sched: Support adding timers to the start of timer_list

If sched_add_timer() is called on a timer that would make it the new
head of the list, then add it and signal the board code that the timer
should be rescheduled.

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

7 files changed, 61 insertions, 44 deletions

diff --git a/src/avr/timer.c b/src/avr/timer.c
index e32a0b42..a87ae206 100644
--- a/src/avr/timer.c
+++ b/src/avr/timer.c
@@ -1,6 +1,6 @@
 // AVR timer interrupt scheduling code.
 //
-// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2016,2017  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
@@ -10,7 +10,7 @@
 #include "board/misc.h" // timer_from_us
 #include "command.h" // shutdown
 #include "irq.h" // irq_save
-#include "sched.h" // sched_timer_kick
+#include "sched.h" // sched_timer_dispatch
 
 
 /****************************************************************
@@ -77,16 +77,13 @@ timer_repeat_set(uint16_t next)
     TIFR1 = 1<<OCF1B;
 }
 
-// Reset the timer - clear settings and dispatch next timer immediately
+// Activate timer dispatch as soon as possible
 void
-timer_reset(void)
+timer_kick(void)
 {
-    uint16_t now = timer_get();
-    timer_set(now + 50);
+    timer_set(timer_get() + 50);
     TIFR1 = 1<<OCF1A;
-    timer_repeat_set(now + 50);
 }
-DECL_SHUTDOWN(timer_reset);
 
 void
 timer_init(void)
@@ -97,7 +94,8 @@ timer_init(void)
     TCCR1B = 1<<CS10;
     // Setup for first irq
     irqstatus_t flag = irq_save();
-    timer_reset();
+    timer_kick();
+    timer_repeat_set(timer_get() + 50);
     TIFR1 = 1<<TOV1;
     // enable interrupt
     TIMSK1 = 1<<OCIE1A;
diff --git a/src/generic/misc.h b/src/generic/misc.h
index b2f1c050..f0f8e5a5 100644
--- a/src/generic/misc.h
+++ b/src/generic/misc.h
@@ -11,6 +11,7 @@ void console_sendf(const struct command_encoder *ce, va_list args);
 uint32_t timer_from_us(uint32_t us);
 uint8_t timer_is_before(uint32_t time1, uint32_t time2);
 uint32_t timer_read_time(void);
+void timer_kick(void);
 void timer_periodic(void);
 
 void *dynmem_start(void);
diff --git a/src/generic/timer_irq.c b/src/generic/timer_irq.c
index 6c14f9ae..127601e2 100644
--- a/src/generic/timer_irq.c
+++ b/src/generic/timer_irq.c
@@ -10,7 +10,7 @@
 #include "board/timer_irq.h" // timer_dispatch_many
 #include "basecmd.h" // stats_note_sleep
 #include "command.h" // shutdown
-#include "sched.h" // sched_timer_kick
+#include "sched.h" // sched_timer_dispatch
 
 DECL_CONSTANT(CLOCK_FREQ, CONFIG_CLOCK_FREQ);
 
diff --git a/src/pru/main.c b/src/pru/main.c
index 1b54e42a..3c2a2725 100644
--- a/src/pru/main.c
+++ b/src/pru/main.c
@@ -51,24 +51,37 @@ irq_wait(void)
     asm("slp 1");
 }
 
+// Set the next timer wake up time
 static void
 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)
 {
     return CT_IEP.TMR_CNT;
 }
 
+// Activate timer dispatch as soon as possible
+void
+timer_kick(void)
+{
+    timer_set(timer_read_time() + 50);
+    CT_IEP.TMR_CMP_STS = 0xff;
+    __delay_cycles(4);
+    CT_INTC.SECR0 = 1 << IEP_EVENT;
+}
+
 static void
 _irq_poll(void)
 {
@@ -87,23 +100,12 @@ irq_poll(void)
 }
 
 void
-timer_shutdown(void)
-{
-    // Reenable timer irq
-    timer_set(timer_read_time() + 50);
-    CT_IEP.TMR_CMP_STS = 0xff;
-    __delay_cycles(4);
-    CT_INTC.SECR0 = 1 << IEP_EVENT;
-}
-DECL_SHUTDOWN(timer_shutdown);
-
-void
 timer_init(void)
 {
     CT_IEP.TMR_CMP_CFG = 0x01 << 1;
     CT_IEP.TMR_GLB_CFG = 0x11;
     CT_IEP.TMR_CNT = 0;
-    timer_shutdown();
+    timer_kick();
 }
 DECL_INIT(timer_init);
 
diff --git a/src/sam3x8e/timer.c b/src/sam3x8e/timer.c
index ba1124cb..3a441062 100644
--- a/src/sam3x8e/timer.c
+++ b/src/sam3x8e/timer.c
@@ -1,6 +1,6 @@
 // SAM3x8e timer interrupt scheduling
 //
-// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2016,2017  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
@@ -9,7 +9,7 @@
 #include "board/timer_irq.h" // timer_dispatch_many
 #include "command.h" // DECL_SHUTDOWN
 #include "sam3x8e.h" // TC0
-#include "sched.h" // sched_timer_kick
+#include "sched.h" // DECL_INIT
 
 // Set the next irq time
 static void
@@ -32,6 +32,14 @@ timer_read_time(void)
     return TC0->TC_CHANNEL[0].TC_CV;
 }
 
+// Activate timer dispatch as soon as possible
+void
+timer_kick(void)
+{
+    timer_set(timer_read_time() + 50);
+    TC0->TC_CHANNEL[0].TC_SR; // read to clear irq pending
+}
+
 void
 timer_init(void)
 {
@@ -39,27 +47,17 @@ timer_init(void)
     // Reset the timer
     tc->TC_CCR = TC_CCR_CLKDIS;
     tc->TC_IDR = 0xFFFFFFFF;
-    tc->TC_SR;
     // Enable it
     PMC->PMC_PCER0 = 1 << ID_TC0;
     tc->TC_CMR = TC_CMR_WAVE | TC_CMR_WAVSEL_UP | TC_CMR_TCCLKS_TIMER_CLOCK1;
     tc->TC_IER = TC_IER_CPAS;
     NVIC_SetPriority(TC0_IRQn, 1);
     NVIC_EnableIRQ(TC0_IRQn);
-    timer_set(1);
+    timer_kick();
     tc->TC_CCR = TC_CCR_CLKEN | TC_CCR_SWTRG;
 }
 DECL_INIT(timer_init);
 
-void
-timer_shutdown(void)
-{
-    // Reenable timer irq
-    timer_set(timer_read_time() + 50);
-    TC0->TC_CHANNEL[0].TC_SR; // read to clear irq pending
-}
-DECL_SHUTDOWN(timer_shutdown);
-
 // IRQ handler
 void __visible __aligned(16) // aligning helps stabilize perf benchmarks
 TC0_Handler(void)
diff --git a/src/sched.c b/src/sched.c
index 758fa71c..92fc5504 100644
--- a/src/sched.c
+++ b/src/sched.c
@@ -20,7 +20,7 @@
  ****************************************************************/
 
 static struct timer periodic_timer, *timer_list = &periodic_timer;
