#define SDR_TIMING0  0x1020 
#define SDR_TIMING1  0x1021 
#define SDR_CONFIG0  0x1008 
#define SDR_CONFIG1  0x1009 
#define RESET_CONTRL 0x1C05 

#define IDLE_CTRL0  0x1c02
#define IDLE_CTRL1  0x1c03

#define CLKCFGMSW   0x1c1f
#define PLL_CNTL1   0x1c20
#define PLL_CNTL2   0x1c21
#define PLL_CNTL3   0x1c22
#define PLL_CNTL4   0x1c23

#define PSRCR       0x1c04
#define PRCR        0x1c05

#define RTC_OSC     0x192C

#define ECDR					 0x1C26
#define CCR1				     0x1C1E
#define EMIF_CNTL            	 0x1C33
#define EMIF_ASYNC_WAIT_0        0x1004
#define EMIF_ASYNC_WAIT_1        0x1005
#define EMIF_ASYNC1_CONFIG_0     0x1010
#define EMIF_ASYNC1_CONFIG_1     0x1011
#define EMIF_ASYNC2_CONFIG_0     0x1014
#define EMIF_ASYNC2_CONFIG_1     0x1015
#define EMIF_ASYNC3_CONFIG_0     0x1018
#define EMIF_ASYNC3_CONFIG_1     0x1019
#define EMIF_ASYNC4_CONFIG_0     0x101C
#define EMIF_ASYNC4_CONFIG_1     0x101D
#define EMIF_NAND_CTRL           0x1060
#define EMIF_NAND_STATUS         0x1064
#define EMIF_PAGEMODCTRL0		 0x1068
#define EMIF_PAGEMODCTRL1		 0x1069


/* The Startup() function is executed when the GEL file is loaded. */
StartUp()
{
    C5515_MapInit();
}

/* The OnTargetConnect() function is executed when the target is connected. */
OnTargetConnect()
{
    GEL_Reset();
    C5515_MapInit();
    ProgramPLL_12p288MHz_clksel0();
	//ProgramPLL_60MHz_clksel0();
	//ProgramPLL_100MHz_clksel0();
	

    GEL_TextOut("Target Connection Complete.\n");
}

menuitem "C5515EVM_Configuration";
hotmenu CPU_Reset()
{
    GEL_Reset();
    GEL_TextOut("CPU Reset Complete.\n");
}

hotmenu C5515EVM_Init()
{
    GEL_Reset();
    C5515_MapInit();
    GEL_TextOut("C5505EVM Initialization Complete.\n");
}

hotmenu SetPLL_1MHz() {
    ProgramPLL_1MHz_clksel0();
}

hotmenu SetPLL_2MHz() {
    ProgramPLL_2MHz_clksel0();
}

hotmenu SetPLL_3MHz() {
    ProgramPLL_3MHz_clksel0();
}

hotmenu SetPLL_4MHz() {
    ProgramPLL_4MHz_clksel0();
}

hotmenu SetPLL_5MHz() {
    ProgramPLL_5MHz_clksel0();
}

hotmenu SetPLL_6MHz() {
    ProgramPLL_6MHz_clksel0();
}

hotmenu SetPLL_7MHz() {
    ProgramPLL_7MHz_clksel0();
}

hotmenu SetPLL_8MHz() {
    ProgramPLL_8MHz_clksel0();
}

hotmenu SetPLL_9MHz() {
    ProgramPLL_9MHz_clksel0();
}

hotmenu SetPLL_10MHz() {
    ProgramPLL_10MHz_clksel0();
}

hotmenu SetPLL_12p288MHz() {
    ProgramPLL_12p288MHz_clksel0();
}

hotmenu SetPLL_20MHz() {
    ProgramPLL_20MHz_clksel0();
}

hotmenu SetPLL_30MHz() {
    ProgramPLL_30MHz_clksel0();
}

hotmenu SetPLL_40MHz() {
    ProgramPLL_40MHz_clksel0();
}

hotmenu SetPLL_50MHz() {
    ProgramPLL_50MHz_clksel0();
}

hotmenu SetPLL_60MHz() {
    ProgramPLL_60MHz_clksel0();
}

hotmenu SetPLL_70MHz() {
    ProgramPLL_70MHz_clksel0();
}

hotmenu SetPLL_75MHz() {
    ProgramPLL_75MHz_clksel0();
}

