irq: Support sleeping when mcu is idle

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

2 files changed, 29 insertions, 1 deletions

diff --git a/src/avr/irq.h b/src/avr/irq.h
index 33172c0f..349c4c43 100644
--- a/src/avr/irq.h
+++ b/src/avr/irq.h
@@ -28,6 +28,10 @@ static inline void irq_restore(irqstatus_t flag) {
     SREG = flag;
 }
 
+static inline void irq_wait(void) {
+    asm("sei\n    sleep\n    cli" : : : "memory");
+}
+
 static inline void irq_poll(void) {
 }
 
diff --git a/src/avr/timer.c b/src/avr/timer.c
index 92459af3..d9397187 100644
--- a/src/avr/timer.c
+++ b/src/avr/timer.c
@@ -6,6 +6,7 @@
 
 #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
@@ -62,6 +63,12 @@ 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)
 {
@@ -103,6 +110,9 @@ timer_init(void)
     // enable interrupt
     TIMSK1 = 1<<OCIE1A;
     irq_restore(flag);
+
+    // Enable idle on sleep instruction
+    SMCR = 0x01;
 }
 DECL_INIT(timer_init);
 
@@ -196,8 +206,22 @@ done:
 void
 timer_task(void)
 {
+    static uint16_t last_timer;
+    uint16_t lst = last_timer;
     irq_disable();
-    timer_repeat_set(timer_get() + TIMER_IDLE_REPEAT_TICKS);
+    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);
     irq_enable();
+    stats_note_sleep(post_sleep - cur);
 }
 DECL_TASK(timer_task);