Fix clock initialization to not try to use 32kHz xtal on P2_3/P2_4

[fw/altos] / src / ao_timer.c

diff --git a/src/ao_timer.c b/src/ao_timer.c
index 81c3b376214ffddac88e19113d2e433cafc89d74..78c6e063f0991eb5c71e628b652cca08e928c02b 100644 (file)
--- a/src/ao_timer.c
+++ b/src/ao_timer.c
@@ -83,3 +83,24 @@ ao_timer_init(void)
        /* enable timer 1 in module mode, dividing by 8 */
        T1CTL = T1CTL_MODE_MODULO | T1CTL_DIV_8;
 }
+
+/*
+ * AltOS always cranks the clock to the max frequency
+ */
+void
+ao_clock_init(void)
+{
+       /* Switch system clock to crystal oscilator */
+       CLKCON = (CLKCON & ~CLKCON_OSC_MASK) | (CLKCON_OSC_XTAL);
+
+       while (!(SLEEP & SLEEP_XOSC_STB))
+               ;
+
+       /* Crank up the timer tick and system clock speed */
+       CLKCON = ((CLKCON & ~(CLKCON_TICKSPD_MASK | CLKCON_CLKSPD_MASK)) |
+                 (CLKCON_TICKSPD_1 | CLKCON_CLKSPD_1));
+
+       while ((CLKCON & (CLKCON_TICKSPD_MASK|CLKCON_CLKSPD_MASK)) !=
+              (CLKCON_TICKSPD_1 | CLKCON_CLKSPD_1))
+               ;
+}