hotmenu SetPLL_80MHz() {
    ProgramPLL_80MHz_clksel0();
}

hotmenu SetPLL_90MHz() {
    ProgramPLL_90MHz_clksel0();
}

hotmenu SetPLL_100MHz() {
    ProgramPLL_100MHz_clksel0();
}

hotmenu SetPLL_120MHz() {
    ProgramPLL_120MHz_clksel0();
}

hotmenu SetPLL_130MHz() {
    ProgramPLL_130MHz_clksel0();
}

hotmenu SetPLL_60MHz_From_12_288MHz() {
    ProgramPLL_60MHz_clksel1();
}

hotmenu SetPLL_60MHz_From_12_288MHz_RTCOff() {
    ProgramPLL_60MHz_clksel1_RTCOFF();
}

hotmenu SetPLL_100MHz_From_12MHz() {
    ProgramPLL_100MHz_clksel1();
}

hotmenu SetPLL_120MHz_From_12MHz() {
    ProgramPLL_120MHz_clksel1();
}

hotmenu SetPLL_130MHz_From_12MHz() {
    ProgramPLL_130MHz_clksel1();
}

hotmenu SetPLL_140MHz_From_12MHz() {
    ProgramPLL_140MHz_clksel1();
}

hotmenu SetPLL_150MHz_From_12MHz() {
    ProgramPLL_150MHz_clksel1();
}

hotmenu Peripheral_Reset() {
	Reset_Peripheral();
}

/*
	*(short *)CCR1@IO = 0x0001 // enable SDRAM clock
	(short *)ECDR@IO = 0 // full rate
	(short *)EMIF_CNTL@IO = 0 // word access
    *(short *)SDR_TIMING0@IO = 0x5810;  0x3510
    *(short *)SDR_TIMING1@IO = 0x4221;  /*0x4225
    *(short *)SDR_CONFIG0@IO = 0x4520;	/*0x4720
    *(short *)SDR_CONFIG1@IO = 0x0031;  /*0x0031
*/

menuitem "SDRAM_Configuration";
hotmenu SDRAM_INIT() {
	Reset_Peripheral();
	*(short *)CCR1@IO = 0x0001;  // enable SDRAM clock
	*(short *)ECDR@IO = 0;		// full rate
	*(short *)EMIF_CNTL@IO = 0;	 // word access
    *(short *)SDR_TIMING0@IO = 0x4710; 
    *(short *)SDR_TIMING1@IO = 0x3911;  
    *(short *)SDR_CONFIG0@IO = 0x4520;	
    *(short *)SDR_CONFIG1@IO = 0x0031;
//    *(short *)SDR_TIMING0@IO = 0x5810; 
//    *(short *)SDR_TIMING1@IO = 0x4221;  
      
    GEL_TextOut("SDRAM Initilization Complete.\n");
}

menuitem "EMIF_Configuration";
hotmenu EMIF_3_3_INIT() {
    /* Config EMIF - System Control Regsiter, 16bit mode */
    *(short *)EMIF_CNTL@IO =0;

    /*for SRAM in memory card (Async_CE1)*/
    /* Config EMIF - ASYNC Regsiters */
    *(short *)EMIF_ASYNC_WAIT_0@IO =0x0080;
    *(short *)EMIF_ASYNC_WAIT_1@IO =0x00E4;

	/* Configure as 16-bit data bus */    
    /* Async4 ==> Async_CE1 (SRAM)*/	
    *(short *)EMIF_ASYNC4_CONFIG_0@IO =0x40AD;
    *(short *)EMIF_ASYNC4_CONFIG_1@IO =0x0020;

    /* Async3 ==> Async_CE0(Flash)*/
    *(short *)EMIF_ASYNC3_CONFIG_0@IO =0xFFFD;
    *(short *)EMIF_ASYNC3_CONFIG_1@IO =0x3FFF;

    *(short *)EMIF_ASYNC2_CONFIG_0@IO =0xFFFD;
    *(short *)EMIF_ASYNC2_CONFIG_1@IO =0x3FFF;

	*(short *)EMIF_ASYNC1_CONFIG_0@IO =0xFFFD;
    *(short *)EMIF_ASYNC1_CONFIG_1@IO =0x3FFF;


	/*do not assign Async_CE0 and Async_CE1 for NAND*/
    *(short *)EMIF_NAND_CTRL@IO =0x3101;

    /* Turn off page mode for all Chip Selects*/
    *(short *)EMIF_PAGEMODCTRL0@IO =0xFCFC;
    *(short *)EMIF_PAGEMODCTRL1@IO =0xFCFC;
    GEL_TextOut("EMIF Initilization for 3.3V card is complete.\n");
}