-static struct timer sentinel_timer;
+static struct timer sentinel_timer, deleted_timer;
 
 // The periodic_timer simplifies the timer code by ensuring there is
 // always a timer on the timer list and that there is always a timer
@@ -79,10 +79,22 @@ sched_add_timer(struct timer *add)
 {
     uint32_t waketime = add->waketime;
     irqstatus_t flag = irq_save();
-    if (timer_is_before(waketime, timer_list->waketime))
-        // Timer in past (or very near future)
-        shutdown("Timer too close");
-    insert_timer(add, waketime);
+    if (unlikely(timer_is_before(waketime, timer_list->waketime))) {
+        // This timer is before all other scheduled timers
+        struct timer *tl = timer_list;
+        if (timer_is_before(waketime, timer_read_time() + timer_from_us(2000)))
+            try_shutdown("Timer too close");
+        if (tl == &deleted_timer)
+            add->next = deleted_timer.next;
+        else
+            add->next = tl;
+        deleted_timer.waketime = waketime;
+        deleted_timer.next = add;
+        timer_list = &deleted_timer;
+        timer_kick();
+    } else {
+        insert_timer(add, waketime);
+    }
     irq_restore(flag);
 }
 
@@ -158,6 +170,7 @@ sched_timer_shutdown(void)
     deleted_timer.waketime = periodic_timer.waketime;
     deleted_timer.next = &periodic_timer;
     periodic_timer.next = &sentinel_timer;
+    timer_kick();
 }
 DECL_SHUTDOWN(sched_timer_shutdown);
 
diff --git a/src/simulator/main.c b/src/simulator/main.c
index ccd6dd93..7519a35b 100644
--- a/src/simulator/main.c
+++ b/src/simulator/main.c
@@ -74,15 +74,20 @@ timer_is_before(uint32_t time1, uint32_t time2)
     return (int32_t)(time1 - time2) < 0;
 }
 
+uint32_t
+timer_read_time(void)
+{
+    return 0; // XXX
+}
+
 void
-timer_periodic(void)
+timer_kick(void)
 {
 }
 
-uint32_t
-timer_read_time(void)
+void
+timer_periodic(void)
 {
-    return 0; // XXX
 }