From 118fd21cb815d1c27e7e5bd6b394369bdf095919 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Wed, 12 Jul 2017 22:16:16 -0400
Subject: [PATCH] irq: Support sleeping when mcu is idle

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/mcu.py           |  6 ++++--
 src/avr/irq.h           |  4 ++++
 src/avr/timer.c         | 26 +++++++++++++++++++++++++-
 src/basecmd.c           | 18 ++++++++++++++----
 src/basecmd.h           |  1 +
 src/generic/armcm_irq.c |  6 ++++++
 src/generic/irq.h       |  1 +
 src/generic/timer_irq.c | 17 ++++++++++++++++-
 src/generic/timer_irq.h |  1 +
 src/pru/main.c          | 12 ++++++++++++
 src/pru/pru0.c          |  7 +++++--
 src/sam3x8e/timer.c     |  7 +++++++
 src/simulator/main.c    |  5 +++++
 13 files changed, 101 insertions(+), 10 deletions(-)

diff --git a/klippy/mcu.py b/klippy/mcu.py
index cf85b740..f4e3e861 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -420,6 +420,7 @@ class MCU:
         self._stats_sumsq_base = 0.
         self._mcu_tick_avg = 0.
         self._mcu_tick_stddev = 0.
+        self._mcu_tick_awake = 0.
     def handle_mcu_stats(self, params):
         count = params['count']
         tick_sum = params['sum']
@@ -427,6 +428,7 @@ class MCU:
         self._mcu_tick_avg = tick_sum * c
         tick_sumsq = params['sumsq'] * self._stats_sumsq_base
         self._mcu_tick_stddev = c * math.sqrt(count*tick_sumsq - tick_sum**2)
+        self._mcu_tick_awake = tick_sum / self._mcu_freq
     def handle_shutdown(self, params):
         if self.is_shutdown:
             return
@@ -493,9 +495,9 @@ class MCU:
             self._ffi_lib.steppersync_free(self._steppersync)
             self._steppersync = None
     def stats(self, eventtime):
-        return "%s mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
+        return "%s mcu_awake=%.03f mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
             self.serial.stats(eventtime),
-            self._mcu_tick_avg, self._mcu_tick_stddev)
+            self._mcu_tick_awake, self._mcu_tick_avg, self._mcu_tick_stddev)
     def force_shutdown(self):
         self.send(self._emergency_stop_cmd.encode())
     def microcontroller_restart(self):
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);
diff --git a/src/basecmd.c b/src/basecmd.c
index 742a9e26..7a2f7882 100644
--- a/src/basecmd.c
+++ b/src/basecmd.c
@@ -188,6 +188,7 @@ command_get_status(uint32_t *args)
 DECL_COMMAND_FLAGS(command_get_status, HF_IN_SHUTDOWN, "get_status");
 
 static uint32_t stats_send_time, stats_send_time_high;
+static uint32_t stats_last_time, stats_sleep_time;
 
 void
 command_get_uptime(uint32_t *args)
@@ -198,16 +199,23 @@ command_get_uptime(uint32_t *args)
 }
 DECL_COMMAND_FLAGS(command_get_uptime, HF_IN_SHUTDOWN, "get_uptime");
 
+void
+stats_note_sleep(uint32_t sleep_time)
+{
+    stats_sleep_time += sleep_time;
+    stats_last_time += sleep_time;
+}
+
 #define SUMSQ_BASE 256
 DECL_CONSTANT(STATS_SUMSQ_BASE, SUMSQ_BASE);
 
 void
 stats_task(void)
 {
-    static uint32_t last, count, sumsq;
+    static uint32_t count, sumsq;
     uint32_t cur = timer_read_time();
-    uint32_t diff = cur - last;
-    last = cur;
+    uint32_t diff = cur - stats_last_time;
+    stats_last_time = cur;
     count++;
     // Calculate sum of diff^2 - be careful of integer overflow
     uint32_t nextsumsq;
@@ -224,10 +232,12 @@ stats_task(void)
 
     if (timer_is_before(cur, stats_send_time + timer_from_us(5000000)))
         return;
-    sendf("stats count=%u sum=%u sumsq=%u", count, cur - stats_send_time, sumsq);
+    sendf("stats count=%u sum=%u sumsq=%u"
+          , count, cur - stats_send_time - stats_sleep_time, sumsq);
     if (cur < stats_send_time)
         stats_send_time_high++;
     stats_send_time = cur;
+    stats_sleep_time = 0;
     count = sumsq = 0;
 }
 DECL_TASK(stats_task);
