pwmcmds: Use move queue for hard PWM

Signed-off-by: Pascal Pieper <accounts@pascalpieper.de>
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

3 files changed, 61 insertions, 15 deletions

diff --git a/docs/MCU_Commands.md b/docs/MCU_Commands.md
index 133476d8..536d1ce4 100644
--- a/docs/MCU_Commands.md
+++ b/docs/MCU_Commands.md
@@ -197,8 +197,8 @@ only of interest to developers looking to gain insight into Klipper.
   same 'oid' parameter must have been issued during micro-controller
   configuration.
 
-* `schedule_pwm_out oid=%c clock=%u value=%hu` : Schedules a change to
-  a hardware PWM output pin. See the 'schedule_digital_out' and
+* `queue_pwm_out oid=%c clock=%u value=%hu` : Schedules a change to a
+  hardware PWM output pin. See the 'schedule_digital_out' and
   'config_pwm_out' commands for more info.
 
 * `schedule_soft_pwm_out oid=%c clock=%u on_ticks=%u` : Schedules a
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 0cd6a22e..2bf3200c 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -208,6 +208,7 @@ class MCU_pwm:
                     % (self._pin, cycle_ticks,
                        self._start_value * self._pwm_max))
                 return
+            self._mcu.request_move_queue_slot()
             self._oid = self._mcu.create_oid()
             self._mcu.add_config_cmd(
                 "config_pwm_out oid=%d pin=%s cycle_ticks=%d value=%d"
@@ -217,11 +218,11 @@ class MCU_pwm:
                    self._shutdown_value * self._pwm_max,
                    self._mcu.seconds_to_clock(self._max_duration)))
             svalue = int(self._start_value * self._pwm_max + 0.5)
-            self._mcu.add_config_cmd("schedule_pwm_out oid=%d clock=%d value=%d"
+            self._mcu.add_config_cmd("queue_pwm_out oid=%d clock=%d value=%d"
                                      % (self._oid, self._last_clock, svalue),
                                      on_restart=True)
             self._set_cmd = self._mcu.lookup_command(
-                "schedule_pwm_out oid=%c clock=%u value=%hu", cq=cmd_queue)
+                "queue_pwm_out oid=%c clock=%u value=%hu", cq=cmd_queue)
             return
         # Software PWM
         if self._shutdown_value not in [0., 1.]:
diff --git a/src/pwmcmds.c b/src/pwmcmds.c
index fd380f8b..d214a95a 100644
--- a/src/pwmcmds.c
+++ b/src/pwmcmds.c
@@ -6,6 +6,8 @@
 
 #include "basecmd.h" // oid_alloc
 #include "board/gpio.h" // struct gpio_pwm
+#include "board/irq.h" // irq_disable
+#include "board/misc.h" // timer_is_before
 #include "command.h" // DECL_COMMAND
 #include "sched.h" // sched_add_timer
 
@@ -13,7 +15,14 @@ struct pwm_out_s {
     struct timer timer;
     struct gpio_pwm pin;
     uint32_t max_duration;
-    uint16_t value, default_value;
+    uint16_t default_value;
+    struct move_queue_head mq;
+};
+
+struct pwm_move {
+    struct move_node node;
+    uint32_t waketime;
+    uint16_t value;
 };
 
 static uint_fast8_t
@@ -25,12 +34,32 @@ pwm_end_event(struct timer *timer)
 static uint_fast8_t
 pwm_event(struct timer *timer)
 {
+    // Apply next update and remove it from queue
     struct pwm_out_s *p = container_of(timer, struct pwm_out_s, timer);
-    gpio_pwm_write(p->pin, p->value);
-    if (p->value == p->default_value || !p->max_duration)
-        return SF_DONE;
-    p->timer.waketime += p->max_duration;
-    p->timer.func = pwm_end_event;
+    struct move_node *mn = move_queue_pop(&p->mq);
+    struct pwm_move *m = container_of(mn, struct pwm_move, node);
+    uint16_t value = m->value;
+    gpio_pwm_write(p->pin, value);
+    move_free(m);
+
+    // Check if more updates queued
+    if (move_queue_empty(&p->mq)) {
+        if (value == p->default_value || !p->max_duration)
+            return SF_DONE;
+
+        // Start the safety timeout
+        p->timer.waketime += p->max_duration;
+        p->timer.func = pwm_end_event;
+        return SF_RESCHEDULE;
+    }
+
+    // Schedule next update
+    struct move_node *nn = move_queue_first(&p->mq);
+    uint32_t wake = container_of(nn, struct pwm_move, node)->waketime;
+    if (value != p->default_value && p->max_duration
+        && timer_is_before(p->timer.waketime + p->max_duration, wake))
+        shutdown("Scheduled pwm event will exceed max_duration");
+    p->timer.waketime = wake;
     return SF_RESCHEDULE;
 }
 
@@ -43,23 +72,37 @@ command_config_pwm_out(uint32_t *args)
     p->pin = pin;
     p->default_value = args[4];
     p->max_duration = args[5];
+    p->timer.func = pwm_event;
+    move_queue_setup(&p->mq, sizeof(struct pwm_move));
 }
 DECL_COMMAND(command_config_pwm_out,
              "config_pwm_out oid=%c pin=%u cycle_ticks=%u value=%hu"
              " default_value=%hu max_duration=%u");
 
 void
-command_schedule_pwm_out(uint32_t *args)
+command_queue_pwm_out(uint32_t *args)
 {
     struct pwm_out_s *p = oid_lookup(args[0], command_config_pwm_out);
+    struct pwm_move *m = move_alloc();
+    m->waketime = args[1];
+    m->value = args[2];
+
+    irq_disable();
+    int need_add_timer = move_queue_push(&m->node, &p->mq);
+    irq_enable();
+    if (!need_add_timer)
+        return;
+
+    // queue was empty and a timer needs to be added
     sched_del_timer(&p->timer);
+    if (p->timer.func == pwm_end_event
+        && timer_is_before(p->timer.waketime, m->waketime))
+        shutdown("Scheduled pwm event will exceed max_duration");
     p->timer.func = pwm_event;
-    p->timer.waketime = args[1];
-    p->value = args[2];
+    p->timer.waketime = m->waketime;
     sched_add_timer(&p->timer);
 }
-DECL_COMMAND(command_schedule_pwm_out,
-             "schedule_pwm_out oid=%c clock=%u value=%hu");
+DECL_COMMAND(command_queue_pwm_out, "queue_pwm_out oid=%c clock=%u value=%hu");
 
 void
 pwm_shutdown(void)
@@ -68,6 +111,8 @@ pwm_shutdown(void)
     struct pwm_out_s *p;
     foreach_oid(i, p, command_config_pwm_out) {
         gpio_pwm_write(p->pin, p->default_value);
+        p->timer.func = pwm_event;
+        move_queue_clear(&p->mq);
     }
 }
 DECL_SHUTDOWN(pwm_shutdown);