sched: Implement generic sleep mechanism based on tasks pending

Track when tasks are pending and spin in irq_wait() when no tasks are
pending.  This improves the mechanism for sleeping the processor -
it's simpler for the board specific code and it reduces the
possibility of the processor sleeping when tasks are busy.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

9 files changed, 60 insertions, 80 deletions

diff --git a/src/avr/timer.c b/src/avr/timer.c
index 17f3e913..a49d9e6d 100644
--- a/src/avr/timer.c
+++ b/src/avr/timer.c
@@ -6,7 +6,6 @@
 
 #include <avr/interrupt.h> // TCNT1
 #include "autoconf.h" // CONFIG_AVR_CLKPR
-#include "basecmd.h" // stats_note_sleep
 #include "board/misc.h" // timer_from_us
 #include "command.h" // shutdown
 #include "irq.h" // irq_save
@@ -63,12 +62,6 @@ timer_set(uint16_t next)
     OCR1A = next;
 }
 
-static inline uint16_t
-timer_get_next(void)
-{
-    return OCR1A;
-}
-
 static inline void
 timer_repeat_set(uint16_t next)
 {
@@ -213,22 +206,8 @@ done:
 void
 timer_task(void)
 {
-    static uint16_t last_timer;
-    uint16_t lst = last_timer;
     irq_disable();
-    uint16_t next = timer_get_next(), cur = timer_get();
-    if (lst != next) {
-        timer_repeat_set(cur + TIMER_IDLE_REPEAT_TICKS);
-        irq_enable();
-        last_timer = next;
-        return;
-    }
-
-    // Sleep the processor
-    irq_wait();
-    uint16_t post_sleep = timer_get();
-    timer_repeat_set(post_sleep + TIMER_IDLE_REPEAT_TICKS);
+    timer_repeat_set(timer_get() + TIMER_IDLE_REPEAT_TICKS);
     irq_enable();
-    stats_note_sleep(post_sleep - cur);
 }
 DECL_TASK(timer_task);
diff --git a/src/basecmd.c b/src/basecmd.c
index da21fad0..831b12e4 100644
--- a/src/basecmd.c
+++ b/src/basecmd.c
@@ -228,7 +228,6 @@ command_get_status(uint32_t *args)
 DECL_COMMAND_FLAGS(command_get_status, HF_IN_SHUTDOWN, "get_status");
 
 static uint32_t stats_send_time, stats_send_time_high;
-static uint32_t stats_last_time, stats_sleep_time;
 
 void
 command_get_uptime(uint32_t *args)
@@ -239,24 +238,16 @@ command_get_uptime(uint32_t *args)
 }
 DECL_COMMAND_FLAGS(command_get_uptime, HF_IN_SHUTDOWN, "get_uptime");
 
-void
-stats_note_sleep(uint32_t sleep_time)
-{
-    stats_sleep_time += sleep_time;
-    stats_last_time += sleep_time;
-}
-
 #define SUMSQ_BASE 256
 DECL_CONSTANT(STATS_SUMSQ_BASE, SUMSQ_BASE);
 
 void