// ***************************************************************************

/* Memory map based on MP/MC value=0 (BOOTM[2:0]=0).    */
C5515_MapInit() {
    GEL_MapOn();
    GEL_MapReset();

    /*Program Space*/

    /* DARAM */
    GEL_MapAdd(0x0000C0,0,0x001F40,1,1);    /* DARAM0 */
    GEL_MapAdd(0x002000,0,0x002000,1,1);    /* DARAM1 */
    GEL_MapAdd(0x004000,0,0x002000,1,1);    /* DARAM2 */
    GEL_MapAdd(0x006000,0,0x002000,1,1);    /* DARAM3 */
    GEL_MapAdd(0x008000,0,0x002000,1,1);    /* DARAM4 */
    GEL_MapAdd(0x00A000,0,0x002000,1,1);    /* DARAM5 */
    GEL_MapAdd(0x00C000,0,0x002000,1,1);    /* DARAM6 */
    GEL_MapAdd(0x00E000,0,0x002000,1,1);    /* DARAM7 */

    /* SARAM */
    GEL_MapAdd(0x010000,0,0x002000,1,1);    /* SARAM0 */
    GEL_MapAdd(0x012000,0,0x002000,1,1);    /* SARAM1 */
    GEL_MapAdd(0x014000,0,0x002000,1,1);    /* SARAM2 */
    GEL_MapAdd(0x016000,0,0x002000,1,1);    /* SARAM3 */
    GEL_MapAdd(0x018000,0,0x002000,1,1);    /* SARAM4 */
    GEL_MapAdd(0x01A000,0,0x002000,1,1);    /* SARAM5 */
    GEL_MapAdd(0x01C000,0,0x002000,1,1);    /* SARAM6 */
    GEL_MapAdd(0x01E000,0,0x002000,1,1);    /* SARAM7 */
    GEL_MapAdd(0x020000,0,0x002000,1,1);    /* SARAM8 */
    GEL_MapAdd(0x022000,0,0x002000,1,1);    /* SARAM9 */
    GEL_MapAdd(0x024000,0,0x002000,1,1);    /* SARAM10 */
    GEL_MapAdd(0x026000,0,0x002000,1,1);    /* SARAM11 */
    GEL_MapAdd(0x028000,0,0x002000,1,1);    /* SARAM12 */
    GEL_MapAdd(0x02A000,0,0x002000,1,1);    /* SARAM13 */
    GEL_MapAdd(0x02C000,0,0x002000,1,1);    /* SARAM14 */
    GEL_MapAdd(0x02E000,0,0x002000,1,1);    /* SARAM15 */
    GEL_MapAdd(0x030000,0,0x002000,1,1);    /* SARAM16 */
    GEL_MapAdd(0x032000,0,0x002000,1,1);    /* SARAM17 */
    GEL_MapAdd(0x034000,0,0x002000,1,1);    /* SARAM18 */
    GEL_MapAdd(0x036000,0,0x002000,1,1);    /* SARAM19 */
    GEL_MapAdd(0x038000,0,0x002000,1,1);    /* SARAM20 */
    GEL_MapAdd(0x03A000,0,0x002000,1,1);    /* SARAM21 */
    GEL_MapAdd(0x03C000,0,0x002000,1,1);    /* SARAM22 */
    GEL_MapAdd(0x03E000,0,0x002000,1,1);    /* SARAM23 */
    GEL_MapAdd(0x040000,0,0x002000,1,1);    /* SARAM24 */
    GEL_MapAdd(0x042000,0,0x002000,1,1);    /* SARAM25 */
    GEL_MapAdd(0x044000,0,0x002000,1,1);    /* SARAM26 */
    GEL_MapAdd(0x046000,0,0x002000,1,1);    /* SARAM27 */
    GEL_MapAdd(0x048000,0,0x002000,1,1);    /* SARAM28 */
    GEL_MapAdd(0x04A000,0,0x002000,1,1);    /* SARAM29 */
    GEL_MapAdd(0x04C000,0,0x002000,1,1);    /* SARAM30 */
    GEL_MapAdd(0x04E000,0,0x002000,1,1);    /* SARAM31 */

    /* External-Memory */
    GEL_MapAdd(0x050000,0,0x7B0000,1,1);    /* External-SDRAM */
    GEL_MapAdd(0x800000,0,0x400000,1,1);    /* External-Async */
    GEL_MapAdd(0xC00000,0,0x200000,1,1);    /* External-Async */
    GEL_MapAdd(0xE00000,0,0x100000,1,1);    /* External-Async */
    GEL_MapAdd(0xF00000,0,0x0E0000,1,1);    /* External-Async */

    /* ROM */
    GEL_MapAdd(0xFE0000,0,0x008000,1,0);    /* SAROM0 */
    GEL_MapAdd(0xFE8000,0,0x008000,1,0);    /* SAROM1 */
    GEL_MapAdd(0xFF0000,0,0x008000,1,0);    /* SAROM2 */
    GEL_MapAdd(0xFF8000,0,0x008000,1,0);    /* SAROM3 */

    /* Data Space */

    /* DARAM */
    GEL_MapAdd(0x000000,1,0x000060,1,1);    /* MMRs */
    GEL_MapAdd(0x000060,1,0x000FA0,1,1);    /* DARAM0 */
    GEL_MapAdd(0x001000,1,0x001000,1,1);    /* DARAM1 */
    GEL_MapAdd(0x002000,1,0x001000,1,1);    /* DARAM2 */
    GEL_MapAdd(0x003000,1,0x001000,1,1);    /* DARAM3 */
    GEL_MapAdd(0x004000,1,0x001000,1,1);    /* DARAM4 */
    GEL_MapAdd(0x005000,1,0x001000,1,1);    /* DARAM5 */
    GEL_MapAdd(0x006000,1,0x001000,1,1);    /* DARAM6 */
    GEL_MapAdd(0x007000,1,0x001000,1,1);    /* DARAM7 */

    /* SARAM */
    GEL_MapAdd(0x008000,1,0x001000,1,1);    /* SARAM0 */
    GEL_MapAdd(0x009000,1,0x001000,1,1);    /* SARAM1 */
    GEL_MapAdd(0x00A000,1,0x001000,1,1);    /* SARAM2 */
    GEL_MapAdd(0x00B000,1,0x001000,1,1);    /* SARAM3 */
    GEL_MapAdd(0x00C000,1,0x001000,1,1);    /* SARAM4 */
    GEL_MapAdd(0x00D000,1,0x001000,1,1);    /* SARAM5 */
    GEL_MapAdd(0x00E000,1,0x001000,1,1);    /* SARAM6 */
    GEL_MapAdd(0x00F000,1,0x001000,1,1);    /* SARAM7 */
    GEL_MapAdd(0x010000,1,0x001000,1,1);    /* SARAM8 */
    GEL_MapAdd(0x011000,1,0x001000,1,1);    /* SARAM9 */
    GEL_MapAdd(0x012000,1,0x001000,1,1);    /* SARAM10 */
    GEL_MapAdd(0x013000,1,0x001000,1,1);    /* SARAM11 */
    GEL_MapAdd(0x014000,1,0x001000,1,1);    /* SARAM12 */
    GEL_MapAdd(0x015000,1,0x001000,1,1);    /* SARAM13 */
    GEL_MapAdd(0x016000,1,0x001000,1,1);    /* SARAM14 */
    GEL_MapAdd(0x017000,1,0x001000,1,1);    /* SARAM15 */
    GEL_MapAdd(0x018000,1,0x001000,1,1);    /* SARAM16 */
    GEL_MapAdd(0x019000,1,0x001000,1,1);    /* SARAM17 */
    GEL_MapAdd(0x01A000,1,0x001000,1,1);    /* SARAM18 */
    GEL_MapAdd(0x01B000,1,0x001000,1,1);    /* SARAM19 */
    GEL_MapAdd(0x01C000,1,0x001000,1,1);    /* SARAM20 */
    GEL_MapAdd(0x01D000,1,0x001000,1,1);    /* SARAM21 */
    GEL_MapAdd(0x01E000,1,0x001000,1,1);    /* SARAM22 */
    GEL_MapAdd(0x01F000,1,0x001000,1,1);    /* SARAM23 */
    GEL_MapAdd(0x020000,1,0x001000,1,1);    /* SARAM24 */
    GEL_MapAdd(0x021000,1,0x001000,1,1);    /* SARAM25 */
    GEL_MapAdd(0x022000,1,0x001000,1,1);    /* SARAM26 */
    GEL_MapAdd(0x023000,1,0x001000,1,1);    /* SARAM27 */
    GEL_MapAdd(0x024000,1,0x001000,1,1);    /* SARAM28 */
    GEL_MapAdd(0x025000,1,0x001000,1,1);    /* SARAM29 */
    GEL_MapAdd(0x026000,1,0x001000,1,1);    /* SARAM30 */
    GEL_MapAdd(0x027000,1,0x001000,1,1);    /* SARAM31 */

    /* External-Memory */
    GEL_MapAdd(0x028000,1,0x3D8000,1,1);    /* External-SDRAM */
    GEL_MapAdd(0x400000,1,0x200000,1,1);    /* External-Async */
    GEL_MapAdd(0x600000,1,0x100000,1,1);    /* External-Async */
    GEL_MapAdd(0x700000,1,0x080000,1,1);    /* External-Async */
    GEL_MapAdd(0x780000,1,0x070000,1,1);    /* External-Async */

    /* ROM */
    GEL_MapAdd(0x7F0000,1,0x004000,1,0);    /* SAROM0 */
    GEL_MapAdd(0x7F4000,1,0x004000,1,0);    /* SAROM1 */
    GEL_MapAdd(0x7F8000,1,0x004000,1,0);    /* SAROM2 */
    GEL_MapAdd(0x7FC000,1,0x004000,1,0);    /* SAROM3 */

    /* IO Space */
    GEL_MapAdd(0x0000,2,0xFFFF,1,1);        /* XPORT */
}

