gpiocmds: Use move queue for digital output pins

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

3 files changed, 77 insertions, 30 deletions

diff --git a/docs/MCU_Commands.md b/docs/MCU_Commands.md
index 536d1ce4..8b20c6a4 100644
--- a/docs/MCU_Commands.md
+++ b/docs/MCU_Commands.md
@@ -113,18 +113,18 @@ This section lists some commonly used config commands.
   digital output mode and set to an initial value as specified by
   'value' (0 for low, 1 for high). Creating a digital_out object
   allows the host to schedule GPIO updates for the given pin at
-  specified times (see the schedule_digital_out command described
-  below). Should the micro-controller software go into shutdown mode
-  then all configured digital_out objects will be set to
-  'default_value'. The 'max_duration' parameter is used to implement a
-  safety check - if it is non-zero then it is the maximum number of
-  clock ticks that the host may set the given GPIO to a non-default
-  value without further updates. For example, if the default_value is
-  zero and the max_duration is 16000 then if the host sets the gpio to
-  a value of one then it must schedule another update to the gpio pin
-  (to either zero or one) within 16000 clock ticks. This safety
-  feature can be used with heater pins to ensure the host does not
-  enable the heater and then go off-line.
+  specified times (see the queue_digital_out command described below).
+  Should the micro-controller software go into shutdown mode then all
+  configured digital_out objects will be set to 'default_value'. The
+  'max_duration' parameter is used to implement a safety check - if it
+  is non-zero then it is the maximum number of clock ticks that the
+  host may set the given GPIO to a non-default value without further
+  updates. For example, if the default_value is zero and the
+  max_duration is 16000 then if the host sets the gpio to a value of
+  one then it must schedule another update to the gpio pin (to either
+  zero or one) within 16000 clock ticks. This safety feature can be
+  used with heater pins to ensure the host does not enable the heater
+  and then go off-line.
 
 * `config_pwm_out oid=%c pin=%u cycle_ticks=%u value=%hu
   default_value=%hu max_duration=%u` : This command creates an
@@ -191,21 +191,21 @@ Common commands
 This section lists some commonly used run-time commands. It is likely
 only of interest to developers looking to gain insight into Klipper.
 
-* `schedule_digital_out oid=%c clock=%u value=%c` : This command will
+* `queue_digital_out oid=%c clock=%u value=%c` : This command will
   schedule a change to a digital output GPIO pin at the given clock
   time. To use this command a 'config_digital_out' command with the
   same 'oid' parameter must have been issued during micro-controller
   configuration.
 
 * `queue_pwm_out oid=%c clock=%u value=%hu` : Schedules a change to a
-  hardware PWM output pin. See the 'schedule_digital_out' and
+  hardware PWM output pin. See the 'queue_digital_out' and
   'config_pwm_out' commands for more info.
 
 * `schedule_soft_pwm_out oid=%c clock=%u on_ticks=%u` : Schedules a
   change to a software PWM output pin. Because the output switching is
   implemented in the micro-controller software, it is recommended that
   the sum of on_ticks and off_ticks parameters corresponds to a time
