altos: Make stm-flash capable of switching to application

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

diff --git a/src/core/ao_task.c b/src/core/ao_task.c
index a11979f0a81fb78a5063ee9066189647c649285e..0aad650898d9c54dddca6d53e6fa2c799aec89d8 100644 (file)
--- a/src/core/ao_task.c
+++ b/src/core/ao_task.c
@@ -114,6 +114,8 @@ ao_task_validate_alarm_queue(void)
 #define ao_task_validate_alarm_queue()
 #endif
 
+uint16_t       ao_task_alarm_tick;
+
 static void
 ao_task_to_alarm_queue(struct ao_task *task)
 {
@@ -126,6 +128,7 @@ ao_task_to_alarm_queue(struct ao_task *task)
                }
        }
        ao_list_append(&task->alarm_queue, &alarm_queue);
+       ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
        ao_task_validate_alarm_queue();
 }
 
@@ -133,6 +136,10 @@ static void
 ao_task_from_alarm_queue(struct ao_task *task)
 {
        ao_list_del(&task->alarm_queue);
+       if (ao_list_is_empty(&alarm_queue))
+               ao_task_alarm_tick = 0;
+       else
+               ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm;
        ao_task_validate_alarm_queue();
 }
 
@@ -154,10 +161,7 @@ void
 ao_task_check_alarm(uint16_t tick)
 {
        struct ao_task  *alarm, *next;
-       int             i;
 
-       if (ao_num_tasks == 0)
-               return;
        ao_list_for_each_entry_safe(alarm, next, &alarm_queue, struct ao_task, alarm_queue) {
                if ((int16_t) (tick - alarm->alarm) < 0)
                        break;
@@ -173,6 +177,7 @@ ao_task_init(void)
        uint8_t i;
        ao_list_init(&run_queue);
        ao_list_init(&alarm_queue);
+       ao_task_alarm_tick = 0;
        for (i = 0; i < SLEEP_HASH_SIZE; i++)
                ao_list_init(&sleep_queue[i]);
 }
@@ -264,14 +269,9 @@ ao_task_validate(void)
                }
        }
 }
-#else
-#define ao_task_validate()
-#endif
+#endif /* DEBUG */
 
-#else
-#define ao_task_to_run_queue(task)
-#define ao_task_to_alarm_queue(task)
-#endif
+#endif /* HAS_TASK_QUEUE */
 
 void
 ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant
@@ -305,6 +305,8 @@ ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *nam
                );
 }
 
+__data uint8_t ao_task_minimize_latency;
+
 /* Task switching function. This must not use any stack variables */
 void
 ao_yield(void) ao_arch_naked_define
@@ -331,6 +333,12 @@ ao_yield(void) ao_arch_naked_define
        }
 
        ao_arch_isr_stack();
+#if !HAS_TASK_QUEUE
+       if (ao_task_minimize_latency)
+               ao_arch_release_interrupts();
+       else
+#endif
+               ao_arch_block_interrupts();
 
 #if AO_CHECK_STACK
        in_yield = 1;
@@ -339,21 +347,19 @@ ao_yield(void) ao_arch_naked_define
         * this loop will run forever, which is just fine
         */
 #if HAS_TASK_QUEUE
-       if (ao_cur_task->wchan == NULL) {
-               uint32_t flags;
-               flags = ao_arch_irqsave();
+       /* If the current task is running, move it to the
+        * end of the queue to allow other tasks a chance
+        */
+       if (ao_cur_task->wchan == NULL)
                ao_task_to_run_queue(ao_cur_task);
-               ao_arch_irqrestore(flags);
-       }
        ao_cur_task = NULL;
-
        for (;;) {
                ao_arch_memory_barrier();
                if (!ao_list_is_empty(&run_queue))
                        break;
-               ao_arch_cpu_idle();
+               /* Wait for interrupts when there's nothing ready */
+               ao_arch_wait_interrupt();
        }
-               
        ao_cur_task = ao_list_first_entry(&run_queue, struct ao_task, queue);
 #else
        {
@@ -374,20 +380,19 @@ ao_yield(void) ao_arch_naked_define
                            (int16_t) (ao_time() - ao_cur_task->alarm) >= 0)
                                break;
 
-                       /* Enter lower power mode when there isn't anything to do */
-                       if (ao_cur_task_index == ao_last_task_index)
-                               ao_arch_cpu_idle();
+                       /* Wait for interrupts when there's nothing ready */
+                       if (ao_cur_task_index == ao_last_task_index && !ao_task_minimize_latency)
+                               ao_arch_wait_interrupt();
                }
-#if HAS_SAMPLE_PROFILE
-               ao_cur_task->start = ao_sample_profile_timer_value();
-#endif
        }
 #endif
+#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
-       ao_arch_block_interrupts();
        in_yield = 0;
 #endif
        ao_arch_restore_stack();
@@ -507,6 +512,7 @@ ao_exit(void)
        /* we'll never get back here */
 }
 
+#if HAS_TASK_INFO
 void
 ao_task_info(void)
 {
@@ -519,10 +525,11 @@ ao_task_info(void)
                       task->name,
                       (int) task->wchan);
        }
-#if HAS_TASK_QUEUE
+#if HAS_TASK_QUEUE && DEBUG
        ao_task_validate();
 #endif
 }
+#endif
 
 void
 ao_start_scheduler(void)
@@ -531,5 +538,8 @@ ao_start_scheduler(void)
        ao_cur_task_index = AO_NO_TASK_INDEX;
 #endif
        ao_cur_task = NULL;
+#if HAS_ARCH_START_SCHEDULER
+       ao_arch_start_scheduler();
+#endif
        ao_yield();
 }