#include "clock.h"
#include <gint/hardware.h>
#include <gint/defs/types.h>
#include <gint/cpu.h>
#include <gint/mpu/bsc.h>
#include <gint/mpu/cpg.h>
#include <gint/clock.h>


#define FLLFRQ_default_CG50	0x00004384
#define FRQCR_default_CG50	0x0F011112

#define CS0BCR_default_CG50	0x36DA0400	// CG50
#define CS2BCR_default_CG50	0x36DA3400	// CG50
#define CS3BCR_default_CG50  0x36DB4400	// CG50
#define CS4BCR_default_CG50  0x36DB0400
#define CS5aBCR_default_CG50 0x17DF0400
#define CS5bBCR_default_CG50 0x17DF0400
#define CS6aBCR_default_CG50 0x34D30200
#define CS6bBCR_default_CG50 0x34D30200

#define CS0WCR_default_CG50	0x000003C0	// CG50
#define CS2WCR_default_CG50	0x000003C0	// CG50
#define CS3WCR_default_CG50  0x000024D1	// CG50
#define CS4WCR_default_CG50  0x00000540
#define CS5aWCR_default_CG50 0x000203C1
#define CS5bWCR_default_CG50 0x000203C1
#define CS6aWCR_default_CG50 0x000302C0
#define CS6bWCR_default_CG50 0x000302C0
//#define SDCR_default    0x00000A08


#define FLLFRQ_default_CG10_20	0x00004384
#define FRQCR_default_CG10_20	0x0F102203

#define CS0BCR_default_CG10_20	0x24920400
#define CS2BCR_default_CG10_20	0x24923400
#define CS3BCR_default_CG10_20  0x24924400
#define CS4BCR_default_CG10_20  0x36DB0400
#define CS5aBCR_default_CG10_20 0x15140400
#define CS5bBCR_default_CG10_20 0x15140400
#define CS6aBCR_default_CG10_20 0x34D30200
#define CS6bBCR_default_CG10_20 0x34D30200

#define CS0WCR_default_CG10_20	0x000001C0
#define CS2WCR_default_CG10_20	0x00000140
#define CS3WCR_default_CG10_20  0x000024D0
#define CS4WCR_default_CG10_20  0x00000540
#define CS5aWCR_default_CG10_20 0x00010240
#define CS5bWCR_default_CG10_20 0x00010240
#define CS6aWCR_default_CG10_20 0x000302C0
#define CS6bWCR_default_CG10_20 0x000302C0



#define PLL_32x 0b011111 //
#define PLL_26x 0b011001 //
#define PLL_16x 0b001111 // default

#define DIV_2 0b0000	// 1/2
#define DIV_4 0b0001	// 1/4
#define DIV_8 0b0010	// 1/8
#define DIV16 0b0011	// 1/16
#define DIV32 0b0100    // 1/32

#define WAIT18 0b1011

#define CPG SH7305_CPG
#define BSC SH7305_BSC
#define SDMR3_CL2    *(volatile uint8_t *)0xFEC15040    // SDMR2    Address
#define SDMR3_CL3    *(volatile uint8_t *)0xFEC15060    // SDMR2    Address


