From 697412d25c572d17821561db88abb992f944b18b Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Fri, 20 Nov 2020 18:57:47 -0500 Subject: [PATCH] stepper: Use a reusable interface to the "move queue" Signed-off-by: Kevin O'Connor --- src/basecmd.c | 69 +++++++++++++++++++++++++++++++++++++++++---------- src/basecmd.h | 14 ++++++++++- src/stepper.c | 31 ++++++++++------------- 3 files changed, 82 insertions(+), 32 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() -void -move_request_size(int size) +// 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_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; inext = 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 // size_t #include // 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); } }