Reset_Peripheral() {
    int i;

    *(short *)PSRCR@IO = 0x0002;
    *(short *)PRCR@IO  = 0x00FF;

    for(i=0;i<0xff;i++);

    GEL_TextOut("Reset Peripherals is complete.\n");
}


ProgramPLL_1MHz_clksel0() {
    GEL_TextOut("Configure PLL (1.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x023B;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (1.00 MHz).\n");
}

ProgramPLL_2MHz_clksel0() {
    GEL_TextOut("Configure PLL (2.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x021D;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (2.00 MHz).\n");
}

ProgramPLL_3MHz_clksel0() {
    GEL_TextOut("Configure PLL (3.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0213;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (3.00 MHz).\n");
}

ProgramPLL_4MHz_clksel0() {
    GEL_TextOut("Configure PLL (4.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x020E;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (4.00 MHz).\n");
}

ProgramPLL_5MHz_clksel0() {
    GEL_TextOut("Configure PLL (5.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x020B;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (5.00 MHz).\n");
}

ProgramPLL_6MHz_clksel0() {
    GEL_TextOut("Configure PLL (6.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0209;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (6.00 MHz).\n");
}

ProgramPLL_7MHz_clksel0() {
    GEL_TextOut("Configure PLL (7.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0209;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8855;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (7.00 MHz).\n");
}