#include <gint/mpu/tmu.h>
#include <gint/timer.h>
/* 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[3];      //  9 timers : 3 TMUs + 6 ETMUs
uint32_t initTimersTCNT[3];
uint32_t initTimersTCOR[3];
uint32_t newTimersTCNT[3];
uint32_t newTimersTCOR[3];
uint32_t initPphi;
uint32_t newPphi;


// Return the value of Pphi_Freq as per the current overclocking configuration
uint32_t getPphi_sh7305(void)
{
    uint32_t fll_freq;
	uint32_t pll_freq;
	uint32_t per_freq;

	fll_freq  = (SH7305_CPG.FLLFRQ.FLF * 32768) / (1 << SH7305_CPG.FLLFRQ.SELXM);
	pll_freq = fll_freq * (SH7305_CPG.FRQCR.STC + 1);
	per_freq = pll_freq / (1 << (SH7305_CPG.FRQCR.P1FC + 1));

	return per_freq;
}

//We list all running timers and store this in a table (true/false)
void listTimerStatus( void )
{
    for(int k=0;k<3; k++)
    {
            tmu_t *T = &TMU[k];
            //runningTimers[k]= (!T->TCR.UNIE && !(*TSTR & (1 << k)));
            runningTimers[k] = (T->TCNT!=0xffffffff || T->TCOR!=0xffffffff);        // as per Lephe's proposal for Libprof compatibility
    }
}

// We get all TCNT and TCOR of currently used timers
// And store these value into the
void getInitialTimersParameters( void )
{
    for(int k=0;k<3; k++)
    {
        if (runningTimers[k]==true)
        {
                tmu_t *T = &TMU[k];
                initTimersTCNT[k]= T->TCNT;
                initTimersTCOR[k]= T->TCOR;
        }
    }
}

static int callback(void)
{
	return TIMER_CONTINUE;
}


//We update the timers with the new TCNT and new TCOR
void updateNewTimersParameters( void )
{
    for(int k=0;k<3; k++)
    {
        if (runningTimers[k]==true)
        {
            timer_pause( k );

                tmu_t *T = &TMU[k];
                T->TCNT = newTimersTCNT[k];
                T->TCOR = newTimersTCOR[k];

            timer_start(k);
        }
    }
}

//We compute the new TCNT and new TCOR
void computeNewTimersParameters( uint32_t initPphi_f, uint32_t newPphi_f )
{
    for(int k=0;k<3; k++)
    {
        if (runningTimers[k]==true)
        {
            newTimersTCNT[k] = (uint32_t) ((uint64_t) initTimersTCNT[k] * (uint64_t) newPphi_f / (uint64_t) initPphi_f);

            if(initTimersTCOR[k] == 0xffffffff)
                newTimersTCOR[k] = 0xffffffff;
            else
                newTimersTCOR[k] = (uint32_t) ((uint64_t) initTimersTCOR[k] * (uint64_t) newPphi_f / (uint64_t) initPphi_f);
        }
    }
}


static overclock_level current_clock_state = -1;
bool overclock_config_changed = false;


void SetOCDefault_CG50( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = FLLFRQ_default_CG50;
    CPG.FRQCR.lword  = FRQCR_default_CG50;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = CS0BCR_default_CG50;
    BSC.CS0WCR.lword  = CS0WCR_default_CG50;
    BSC.CS2BCR.lword  = CS2BCR_default_CG50;
    BSC.CS2WCR.lword  = CS2WCR_default_CG50;
    BSC.CS3BCR.lword  = CS3BCR_default_CG50;
    BSC.CS3WCR.lword  = CS3WCR_default_CG50;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG50;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG50;
}

void SetOCPtuneF2_CG50( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_16x<<24)+(DIV_4<<20)+(DIV_8<<12)+(DIV_8<<8)+DIV_8;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = 0x24920400;
    BSC.CS0WCR.lword  = 0x00000340;
    BSC.CS2BCR.lword  = 0x24923400;
    BSC.CS2WCR.lword  = CS2WCR_default_CG50;
    BSC.CS3BCR.lword  = 0x24924400;
    BSC.CS3WCR.lword  = CS3WCR_default_CG50;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG50;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG50;
}

void SetOCPtuneF3_CG50( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_26x<<24)+(DIV_4<<20)+(DIV_8<<12)+(DIV_8<<8)+DIV_8;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = 0x24920400;
    BSC.CS0WCR.lword  = 0x00000240;
    BSC.CS2BCR.lword  = 0x24923400;
    BSC.CS2WCR.lword  = CS2WCR_default_CG50;
    BSC.CS3BCR.lword  = 0x24924400;
    BSC.CS3WCR.lword  = CS3WCR_default_CG50;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG50;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG50;
}

void SetOCPtuneF4_CG50( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_32x<<24)+(DIV_2<<20)+(DIV_4<<12)+(DIV_8<<8)+DIV16;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)


    BSC.CS0BCR.lword  = 0x24920400;
    BSC.CS0WCR.lword  = 0x000002C0;
    BSC.CS2BCR.lword  = 0x24923400;
    BSC.CS2WCR.lword  = CS2WCR_default_CG50;
    BSC.CS3BCR.lword  = 0x24924400;
    BSC.CS3WCR.lword  = CS3WCR_default_CG50;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG50;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG50;
}

void SetOCPtuneF5_CG50( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_26x<<24)+(DIV_2<<20)+(DIV_4<<12)+(DIV_4<<8)+DIV_8;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = 0x24920400;
    BSC.CS0WCR.lword  = 0x00000440;
    BSC.CS2BCR.lword  = 0x24923400;
    BSC.CS2WCR.lword  = CS2WCR_default_CG50;
    BSC.CS3BCR.lword  = 0x24924400;
    BSC.CS3WCR.lword  = CS3WCR_default_CG50;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG50;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG50;
}


void SetOCDefault_CG10_20( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = FLLFRQ_default_CG10_20;
    CPG.FRQCR.lword  = FRQCR_default_CG10_20;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = CS0BCR_default_CG10_20;
    BSC.CS0WCR.lword  = CS0WCR_default_CG10_20;
    BSC.CS2BCR.lword  = CS2BCR_default_CG10_20;
    BSC.CS2WCR.lword  = CS2WCR_default_CG10_20;
    BSC.CS3BCR.lword  = CS3BCR_default_CG10_20;
    BSC.CS3WCR.lword  = CS3WCR_default_CG10_20;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG10_20;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG10_20;
}

void SetOCPtuneF2_CG10_20( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_32x<<24)+(DIV_8<<20)+(DIV16<<12)+(DIV16<<8)+DIV32;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = 0x04900400;
    BSC.CS0WCR.lword  = 0x00000140;
    BSC.CS2BCR.lword  = 0x04903400;
    BSC.CS2WCR.lword  = 0x000100C0;
    BSC.CS3BCR.lword  = CS3BCR_default_CG10_20;
    BSC.CS3WCR.lword  = CS3WCR_default_CG10_20;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG10_20;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG10_20;
}

void SetOCPtuneF3_CG10_20( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_32x<<24)+(DIV_4<<20)+(DIV_8<<12)+(DIV_8<<8)+DIV32;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = 0x24900400;
    BSC.CS0WCR.lword  = 0x000002C0;
    BSC.CS2BCR.lword  = 0x04903400;
    BSC.CS2WCR.lword  = 0x000201C0;
    BSC.CS3BCR.lword  = CS3BCR_default_CG10_20;
    BSC.CS3WCR.lword  = CS3WCR_default_CG10_20;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG10_20;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG10_20;
}

void SetOCPtuneF4_CG10_20( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_32x<<24)+(DIV_4<<20)+(DIV_4<<12)+(DIV_4<<8)+DIV32;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = 0x44900400;
    BSC.CS0WCR.lword  = 0x00000440;
    BSC.CS2BCR.lword  = 0x04903400;
    BSC.CS2WCR.lword  = 0x00040340;
    BSC.CS3BCR.lword  = CS3BCR_default_CG10_20;
    BSC.CS3WCR.lword  = CS3WCR_default_CG10_20;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG10_20;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG10_20;
}

void SetOCPtuneF5_CG10_20( void )
{
    BSC.CS0WCR.WR = WAIT18;

    CPG.FLLFRQ.lword  = 0x00004000+900;
    CPG.FRQCR.lword  = (PLL_26x<<24)+(DIV_2<<20)+(DIV_4<<12)+(DIV_4<<8)+DIV16;

    CPG.FRQCR.KICK = 1 ;
    while((CPG.LSTATS & 1)!=0)

    BSC.CS0BCR.lword  = 0x34900400;
    BSC.CS0WCR.lword  = 0x000003C0;
    BSC.CS2BCR.lword  = 0x04903400;
    BSC.CS2WCR.lword  = 0x000402C0;
    BSC.CS3BCR.lword  = CS3BCR_default_CG10_20;
    BSC.CS3WCR.lword  = CS3WCR_default_CG10_20;

    if ( BSC.CS3WCR.A3CL == 1 ) SDMR3_CL2 = 0; else SDMR3_CL3 = 0;

    BSC.CS5ABCR.lword = CS5aBCR_default_CG10_20;
    BSC.CS5AWCR.lword = CS5aWCR_default_CG10_20;
}



overclock_level overclock_detect( void )
{
    if(gint[HWCALC] == HWCALC_FXCG50)
    {

        // OC_Default configuration
        if (CPG.FLLFRQ.lword  == FLLFRQ_default_CG50 &&    CPG.FRQCR.lword  == FRQCR_default_CG50 &&    BSC.CS0BCR.lword  == CS0BCR_default_CG50 &&    BSC.CS0WCR.lword  == CS0WCR_default_CG50 &&
        BSC.CS2BCR.lword  == CS2BCR_default_CG50 &&    BSC.CS2WCR.lword  == CS2WCR_default_CG50 &&    BSC.CS3BCR.lword  == CS3BCR_default_CG50 &&    BSC.CS3WCR.lword  == CS3WCR_default_CG50 &&
        BSC.CS5ABCR.lword == CS5aBCR_default_CG50 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG50 ) return OC_Default;

        // OC_PtuneF2 configuration
        else if (CPG.FLLFRQ.lword  == 0x00004000+900 &&  CPG.FRQCR.lword  == (PLL_16x<<24)+(DIV_4<<20)+(DIV_8<<12)+(DIV_8<<8)+DIV_8 && BSC.CS0BCR.lword  == 0x24920400 &&
        BSC.CS0WCR.lword  == 0x00000340 &&    BSC.CS2BCR.lword  == 0x24923400 &&    BSC.CS2WCR.lword  == CS2WCR_default_CG50 &&    BSC.CS3BCR.lword  == 0x24924400 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG50 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG50 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG50 ) return OC_PtuneF2;

        // OC_PtuneF3 configuration
        else if (CPG.FLLFRQ.lword  == 0x00004000+900 &&     CPG.FRQCR.lword  == (PLL_26x<<24)+(DIV_4<<20)+(DIV_8<<12)+(DIV_8<<8)+DIV_8 &&     BSC.CS0BCR.lword  == 0x24920400 &&
        BSC.CS0WCR.lword  == 0x00000240 &&    BSC.CS2BCR.lword  == 0x24923400 &&    BSC.CS2WCR.lword  == CS2WCR_default_CG50 &&    BSC.CS3BCR.lword  == 0x24924400 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG50 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG50 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG50) return OC_PtuneF3;

        // OC_PtuneF4 configuration
        else if (CPG.FLLFRQ.lword  == 0x00004000+900 &&     CPG.FRQCR.lword  == (PLL_32x<<24)+(DIV_2<<20)+(DIV_4<<12)+(DIV_8<<8)+DIV16 &&    BSC.CS0BCR.lword  == 0x24920400 &&
        BSC.CS0WCR.lword  == 0x000002C0 &&    BSC.CS2BCR.lword  == 0x24923400 &&    BSC.CS2WCR.lword  == CS2WCR_default_CG50 &&    BSC.CS3BCR.lword  == 0x24924400 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG50 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG50 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG50) return OC_PtuneF4;

        // OC_PtuneF5 configuration
        else if (CPG.FLLFRQ.lword == 0x00004000+900 &&    CPG.FRQCR.lword  == (PLL_26x<<24)+(DIV_2<<20)+(DIV_4<<12)+(DIV_4<<8)+DIV_8 &&    BSC.CS0BCR.lword  == 0x24920400 &&
        BSC.CS0WCR.lword  == 0x00000440 &&    BSC.CS2BCR.lword  == 0x24923400 &&    BSC.CS2WCR.lword  == CS2WCR_default_CG50 &&    BSC.CS3BCR.lword  == 0x24924400 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG50 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG50 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG50) return OC_PtuneF5;

        // Not recognized
        else return OC_Undefined;

    }
    else if(gint[HWCALC] == HWCALC_PRIZM)
    {

        // OC_Default configuration
        if (CPG.FLLFRQ.lword  == FLLFRQ_default_CG10_20 &&    CPG.FRQCR.lword  == FRQCR_default_CG10_20 &&    BSC.CS0BCR.lword  == CS0BCR_default_CG10_20 &&    BSC.CS0WCR.lword  == CS0WCR_default_CG10_20 &&
        BSC.CS2BCR.lword  == CS2BCR_default_CG10_20 &&    BSC.CS2WCR.lword  == CS2WCR_default_CG10_20 &&    BSC.CS3BCR.lword  == CS3BCR_default_CG10_20 &&    BSC.CS3WCR.lword  == CS3WCR_default_CG10_20 &&
        BSC.CS5ABCR.lword == CS5aBCR_default_CG10_20 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG10_20  ) return OC_Default;

        // OC_PtuneF2 configuration
        else if (CPG.FLLFRQ.lword  == 0x00004000+900 &&     CPG.FRQCR.lword  == (PLL_32x<<24)+(DIV_8<<20)+(DIV16<<12)+(DIV16<<8)+DIV32 &&    BSC.CS0BCR.lword  == 0x04900400 &&
        BSC.CS0WCR.lword  == 0x00000140 &&    BSC.CS2BCR.lword  == 0x04903400 &&    BSC.CS2WCR.lword  == 0x000100C0 &&    BSC.CS3BCR.lword  == CS3BCR_default_CG10_20 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG10_20 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG10_20 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG10_20 ) return OC_PtuneF2;

        // OC_PtuneF3 configuration
        else if (CPG.FLLFRQ.lword  == 0x00004000+900 &&     CPG.FRQCR.lword  == (PLL_32x<<24)+(DIV_4<<20)+(DIV_8<<12)+(DIV_8<<8)+DIV32 &&    BSC.CS0BCR.lword  == 0x24900400 &&
        BSC.CS0WCR.lword  == 0x000002C0 &&    BSC.CS2BCR.lword  == 0x04903400 &&    BSC.CS2WCR.lword  == 0x000201C0 &&    BSC.CS3BCR.lword  == CS3BCR_default_CG10_20 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG10_20 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG10_20 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG10_20 ) return OC_PtuneF3;

        // OC_PtuneF4 configuration
        else if (CPG.FLLFRQ.lword  == 0x00004000+900 &&    CPG.FRQCR.lword  == (PLL_32x<<24)+(DIV_4<<20)+(DIV_4<<12)+(DIV_4<<8)+DIV32 &&    BSC.CS0BCR.lword  == 0x44900400 &&
        BSC.CS0WCR.lword  == 0x00000440 &&    BSC.CS2BCR.lword  == 0x04903400 &&    BSC.CS2WCR.lword  == 0x00040340 &&    BSC.CS3BCR.lword  == CS3BCR_default_CG10_20 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG10_20 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG10_20 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG10_20 ) return OC_PtuneF4;

        // OC_PtuneF5 configuration
        else if (CPG.FLLFRQ.lword  == 0x00004000+900 &&    CPG.FRQCR.lword  == (PLL_26x<<24)+(DIV_2<<20)+(DIV_4<<12)+(DIV_4<<8)+DIV16 &&    BSC.CS0BCR.lword  == 0x34900400 &&
        BSC.CS0WCR.lword  == 0x000003C0 &&    BSC.CS2BCR.lword  == 0x04903400 &&    BSC.CS2WCR.lword  == 0x000402C0 &&    BSC.CS3BCR.lword  == CS3BCR_default_CG10_20 &&
        BSC.CS3WCR.lword  == CS3WCR_default_CG10_20 &&    BSC.CS5ABCR.lword == CS5aBCR_default_CG10_20 &&    BSC.CS5AWCR.lword == CS5aWCR_default_CG10_20) return OC_PtuneF5;

        // Not recognized
        else return OC_Undefined;
    }
}



// return 0 if no need to change
// return 1 if successful change
// return -1 on error

int clock_overclock( overclock_level level )
{
    uint32_t count=0;

    if (level==OC_Undefined) return 0;

    if(gint[HWCALC] == HWCALC_FXCG50 && current_clock_state!=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_CG50(), current_clock_state= OC_Default, overclock_config_changed=true;
        if (level == OC_PtuneF2 && current_clock_state!=OC_PtuneF2)    SetOCPtuneF2_CG50(), current_clock_state= OC_PtuneF2, overclock_config_changed=true;
        if (level == OC_PtuneF3 && current_clock_state!=OC_PtuneF3)    SetOCPtuneF3_CG50(), current_clock_state= OC_PtuneF3, overclock_config_changed=true;
        if (level == OC_PtuneF4 && current_clock_state!=OC_PtuneF4)    SetOCPtuneF4_CG50(), current_clock_state= OC_PtuneF4, overclock_config_changed=true;
        if (level == OC_PtuneF5 && current_clock_state!=OC_PtuneF5)    SetOCPtuneF5_CG50(), current_clock_state= OC_PtuneF5, overclock_config_changed=true;

        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();

        cpg_compute_freq();

        return 1;
    }
    else if(gint[HWCALC] == HWCALC_PRIZM && current_clock_state!=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_CG10_20(), current_clock_state= OC_Default, overclock_config_changed=true;
        if (level == OC_PtuneF2 && current_clock_state!=OC_PtuneF2)    SetOCPtuneF2_CG10_20(), current_clock_state= OC_PtuneF2, overclock_config_changed=true;
        if (level == OC_PtuneF3 && current_clock_state!=OC_PtuneF3)    SetOCPtuneF3_CG10_20(), current_clock_state= OC_PtuneF3, overclock_config_changed=true;
        if (level == OC_PtuneF4 && current_clock_state!=OC_PtuneF4)    SetOCPtuneF4_CG10_20(), current_clock_state= OC_PtuneF4, overclock_config_changed=true;
        if (level == OC_PtuneF5 && current_clock_state!=OC_PtuneF5)    SetOCPtuneF5_CG10_20(), current_clock_state= OC_PtuneF5, overclock_config_changed=true;

        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();

        cpg_compute_freq();

        return 1;
    }



    else return 0;
}