diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 59971c1c4..b9ad9747d 100644 --- a/klippy/chelper/__init__.py +++ b/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 = """ diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index 52dd40773..ab261d129 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -15,6 +15,7 @@ // efficiency - the repetitive integer math is vastly faster in C. #include // sqrt +#include // pthread_mutex_lock #include // offsetof #include // uint32_t #include // 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); +} diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index 7ca0f2e43..5ebf8bf08 100644 --- a/klippy/chelper/stepcompress.h +++ b/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 - , double gen_steps_time - , uint64_t flush_clock); +void stepcompress_start_gen_steps(struct stepcompress *sc, double gen_steps_time + , uint64_t flush_clock); +int32_t stepcompress_finalize_gen_steps(struct stepcompress *sc); #endif // stepcompress.h diff --git a/klippy/chelper/steppersync.c b/klippy/chelper/steppersync.c index 745578c75..0ff5bcab1 100644 --- a/klippy/chelper/steppersync.c +++ b/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; isc_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; isc_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; isc_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; } diff --git a/klippy/chelper/steppersync.h b/klippy/chelper/steppersync.h index 1320bbaa0..41cd03bbd 100644 --- a/klippy/chelper/steppersync.h +++ b/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 diff --git a/klippy/extras/motion_queuing.py b/klippy/extras/motion_queuing.py index 9d2ef5137..a61ba5cc0 100644 --- a/klippy/extras/motion_queuing.py +++ b/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_flush = ffi_lib.steppersync_flush + self.steppersync_start_gen_steps = ffi_lib.steppersync_start_gen_steps + 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 - ret = self.steppersync_generate_steps(ss, max_step_gen_time, clock) - if ret: - raise mcu.error("Internal error in MCU '%s' stepcompress" - % (mcu.get_name(),)) - # Flush steps from steppersync - ret = self.steppersync_flush(ss, clock) + self.steppersync_start_gen_steps(ss, max_step_gen_time, clock) + for mcu, ss in self.steppersyncs: + clock = max(0, mcu.print_time_to_clock(must_flush_time)) + ret = self.steppersync_finalize_gen_steps(ss, clock) if ret: raise mcu.error("Internal error in MCU '%s' stepcompress" % (mcu.get_name(),)) diff --git a/klippy/extras/pwm_tool.py b/klippy/extras/pwm_tool.py index d9e72c5e1..cec7e3791 100644 --- a/klippy/extras/pwm_tool.py +++ b/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) diff --git a/klippy/stepper.py b/klippy/stepper.py index d5b3cecde..046b5280d 100644 --- a/klippy/stepper.py +++ b/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