ProgramPLL_8MHz_clksel0() {
    GEL_TextOut("Configure PLL (8.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0209;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8985;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (8.00 MHz).\n");
}

ProgramPLL_9MHz_clksel0() {
    GEL_TextOut("Configure PLL (9.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0209;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8AB5;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (9.00 MHz).\n");
}

ProgramPLL_10MHz_clksel0() {
    GEL_TextOut("Configure PLL (10.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0205;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (10.00 MHz).\n");
}

ProgramPLL_12p288MHz_clksel0() {
    GEL_TextOut("Configure PLL (12.288 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0207;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8BB4;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (12.288 MHz).\n");
}

ProgramPLL_20MHz_clksel0() {
    GEL_TextOut("Configure PLL (20.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0202;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (20.00 MHz).\n");
}

ProgramPLL_30MHz_clksel0() {
    GEL_TextOut("Configure PLL (30.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0201;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (30.00 MHz).\n");
}


ProgramPLL_40MHz_clksel0() {
    GEL_TextOut("Configure PLL (40.04 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0201;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8986;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (40.04 MHz).\n");
}

ProgramPLL_50MHz_clksel0() {
    GEL_TextOut("Configure PLL (50.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0201;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8BE8;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (50.00 MHz).\n");
}

ProgramPLL_60MHz_clksel0() {
    GEL_TextOut("Configure PLL (60.00 MHz).\n");
    // Enable clocks to all peripherals
    //*(short *)IDLE_CTRL0@IO = 0x0;
    //*(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0000;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8724;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (60.00 MHz).\n");
}
ProgramPLL_70MHz_clksel0() {
    GEL_TextOut("Configure PLL (70.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8855;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (70.00 MHz).\n");
}