diff --git a/src/basecmd.h b/src/basecmd.h
index 987d7498..713343e2 100644
--- a/src/basecmd.h
+++ b/src/basecmd.h
@@ -9,6 +9,7 @@ void move_request_size(int size);
 void *oid_lookup(uint8_t oid, void *type);
 void *oid_alloc(uint8_t oid, void *type, uint16_t size);
 void *oid_next(uint8_t *i, void *type);
+void stats_note_sleep(uint32_t sleep_time);
 
 #define foreach_oid(pos,data,oidtype)                   \
     for (pos=-1; (data=oid_next(&pos, oidtype)); )
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
@@ -34,6 +34,12 @@ irq_restore(irqstatus_t flag)
     asm volatile("msr primask, %0" :: "r" (flag) : "memory");
 }
 
+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
diff --git a/src/pru/main.c b/src/pru/main.c
index 3ead0bde..021bee3f 100644
--- a/src/pru/main.c
+++ b/src/pru/main.c
@@ -46,12 +46,24 @@ irq_restore(irqstatus_t flag)
 {
 }
 
+void
+irq_wait(void)
+{
+    asm("slp 1");
+}
+
 static void
 timer_set(uint32_t value)
 {
     CT_IEP.TMR_CMP0 = value;
 }
 
+uint32_t
+timer_get_next(void)
+{
+    return CT_IEP.TMR_CMP0;
+}
+
 uint32_t
 timer_read_time(void)
 {
diff --git a/src/pru/pru0.c b/src/pru/pru0.c
index 3a8fd421..c6a63cb9 100644
--- a/src/pru/pru0.c
+++ b/src/pru/pru0.c
@@ -135,8 +135,7 @@ static void
 process_io(void)
 {
     for (;;) {
-        if (!(read_r31() & (1 << (WAKE_PRU0_IRQ + R31_IRQ_OFFSET))))
-            continue;
+        asm("slp 1");
         CT_INTC.SECR0 = (1 << KICK_PRU0_FROM_ARM_EVENT) | (1 << KICK_PRU0_EVENT);
         check_can_send();
         check_can_read();
@@ -327,6 +326,10 @@ main(void)
                              , CHAN_DESC, CHAN_PORT) != PRU_RPMSG_SUCCESS)
         ;
 
+    // Allow PRU0 and PRU1 to wake from sleep
+    PRU0_CTRL.WAKEUP_EN = 1 << (WAKE_PRU0_IRQ + R31_IRQ_OFFSET);
+    PRU1_CTRL.WAKEUP_EN = 1 << (WAKE_PRU1_IRQ + R31_IRQ_OFFSET);
+
     // Wait for PRU1 to be ready
     memset(SHARED_MEM, 0, sizeof(*SHARED_MEM));
     writel(&SHARED_MEM->signal, SIGNAL_PRU0_WAITING);
diff --git a/src/sam3x8e/timer.c b/src/sam3x8e/timer.c
index 0bc21dae..ba1124cb 100644
--- a/src/sam3x8e/timer.c
+++ b/src/sam3x8e/timer.c
@@ -18,6 +18,13 @@ timer_set(uint32_t value)
     TC0->TC_CHANNEL[0].TC_RA = value;
 }
 
+// Return the next scheduled wake up time
+uint32_t
+timer_get_next(void)
+{
+    return TC0->TC_CHANNEL[0].TC_RA;
+}
+
 // Return the current time (in absolute clock ticks).
 uint32_t
 timer_read_time(void)
diff --git a/src/simulator/main.c b/src/simulator/main.c
index f0e18737..ccd6dd93 100644
--- a/src/simulator/main.c
+++ b/src/simulator/main.c
@@ -47,6 +47,11 @@ irq_restore(irqstatus_t flag)
     Interrupt_off = flag;
 }
 
+void
+irq_wait(void)
+{
+}
+
 void
 irq_poll(void)
 {