+void sysinit() {
+ // xtal_freq = 12000000;
+
+ volatile int *scs;
+ scs = (int *)0x400fc1a0;
+ // enable main oscillator
+ *scs = 0x20;
+
+ // wait for oscillator to get ready
+ while ((*scs & (1<<6)) == 0);
+
+ volatile int *cclkcfg;
+ cclkcfg = (int *)0x400FC104;
+ // set clockdivider to 1/4x
+ *cclkcfg = 0x03;
+
+ // peripheral clocks (all set to 1/4x cclk)
+ volatile int *pclksel0, *pclksel1;
+ pclksel0=(int *)0x400FC1A8;
+ *pclksel0 = 0;
+ pclksel1=(int *)0x400FC1AC;
+ *pclksel1 = 0;
+
+ // main clock set (pll0)
+ volatile int *clksrcsel;
+ clksrcsel = (int *)0x400FC10C;
+ *clksrcsel = 0x01;
+
+ volatile int *pll0con, *pll0stat, *pll0feed, *pll0cfg;
+ pll0con=(int*)0x400FC080;
+ pll0stat=(int *)0x400FC088;
+ // pll0feed : feed 0xAA followed by 0x55 to feed
+ pll0feed=(int *)0x400FC08C;
+ pll0cfg=(int *)0x400FC084;
+
+ // FORMULA = FCCO = (2 * M * FIN) / N
+ // as Xtal (FIN) is 12MHz, N = 6 , M = 25 gives 100 MHz
+ // as both msel and nsel stores "N-1" store 24 and 5
+ *pll0cfg = 24 | (5 << 16);
+
+ // feed to set clock
+ *pll0feed = 0xaa;
+ *pll0feed = 0x55;
+
+
+ // enable pll0
+ *pll0con = 0x01;
+ *pll0feed = 0xaa;
+ *pll0feed = 0x55;
+
+ // wait for enable
+ while (!(*pll0stat & (1<<26)));
+
+ // lock pll0
+ *pll0con = 0x03;
+ *pll0feed = 0xaa;
+ *pll0feed = 0x55;
+
+ // wait for lock
+ while (!(*pll0stat & ((1<<25) | (1<<24))));
+
+ volatile int *pll1con, *pll1cfg, *pll1stat, *pll1feed;
+ pll1con=(int *)0x400FC0A0;
+ pll1cfg=(int *)0x400FC0A4;
+ pll1stat=(int *)0x400FC0A8;
+ pll1feed=(int *)0x400FC0AC;
+
+ // setup pll1 (multiplier 4, divider 6)
+ *pll1cfg = 3 | (0x01 << 5);
+
+ // enable pll1
+ *pll1con = 0x01;
+ *pll1feed = 0xaa;
+ *pll1feed = 0x55;
+ while (!(*pll1stat & (1<<10)));
+
+ // connect pll1
+ *pll1con = 0x03;
+ *pll1feed = 0xaa;
+ *pll1feed = 0x55;
+ while (!(*pll1stat & ((1<< 9) | (1<< 8))));
+}