1 files changed, 56 insertions, 11 deletions

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);