diff --git a/Demo.layout b/Demo.layout
index a70778b..354c1bd 100644
--- a/Demo.layout
+++ b/Demo.layout
@@ -2,19 +2,29 @@
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
-
+
diff --git a/src/clock.c b/src/clock.c
index 77cfeb9..0944de5 100644
--- a/src/clock.c
+++ b/src/clock.c
@@ -44,10 +44,132 @@
#define SDMR3_CL2 *(volatile uint8_t *)0xFEC15040 // SDMR2 Address
#define SDMR3_CL3 *(volatile uint8_t *)0xFEC15060 // SDMR2 Address
+
+#include
+/* Arrays of standard and extra timers */
+static tmu_t *TMU = SH7305_TMU.TMU;
+static etmu_t *ETMU = SH7305_ETMU;
+/* TSTR register for standard timers */
+static volatile uint8_t *TSTR = &SH7305_TMU.TSTR;
+
+bool runningTimers[9]; // 9 timers : 3 TMUs + 6 ETMUs
+uint32_t initTimersTCNT[9];
+uint32_t initTimersTCOR[9];
+uint32_t newTimersTCNT[9];
+uint32_t newTimersTCOR[9];
+int initPphi;
+int newPphi;
+
+static int getPphi_sh7305(void)
+{
+ /* The meaning of the PLL setting on SH7305 differs from the
+ documentation of SH7224; the value must not be doubled. */
+ int pll = CPG.FRQCR.STC + 1;
+
+
+ /* The FLL ratio is the value of the setting, halved if SELXM=1 */
+ int fll = CPG.FLLFRQ.FLF;
+ if(CPG.FLLFRQ.SELXM == 1) fll >>= 1;
+
+ /* On SH7724, the divider ratio is given by 1 / (setting + 1), but on
+ the SH7305 it is 1 / (2^setting + 1). */
+
+ int divb = CPG.FRQCR.BFC;
+ int divi = CPG.FRQCR.IFC;
+ int divp = CPG.FRQCR.P1FC;
+
+ /* Deduce the input frequency of divider 1 */
+ int base = 32768;
+ if(CPG.PLLCR.FLLE) base *= fll;
+ if(CPG.PLLCR.PLLE) base *= pll;
+ return (base >> (divp + 1));
+}
+
+//We list all running timers and store this in a table (true/false)
+void listTimerStatus( void )
+{
+ for(int k=0;k<9; k++)
+ {
+ if(k < 3)
+ {
+ tmu_t *T = &TMU[k];
+ runningTimers[k]= (!T->TCR.UNIE && !(*TSTR & (1 << k)));
+ }
+ else
+ {
+ etmu_t *T = &ETMU[k-3];
+ runningTimers[k]= (!T->TCR.UNIE && !T->TSTR);
+ }
+ }
+}
+
+// We get all TCNT and TCOR of currently used timers
+// And store these value into the
+void getInitialTimersParameters( void )
+{
+ for(int k=0;k<9; k++)
+ {
+ if (runningTimers[k]==true)
+ {
+ if(k < 3)
+ {
+ tmu_t *T = &TMU[k];
+ initTimersTCNT[k]= T->TCNT;
+ initTimersTCOR[k]= T->TCOR;
+ }
+ else
+ {
+ etmu_t *T = &ETMU[k-3];
+ initTimersTCNT[k]= T->TCNT;
+ initTimersTCOR[k]= T->TCOR;
+ }
+ }
+ }
+}
+
+//We update the timers with the new TCNT and new TCOR
+void updateNewTimersParameters( void )
+{
+ for(int k=0;k<9; k++)
+ {
+ if (runningTimers[k]==true)
+ {
+ if(k < 3)
+ {
+ tmu_t *T = &TMU[k];
+ T->TCNT = newTimersTCNT[k];
+ T->TCOR = newTimersTCOR[k];
+ }
+ else
+ {
+ etmu_t *T = &ETMU[k-3];
+ T->TCNT = newTimersTCNT[k];
+ T->TCOR = newTimersTCOR[k];
+ }
+ }
+ }
+}
+
+//We compute the new TCNT and new TCOR
+void computeNewTimersParameters( int initPphi_f, int newPphi_f )
+{
+ for(int k=0;k<9; k++)
+ {
+ if (runningTimers[k]==true)
+ {
+ newTimersTCNT[k] = initTimersTCNT[k] * newPphi_f / initPphi_f;
+ newTimersTCOR[k] = initTimersTCOR[k] * newPphi_f / initPphi_f;
+ }
+ }
+}
+
+
static overclock_level current_clock_state = OC_Default;
bool overclock_config_changed = false;
+
+
void SetOCDefault( void )
{
BSC.CS0WCR.WR = WAIT18;
@@ -174,23 +296,22 @@ int clock_overclock( overclock_level level )
{
cpu_atomic_start();
+ listTimerStatus(); // we list the running timers
+ initPphi = getPphi_sh7305(); // we get the current P_Phi_f
+ getInitialTimersParameters(); // we collect the current TCNT and TCOR
+
if (level == OC_Default && current_clock_state!=OC_Default) SetOCDefault(), current_clock_state= OC_Default, overclock_config_changed=true;
if (level == OC_PtuneF2 && current_clock_state!=OC_PtuneF2) SetOCPtuneF2(), current_clock_state= OC_PtuneF2, overclock_config_changed=true;
if (level == OC_PtuneF3 && current_clock_state!=OC_PtuneF3) SetOCPtuneF3(), current_clock_state= OC_PtuneF3, overclock_config_changed=true;
if (level == OC_PtuneF4 && current_clock_state!=OC_PtuneF4) SetOCPtuneF4(), current_clock_state= OC_PtuneF4, overclock_config_changed=true;
if (level == OC_PtuneF5 && current_clock_state!=OC_PtuneF5) SetOCPtuneF5(), current_clock_state= OC_PtuneF5, overclock_config_changed=true;
-/*
- if (overclock_config_changed==true)
- {
- CPG.FRQCR.KICK = 1 ;
- while((CPG.LSTATS & 1)!=0 && count<=1000)
- { count++; }
- }
-*/
+
+ newPphi = getPphi_sh7305(); // we get the new P_Phi_f after OC
+ computeNewTimersParameters( initPphi, newPphi ); // we compute the new TCNT and TCOR as per the new frequency
+ updateNewTimersParameters(); // we adjust the timers accordingly
+
cpu_atomic_end();
-// if (count >= 1000) return -1;
-// else return 1;
return 1;
}
else return 0;
diff --git a/src/clock.h b/src/clock.h
index b7fd398..e0459b4 100644
--- a/src/clock.h
+++ b/src/clock.h
@@ -1,5 +1,10 @@
-#ifndef CLOCK_H
-#define CLOCK_H
+#ifndef OVERCLOCK_H
+#define OVERCLOCK_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
typedef enum
{
@@ -18,4 +23,9 @@ typedef enum
int clock_overclock( overclock_level level );
-#endif // CLOCK_H
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* OVERCLOCK_H */
+