From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Sat, 10 Sep 2016 11:21:14 +0000 (-0700)
Subject: Cleaned up buffer.c
X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=c82c466cd4ce0d55435675139f5ef08d8c407a6e;p=bbctrl-firmware

Cleaned up buffer.c
---

diff --git a/src/command.c b/src/command.c
index 16397d2..dc94728 100644
--- a/src/command.c
+++ b/src/command.c
@@ -227,7 +227,7 @@ void command_callback() {
   default:
     if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;}
     else if (mp_is_flushing()) break; // Flush GCode command
-    else if (!mp_get_planner_buffer_room() ||
+    else if (!mp_queue_get_room() ||
              mp_is_resuming() ||
              mach_arc_active() ||
              mach_is_homing() ||
diff --git a/src/machine.c b/src/machine.c
index 0ec4f90..d46f41f 100644
--- a/src/machine.c
+++ b/src/machine.c
@@ -44,12 +44,12 @@
  *   - Call the mach_xxx_xxx() function which will do any input validation and
  *     return an error if it detects one.
  *
- *   - The mach_ function calls mp_queue_command().  Arguments are a callback to
+ *   - The mach_ function calls mp_queue__command().  Arguments are a callback to
  *     the _exec_...() function, which is the runtime execution routine, and any
  *     arguments that are needed by the runtime. See typedef for *exec in
  *     planner.h for details
  *
- *   - mp_queue_command() stores the callback and the args in a planner buffer.
+ *   - mp_queue__command() stores the callback and the args in a planner buffer.
  *
  *   - When planner execution reaches the buffer it executes the callback w/ the
  *     args.  Take careful note that the callback executes under an interrupt,
@@ -285,7 +285,7 @@ static void _exec_spindle_speed(float *value, float *flag) {
 /// Queue the S parameter to the planner buffer
 void mach_set_spindle_speed(float speed) {
   float value[AXES] = {speed};
-  mp_queue_command(_exec_spindle_speed, value, value);
+  mp_queue__command(_exec_spindle_speed, value, value);
 }
 
 
@@ -300,7 +300,7 @@ static void _exec_spindle_mode(float *value, float *flag) {
 /// Queue the spindle command to the planner buffer
 void mach_set_spindle_mode(spindle_mode_t mode) {
   float value[AXES] = {mode};
-  mp_queue_command(_exec_spindle_mode, value, value);
+  mp_queue__command(_exec_spindle_mode, value, value);
 }
 
 
@@ -411,11 +411,9 @@ void mach_update_work_offsets() {
   }
 
   if (!same) {
-    mp_buffer_t *bf = mp_get_write_buffer();
-    bf->bf_func = _exec_update_work_offsets;
+    mp_buffer_t *bf = mp_queue_get_tail();
     copy_vector(bf->target, work_offset);
-
-    mp_commit_write_buffer(mach.gm.line);
+    mp_queue_push(_exec_update_work_offsets, mach.gm.line);
   }
 }
 
@@ -783,7 +781,7 @@ void mach_set_absolute_origin(float origin[], float flag[]) {
       mp_set_axis_position(axis, value[axis]);     // set mm position
     }
 
-  mp_queue_command(_exec_absolute_origin, value, flag);
+  mp_queue__command(_exec_absolute_origin, value, flag);
 }
 
 
@@ -956,7 +954,7 @@ static void _exec_mist_coolant_control(float *value, float *flag) {
 void mach_mist_coolant_control(bool mist_coolant) {
   mach.gm.mist_coolant = mist_coolant;
   float value[AXES] = {mist_coolant};
-  mp_queue_command(_exec_mist_coolant_control, value, value);
+  mp_queue__command(_exec_mist_coolant_control, value, value);
 }
 
 
@@ -970,7 +968,7 @@ static void _exec_flood_coolant_control(float *value, float *flag) {
 void mach_flood_coolant_control(bool flood_coolant) {
   mach.gm.flood_coolant = flood_coolant;
   float value[AXES] = {flood_coolant};
-  mp_queue_command(_exec_flood_coolant_control, value, value);
+  mp_queue__command(_exec_flood_coolant_control, value, value);
 }
 
 
@@ -1047,11 +1045,7 @@ static stat_t _exec_program_stop(mp_buffer_t *bf) {
 
 /// M0 Queue a program stop
 void mach_program_stop() {
-  mp_buffer_t *bf = mp_get_write_buffer();
-  if (!bf) {CM_ALARM(STAT_BUFFER_FULL_FATAL); return;} // Should not fail
-
-  bf->bf_func = _exec_program_stop;
-  mp_commit_write_buffer(mach.gm.line);
+  mp_queue_push(_exec_program_stop, mach.gm.line);
 }
 
 
diff --git a/src/plan/arc.c b/src/plan/arc.c
index 6578f73..db5308b 100644
--- a/src/plan/arc.c
+++ b/src/plan/arc.c
@@ -447,7 +447,7 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints
  *  as many arc segments (lines) as it can before it blocks, then returns.
  */
 void mach_arc_callback() {
-  while (arc.running && mp_get_planner_buffer_room()) {
+  while (arc.running && mp_queue_get_room()) {
     if (arc.segments == 1) { // Final segment
       arc.position[arc.plane_axis_0] = arc.target[arc.plane_axis_0];
       arc.position[arc.plane_axis_1] = arc.target[arc.plane_axis_1];
diff --git a/src/plan/buffer.c b/src/plan/buffer.c
index b09cdfc..7ed8cf3 100644
--- a/src/plan/buffer.c
+++ b/src/plan/buffer.c
@@ -28,25 +28,8 @@
 \******************************************************************************/
 
 /* Planner buffers are used to queue and operate on Gcode blocks. Each
- * buffer contains one Gcode block which may be a move, and M code, or
+ * buffer contains one Gcode block which may be a move, M code or
  * other command that must be executed synchronously with movement.
- *
- * Buffers are in a circularly linked list managed by a WRITE pointer
- * and a RUN pointer.  New blocks are populated by (1) getting a write
- * buffer, (2) populating the buffer, then (3) placing it in the queue
- * (queue write buffer). If an exception occurs during population you
- * can unget the write buffer before queuing it, which returns it to
- * the pool of available buffers.
- *
- * The RUN buffer is the buffer currently executing. It may be
- * retrieved once for simple commands, or multiple times for
- * long-running commands like moves. When the command is complete the
- * run buffer is returned to the pool by freeing it.
- *
- * Notes:
- *    The write buffer pointer only moves forward on _queue_write_buffer, and
- *    the read buffer pointer only moves forward on free_read calls.
- *    (test, get and unget have no effect)
  */
 
 #include "buffer.h"
@@ -56,153 +39,115 @@
 #include <string.h>
 
 
-typedef struct {                            // ring buffer for sub-moves
-  volatile uint8_t buffers_available;       // count of available buffers
-  mp_buffer_t *w;                           // get_write_buffer pointer
-  mp_buffer_t *q;                           // queue_write_buffer pointer
-  mp_buffer_t *r;                           // get/end_run_buffer pointer
-  mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE]; // buffer storage
+typedef struct {
+  uint8_t space;
+  mp_buffer_t *tail;
+  mp_buffer_t *head;
+  mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE];
 } buffer_pool_t;
 
 
-buffer_pool_t mb; // move buffer queue
-
+buffer_pool_t mb;
 
-/// buffer incr & wrap
-#define _bump(a) ((a < PLANNER_BUFFER_POOL_SIZE - 1) ? a + 1 : 0)
 
+/// Zeroes the contents of a buffer
+static void _clear_buffer(mp_buffer_t *bf) {
+  mp_buffer_t *next = bf->next;            // save pointers
+  mp_buffer_t *prev = bf->prev;
+  memset(bf, 0, sizeof(mp_buffer_t));
+  bf->next = next;                         // restore pointers
+  bf->prev = prev;
+}
 
-/// Initializes or resets buffers
-void mp_init_buffers() {
-  mp_buffer_t *pv;
-
-  memset(&mb, 0, sizeof(mb));      // clear all values, pointers and status
-
-  mb.w = mb.q = mb.r = &mb.bf[0];  // init write and read buffer pointers
-  pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE - 1];
 
-  // setup ring pointers
-  for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
-    mb.bf[i].nx = &mb.bf[_bump(i)];
-    mb.bf[i].pv = pv;
-    pv = &mb.bf[i];
+static void _push() {
+  if (!mb.space) {
+    CM_ALARM(STAT_INTERNAL_ERROR);
+    return;
   }
 
-  mb.buffers_available = PLANNER_BUFFER_POOL_SIZE;
-
-  mp_state_idle();
+  mb.tail = mb.tail->next;
+  mb.space--;
 }
 
 
-uint8_t mp_get_planner_buffer_room() {
-  uint16_t n = mb.buffers_available;
-  return n < PLANNER_BUFFER_HEADROOM ? 0 : n - PLANNER_BUFFER_HEADROOM;
-}
-
+static void _pop() {
+  if (mb.space == PLANNER_BUFFER_POOL_SIZE) {
+    CM_ALARM(STAT_INTERNAL_ERROR);
+    return;
+  }
 
-uint8_t mp_get_planner_buffer_fill() {
-  return PLANNER_BUFFER_POOL_SIZE - mb.buffers_available;
+  mb.head = mb.head->next;
+  mb.space++;
 }
 
 
-void mp_wait_for_buffer() {while (!mb.buffers_available) continue;}
-bool mp_queue_empty() {return mb.w == mb.r;}
-
+/// Initializes or resets buffers
+void mp_queue_init() {
+  memset(&mb, 0, sizeof(mb));     // clear all values
 
-/// Get pointer to next available write buffer.  Wait until one is available.
-mp_buffer_t *mp_get_write_buffer() {
-  // Wait for a buffer
-  while (!mb.buffers_available) continue;
+  mb.tail = mb.head = &mb.bf[0];  // init head and tail
+  mb.space = PLANNER_BUFFER_POOL_SIZE;
 
-  // Get & clear write buffer
-  mp_buffer_t *w = mb.w;
-  mp_buffer_t *nx = mb.w->nx;               // save linked list pointers
-  mp_buffer_t *pv = mb.w->pv;
-  memset(mb.w, 0, sizeof(mp_buffer_t));     // clear all values
-  w->nx = nx;                               // restore pointers
-  w->pv = pv;
-  w->buffer_state = MP_BUFFER_LOADING;
-  mb.w = w->nx;
+  // Setup ring pointers
+  for (int i = 0; i < mb.space; i++) {
+    mb.bf[i].next = &mb.bf[i + 1];
+    mb.bf[i].prev = &mb.bf[i - 1];
+  }
 
-  mb.buffers_available--;
+  mb.bf[0].prev = &mb.bf[mb.space -1];  // Fix first->prev
+  mb.bf[mb.space - 1].next = &mb.bf[0]; // Fix last->next
 
-  return w;
+  mp_state_idle();
 }
 
 
-/* Commit the next write buffer to the queue
- * Advances write pointer & changes buffer state
- *
- * WARNING: The routine calling mp_commit_write_buffer() must not use the write
- * buffer once it has been queued. Action may start on the buffer immediately,
- * invalidating its contents
- */
-void mp_commit_write_buffer(uint32_t line) {
-  mp_state_running();
-
-  mb.q->ts = rtc_get_time();
-  mb.q->line = line;
-  mb.q->run_state = MOVE_NEW;
-  mb.q->buffer_state = MP_BUFFER_QUEUED;
-  mb.q = mb.q->nx; // advance the queued buffer pointer
+uint8_t mp_queue_get_room() {
+  return mb.space < PLANNER_BUFFER_HEADROOM ?
+    0 : mb.space - PLANNER_BUFFER_HEADROOM;
 }
 
 
-/* Get pointer to the next or current run buffer
- * Returns a new run buffer if prev buf was ENDed
- * Returns same buf if called again before ENDing
- * Returns 0 if no buffer available
- * The behavior supports continuations (iteration)
- */
-mp_buffer_t *mp_get_run_buffer() {
-  switch (mb.r->buffer_state) {
-  case MP_BUFFER_QUEUED: // fresh buffer; becomes running if queued or pending
-    mb.r->buffer_state = MP_BUFFER_RUNNING;
-    // Fall through
+uint8_t mp_queue_get_fill() {
+  return PLANNER_BUFFER_POOL_SIZE - mb.space;
+}
 
-  case MP_BUFFER_RUNNING: // asking for the same run buffer for the Nth time
-    return mb.r; // return same buffer
 
-  default: return 0; // no queued buffers
-  }
-}
+bool mp_queue_is_empty() {return mb.tail == mb.head;}
 
 
-/// Release the run buffer & return to buffer pool.
-void mp_free_run_buffer() {           // EMPTY current run buf & adv to next
-  mp_clear_buffer(mb.r);              // clear it out (& reset replannable)
-  mb.r = mb.r->nx;                    // advance to next run buffer
-  mb.buffers_available++;
+/// Get pointer to next buffer, waiting until one is available.
+mp_buffer_t *mp_queue_get_tail() {
+  while (!mb.space) continue; // Wait for a buffer
+  return mb.tail;
 }
 
 
-/// Returns pointer to last buffer, i.e. last block (zero)
-mp_buffer_t *mp_get_last_buffer() {
-  mp_buffer_t *bf = mp_get_run_buffer();
-  mp_buffer_t *bp;
+/*** Commit the next buffer to the queue.
+ *
+ * WARNING: The routine calling mp_queue_push() must not use the write
+ * buffer once it has been queued.  Action may start on the buffer immediately,
+ * invalidating its contents
+ */
+void mp_queue_push(buffer_cb_t cb, uint32_t line) {
+  mp_state_running();
 
-  for (bp = bf; bp && bp->nx != bf; bp = mp_buffer_next(bp))
-    if (bp->nx->run_state == MOVE_OFF) break;
+  mb.tail->ts = rtc_get_time();
+  mb.tail->cb = cb;
+  mb.tail->line = line;
+  mb.tail->run_state = MOVE_NEW;
 
-  return bp;
+  _push();
 }
 
 
-/// Zeroes the contents of the buffer
-void mp_clear_buffer(mp_buffer_t *bf) {
-  mp_buffer_t *nx = bf->nx;            // save pointers
-  mp_buffer_t *pv = bf->pv;
-  memset(bf, 0, sizeof(mp_buffer_t));
-  bf->nx = nx;                         // restore pointers
-  bf->pv = pv;
+mp_buffer_t *mp_queue_get_head() {
+  return mp_queue_is_empty() ? 0 : mb.head;
 }
 
 
-///  Copies the contents of bp into bf - preserves links
-void mp_copy_buffer(mp_buffer_t *bf, const mp_buffer_t *bp) {
-  mp_buffer_t *nx = bf->nx;            // save pointers
-  mp_buffer_t *pv = bf->pv;
-  memcpy(bf, bp, sizeof(mp_buffer_t));
-  bf->nx = nx;                         // restore pointers
-  bf->pv = pv;
+/// Clear and release buffer to pool
+void mp_queue_pop() {
+  _clear_buffer(mb.head);
+  _pop();
 }
diff --git a/src/plan/buffer.h b/src/plan/buffer.h
index 3108fb4..8c708c2 100644
--- a/src/plan/buffer.h
+++ b/src/plan/buffer.h
@@ -34,42 +34,33 @@
 
 
 typedef enum {
-  MOVE_OFF,                // move inactive (must be zero)
+  MOVE_OFF,                // move inactive
   MOVE_NEW,                // initial value
   MOVE_INIT,               // first run
-  MOVE_RUN,                // general run state (for non-acceleration moves)
+  MOVE_RUN,                // subsequent runs
   MOVE_RESTART,            // restart buffer when done
 } run_state_t;
 
 
-// All the enums that equal zero must be zero. Don't change this
-typedef enum {
-  MP_BUFFER_EMPTY,                // struct is available for use (MUST BE 0)
-  MP_BUFFER_LOADING,              // being written ("checked out")
-  MP_BUFFER_QUEUED,               // in queue
-  MP_BUFFER_RUNNING,              // current running buffer
-} buffer_state_t;
-
-
 // Callbacks
-typedef void (*mach_func_t)(float[], float[]);
 struct mp_buffer_t;
-typedef stat_t (*bf_func_t)(struct mp_buffer_t *bf);
+typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf);
+typedef void (*mach_cb_t)(float[], float[]);
 
 
 typedef struct mp_buffer_t {      // See Planning Velocity Notes
-  struct mp_buffer_t *pv;         // pointer to previous buffer
-  struct mp_buffer_t *nx;         // pointer to next buffer
+  struct mp_buffer_t *prev;       // pointer to previous buffer
+  struct mp_buffer_t *next;       // pointer to next buffer
 
   uint32_t ts;                    // Time stamp
-  bf_func_t bf_func;              // callback to buffer exec function
-  mach_func_t mach_func;          // callback to machine
+  int32_t line;                   // gcode block line number
+  buffer_cb_t cb;                 // callback to buffer exec function
 
-  buffer_state_t buffer_state;    // used to manage queuing/dequeuing
   run_state_t run_state;          // run state machine sequence
   bool replannable;               // true if move can be re-planned
 
-  int32_t line;                   // gcode block line number
+  mach_cb_t mach_cb;              // callback to machine
+  float dwell;
 
   float target[AXES];             // XYZABC where the move should go
   float unit[AXES];               // unit vector for axis scaling & planning
@@ -79,7 +70,7 @@ typedef struct mp_buffer_t {      // See Planning Velocity Notes
   float body_length;
   float tail_length;
 
-  // See notes on these variables, in aline()
+  // See notes on these variables, in mp_aline()
   float entry_velocity;           // entry velocity requested for the move
   float cruise_velocity;          // cruise velocity requested & achieved
   float exit_velocity;            // exit velocity requested for the move
@@ -93,22 +84,21 @@ typedef struct mp_buffer_t {      // See Planning Velocity Notes
   float jerk;                     // maximum linear jerk term for this move
   float recip_jerk;               // 1/Jm used for planning (computed & cached)
   float cbrt_jerk;                // cube root of Jm (computed & cached)
-
-  float dwell;
 } mp_buffer_t;
 
 
-void mp_init_buffers();
-uint8_t mp_get_planner_buffer_room();
-uint8_t mp_get_planner_buffer_fill();
-void mp_wait_for_buffer();
-bool mp_queue_empty();
-mp_buffer_t *mp_get_write_buffer();
-void mp_commit_write_buffer(uint32_t line);
-mp_buffer_t *mp_get_run_buffer();
-void mp_free_run_buffer();
-mp_buffer_t *mp_get_last_buffer();
-static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->pv;}
-static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->nx;}
-void mp_clear_buffer(mp_buffer_t *bf);
-void mp_copy_buffer(mp_buffer_t *bf, const mp_buffer_t *bp);
+void mp_queue_init();
+
+uint8_t mp_queue_get_room();
+uint8_t mp_queue_get_fill();
+
+bool mp_queue_is_empty();
+
+mp_buffer_t *mp_queue_get_tail();
+void mp_queue_push(buffer_cb_t func, uint32_t line);
+
+mp_buffer_t *mp_queue_get_head();
+void mp_queue_pop();
+
+static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;}
+static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;}
diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c
index 696d8fd..3e4919a 100644
--- a/src/plan/calibrate.c
+++ b/src/plan/calibrate.c
@@ -169,9 +169,7 @@ uint8_t command_calibrate(int argc, char *argv[]) {
   mp_set_cycle(CYCLE_CALIBRATING);
   cal.motor = 1;
 
-  mp_buffer_t *bf = mp_get_write_buffer();
-  bf->bf_func = _exec_calibrate; // register callback
-  mp_commit_write_buffer(-1);
+  mp_queue_push(_exec_calibrate, -1);
 
   return 0;
 }
diff --git a/src/plan/command.c b/src/plan/command.c
index 7b2654c..98414a7 100644
--- a/src/plan/command.c
+++ b/src/plan/command.c
@@ -30,7 +30,7 @@
 /* How this works:
  *   - A command is called by the Gcode interpreter (mach_<command>,
  *     e.g. M code)
- *   - mach_ function calls mp_queue_command which puts it in the planning queue
+ *   - mach_ function calls mp_queue__command which puts it in the planning queue
  *     (bf buffer) which sets some parameters and registers a callback to the
  *     execution function in the machine.
  *   - When the planning queue gets to the function it calls _exec_command()
@@ -49,28 +49,21 @@
 #include "buffer.h"
 #include "machine.h"
 #include "stepper.h"
+#include "util.h"
 
 
 /// Callback to execute command
 static stat_t _exec_command(mp_buffer_t *bf) {
-  st_prep_command(bf->mach_func, bf->target, bf->unit);
+  st_prep_command(bf->mach_cb, bf->target, bf->unit);
   return STAT_OK; // Done
 }
 
 
 /// Queue a synchronous Mcode, program control, or other command
-void mp_queue_command(mach_func_t mach_func, float values[], float flags[]) {
-  mp_buffer_t *bf = mp_get_write_buffer();
-
-  bf->bf_func = _exec_command;    // callback to planner queue exec function
-  bf->mach_func = mach_func;      // callback to machine exec function
-
-  // Store values and flags in planner buffer
-  for (int axis = 0; axis < AXES; axis++) {
-    bf->target[axis] = values[axis];
-    bf->unit[axis] = flags[axis]; // flag vector in unit
-  }
-
-  // Must be final operation before exit
-  mp_commit_write_buffer(mach_get_line());
+void mp_queue__command(mach_cb_t mach_cb, float values[], float flags[]) {
+  mp_buffer_t *bf = mp_queue_get_tail();
+  bf->mach_cb = mach_cb;
+  copy_vector(bf->target, values);
+  copy_vector(bf->unit, flags);
+  mp_queue_push(_exec_command, mach_get_line());
 }
diff --git a/src/plan/command.h b/src/plan/command.h
index 2a3c46e..0eb23cf 100644
--- a/src/plan/command.h
+++ b/src/plan/command.h
@@ -29,4 +29,4 @@
 
 #include "plan/buffer.h"
 
-void mp_queue_command(mach_func_t mach_exec, float *value, float *flag);
+void mp_queue__command(mach_cb_t mach_exec, float *value, float *flag);
diff --git a/src/plan/dwell.c b/src/plan/dwell.c
index c2dd079..0093b12 100644
--- a/src/plan/dwell.c
+++ b/src/plan/dwell.c
@@ -46,12 +46,9 @@ static stat_t _exec_dwell(mp_buffer_t *bf) {
 
 /// Queue a dwell
 stat_t mp_dwell(float seconds, int32_t line) {
-  mp_buffer_t *bf = mp_get_write_buffer();
-  bf->bf_func = _exec_dwell; // register callback to dwell start
-  bf->dwell = seconds;       // in seconds, not minutes
-
-  // must be final operation before exit
-  mp_commit_write_buffer(line);
+  mp_buffer_t *bf = mp_queue_get_tail();
+  bf->dwell = seconds; // in seconds, not minutes
+  mp_queue_push(_exec_dwell, line);
 
   return STAT_OK;
 }
diff --git a/src/plan/exec.c b/src/plan/exec.c
index 2c32853..612eb4d 100644
--- a/src/plan/exec.c
+++ b/src/plan/exec.c
@@ -354,7 +354,7 @@ static float _compute_next_segment_velocity() {
  * speed.
  */
 static void _plan_hold() {
-  mp_buffer_t *bf = mp_get_run_buffer(); // working buffer pointer
+  mp_buffer_t *bf = mp_queue_get_head(); // working buffer pointer
   if (!bf) return; // Oops! nothing's running
 
   // Examine and process current buffer and compute length left for decel
@@ -516,13 +516,12 @@ stat_t mp_exec_move() {
   if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING)
     return STAT_NOOP;
 
-  mp_buffer_t *bf = mp_get_run_buffer();
+  mp_buffer_t *bf = mp_queue_get_head();
   if (!bf) return STAT_NOOP; // Nothing running
-  if (!bf->bf_func) return STAT_INTERNAL_ERROR; // Should never happen
 
   if (bf->run_state == MOVE_NEW) {
     // On restart wait a bit to give planner queue a chance to fill
-    if (!mp_runtime_is_busy() && mp_get_planner_buffer_fill() < 4 &&
+    if (!mp_runtime_is_busy() && mp_queue_get_fill() < 4 &&
         !rtc_expired(bf->ts + 250)) return STAT_NOOP;
 
     // Take control of buffer
@@ -534,7 +533,7 @@ stat_t mp_exec_move() {
     mp_runtime_set_line(bf->line);
   }
 
-  stat_t status = bf->bf_func(bf); // Move callback
+  stat_t status = bf->cb(bf); // Move callback
 
   if (status != STAT_EAGAIN) {
     bool idle = false;
@@ -554,10 +553,10 @@ stat_t mp_exec_move() {
       // being run by the steppers.  Planning can overwrite the new move.
       mp_buffer_next(bf)->replannable = false;
 
-      mp_free_run_buffer(); // Free buffer
+      mp_queue_pop(); // Free buffer
 
       // Enter READY state
-      if (mp_queue_empty()) {
+      if (mp_queue_is_empty()) {
         mp_state_idle();
         idle = true;
       }
diff --git a/src/plan/jog.c b/src/plan/jog.c
index 06a6c7f..bab3f81 100644
--- a/src/plan/jog.c
+++ b/src/plan/jog.c
@@ -126,10 +126,7 @@ uint8_t command_jog(int argc, char *argv[]) {
 
   if (!mp_jog_busy()) {
     mp_set_cycle(CYCLE_JOGGING);
-
-    mp_buffer_t *bf = mp_get_write_buffer();
-    bf->bf_func = _exec_jog; // register callback
-    mp_commit_write_buffer(-1);
+    mp_queue_push(_exec_jog, -1);
   }
 
   return STAT_OK;
diff --git a/src/plan/line.c b/src/plan/line.c
index 2b982bd..504734b 100644
--- a/src/plan/line.c
+++ b/src/plan/line.c
@@ -285,10 +285,9 @@ stat_t mp_aline(const float target[], int32_t line) {
   if (fp_ZERO(length)) return STAT_OK;
 
   // Get a buffer.  Note, new buffers are initialized to zero.
-  mp_buffer_t *bf = mp_get_write_buffer(); // current move pointer
+  mp_buffer_t *bf = mp_queue_get_tail(); // current move pointer
 
   // Set buffer values
-  bf->bf_func = mp_exec_aline;
   bf->length = length;
   copy_vector(bf->target, target);
 
@@ -307,7 +306,7 @@ stat_t mp_aline(const float target[], int32_t line) {
   // Note, the following lines must remain in order.
   mp_plan_block_list(bf);       // Plan block list
   mp_set_position(target);      // Set planner position before committing buffer
-  mp_commit_write_buffer(line); // Commit current block after position update
+  mp_queue_push(mp_exec_aline, line); // After position update
 
   return STAT_OK;
 }
diff --git a/src/plan/planner.c b/src/plan/planner.c
index 729de32..7498430 100644
--- a/src/plan/planner.c
+++ b/src/plan/planner.c
@@ -72,7 +72,7 @@
 static float mp_position[AXES]; // final move position for planning purposes
 
 
-void planner_init() {mp_init_buffers();}
+void planner_init() {mp_queue_init();}
 
 
 /// Set planner position for a single axis
@@ -96,7 +96,7 @@ void mp_set_position(const float position[]) {
  * during a hold to reset the planner.  This function should not usually
  * be directly called.  Call mp_request_flush() instead.
  */
-void mp_flush_planner() {mp_init_buffers();}
+void mp_flush_planner() {mp_queue_init();}
 
 
 /* Performs axis mapping & conversion of length units to steps (and deals
@@ -458,7 +458,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) {
  * first block and the bf.  It sets entry, exit and cruise v's from vmax's then
  * calls trapezoid generation.
  *
- * Variables that must be provided in the mp_buffer_ts that will be processed:
+ * Variables that must be provided in the mp_buffer_t that will be processed:
  *
  *   bf (function arg)     - end of block list (last block in time)
  *   bf->replannable       - start of block list set by last FALSE value
@@ -491,7 +491,6 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) {
  *   bf->run_state         - NEW for all blocks but the earliest
  *   bf->target[]          - block target position
  *   bf->unit[]            - block unit vector
- *   bf->time              - gets set later
  *   bf->jerk              - source of the other jerk variables.
  *
  * Notes:
@@ -552,7 +551,7 @@ void mp_plan_block_list(mp_buffer_t *bf) {
 
 
 void mp_replan_blocks() {
-  mp_buffer_t *bf = mp_get_run_buffer();
+  mp_buffer_t *bf = mp_queue_get_head();
   if (!bf) return;
 
   mp_buffer_t *bp = bf;
diff --git a/src/plan/state.c b/src/plan/state.c
index 09c99fe..66fafc6 100644
--- a/src/plan/state.c
+++ b/src/plan/state.c
@@ -181,7 +181,7 @@ void mp_state_callback() {
   if (ps.flush_requested && mp_is_quiescent()) {
     mach_abort_arc();
 
-    if (!mp_queue_empty()) {
+    if (!mp_queue_is_empty()) {
       mp_flush_planner();
 
       // NOTE The following uses low-level mp calls for absolute position.
@@ -205,7 +205,7 @@ void mp_state_callback() {
 
     if (mp_get_state() == STATE_HOLDING) {
       // Check if any moves are buffered
-      if (!mp_queue_empty()) {
+      if (!mp_queue_is_empty()) {
         mp_replan_blocks();
         _set_state(STATE_RUNNING);
 
diff --git a/src/stepper.c b/src/stepper.c
index 8b60c30..f8ce6bc 100644
--- a/src/stepper.c
+++ b/src/stepper.c
@@ -63,7 +63,7 @@ typedef struct {
   move_type_t move_type;
   uint16_t seg_period;
   uint32_t prep_dwell;
-  mach_func_t mach_func; // used for command moves
+  mach_cb_t mach_cb; // used for command moves
   float values[AXES];
   float flags[AXES];
 } stepper_t;
@@ -169,7 +169,7 @@ ISR(STEP_TIMER_ISR) {
     st.dwell = st.prep_dwell;
 
   } else if (st.move_type == MOVE_TYPE_COMMAND)
-    st.mach_func(st.values, st.flags); // Execute command
+    st.mach_cb(st.values, st.flags); // Execute command
 
   // We are done with this move
   st.move_type = MOVE_TYPE_NULL;
@@ -224,10 +224,10 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) {
 
 
 /// Stage command to execution
-void st_prep_command(mach_func_t mach_func, float values[], float flags[]) {
+void st_prep_command(mach_cb_t mach_cb, float values[], float flags[]) {
   if (st.move_ready) CM_ALARM(STAT_INTERNAL_ERROR);
   st.move_type = MOVE_TYPE_COMMAND;
-  st.mach_func = mach_func;
+  st.mach_cb = mach_cb;
   copy_vector(st.values, values);
   copy_vector(st.flags, flags);
   st.move_ready = true; // signal prep buffer ready
diff --git a/src/stepper.h b/src/stepper.h
index 12edee8..ad1cdac 100644
--- a/src/stepper.h
+++ b/src/stepper.h
@@ -40,5 +40,5 @@ void st_shutdown();
 bool st_is_busy();
 stat_t st_prep_line(float travel_steps[], float following_error[],
                     float segment_time);
-void st_prep_command(mach_func_t mach_func, float values[], float flags[]);
+void st_prep_command(mach_cb_t mach_cb, float values[], float flags[]);
 void st_prep_dwell(float seconds);