altos: Add stack-guard code. Uses STM MPU to trap stack overflow.

[fw/altos] / src / core / ao_task.c

diff --git a/src/core/ao_task.c b/src/core/ao_task.c
index 4593bd790c4717586d258489749212d5cf4a5aae..df70b906ccfa6bec6c793c417a25bc4d4e652ff6 100644 (file)
--- a/src/core/ao_task.c
+++ b/src/core/ao_task.c
@@ -16,6 +16,13 @@
  */
 
 #include <ao.h>
+#include <ao_task.h>
+#if HAS_SAMPLE_PROFILE
+#include <ao_sample_profile.h>
+#endif
+#if HAS_STACK_GUARD
+#include <ao_mpu.h>
+#endif
 
 #define AO_NO_TASK_INDEX       0xff
 
@@ -67,6 +74,8 @@ ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *nam
        ao_arch_init_stack(task, start);
 }
 
+__xdata uint8_t        ao_idle;
+
 /* Task switching function. This must not use any stack variables */
 void
 ao_yield(void) ao_arch_naked_define
@@ -77,6 +86,13 @@ ao_yield(void) ao_arch_naked_define
                ao_cur_task_index = ao_num_tasks-1;
        else
        {
+#if HAS_SAMPLE_PROFILE
+               uint16_t        tick = ao_sample_profile_timer_value();
+               uint16_t        run = tick - ao_cur_task->start;
+               if (run > ao_cur_task->max_run)
+                       ao_cur_task->max_run = run;
+               ++ao_cur_task->yields;
+#endif
                ao_arch_save_stack();
        }
 
@@ -110,7 +126,13 @@ ao_yield(void) ao_arch_naked_define
                        if (ao_cur_task_index == ao_last_task_index)
                                ao_arch_cpu_idle();
                }
+#if HAS_SAMPLE_PROFILE
+       ao_cur_task->start = ao_sample_profile_timer_value();
+#endif
        }
+#if HAS_STACK_GUARD
+       ao_mpu_stack_guard(ao_cur_task->stack);
+#endif
 #if AO_CHECK_STACK
        cli();
        in_yield = 0;
@@ -125,6 +147,7 @@ ao_sleep(__xdata void *wchan)
        ao_yield();
        if (ao_cur_task->wchan) {
                ao_cur_task->wchan = NULL;
+               ao_cur_task->alarm = 0;
                return 1;
        }
        return 0;
@@ -157,6 +180,16 @@ ao_clear_alarm(void)
        ao_cur_task->alarm = 0;
 }
 
+static __xdata uint8_t ao_forever;
+
+void
+ao_delay(uint16_t ticks)
+{
+       ao_alarm(ticks);
+       ao_sleep(&ao_forever);
+       ao_clear_alarm();
+}
+
 void
 ao_exit(void)
 {