From c520bf981dc37bb3dccdad6dcd78012ad13abd1f Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 5 Aug 2025 16:32:27 -0400 Subject: [PATCH] steppersync: Split steppersync code from stepcompress.c to new file Signed-off-by: Kevin O'Connor --- klippy/chelper/__init__.py | 12 ++- klippy/chelper/stepcompress.c | 173 ++-------------------------------- klippy/chelper/stepcompress.h | 15 +-- klippy/chelper/steppersync.c | 168 +++++++++++++++++++++++++++++++++ klippy/chelper/steppersync.h | 16 ++++ 5 files changed, 206 insertions(+), 178 deletions(-) create mode 100644 klippy/chelper/steppersync.c create mode 100644 klippy/chelper/steppersync.h diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index d56b720c..3800834b 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -17,16 +17,16 @@ COMPILE_ARGS = ("-Wall -g -O2 -shared -fPIC" " -o %s %s") SSE_FLAGS = "-mfpmath=sse -msse2" SOURCE_FILES = [ - 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c', - 'pollreactor.c', 'msgblock.c', 'trdispatch.c', + 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'steppersync.c', + 'itersolve.c', 'trapq.c', 'pollreactor.c', 'msgblock.c', 'trdispatch.c', 'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c', 'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', 'kin_generic.c' ] DEST_LIB = "c_helper.so" OTHER_FILES = [ - 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h', - 'trapq.h', 'pollreactor.h', 'msgblock.h' + 'list.h', 'serialqueue.h', 'stepcompress.h', 'steppersync.h', + 'itersolve.h', 'pyhelper.h', 'trapq.h', 'pollreactor.h', 'msgblock.h' ] defs_stepcompress = """ @@ -54,7 +54,9 @@ defs_stepcompress = """ int stepcompress_extract_old(struct stepcompress *sc , struct pull_history_steps *p, int max , uint64_t start_clock, uint64_t end_clock); +""" +defs_steppersync = """ struct steppersync *steppersync_alloc(struct serialqueue *sq , struct stepcompress **sc_list, int sc_num, int move_num); void steppersync_free(struct steppersync *ss); @@ -228,7 +230,7 @@ defs_std = """ defs_all = [ defs_pyhelper, defs_serialqueue, defs_std, defs_stepcompress, - defs_itersolve, defs_trapq, defs_trdispatch, + defs_steppersync, defs_itersolve, defs_trapq, defs_trdispatch, defs_kin_cartesian, defs_kin_corexy, defs_kin_corexz, defs_kin_delta, defs_kin_deltesian, defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch, defs_kin_extruder, defs_kin_shaper, defs_kin_idex, diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index 939ac2c1..470feb71 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -1,6 +1,6 @@ // Stepper pulse schedule compression // -// Copyright (C) 2016-2021 Kevin O'Connor +// Copyright (C) 2016-2025 Kevin O'Connor // // This file may be distributed under the terms of the GNU GPLv3 license. @@ -277,7 +277,7 @@ stepcompress_set_invert_sdir(struct stepcompress *sc, uint32_t invert_sdir) } // Expire the stepcompress history older than the given clock -static void +void stepcompress_history_expire(struct stepcompress *sc, uint64_t end_clock) { while (!list_empty(&sc->history_list)) { @@ -314,6 +314,12 @@ stepcompress_get_step_dir(struct stepcompress *sc) return sc->next_step_dir; } +struct list_head * +stepcompress_get_msg_queue(struct stepcompress *sc) +{ + return &sc->msg_queue; +} + // Determine the "print time" of the last_step_clock static void calc_last_step_print_time(struct stepcompress *sc) @@ -323,7 +329,7 @@ calc_last_step_print_time(struct stepcompress *sc) } // Set the conversion rate of 'print_time' to mcu clock -static void +void stepcompress_set_time(struct stepcompress *sc , double time_offset, double mcu_freq) { @@ -532,7 +538,7 @@ stepcompress_commit(struct stepcompress *sc) } // Flush pending steps -static int +int stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) { if (sc->next_step_clock && move_clock >= sc->next_step_clock) { @@ -656,162 +662,3 @@ stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p } return res; } - - -/**************************************************************** - * Step compress synchronization - ****************************************************************/ - -// The steppersync object is used to synchronize the output of mcu -// step commands. The mcu can only queue a limited number of step -// commands - this code tracks when items on the mcu step queue become -// free so that new commands can be transmitted. It also ensures the -// mcu step queue is ordered between steppers so that no stepper -// starves the other steppers of space in the mcu step queue. - -struct steppersync { - // Serial port - struct serialqueue *sq; - struct command_queue *cq; - // Storage for associated stepcompress objects - struct stepcompress **sc_list; - int sc_num; - // Storage for list of pending move clocks - uint64_t *move_clocks; - int num_move_clocks; -}; - -// Allocate a new 'steppersync' object -struct steppersync * __visible -steppersync_alloc(struct serialqueue *sq, struct stepcompress **sc_list - , int sc_num, int move_num) -{ - struct steppersync *ss = malloc(sizeof(*ss)); - memset(ss, 0, sizeof(*ss)); - ss->sq = sq; - ss->cq = serialqueue_alloc_commandqueue(); - - ss->sc_list = malloc(sizeof(*sc_list)*sc_num); - memcpy(ss->sc_list, sc_list, sizeof(*sc_list)*sc_num); - ss->sc_num = sc_num; - - ss->move_clocks = malloc(sizeof(*ss->move_clocks)*move_num); - memset(ss->move_clocks, 0, sizeof(*ss->move_clocks)*move_num); - ss->num_move_clocks = move_num; - - return ss; -} - -// Free memory associated with a 'steppersync' object -void __visible -steppersync_free(struct steppersync *ss) -{ - if (!ss) - return; - free(ss->sc_list); - free(ss->move_clocks); - serialqueue_free_commandqueue(ss->cq); - free(ss); -} - -// Set the conversion rate of 'print_time' to mcu clock -void __visible -steppersync_set_time(struct steppersync *ss, double time_offset - , double mcu_freq) -{ - int i; - for (i=0; isc_num; i++) { - struct stepcompress *sc = ss->sc_list[i]; - stepcompress_set_time(sc, time_offset, mcu_freq); - } -} - -// Expire the stepcompress history before the given clock time -void __visible -steppersync_history_expire(struct steppersync *ss, uint64_t end_clock) -{ - int i; - for (i = 0; i < ss->sc_num; i++) { - struct stepcompress *sc = ss->sc_list[i]; - stepcompress_history_expire(sc, end_clock); - } -} - -// Implement a binary heap algorithm to track when the next available -// 'struct move' in the mcu will be available -static void -heap_replace(struct steppersync *ss, uint64_t req_clock) -{ - uint64_t *mc = ss->move_clocks; - int nmc = ss->num_move_clocks, pos = 0; - for (;;) { - int child1_pos = 2*pos+1, child2_pos = 2*pos+2; - uint64_t child2_clock = child2_pos < nmc ? mc[child2_pos] : UINT64_MAX; - uint64_t child1_clock = child1_pos < nmc ? mc[child1_pos] : UINT64_MAX; - if (req_clock <= child1_clock && req_clock <= child2_clock) { - mc[pos] = req_clock; - break; - } - if (child1_clock < child2_clock) { - mc[pos] = child1_clock; - pos = child1_pos; - } else { - mc[pos] = child2_clock; - pos = child2_pos; - } - } -} - -// Find and transmit any scheduled steps prior to the given 'move_clock' -int __visible -steppersync_flush(struct steppersync *ss, uint64_t move_clock) -{ - // Flush each stepcompress to the specified move_clock - int i; - for (i=0; isc_num; i++) { - int ret = stepcompress_flush(ss->sc_list[i], move_clock); - if (ret) - return ret; - } - - // Order commands by the reqclock of each pending command - struct list_head msgs; - list_init(&msgs); - for (;;) { - // Find message with lowest reqclock - uint64_t req_clock = MAX_CLOCK; - struct queue_message *qm = NULL; - for (i=0; isc_num; i++) { - struct stepcompress *sc = ss->sc_list[i]; - if (!list_empty(&sc->msg_queue)) { - struct queue_message *m = list_first_entry( - &sc->msg_queue, struct queue_message, node); - if (m->req_clock < req_clock) { - qm = m; - req_clock = m->req_clock; - } - } - } - if (!qm || (qm->min_clock && req_clock > move_clock)) - break; - - uint64_t next_avail = ss->move_clocks[0]; - if (qm->min_clock) - // The qm->min_clock field is overloaded to indicate that - // the command uses the 'move queue' and to store the time - // that move queue item becomes available. - heap_replace(ss, qm->min_clock); - // Reset the min_clock to its normal meaning (minimum transmit time) - qm->min_clock = next_avail; - - // Batch this command - list_del(&qm->node); - list_add_tail(&qm->node, &msgs); - } - - // Transmit commands - if (!list_empty(&msgs)) - serialqueue_send_batch(ss->sq, ss->cq, &msgs); - - return 0; -} diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index 06bc7a49..1850436e 100644 --- a/klippy/chelper/stepcompress.h +++ b/klippy/chelper/stepcompress.h @@ -17,12 +17,17 @@ void stepcompress_fill(struct stepcompress *sc, uint32_t max_error , int32_t set_next_step_dir_msgtag); void stepcompress_set_invert_sdir(struct stepcompress *sc , uint32_t invert_sdir); +void stepcompress_history_expire(struct stepcompress *sc, uint64_t end_clock); void stepcompress_free(struct stepcompress *sc); uint32_t stepcompress_get_oid(struct stepcompress *sc); int stepcompress_get_step_dir(struct stepcompress *sc); +struct list_head *stepcompress_get_msg_queue(struct stepcompress *sc); +void stepcompress_set_time(struct stepcompress *sc + , double time_offset, double mcu_freq); int stepcompress_append(struct stepcompress *sc, int sdir , double print_time, double step_time); int stepcompress_commit(struct stepcompress *sc); +int stepcompress_flush(struct stepcompress *sc, uint64_t move_clock); int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); int stepcompress_set_last_position(struct stepcompress *sc, uint64_t clock , int64_t last_position); @@ -35,14 +40,4 @@ int stepcompress_extract_old(struct stepcompress *sc , struct pull_history_steps *p, int max , uint64_t start_clock, uint64_t end_clock); -struct serialqueue; -struct steppersync *steppersync_alloc( - struct serialqueue *sq, struct stepcompress **sc_list, int sc_num - , int move_num); -void steppersync_free(struct steppersync *ss); -void steppersync_set_time(struct steppersync *ss, double time_offset - , double mcu_freq); -void steppersync_history_expire(struct steppersync *ss, uint64_t end_clock); -int steppersync_flush(struct steppersync *ss, uint64_t move_clock); - #endif // stepcompress.h diff --git a/klippy/chelper/steppersync.c b/klippy/chelper/steppersync.c new file mode 100644 index 00000000..0542ee4f --- /dev/null +++ b/klippy/chelper/steppersync.c @@ -0,0 +1,168 @@ +// Stepper step transmit synchronization +// +// Copyright (C) 2016-2025 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +// The steppersync object is used to synchronize the output of mcu +// step commands. The mcu can only queue a limited number of step +// commands - this code tracks when items on the mcu step queue become +// free so that new commands can be transmitted. It also ensures the +// mcu step queue is ordered between steppers so that no stepper +// starves the other steppers of space in the mcu step queue. + +#include // offsetof +#include // malloc +#include // memset +#include "compiler.h" // __visible +#include "serialqueue.h" // struct queue_message +#include "stepcompress.h" // stepcompress_flush +#include "steppersync.h" // steppersync_alloc + +struct steppersync { + // Serial port + struct serialqueue *sq; + struct command_queue *cq; + // Storage for associated stepcompress objects + struct stepcompress **sc_list; + int sc_num; + // Storage for list of pending move clocks + uint64_t *move_clocks; + int num_move_clocks; +}; + +// Allocate a new 'steppersync' object +struct steppersync * __visible +steppersync_alloc(struct serialqueue *sq, struct stepcompress **sc_list + , int sc_num, int move_num) +{ + struct steppersync *ss = malloc(sizeof(*ss)); + memset(ss, 0, sizeof(*ss)); + ss->sq = sq; + ss->cq = serialqueue_alloc_commandqueue(); + + ss->sc_list = malloc(sizeof(*sc_list)*sc_num); + memcpy(ss->sc_list, sc_list, sizeof(*sc_list)*sc_num); + ss->sc_num = sc_num; + + ss->move_clocks = malloc(sizeof(*ss->move_clocks)*move_num); + memset(ss->move_clocks, 0, sizeof(*ss->move_clocks)*move_num); + ss->num_move_clocks = move_num; + + return ss; +} + +// Free memory associated with a 'steppersync' object +void __visible +steppersync_free(struct steppersync *ss) +{ + if (!ss) + return; + free(ss->sc_list); + free(ss->move_clocks); + serialqueue_free_commandqueue(ss->cq); + free(ss); +} + +// Set the conversion rate of 'print_time' to mcu clock +void __visible +steppersync_set_time(struct steppersync *ss, double time_offset + , double mcu_freq) +{ + int i; + for (i=0; isc_num; i++) { + struct stepcompress *sc = ss->sc_list[i]; + stepcompress_set_time(sc, time_offset, mcu_freq); + } +} + +// Expire the stepcompress history before the given clock time +void __visible +steppersync_history_expire(struct steppersync *ss, uint64_t end_clock) +{ + int i; + for (i = 0; i < ss->sc_num; i++) { + struct stepcompress *sc = ss->sc_list[i]; + stepcompress_history_expire(sc, end_clock); + } +} + +// Implement a binary heap algorithm to track when the next available +// 'struct move' in the mcu will be available +static void +heap_replace(struct steppersync *ss, uint64_t req_clock) +{ + uint64_t *mc = ss->move_clocks; + int nmc = ss->num_move_clocks, pos = 0; + for (;;) { + int child1_pos = 2*pos+1, child2_pos = 2*pos+2; + uint64_t child2_clock = child2_pos < nmc ? mc[child2_pos] : UINT64_MAX; + uint64_t child1_clock = child1_pos < nmc ? mc[child1_pos] : UINT64_MAX; + if (req_clock <= child1_clock && req_clock <= child2_clock) { + mc[pos] = req_clock; + break; + } + if (child1_clock < child2_clock) { + mc[pos] = child1_clock; + pos = child1_pos; + } else { + mc[pos] = child2_clock; + pos = child2_pos; + } + } +} + +// Find and transmit any scheduled steps prior to the given 'move_clock' +int __visible +steppersync_flush(struct steppersync *ss, uint64_t move_clock) +{ + // Flush each stepcompress to the specified move_clock + int i; + for (i=0; isc_num; i++) { + int ret = stepcompress_flush(ss->sc_list[i], move_clock); + if (ret) + return ret; + } + + // Order commands by the reqclock of each pending command + struct list_head msgs; + list_init(&msgs); + for (;;) { + // Find message with lowest reqclock + uint64_t req_clock = MAX_CLOCK; + struct queue_message *qm = NULL; + for (i=0; isc_num; i++) { + struct stepcompress *sc = ss->sc_list[i]; + struct list_head *sc_mq = stepcompress_get_msg_queue(sc); + if (!list_empty(sc_mq)) { + struct queue_message *m = list_first_entry( + sc_mq, struct queue_message, node); + if (m->req_clock < req_clock) { + qm = m; + req_clock = m->req_clock; + } + } + } + if (!qm || (qm->min_clock && req_clock > move_clock)) + break; + + uint64_t next_avail = ss->move_clocks[0]; + if (qm->min_clock) + // The qm->min_clock field is overloaded to indicate that + // the command uses the 'move queue' and to store the time + // that move queue item becomes available. + heap_replace(ss, qm->min_clock); + // Reset the min_clock to its normal meaning (minimum transmit time) + qm->min_clock = next_avail; + + // Batch this command + list_del(&qm->node); + list_add_tail(&qm->node, &msgs); + } + + // Transmit commands + if (!list_empty(&msgs)) + serialqueue_send_batch(ss->sq, ss->cq, &msgs); + + return 0; +} diff --git a/klippy/chelper/steppersync.h b/klippy/chelper/steppersync.h new file mode 100644 index 00000000..ffc8d569 --- /dev/null +++ b/klippy/chelper/steppersync.h @@ -0,0 +1,16 @@ +#ifndef STEPPERSYNC_H +#define STEPPERSYNC_H + +#include // uint64_t + +struct serialqueue; +struct steppersync *steppersync_alloc( + struct serialqueue *sq, struct stepcompress **sc_list, int sc_num + , int move_num); +void steppersync_free(struct steppersync *ss); +void steppersync_set_time(struct steppersync *ss, double time_offset + , double mcu_freq); +void steppersync_history_expire(struct steppersync *ss, uint64_t end_clock); +int steppersync_flush(struct steppersync *ss, uint64_t move_clock); + +#endif // steppersync.h