1 files changed, 4 insertions, 75 deletions

diff --git a/src/stm32f0/timer.c b/src/stm32f0/timer.c
index e5628f93..0b285d4e 100644
--- a/src/stm32f0/timer.c
+++ b/src/stm32f0/timer.c
@@ -16,30 +16,13 @@
 #include "log.h"
 #include "sched.h" // DECL_INIT
 #include "command.h" // shutdown
-
-DECL_CONSTANT("CLOCK_FREQ", CONFIG_CLOCK_FREQ);
-
-// Return the number of clock ticks for a given number of microseconds
-uint32_t
-timer_from_us(uint32_t us)
-{
-    return us * (CONFIG_CLOCK_FREQ / 1000000);
-}
-
-// Return true if time1 is before time2.  Always use this function to
-// compare times as regular C comparisons can fail if the counter
-// rolls over.
-uint8_t
-timer_is_before(uint32_t time1, uint32_t time2)
-{
-    return (int32_t)(time1 - time2) < 0;
-}
+#include "board/timer_irq.h" // timer_dispatch_many
 
 // Set the next irq time
 static void
 timer_set_diff(uint32_t value)
 {
-    SysTick->LOAD = value;
+    SysTick->LOAD = value & SysTick_LOAD_RELOAD_Msk;
     SysTick->VAL = 0;
     SysTick->LOAD = 0;
 }
@@ -84,69 +67,15 @@ timer_read_time(void)
     return TIM2->CNT;
 }
 
-static uint32_t timer_repeat_until;
-#define TIMER_IDLE_REPEAT_TICKS timer_from_us(500)
-#define TIMER_REPEAT_TICKS timer_from_us(100)
-
-#define TIMER_MIN_TRY_TICKS timer_from_us(2)
-#define TIMER_DEFER_REPEAT_TICKS timer_from_us(5)
-
-// Invoke timers
-static uint32_t
-timer_dispatch_many(void)
-{
-    uint32_t tru = timer_repeat_until;
-    for (;;) {
-        // Run the next software timer
-        uint32_t next = sched_timer_dispatch();
-
-        uint32_t now = timer_read_time();
-        int32_t diff = next - now;
-        if (diff > (int32_t)TIMER_MIN_TRY_TICKS)
-            // Schedule next timer normally.
-            return diff;
-
-        if (unlikely(timer_is_before(tru, now))) {
-            // Check if there are too many repeat timers
-            if (diff < (int32_t)(-timer_from_us(1000)))
-                try_shutdown("Rescheduled timer in the past");
-            if (sched_tasks_busy()) {
-                timer_repeat_until = now + TIMER_REPEAT_TICKS;
-                return TIMER_DEFER_REPEAT_TICKS;
-            }
-            timer_repeat_until = tru = now + TIMER_IDLE_REPEAT_TICKS;
-        }
-
-        // Next timer in the past or near future - wait for it to be ready
-        irq_enable();
-        while (unlikely(diff > 0))
-            diff = next - timer_read_time();
-        irq_disable();
-    }
-}
-
 // IRQ handler
 void __visible __aligned(16) // aligning helps stabilize perf benchmarks
 SysTick_Handler(void)
 {
     irq_disable();
-    uint32_t diff = timer_dispatch_many();
-    timer_set_diff(diff);
-    irq_enable();
-}
-
-// Make sure timer_repeat_until doesn't wrap 32bit comparisons
-void
-timer_task(void)
-{
-    uint32_t now = timer_read_time();
-    irq_disable();
-    if (timer_is_before(timer_repeat_until, now))
-        timer_repeat_until = now;
+    uint32_t next = timer_dispatch_many();
+    timer_set_diff(next-timer_read_time());
     irq_enable();
 }
-DECL_TASK(timer_task);
-
 
 void TIM2_IRQHandler(void)
 {