ProgramPLL_75MHz_clksel0() {
    GEL_TextOut("Configure PLL (75.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x88ED;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (75.00 MHz).\n");
}

ProgramPLL_80MHz_clksel0() {
    GEL_TextOut("Configure PLL (80.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8985;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (80.00 MHz).\n");
}

ProgramPLL_90MHz_clksel0() {
    GEL_TextOut("Configure PLL (90.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8AB5;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (90.00 MHz).\n");
}

ProgramPLL_100MHz_clksel0() {
    GEL_TextOut("Configure PLL (100.00 MHz).\n");
    // Enable clocks to all peripherals
    //*(short *)IDLE_CTRL0@IO = 0x0;
    //*(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0000;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8BE8;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (100.00 MHz).\n");
}

ProgramPLL_120MHz_clksel0() {
    GEL_TextOut("Configure PLL (120.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8E48;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (120.00 MHz).\n");
}

ProgramPLL_130MHz_clksel0() {
    GEL_TextOut("Configure PLL (130.00 MHz).\n");
    // Enable clocks to all peripherals
    //*(short *)IDLE_CTRL0@IO = 0x0;
    //*(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x8000;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8F7B;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (130.00 MHz).\n");
}


ProgramPLL_60MHz_clksel1() {
    GEL_TextOut("Configure PLL (60.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x4044;
    *(short *)PLL_CNTL4@IO = 0x0000;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8058;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (60.00 MHz).\n");
}

ProgramPLL_60MHz_clksel1_RTCOFF() {
    GEL_TextOut("Configure PLL (60.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x4044;
    *(short *)PLL_CNTL4@IO = 0x0000;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8058;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    // disable RTC OSC
	*(short *)RTC_OSC@IO = 0x0018;

    GEL_TextOut("PLL Init Done (60.00 MHz).\n");
}

ProgramPLL_100MHz_clksel1() {
    GEL_TextOut("Configure PLL (100.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x616E;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8BE8;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (100.00 MHz).\n");
}


ProgramPLL_120MHz_clksel1() {
    GEL_TextOut("Configure PLL (120.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x616E;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8E48;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (120.00 MHz).\n");
}


ProgramPLL_130MHz_clksel1() {
    GEL_TextOut("Configure PLL (130.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x616E;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8F7B;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (130.00 MHz).\n");
}

ProgramPLL_140MHz_clksel1() {
    GEL_TextOut("Configure PLL (140.00 MHz).\n");
    // Enable clocks to all peripherals
    *(short *)IDLE_CTRL0@IO = 0x0;
    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x6154;
    *(short *)PLL_CNTL4@IO = 0x0001;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8F7B;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (140.00 MHz).\n");
}

ProgramPLL_150MHz_clksel1() {
    GEL_TextOut("Configure PLL (150.00 MHz).\n");
    // Enable clocks to all peripherals
//    *(short *)IDLE_CTRL0@IO = 0x0;
//    *(short *)IDLE_CTRL1@IO = 0x0;

    // bypass PLL
    *(short *)CLKCFGMSW@IO = 0x0;

    *(short *)PLL_CNTL2@IO = 0x0044;
    *(short *)PLL_CNTL4@IO = 0x0000;
    *(short *)PLL_CNTL3@IO = 0x0806;
    *(short *)PLL_CNTL1@IO = 0x8380;

    // Busy wait until TESTLOCKMON is high or timeout
    GEL_TextOut("Wait until TESTLOCKMON is high...\n");
    while ( ((*(short *)PLL_CNTL3@IO) & 8) == 0) ;
    GEL_TextOut("After checking TESTLOCKMON bit...\n");

    // Switch to PLL clk
    *(short *)CLKCFGMSW@IO = 0x1;

    GEL_TextOut("PLL Init Done (150.00 MHz).\n");
}