stepper: Use a reusable interface to the "move queue"

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

3 files changed, 81 insertions, 31 deletions

diff --git a/src/basecmd.c b/src/basecmd.c
index 383821a6..0cf8afec 100644
--- a/src/basecmd.c
+++ b/src/basecmd.c
@@ -58,11 +58,7 @@ alloc_chunks(size_t size, size_t count, uint16_t *avail)
  * Move queue
  ****************************************************************/
 
-struct move_freed {
-    struct move_freed *next;
-};
-
-static struct move_freed *move_free_list;
+static struct move_node *move_free_list;
 static void *move_list;
 static uint16_t move_count;
 static uint8_t move_item_size;
@@ -79,7 +75,7 @@ is_finalized(void)
 void
 move_free(void *m)
 {
-    struct move_freed *mf = m;
+    struct move_node *mf = m;
     mf->next = move_free_list;
     move_free_list = mf;
 }
@@ -89,7 +85,7 @@ void *
 move_alloc(void)
 {
     irqstatus_t flag = irq_save();
-    struct move_freed *mf = move_free_list;
+    struct move_node *mf = move_free_list;
     if (!mf)
         shutdown("Move queue overflow");
     move_free_list = mf->next;
@@ -97,10 +93,56 @@ move_alloc(void)
     return mf;
 }
 
-// Request minimum size of runtime allocations returned by move_alloc()
+// Check if a move_queue is empty
+int
+move_queue_empty(struct move_queue_head *mh)
+{
+    return mh->first == NULL;
+}
+
+// Return first node in a move queue
+struct move_node *
+move_queue_first(struct move_queue_head *mh)
+{
+    return mh->first;
+}
+
+// Add move to queue
+int
+move_queue_push(struct move_node *m, struct move_queue_head *mh)
+{
+    m->next = NULL;
+    if (mh->first) {
+        mh->last->next = m;
+        mh->last = m;
+        return 0;
+    }
+    mh->first = mh->last = m;
+    return 1;
+}
+
+// Remove first item from queue (caller must ensure queue not empty)
+struct move_node *
+move_queue_pop(struct move_queue_head *mh)
+{
+    struct move_node *mn = mh->first;
+    mh->first = mn->next;
+    return mn;
+}
+
+// Completely clear move queue (used in shutdown handlers)
 void
-move_request_size(int size)
+move_queue_clear(struct move_queue_head *mh)
 {
+    mh->first = NULL;
+}
+
+// Initialize a move_queue with nodes of the give size
+void
+move_queue_setup(struct move_queue_head *mh, int size)
+{
+    mh->first = mh->last = NULL;
+
     if (size > UINT8_MAX || is_finalized())
         shutdown("Invalid move request size");
     if (size > move_item_size)
@@ -115,10 +157,10 @@ move_reset(void)
     // Add everything in move_list to the free list.
     uint32_t i;
     for (i=0; i<move_count-1; i++) {
-        struct move_freed *mf = move_list + i*move_item_size;
+        struct move_node *mf = move_list + i*move_item_size;
         mf->next = move_list + (i + 1)*move_item_size;
     }
-    struct move_freed *mf = move_list + (move_count - 1)*move_item_size;
+    struct move_node *mf = move_list + (move_count - 1)*move_item_size;
     mf->next = NULL;
     move_free_list = move_list;
 }
@@ -129,7 +171,8 @@ move_finalize(void)
 {
     if (is_finalized())
         shutdown("Already finalized");
-    move_request_size(sizeof(*move_free_list));
+    struct move_queue_head dummy;
+    move_queue_setup(&dummy, sizeof(*move_free_list));
     move_list = alloc_chunks(move_item_size, 1024, &move_count);
     move_reset();
 }
diff --git a/src/basecmd.h b/src/basecmd.h
index f4ea1cc7..a6464cad 100644
--- a/src/basecmd.h
+++ b/src/basecmd.h
@@ -4,10 +4,22 @@
 #include <stddef.h> // size_t
 #include <stdint.h> // uint8_t
 
+struct move_node {
+    struct move_node *next;
+};
+struct move_queue_head {
+    struct move_node *first, *last;
+};
+
 void *alloc_chunk(size_t size);
 void move_free(void *m);
 void *move_alloc(void);
