stepcompress: Generate steps in a per-stepper background thread

Create a thread for each stepper and use it for step generation and
step compression.

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

klippy/chelper/__init__.py

@@ -36,7 +36,7 @@ defs_stepcompress = """
         int step_count, interval, add;
     };
 
-    struct stepcompress *stepcompress_alloc(uint32_t oid);
+    struct stepcompress *stepcompress_alloc(uint32_t oid, char name[16]);
     void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
         , int32_t queue_step_msgtag, int32_t set_next_step_dir_msgtag);
     void stepcompress_set_invert_sdir(struct stepcompress *sc
@@ -66,10 +66,11 @@ defs_steppersync = """
     void steppersync_free(struct steppersync *ss);
     void steppersync_set_time(struct steppersync *ss
         , double time_offset, double mcu_freq);
-    int32_t steppersync_generate_steps(struct steppersync *ss
-        , double gen_steps_time, uint64_t flush_clock);
     void steppersync_history_expire(struct steppersync *ss, uint64_t end_clock);
-    int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+    void steppersync_start_gen_steps(struct steppersync *ss
+        , double gen_steps_time, uint64_t flush_clock);
+    int32_t steppersync_finalize_gen_steps(struct steppersync *ss
+        , uint64_t flush_clock);
 """
 
 defs_itersolve = """

klippy/chelper/stepcompress.c

@@ -15,6 +15,7 @@
 // efficiency - the repetitive integer math is vastly faster in C.
 
 #include <math.h> // sqrt
+#include <pthread.h> // pthread_mutex_lock
 #include <stddef.h> // offsetof
 #include <stdint.h> // uint32_t
 #include <stdio.h> // fprintf
@@ -47,8 +48,16 @@ struct stepcompress {
     // History tracking
     int64_t last_position;
     struct list_head history_list;
-    // Itersolve reference
+    // Thread for step generation
     struct stepper_kinematics *sk;
+    char name[16];
+    pthread_t tid;
+    pthread_mutex_t lock; // protects variables below
+    pthread_cond_t cond;
+    int have_work;
+    double bg_gen_steps_time;
+    uint64_t bg_flush_clock;
+    int32_t bg_result;
 };
 
 struct step_move {
@@ -244,9 +253,12 @@ check_line(struct stepcompress *sc, struct step_move move)
  * Step compress interface
  ****************************************************************/
 
+static int sc_thread_alloc(struct stepcompress *sc, char name[16]);
+static void sc_thread_free(struct stepcompress *sc);
+
 // Allocate a new 'stepcompress' object
 struct stepcompress * __visible
-stepcompress_alloc(uint32_t oid)
+stepcompress_alloc(uint32_t oid, char name[16])
 {
     struct stepcompress *sc = malloc(sizeof(*sc));
     memset(sc, 0, sizeof(*sc));
@@ -254,6 +266,10 @@ stepcompress_alloc(uint32_t oid)
     list_init(&sc->history_list);
     sc->oid = oid;
     sc->sdir = -1;
+
+    int ret = sc_thread_alloc(sc, name);
+    if (ret)
+        return NULL;
     return sc;
 }
 
@@ -299,6 +315,7 @@ stepcompress_free(struct stepcompress *sc)
 {
     if (!sc)
         return;
+    sc_thread_free(sc);
     free(sc->queue);
     message_queue_free(&sc->msg_queue);
     stepcompress_history_expire(sc, UINT64_MAX);
@@ -666,6 +683,11 @@ stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p
     return res;
 }
 
+
+/****************************************************************
+ * Step generation thread
+ ****************************************************************/
+
 // Store a reference to stepper_kinematics
 void __visible
 stepcompress_set_stepper_kinematics(struct stepcompress *sc
@@ -682,7 +704,7 @@ stepcompress_get_stepper_kinematics(struct stepcompress *sc)
 }
 
 // Generate steps (via itersolve) and flush
-int32_t
+static int32_t
 stepcompress_generate_steps(struct stepcompress *sc, double gen_steps_time
                             , uint64_t flush_clock)
 {
@@ -695,3 +717,96 @@ stepcompress_generate_steps(struct stepcompress *sc, double gen_steps_time
     // Flush steps
     return stepcompress_flush(sc, flush_clock);
 }
+
+// Main background thread for generating steps
+static void *
+sc_background_thread(void *data)
+{
+    struct stepcompress *sc = data;
+    set_thread_name(sc->name);
+
+    pthread_mutex_lock(&sc->lock);
+    for (;;) {
+        if (!sc->have_work) {
+            pthread_cond_wait(&sc->cond, &sc->lock);
+            continue;
+        }
+        if (sc->have_work < 0)
+            // Exit request
+            break;
+
+        // Request to generate steps
+        sc->bg_result = stepcompress_generate_steps(sc, sc->bg_gen_steps_time
+                                                    , sc->bg_flush_clock);
+        sc->have_work = 0;
+        pthread_cond_signal(&sc->cond);
+    }
+    pthread_mutex_unlock(&sc->lock);
+
+    return NULL;
+}
+
+// Signal background thread to start step generation
+void
+stepcompress_start_gen_steps(struct stepcompress *sc, double gen_steps_time
+                             , uint64_t flush_clock)
+{
+    if (!sc->sk)
+        return;
+    pthread_mutex_lock(&sc->lock);
+    while (sc->have_work)
+        pthread_cond_wait(&sc->cond, &sc->lock);
+    sc->bg_gen_steps_time = gen_steps_time;
+    sc->bg_flush_clock = flush_clock;
+    sc->have_work = 1;
+    pthread_mutex_unlock(&sc->lock);
+    pthread_cond_signal(&sc->cond);
+}
+
+// Wait for background thread to complete last step generation request
+int32_t
+stepcompress_finalize_gen_steps(struct stepcompress *sc)
+{
+    pthread_mutex_lock(&sc->lock);
+    while (sc->have_work)
+        pthread_cond_wait(&sc->cond, &sc->lock);
+    int32_t res = sc->bg_result;
+    pthread_mutex_unlock(&sc->lock);
+    return res;
+}
+
+// Internal helper to start thread
+static int
+sc_thread_alloc(struct stepcompress *sc, char name[16])
+{
+    strncpy(sc->name, name, sizeof(sc->name));
+    sc->name[sizeof(sc->name)-1] = '\0';
+    int ret = pthread_mutex_init(&sc->lock, NULL);
+    if (ret)
+        goto fail;
+    ret = pthread_cond_init(&sc->cond, NULL);
+    if (ret)
+        goto fail;
+    ret = pthread_create(&sc->tid, NULL, sc_background_thread, sc);
+    if (ret)
+        goto fail;
+    return 0;
+fail:
+    report_errno("sc init", ret);
+    return -1;
+}
+
+// Request background thread to exit
+static void
+sc_thread_free(struct stepcompress *sc)
+{
+    pthread_mutex_lock(&sc->lock);
+    while (sc->have_work)
+        pthread_cond_wait(&sc->cond, &sc->lock);
+    sc->have_work = -1;
+    pthread_cond_signal(&sc->cond);
+    pthread_mutex_unlock(&sc->lock);
+    int ret = pthread_join(sc->tid, NULL);
+    if (ret)
+        report_errno("sc pthread_join", ret);
+}

klippy/chelper/stepcompress.h

@@ -11,7 +11,7 @@ struct pull_history_steps {
     int step_count, interval, add;
 };
 
-struct stepcompress *stepcompress_alloc(uint32_t oid);
+struct stepcompress *stepcompress_alloc(uint32_t oid, char name[16]);
 void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
                        , int32_t queue_step_msgtag
                        , int32_t set_next_step_dir_msgtag);
@@ -43,8 +43,8 @@ void stepcompress_set_stepper_kinematics(struct stepcompress *sc
                                          , struct stepper_kinematics *sk);
 struct stepper_kinematics *stepcompress_get_stepper_kinematics(
     struct stepcompress *sc);
-int32_t stepcompress_generate_steps(struct stepcompress *sc
+void stepcompress_start_gen_steps(struct stepcompress *sc, double gen_steps_time
-                                    , double gen_steps_time
+                                  , uint64_t flush_clock);
-                                    , uint64_t flush_clock);
+int32_t stepcompress_finalize_gen_steps(struct stepcompress *sc);
 
 #endif // stepcompress.h

klippy/chelper/steppersync.c

@@ -76,22 +76,6 @@ steppersync_set_time(struct steppersync *ss, double time_offset
     }
 }
 