-  of 10ms or greater. See the 'schedule_digital_out' and
+  of 10ms or greater. See the 'queue_digital_out' and
   'config_soft_pwm_out' commands for more info.
 
 * `query_analog_in oid=%c clock=%u sample_ticks=%u sample_count=%c
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 2bf3200c..a4924163 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -143,18 +143,19 @@ class MCU_digital_out:
             self._mcu.add_config_cmd("set_digital_out pin=%s value=%d"
                                      % (self._pin, self._start_value))
             return
+        self._mcu.request_move_queue_slot()
         self._oid = self._mcu.create_oid()
         self._mcu.add_config_cmd(
             "config_digital_out oid=%d pin=%s value=%d default_value=%d"
-            " max_duration=%d" % (
-                self._oid, self._pin, self._start_value, self._shutdown_value,
-                self._mcu.seconds_to_clock(self._max_duration)))
+            " max_duration=%d"
+            % (self._oid, self._pin, self._start_value, self._shutdown_value,
+               self._mcu.seconds_to_clock(self._max_duration)))
         self._mcu.add_config_cmd("update_digital_out oid=%d value=%d"
                                  % (self._oid, self._start_value),
                                  on_restart=True)
         cmd_queue = self._mcu.alloc_command_queue()
         self._set_cmd = self._mcu.lookup_command(
-            "schedule_digital_out oid=%c clock=%u value=%c", cq=cmd_queue)
+            "queue_digital_out oid=%c clock=%u value=%c", cq=cmd_queue)
     def set_digital(self, print_time, value):
         clock = self._mcu.print_time_to_clock(print_time)
         self._set_cmd.send([self._oid, clock, (not not value) ^ self._invert],
diff --git a/src/gpiocmds.c b/src/gpiocmds.c
index a9eabdac..7871d33d 100644
--- a/src/gpiocmds.c
+++ b/src/gpiocmds.c
@@ -20,7 +20,14 @@ struct digital_out_s {
     struct timer timer;
     struct gpio_out pin;
     uint32_t max_duration;
-    uint8_t value, default_value;
+    uint8_t default_value;
+    struct move_queue_head mq;
+};
+
+struct digital_move {
+    struct move_node node;
+    uint32_t waketime;
+    uint8_t value;
 };
 
 static uint_fast8_t
@@ -32,12 +39,32 @@ digital_end_event(struct timer *timer)
 static uint_fast8_t
 digital_out_event(struct timer *timer)
 {
+    // Apply next update and remove it from queue
     struct digital_out_s *d = container_of(timer, struct digital_out_s, timer);
-    gpio_out_write(d->pin, d->value);
-    if (d->value == d->default_value || !d->max_duration)
-        return SF_DONE;
-    d->timer.waketime += d->max_duration;
-    d->timer.func = digital_end_event;
+    struct move_node *mn = move_queue_pop(&d->mq);
+    struct digital_move *m = container_of(mn, struct digital_move, node);
+    uint8_t value = m->value;
+    gpio_out_write(d->pin, value);
+    move_free(m);
+
+    // Check if more updates queued
+    if (move_queue_empty(&d->mq)) {
+        if (value == d->default_value || !d->max_duration)
+            return SF_DONE;
+
+        // Start the safety timeout
+        d->timer.waketime += d->max_duration;
+        d->timer.func = digital_end_event;
+        return SF_RESCHEDULE;
+    }
+
+    // Schedule next update
+    struct move_node *nn = move_queue_first(&d->mq);
+    uint32_t wake = container_of(nn, struct digital_move, node)->waketime;
+    if (value != d->default_value && d->max_duration
+        && timer_is_before(d->timer.waketime + d->max_duration, wake))
+        shutdown("Scheduled digital out event will exceed max_duration");
+    d->timer.waketime = wake;
     return SF_RESCHEDULE;
 }
 
@@ -50,29 +77,46 @@ command_config_digital_out(uint32_t *args)
     d->pin = pin;
     d->default_value = args[3];
     d->max_duration = args[4];
+    d->timer.func = digital_out_event;
+    move_queue_setup(&d->mq, sizeof(struct digital_move));
 }
 DECL_COMMAND(command_config_digital_out,
              "config_digital_out oid=%c pin=%u value=%c default_value=%c"
              " max_duration=%u");
 
 void
-command_schedule_digital_out(uint32_t *args)
+command_queue_digital_out(uint32_t *args)
 {
     struct digital_out_s *d = oid_lookup(args[0], command_config_digital_out);
+    struct digital_move *m = move_alloc();
+    m->waketime = args[1];
+    m->value = args[2];
+
+    irq_disable();
+    int need_add_timer = move_queue_push(&m->node, &d->mq);
+    irq_enable();
+    if (!need_add_timer)
+        return;
+
+    // queue was empty and a timer needs to be added
     sched_del_timer(&d->timer);
+    if (d->timer.func == digital_end_event
+        && timer_is_before(d->timer.waketime, m->waketime))
+        shutdown("Scheduled digital out event will exceed max_duration");
     d->timer.func = digital_out_event;
-    d->timer.waketime = args[1];
-    d->value = args[2];
+    d->timer.waketime = m->waketime;
     sched_add_timer(&d->timer);
 }
-DECL_COMMAND(command_schedule_digital_out,
-             "schedule_digital_out oid=%c clock=%u value=%c");
+DECL_COMMAND(command_queue_digital_out,
+             "queue_digital_out oid=%c clock=%u value=%c");
 
 void
 command_update_digital_out(uint32_t *args)
 {
     struct digital_out_s *d = oid_lookup(args[0], command_config_digital_out);
     sched_del_timer(&d->timer);
+    if (!move_queue_empty(&d->mq))
+        shutdown("update_digital_out not valid with active queue");
     uint8_t value = args[1];
     gpio_out_write(d->pin, value);
     if (value != d->default_value && d->max_duration) {
@@ -90,6 +134,8 @@ digital_out_shutdown(void)
     struct digital_out_s *d;
     foreach_oid(i, d, command_config_digital_out) {
         gpio_out_write(d->pin, d->default_value);
+        d->timer.func = digital_out_event;
+        move_queue_clear(&d->mq);
     }
 }
 DECL_SHUTDOWN(digital_out_shutdown);