X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_timer.c;h=e81f937d9604096bc3995620a0b17730d55d74c9;hp=81c3b376214ffddac88e19113d2e433cafc89d74;hb=b75aa1c825b84bd7fa1578320fbc7e904c373a7d;hpb=17d2432a8b9c15963cd3b821f025ad33972ef477

diff --git a/src/ao_timer.c b/src/ao_timer.c
index 81c3b376..e81f937d 100644
--- a/src/ao_timer.c
+++ b/src/ao_timer.c
@@ -24,13 +24,13 @@ uint16_t ao_time(void) __critical
 	return ao_tick_count;
 }
 
+static __xdata uint8_t ao_forever;
+
 void
 ao_delay(uint16_t ticks)
 {
-	uint16_t until = ao_time() + ticks;
-
-	while ((int16_t) (until - ao_time()) > 0)
-		ao_sleep(DATA_TO_XDATA(&ao_tick_count));
+	ao_alarm(ticks);
+	ao_sleep(&ao_forever);
 }
 
 #define T1_CLOCK_DIVISOR	8	/* 24e6/8 = 3e6 */
@@ -46,7 +46,6 @@ void ao_timer_isr(void) interrupt 9
 		ao_adc_count = 0;
 		ao_adc_poll();
 	}
-	ao_wakeup(DATA_TO_XDATA(&ao_tick_count));
 }
 
 void
@@ -83,3 +82,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))
+		;
+}