X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_task.c;h=4a78766e2828664b1687b09f0ea28a02ac987066;hp=136444b0899ef0b6112544f507f7ca9e8b7d80fa;hb=00f49c8fb0aa38331360bdb85c32bcebe60dcdc3;hpb=b657aa209b9ea3b3efd33a940283b3ba60a169af

diff --git a/src/ao_task.c b/src/ao_task.c
index 136444b0..4a78766e 100644
--- a/src/ao_task.c
+++ b/src/ao_task.c
@@ -129,6 +129,13 @@ ao_yield(void) _naked
 				break;
 			}
 
+			/* Check if the alarm is set for a time which has passed */
+			if (ao_cur_task->alarm &&
+			    (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) {
+				ao_cur_task_index = ao_next_task_index;
+				break;
+			}
+
 			/* Enter lower power mode when there isn't anything to do */
 			if (ao_next_task_index == ao_cur_task_index)
 				PCON = PCON_IDLE;
@@ -181,13 +188,20 @@ ao_yield(void) _naked
 	_endasm;
 }
 
-void
+uint8_t
 ao_sleep(__xdata void *wchan)
 {
 	__critical {
 		ao_cur_task->wchan = wchan;
 	}
 	ao_yield();
+	if (ao_cur_task->wchan) {
+		ao_cur_task->wchan = NULL;
+		ao_cur_task->alarm = 0;
+		return 1;
+	}
+	ao_cur_task->alarm = 0;
+	return 0;
 }
 
 void
@@ -200,6 +214,16 @@ ao_wakeup(__xdata void *wchan)
 			ao_tasks[i]->wchan = NULL;
 }
 
+void
+ao_alarm(uint16_t delay)
+{
+	/* Make sure we sleep *at least* delay ticks, which means adding
+	 * one to account for the fact that we may be close to the next tick
+	 */
+	if (!(ao_cur_task->alarm = ao_time() + delay + 1))
+		ao_cur_task->alarm = 1;
+}
+
 void
 ao_wake_task(__xdata struct ao_task *task)
 {
@@ -207,7 +231,7 @@ ao_wake_task(__xdata struct ao_task *task)
 }
 
 void
-ao_exit(void)
+ao_exit(void) __critical
 {
 	uint8_t	i;
 	ao_num_tasks--;