-// Generate steps and flush stepcompress objects
-int32_t __visible
-steppersync_generate_steps(struct steppersync *ss, double gen_steps_time
-                           , uint64_t flush_clock)
-{
-    int i;
-    for (i=0; i<ss->sc_num; i++) {
-        struct stepcompress *sc = ss->sc_list[i];
-        int32_t ret = stepcompress_generate_steps(sc, gen_steps_time
-                                                  , flush_clock);
-        if (ret)
-            return ret;
-    }
-    return 0;
-}
-
 // Expire the stepcompress history before the given clock time
 void __visible
 steppersync_history_expire(struct steppersync *ss, uint64_t end_clock)
@@ -129,7 +113,7 @@ heap_replace(struct steppersync *ss, uint64_t req_clock)
 }
 
 // Find and transmit any scheduled steps prior to the given 'move_clock'
-int __visible
+static void
 steppersync_flush(struct steppersync *ss, uint64_t move_clock)
 {
     // Order commands by the reqclock of each pending command
@@ -172,6 +156,34 @@ steppersync_flush(struct steppersync *ss, uint64_t move_clock)
     // Transmit commands
     if (!list_empty(&msgs))
         serialqueue_send_batch(ss->sq, ss->cq, &msgs);
+}
 
+// Start generating steps in stepcompress objects
+void __visible
+steppersync_start_gen_steps(struct steppersync *ss, double gen_steps_time
+                            , uint64_t flush_clock)
+{
+    int i;
+    for (i=0; i<ss->sc_num; i++) {
+        struct stepcompress *sc = ss->sc_list[i];
+        stepcompress_start_gen_steps(sc, gen_steps_time, flush_clock);
+    }
+}
+
+// Finalize step generation and flush
+int32_t __visible
+steppersync_finalize_gen_steps(struct steppersync *ss, uint64_t flush_clock)
+{
+    int i;
+    int32_t res = 0;
+    for (i=0; i<ss->sc_num; i++) {
+        struct stepcompress *sc = ss->sc_list[i];
+        int32_t ret = stepcompress_finalize_gen_steps(sc);
+        if (ret)
+            res = ret;
+    }
+    if (res)
+        return res;
+    steppersync_flush(ss, flush_clock);
     return 0;
 }

