4 files changed, 24 insertions, 1 deletions
diff --git a/src/generic/armcm_irq.c b/src/generic/armcm_irq.c
index 7778c389..9b5a67d5 100644
--- a/src/generic/armcm_irq.c
+++ b/src/generic/armcm_irq.c
@@ -35,6 +35,12 @@ irq_restore(irqstatus_t flag)
 }
 
 void
+irq_wait(void)
+{
+    asm volatile("cpsie i\n    wfi\n    cpsid i\n" ::: "memory");
+}
+
+void
 irq_poll(void)
 {
 }
diff --git a/src/generic/irq.h b/src/generic/irq.h
index 1e39d4e6..bbabf029 100644
--- a/src/generic/irq.h
+++ b/src/generic/irq.h
@@ -9,6 +9,7 @@ void irq_disable(void);
 void irq_enable(void);
 irqstatus_t irq_save(void);
 void irq_restore(irqstatus_t flag);
+void irq_wait(void);
 void irq_poll(void);
 
 #endif // irq.h
diff --git a/src/generic/timer_irq.c b/src/generic/timer_irq.c
index 07161eee..6c14f9ae 100644
--- a/src/generic/timer_irq.c
+++ b/src/generic/timer_irq.c
@@ -8,6 +8,7 @@
 #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_kick
 
@@ -85,9 +86,23 @@ timer_dispatch_many(void)
 void
 timer_task(void)
 {
+    static uint32_t last_timer;
+    uint32_t lst = last_timer;
     irq_disable();
-    timer_repeat_until = timer_read_time() + TIMER_IDLE_REPEAT_TICKS;
+    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;
     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 031f0d6b..ad6ad033 100644
--- a/src/generic/timer_irq.h
+++ b/src/generic/timer_irq.h
@@ -2,5 +2,6 @@
 #define __GENERIC_TIMER_IRQ_H
 
 uint32_t timer_dispatch_many(void);
+uint32_t timer_get_next(void);
 
 #endif // timer_irq.h