-stats_task(void)
+stats_update(uint32_t start, uint32_t cur)
 {
-    static uint32_t count, sumsq;
-    uint32_t cur = timer_read_time();
-    uint32_t diff = cur - stats_last_time;
-    stats_last_time = cur;
+    static uint32_t count, sum, sumsq;
+    uint32_t diff = cur - start;
     count++;
+    sum += diff;
     // Calculate sum of diff^2 - be careful of integer overflow
     uint32_t nextsumsq;
     if (diff <= 0xffff) {
@@ -272,15 +263,12 @@ stats_task(void)
 
     if (timer_is_before(cur, stats_send_time + timer_from_us(5000000)))
         return;
-    sendf("stats count=%u sum=%u sumsq=%u"
-          , count, cur - stats_send_time - stats_sleep_time, sumsq);
+    sendf("stats count=%u sum=%u sumsq=%u", count, sum, sumsq);
     if (cur < stats_send_time)
         stats_send_time_high++;
     stats_send_time = cur;
-    stats_sleep_time = 0;
-    count = sumsq = 0;
+    count = sum = sumsq = 0;
 }
-DECL_TASK(stats_task);
 
 
 /****************************************************************
diff --git a/src/basecmd.h b/src/basecmd.h
index 713343e2..8787b194 100644
--- a/src/basecmd.h
+++ b/src/basecmd.h
@@ -9,7 +9,7 @@ void move_request_size(int size);
 void *oid_lookup(uint8_t oid, void *type);
 void *oid_alloc(uint8_t oid, void *type, uint16_t size);
 void *oid_next(uint8_t *i, void *type);
-void stats_note_sleep(uint32_t sleep_time);
+void stats_update(uint32_t start, uint32_t cur);
 
 #define foreach_oid(pos,data,oidtype)                   \
     for (pos=-1; (data=oid_next(&pos, oidtype)); )
diff --git a/src/generic/timer_irq.c b/src/generic/timer_irq.c
index 605fc129..03881b2e 100644
--- a/src/generic/timer_irq.c
+++ b/src/generic/timer_irq.c
@@ -8,7 +8,6 @@
 #include "board/irq.h" // irq_disable
 #include "board/misc.h" // timer_from_us
 #include "board/timer_irq.h" // timer_dispatch_many
-#include "basecmd.h" // stats_note_sleep
 #include "command.h" // shutdown
 #include "sched.h" // sched_timer_dispatch
 
@@ -80,23 +79,9 @@ timer_dispatch_many(void)
 void
 timer_task(void)
 {
-    static uint32_t last_timer;
-    uint32_t lst = last_timer;
     irq_disable();
-    uint32_t next = timer_get_next(), cur = timer_read_time();
-    if (lst != next) {
-        timer_repeat_until = cur + TIMER_IDLE_REPEAT_TICKS;
-        irq_enable();
-        last_timer = next;
-        return;
-    }
-
-    // Sleep the processor
-    irq_wait();
-    uint32_t post_sleep = timer_read_time();
-    timer_repeat_until = post_sleep + TIMER_IDLE_REPEAT_TICKS;
+    timer_repeat_until = timer_read_time() + TIMER_IDLE_REPEAT_TICKS;
     irq_enable();
-    stats_note_sleep(post_sleep - cur);
 }
 DECL_TASK(timer_task);
 
diff --git a/src/generic/timer_irq.h b/src/generic/timer_irq.h
index ad6ad033..031f0d6b 100644
--- a/src/generic/timer_irq.h
+++ b/src/generic/timer_irq.h
@@ -2,6 +2,5 @@
 #define __GENERIC_TIMER_IRQ_H
 
 uint32_t timer_dispatch_many(void);
-uint32_t timer_get_next(void);
 
 #endif // timer_irq.h
diff --git a/src/pru/main.c b/src/pru/main.c
index 04d440df..45eb8274 100644
--- a/src/pru/main.c
+++ b/src/pru/main.c
@@ -49,6 +49,7 @@ void
 irq_wait(void)
 {
     asm("slp 1");
+    irq_poll();
 }
 
 // Set the next timer wake up time
@@ -58,13 +59,6 @@ timer_set(uint32_t value)
     CT_IEP.TMR_CMP0 = value;
 }
 
-// Return the next scheduled wake up time
-uint32_t
-timer_get_next(void)
-{
-    return CT_IEP.TMR_CMP0;
-}
-
 // Return the current time (in absolute clock ticks).
 uint32_t
 timer_read_time(void)
diff --git a/src/sam3x8e/timer.c b/src/sam3x8e/timer.c
index 3a441062..f7698c65 100644
--- a/src/sam3x8e/timer.c
+++ b/src/sam3x8e/timer.c
@@ -18,13 +18,6 @@ timer_set(uint32_t value)
     TC0->TC_CHANNEL[0].TC_RA = value;
 }
 
-// Return the next scheduled wake up time
-uint32_t
-timer_get_next(void)
-{
-    return TC0->TC_CHANNEL[0].TC_RA;
-}
-
 // Return the current time (in absolute clock ticks).
 uint32_t
 timer_read_time(void)
diff --git a/src/sched.c b/src/sched.c
index a2f5fea8..e6401bbb 100644
--- a/src/sched.c
+++ b/src/sched.c
@@ -6,6 +6,7 @@
 
 #include <setjmp.h> // setjmp
 #include "autoconf.h" // CONFIG_*
+#include "basecmd.h" // stats_update
 #include "board/io.h" // readb
 #include "board/irq.h" // irq_save
 #include "board/misc.h" // timer_from_us
@@ -177,13 +178,23 @@ sched_timer_reset(void)
 
 
 /****************************************************************
- * Task waking
+ * Tasks
  ****************************************************************/
 
+static int_fast8_t tasks_pending;
+
 // Note that at least one task is ready to run
 void
 sched_wake_tasks(void)
 {
+    tasks_pending = 0;
+}
+
+// Check if tasks need to be run
+uint8_t
+sched_tasks_busy(void)
+{
+    return tasks_pending >= 0;
 }
 
 // Note that a task is ready to run
@@ -204,6 +215,40 @@ sched_check_wake(struct task_wake *w)
     return 1;
 }
 
+// Main task dispatch loop
+static void
+run_tasks(void)
+{
+    uint32_t start = timer_read_time();
+    for (;;) {
+        // Check if can sleep
+        irq_disable();
+        if (!tasks_pending) {
+            // Tasks are busy - don't sleep
+            irq_enable();
+        } else {
+            // Sleep processor (only run timers) until tasks pending
+            tasks_pending = -1;
+            uint32_t sleep_start = timer_read_time();
+            do {
+                irq_wait();
+            } while (!sched_tasks_busy());
+            irq_enable();
+            start += timer_read_time() - sleep_start;
+        }
+        tasks_pending = 1;
+
+        // Run all tasks
+        extern void ctr_run_taskfuncs(void);
+        ctr_run_taskfuncs();
+
+        // Update statistics
+        uint32_t cur = timer_read_time();
+        stats_update(start, cur);
+        start = cur;
+    }
+}
+
 
 /****************************************************************
  * Shutdown processing
@@ -274,23 +319,19 @@ sched_shutdown(uint_fast8_t reason)
 
 
 /****************************************************************
- * Startup and background task processing
+ * Startup
  ****************************************************************/
 
-// Auto-generated code in out/compile_time_requests.c
-extern void ctr_run_initfuncs(void);
-extern void ctr_run_taskfuncs(void);
-
 // Main loop of program
 void
 sched_main(void)
 {
+    extern void ctr_run_initfuncs(void);
     ctr_run_initfuncs();
 
     int ret = setjmp(shutdown_jmp);
     if (ret)
         run_shutdown(ret);
 
-    for (;;)
-        ctr_run_taskfuncs();
+    run_tasks();
 }
diff --git a/src/sched.h b/src/sched.h
index ba883778..ffe17e03 100644
--- a/src/sched.h
+++ b/src/sched.h
@@ -30,6 +30,7 @@ void sched_add_timer(struct timer*);
 void sched_del_timer(struct timer *del);
 unsigned int sched_timer_dispatch(void);
 void sched_wake_tasks(void);
+uint8_t sched_tasks_busy(void);
 void sched_wake_task(struct task_wake *w);
 uint8_t sched_check_wake(struct task_wake *w);
 uint8_t sched_is_shutdown(void);