diff --git a/src/linux/timer.c b/src/linux/timer.c
index 696971e2..30e45793 100644
--- a/src/linux/timer.c
+++ b/src/linux/timer.c
@@ -1,13 +1,13 @@
 // Handling of timers on linux systems
 //
-// Copyright (C) 2017-2020  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2017-2021  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
 #include <time.h> // struct timespec
 #include "autoconf.h" // CONFIG_CLOCK_FREQ
-#include "board/misc.h" // timer_from_us
 #include "board/irq.h" // irq_disable
+#include "board/misc.h" // timer_from_us
 #include "command.h" // DECL_CONSTANT
 #include "internal.h" // console_sleep
 #include "sched.h" // DECL_INIT
@@ -17,8 +17,8 @@
  * Timespec helpers
  ****************************************************************/
 
-static uint32_t last_read_time_counter;
-static struct timespec last_read_time, next_wake_time;
+static uint32_t next_wake_time_counter;
+static struct timespec next_wake_time;
 static time_t start_sec;
 
 // Compare two 'struct timespec' times
@@ -41,10 +41,10 @@ timespec_to_time(struct timespec ts)
 static inline struct timespec
 timespec_from_time(uint32_t time)
 {
-    int32_t counter_diff = time - last_read_time_counter;
+    int32_t counter_diff = time - next_wake_time_counter;
     struct timespec ts;
-    ts.tv_sec = last_read_time.tv_sec;
-    ts.tv_nsec = last_read_time.tv_nsec + counter_diff * NSECS_PER_TICK;
+    ts.tv_sec = next_wake_time.tv_sec;
+    ts.tv_nsec = next_wake_time.tv_nsec + counter_diff * NSECS_PER_TICK;
     if ((unsigned long)ts.tv_nsec >= NSECS) {
         if (ts.tv_nsec < 0) {
             ts.tv_sec--;
@@ -57,18 +57,6 @@ timespec_from_time(uint32_t time)
     return ts;
 }
 
-// Add a given number of nanoseconds to a 'struct timespec'
-static inline struct timespec
-timespec_add(struct timespec ts, long ns)
-{
-    ts.tv_nsec += ns;
-    if (ts.tv_nsec >= NSECS) {
-        ts.tv_sec++;
-        ts.tv_nsec -= NSECS;
-    }
-    return ts;
-}
-
 // Return the current time
 static struct timespec
 timespec_read(void)
@@ -78,15 +66,6 @@ timespec_read(void)
     return ts;
 }
 
-// Periodically update last_read_time / last_read_time_counter
-void
-timespec_update(void)
-{
-    last_read_time = timespec_read();
-    last_read_time_counter = timespec_to_time(last_read_time);
-}
-DECL_TASK(timespec_update);
-
 // Check if a given time has past
 int
 timer_check_periodic(struct timespec *ts)
@@ -132,57 +111,74 @@ timer_read_time(void)
 void
 timer_kick(void)
 {
-    next_wake_time = last_read_time;
+    next_wake_time = timespec_read();
+    next_wake_time_counter = timespec_to_time(next_wake_time);
 }
 
-static struct timespec timer_repeat_until;
-#define TIMER_IDLE_REPEAT_NS 500000
-#define TIMER_REPEAT_NS 100000
+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_NS 2000
-#define TIMER_DEFER_REPEAT_NS 5000
+#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 next;
+
+        if (unlikely(timer_is_before(tru, now))) {
+            // Check if there are too many repeat timers
+            if (diff < (int32_t)(-timer_from_us(100000)))
+                try_shutdown("Rescheduled timer in the past");
+            if (sched_tasks_busy()) {
+                timer_repeat_until = now + TIMER_REPEAT_TICKS;
+                return now + 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
+        while (unlikely(diff > 0))
+            diff = next - timer_read_time();
+    }
+}
+
+// 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;
+    irq_enable();
+}
+DECL_TASK(timer_task);
 
 // Invoke timers
 static void
 timer_dispatch(void)
 {
-    struct timespec tru = timer_repeat_until;
-    for (;;) {
-        // Run the next software timer
-        uint32_t next = sched_timer_dispatch();
-        struct timespec nt = timespec_from_time(next);
-
-        struct timespec now = timespec_read();
-        if (!timespec_is_before(nt, timespec_add(now, TIMER_MIN_TRY_NS))) {
-            // Schedule next timer normally.
-            next_wake_time = nt;
-            return;
-        }
-
-        if (unlikely(timespec_is_before(tru, now))) {
-            // Check if there are too many repeat timers
-            if (unlikely(timespec_is_before(timespec_add(nt, 100000000), now)))
-                try_shutdown("Rescheduled timer in the past");
-            if (sched_tasks_busy()) {
-                timer_repeat_until = timespec_add(now, TIMER_REPEAT_NS);
-                next_wake_time = timespec_add(now, TIMER_DEFER_REPEAT_NS);
-                return;
-            }
-            timer_repeat_until = tru = timespec_add(now, TIMER_IDLE_REPEAT_NS);
-        }
-
-        // Next timer in the past or near future - wait for it to be ready
-        while (unlikely(timespec_is_before(now, nt)))
-            now = timespec_read();
-    }
+    uint32_t next = timer_dispatch_many();
+    next_wake_time = timespec_from_time(next);
+    next_wake_time_counter = next;
 }
 
 void
 timer_init(void)
 {
-    start_sec = timespec_read().tv_sec;
-    timer_repeat_until.tv_sec = start_sec + 2;
-    timespec_update();
+    start_sec = timespec_read().tv_sec + 1;
     timer_kick();
 }
 DECL_INIT(timer_init);