klippy/chelper/steppersync.h

@@ -10,9 +10,10 @@ struct steppersync *steppersync_alloc(
 void steppersync_free(struct steppersync *ss);
 void steppersync_set_time(struct steppersync *ss, double time_offset
                           , double mcu_freq);
-int32_t steppersync_generate_steps(struct steppersync *ss, double gen_steps_time
-                                   , uint64_t flush_clock);
 void steppersync_history_expire(struct steppersync *ss, uint64_t end_clock);
-int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+void steppersync_start_gen_steps(struct steppersync *ss, double gen_steps_time
+                                 , uint64_t flush_clock);
+int32_t steppersync_finalize_gen_steps(struct steppersync *ss
+                                       , uint64_t flush_clock);
 
 #endif // steppersync.h

klippy/extras/motion_queuing.py

@@ -29,8 +29,9 @@ class PrinterMotionQueuing:
         # Low-level C flushing calls
         ffi_main, ffi_lib = chelper.get_ffi()
         self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves
-        self.steppersync_generate_steps = ffi_lib.steppersync_generate_steps
+        self.steppersync_start_gen_steps = ffi_lib.steppersync_start_gen_steps
-        self.steppersync_flush = ffi_lib.steppersync_flush
+        self.steppersync_finalize_gen_steps = \
+            ffi_lib.steppersync_finalize_gen_steps
         self.steppersync_history_expire = ffi_lib.steppersync_history_expire
         # Flush notification callbacks
         self.flush_callbacks = []
@@ -58,9 +59,10 @@ class PrinterMotionQueuing:
         trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
         self.trapqs.append(trapq)
         return trapq
-    def allocate_stepcompress(self, mcu, oid):
+    def allocate_stepcompress(self, mcu, oid, name):
+        name = name.encode("utf-8")[:15]
         ffi_main, ffi_lib = chelper.get_ffi()
-        sc = ffi_main.gc(ffi_lib.stepcompress_alloc(oid),
+        sc = ffi_main.gc(ffi_lib.stepcompress_alloc(oid, name),
                          ffi_lib.stepcompress_free)
         self.stepcompress.append((mcu, sc))
         return sc
@@ -90,13 +92,10 @@ class PrinterMotionQueuing:
         # Generate stepper movement and transmit
         for mcu, ss in self.steppersyncs:
             clock = max(0, mcu.print_time_to_clock(must_flush_time))
-            # Generate steps
+            self.steppersync_start_gen_steps(ss, max_step_gen_time, clock)
-            ret = self.steppersync_generate_steps(ss, max_step_gen_time, clock)
+        for mcu, ss in self.steppersyncs:
-            if ret:
+            clock = max(0, mcu.print_time_to_clock(must_flush_time))
-                raise mcu.error("Internal error in MCU '%s' stepcompress"
+            ret = self.steppersync_finalize_gen_steps(ss, clock)
-                                % (mcu.get_name(),))
-            # Flush steps from steppersync
-            ret = self.steppersync_flush(ss, clock)
             if ret:
                 raise mcu.error("Internal error in MCU '%s' stepcompress"
                                 % (mcu.get_name(),))

klippy/extras/pwm_tool.py

@@ -16,8 +16,10 @@ class MCU_queued_pwm:
         self._max_duration = 2.
         self._oid = oid = mcu.create_oid()
         printer = mcu.get_printer()
+        sname = config.get_name().split()[-1]
         self._motion_queuing = printer.load_object(config, 'motion_queuing')
-        self._stepqueue = self._motion_queuing.allocate_stepcompress(mcu, oid)
+        self._stepqueue = self._motion_queuing.allocate_stepcompress(
+            mcu, oid, sname)
         ffi_main, ffi_lib = chelper.get_ffi()
         self._stepcompress_queue_mq_msg = ffi_lib.stepcompress_queue_mq_msg
         mcu.register_config_callback(self._build_config)

klippy/stepper.py

@@ -44,7 +44,8 @@ class MCU_stepper:
         self._reset_cmd_tag = self._get_position_cmd = None
         self._active_callbacks = []
         motion_queuing = printer.load_object(config, 'motion_queuing')
-        self._stepqueue = motion_queuing.allocate_stepcompress(mcu, oid)
+        sname = self._name.split()[-1]
+        self._stepqueue = motion_queuing.allocate_stepcompress(mcu, oid, sname)
         ffi_main, ffi_lib = chelper.get_ffi()
         ffi_lib.stepcompress_set_invert_sdir(self._stepqueue, self._invert_dir)
         self._stepper_kinematics = None