-void move_request_size(int size);
+int move_queue_empty(struct move_queue_head *mh);
+struct move_node *move_queue_first(struct move_queue_head *mh);
+int move_queue_push(struct move_node *m, struct move_queue_head *mh);
+struct move_node *move_queue_pop(struct move_queue_head *mh);
+void move_queue_clear(struct move_queue_head *mh);
+void move_queue_setup(struct move_queue_head *mh, 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);
diff --git a/src/stepper.c b/src/stepper.c
index e8dedaf9..f191be6e 100644
--- a/src/stepper.c
+++ b/src/stepper.c
@@ -21,10 +21,10 @@ DECL_CONSTANT("STEP_DELAY", CONFIG_STEP_DELAY);
  ****************************************************************/
 
 struct stepper_move {
+    struct move_node node;
     uint32_t interval;
     int16_t add;
     uint16_t count;
-    struct stepper_move *next;
     uint8_t flags;
 };
 
@@ -43,7 +43,7 @@ struct stepper {
 #endif
     struct gpio_out step_pin, dir_pin;
     uint32_t position;
-    struct stepper_move *first, **plast;
+    struct move_queue_head mq;
     uint32_t min_stop_interval;
     // gcc (pre v6) does better optimization when uint8_t are bitfields
     uint8_t flags : 8;
@@ -60,8 +60,7 @@ enum {
 static uint_fast8_t
 stepper_load_next(struct stepper *s, uint32_t min_next_time)
 {
-    struct stepper_move *m = s->first;
-    if (!m) {
+    if (move_queue_empty(&s->mq)) {
         // There is no next move - the queue is empty
         if (s->interval - s->add < s->min_stop_interval
             && !(s->flags & SF_NO_NEXT_CHECK))
@@ -71,6 +70,8 @@ stepper_load_next(struct stepper *s, uint32_t min_next_time)
     }
 
     // Load next 'struct stepper_move' into 'struct stepper'
+    struct move_node *mn = move_queue_pop(&s->mq);
+    struct stepper_move *m = container_of(mn, struct stepper_move, node);
     s->next_step_time += m->interval;
     s->add = m->add;
     s->interval = m->interval + m->add;
@@ -101,7 +102,6 @@ stepper_load_next(struct stepper *s, uint32_t min_next_time)
         s->position += m->count;
     }
 
-    s->first = m->next;
     move_free(m);
     return SF_RESCHEDULE;
 }
@@ -191,7 +191,7 @@ command_config_stepper(uint32_t *args)
     s->dir_pin = gpio_out_setup(args[2], 0);
     s->min_stop_interval = args[3];
     s->position = -POSITION_BIAS;
-    move_request_size(sizeof(struct stepper_move));
+    move_queue_setup(&s->mq, sizeof(struct stepper_move));
 }
 DECL_COMMAND(command_config_stepper,
              "config_stepper oid=%c step_pin=%c dir_pin=%c"
@@ -215,7 +215,6 @@ command_queue_step(uint32_t *args)
     if (!m->count)
         shutdown("Invalid count parameter");
     m->add = args[3];
-    m->next = NULL;
     m->flags = 0;
 
     irq_disable();
@@ -231,16 +230,12 @@ command_queue_step(uint32_t *args)
     flags &= ~SF_LAST_RESET;
     if (s->count) {
         s->flags = flags;
-        if (s->first)
-            *s->plast = m;
-        else
-            s->first = m;
-        s->plast = &m->next;
+        move_queue_push(&m->node, &s->mq);
     } else if (flags & SF_NEED_RESET) {
         move_free(m);
     } else {
         s->flags = flags;
-        s->first = m;
+        move_queue_push(&m->node, &s->mq);
         stepper_load_next(s, s->next_step_time + m->interval);
         sched_add_timer(&s->time);
     }
@@ -317,10 +312,10 @@ stepper_stop(struct stepper *s)
     s->flags = (s->flags & SF_INVERT_STEP) | SF_NEED_RESET;
     gpio_out_write(s->dir_pin, 0);
     gpio_out_write(s->step_pin, s->flags & SF_INVERT_STEP);
-    while (s->first) {
-        struct stepper_move *next = s->first->next;
-        move_free(s->first);
-        s->first = next;
+    while (!move_queue_empty(&s->mq)) {
+        struct move_node *mn = move_queue_pop(&s->mq);
+        struct stepper_move *m = container_of(mn, struct stepper_move, node);
+        move_free(m);
     }
 }
 
@@ -330,7 +325,7 @@ stepper_shutdown(void)
     uint8_t i;
     struct stepper *s;
     foreach_oid(i, s, command_config_stepper) {
-        s->first = NULL;
+        move_queue_clear(&s->mq);
         stepper_stop(s);
     }
 }