diff options
Diffstat (limited to 'klippy/chelper')
| -rw-r--r-- | klippy/chelper/__init__.py | 137 | ||||
| -rw-r--r-- | klippy/chelper/list.h | 108 | ||||
| -rw-r--r-- | klippy/chelper/pyhelper.c | 93 | ||||
| -rw-r--r-- | klippy/chelper/pyhelper.h | 14 | ||||
| -rw-r--r-- | klippy/chelper/serialqueue.c | 1090 | ||||
| -rw-r--r-- | klippy/chelper/serialqueue.h | 69 | ||||
| -rw-r--r-- | klippy/chelper/stepcompress.c | 852 |
7 files changed, 2363 insertions, 0 deletions
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py new file mode 100644 index 00000000..7d950ab8 --- /dev/null +++ b/klippy/chelper/__init__.py @@ -0,0 +1,137 @@ +# Wrapper around C helper code +# +# Copyright (C) 2016,2017 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import os, logging +import cffi + + +###################################################################### +# c_helper.so compiling +###################################################################### + +COMPILE_CMD = "gcc -Wall -g -O2 -shared -fPIC -o %s %s" +SOURCE_FILES = ['stepcompress.c', 'serialqueue.c', 'pyhelper.c'] +DEST_LIB = "c_helper.so" +OTHER_FILES = ['list.h', 'serialqueue.h', 'pyhelper.h'] + +defs_stepcompress = """ + struct stepcompress *stepcompress_alloc(uint32_t max_error + , uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid + , uint32_t invert_sdir, uint32_t oid); + void stepcompress_free(struct stepcompress *sc); + int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock); + int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock); + int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len); + + int32_t stepcompress_push(struct stepcompress *sc, double step_clock + , int32_t sdir); + int32_t stepcompress_push_const(struct stepcompress *sc, double clock_offset + , double step_offset, double steps, double start_sv, double accel); + int32_t stepcompress_push_delta(struct stepcompress *sc + , double clock_offset, double move_sd, double start_sv, double accel + , double height, double startxy_sd, double arm_d, double movez_r); + + 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); + int steppersync_flush(struct steppersync *ss, uint64_t move_clock); +""" + +defs_serialqueue = """ + #define MESSAGE_MAX 64 + struct pull_queue_message { + uint8_t msg[MESSAGE_MAX]; + int len; + double sent_time, receive_time; + }; + + struct serialqueue *serialqueue_alloc(int serial_fd, int write_only); + void serialqueue_exit(struct serialqueue *sq); + void serialqueue_free(struct serialqueue *sq); + struct command_queue *serialqueue_alloc_commandqueue(void); + void serialqueue_free_commandqueue(struct command_queue *cq); + void serialqueue_send(struct serialqueue *sq, struct command_queue *cq + , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock); + void serialqueue_encode_and_send(struct serialqueue *sq + , struct command_queue *cq, uint32_t *data, int len + , uint64_t min_clock, uint64_t req_clock); + void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm); + void serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust); + void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq + , double last_clock_time, uint64_t last_clock); + void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len); + int serialqueue_extract_old(struct serialqueue *sq, int sentq + , struct pull_queue_message *q, int max); +""" + +defs_pyhelper = """ + void set_python_logging_callback(void (*func)(const char *)); + double get_monotonic(void); +""" + +# Return the list of file modification times +def get_mtimes(srcdir, filelist): + out = [] + for filename in filelist: + pathname = os.path.join(srcdir, filename) + try: + t = os.path.getmtime(pathname) + except os.error: + continue + out.append(t) + return out + +# Check if the code needs to be compiled +def check_build_code(srcdir, target, sources, cmd, other_files=[]): + src_times = get_mtimes(srcdir, sources + other_files) + obj_times = get_mtimes(srcdir, [target]) + if not obj_times or max(src_times) > min(obj_times): + logging.info("Building C code module %s", target) + srcfiles = [os.path.join(srcdir, fname) for fname in sources] + destlib = os.path.join(srcdir, target) + os.system(cmd % (destlib, ' '.join(srcfiles))) + +FFI_main = None +FFI_lib = None +pyhelper_logging_callback = None + +# Return the Foreign Function Interface api to the caller +def get_ffi(): + global FFI_main, FFI_lib, pyhelper_logging_callback + if FFI_lib is None: + srcdir = os.path.dirname(os.path.realpath(__file__)) + check_build_code(srcdir, DEST_LIB, SOURCE_FILES, COMPILE_CMD + , OTHER_FILES) + FFI_main = cffi.FFI() + FFI_main.cdef(defs_stepcompress) + FFI_main.cdef(defs_serialqueue) + FFI_main.cdef(defs_pyhelper) + FFI_lib = FFI_main.dlopen(os.path.join(srcdir, DEST_LIB)) + # Setup error logging + def logging_callback(msg): + logging.error(FFI_main.string(msg)) + pyhelper_logging_callback = FFI_main.callback( + "void(const char *)", logging_callback) + FFI_lib.set_python_logging_callback(pyhelper_logging_callback) + return FFI_main, FFI_lib + + +###################################################################### +# hub-ctrl hub power controller +###################################################################### + +HC_COMPILE_CMD = "gcc -Wall -g -O2 -o %s %s -lusb" +HC_SOURCE_FILES = ['hub-ctrl.c'] +HC_SOURCE_DIR = '../lib/hub-ctrl' +HC_TARGET = "hub-ctrl" +HC_CMD = "sudo %s/hub-ctrl -h 0 -P 2 -p %d" + +def run_hub_ctrl(enable_power): + srcdir = os.path.dirname(os.path.realpath(__file__)) + hubdir = os.path.join(srcdir, HC_SOURCE_DIR) + check_build_code(hubdir, HC_TARGET, HC_SOURCE_FILES, HC_COMPILE_CMD) + os.system(HC_CMD % (hubdir, enable_power)) diff --git a/klippy/chelper/list.h b/klippy/chelper/list.h new file mode 100644 index 00000000..317a109c --- /dev/null +++ b/klippy/chelper/list.h @@ -0,0 +1,108 @@ +#ifndef __LIST_H +#define __LIST_H + +#define container_of(ptr, type, member) ({ \ + const typeof( ((type *)0)->member ) *__mptr = (ptr); \ + (type *)( (char *)__mptr - offsetof(type,member) );}) + + +/**************************************************************** + * list - Double linked lists + ****************************************************************/ + +struct list_node { + struct list_node *next, *prev; +}; + +struct list_head { + struct list_node root; +}; + +static inline void +list_init(struct list_head *h) +{ + h->root.prev = h->root.next = &h->root; +} + +static inline int +list_empty(const struct list_head *h) +{ + return h->root.next == &h->root; +} + +static inline void +list_del(struct list_node *n) +{ + struct list_node *prev = n->prev; + struct list_node *next = n->next; + next->prev = prev; + prev->next = next; +} + +static inline void +__list_add(struct list_node *n, struct list_node *prev, struct list_node *next) +{ + next->prev = n; + n->next = next; + n->prev = prev; + prev->next = n; +} + +static inline void +list_add_after(struct list_node *n, struct list_node *prev) +{ + __list_add(n, prev, prev->next); +} + +static inline void +list_add_before(struct list_node *n, struct list_node *next) +{ + __list_add(n, next->prev, next); +} + +static inline void +list_add_head(struct list_node *n, struct list_head *h) +{ + list_add_after(n, &h->root); +} + +static inline void +list_add_tail(struct list_node *n, struct list_head *h) +{ + list_add_before(n, &h->root); +} + +static inline void +list_join_tail(struct list_head *add, struct list_head *h) +{ + if (!list_empty(add)) { + struct list_node *prev = h->root.prev; + struct list_node *next = &h->root; + struct list_node *first = add->root.next; + struct list_node *last = add->root.prev; + first->prev = prev; + prev->next = first; + last->next = next; + next->prev = last; + } +} + +#define list_next_entry(pos, member) \ + container_of((pos)->member.next, typeof(*pos), member) + +#define list_first_entry(head, type, member) \ + container_of((head)->root.next, type, member) + +#define list_for_each_entry(pos, head, member) \ + for (pos = list_first_entry((head), typeof(*pos), member) \ + ; &pos->member != &(head)->root \ + ; pos = list_next_entry(pos, member)) + +#define list_for_each_entry_safe(pos, n, head, member) \ + for (pos = list_first_entry((head), typeof(*pos), member) \ + , n = list_next_entry(pos, member) \ + ; &pos->member != &(head)->root \ + ; pos = n, n = list_next_entry(n, member)) + + +#endif // list.h diff --git a/klippy/chelper/pyhelper.c b/klippy/chelper/pyhelper.c new file mode 100644 index 00000000..6fa4817f --- /dev/null +++ b/klippy/chelper/pyhelper.c @@ -0,0 +1,93 @@ +// Helper functions for C / Python interface +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include <errno.h> // errno +#include <stdarg.h> // va_start +#include <stdint.h> // uint8_t +#include <stdio.h> // fprintf +#include <string.h> // strerror +#include <time.h> // struct timespec +#include "pyhelper.h" // get_monotonic + +// Return the monotonic system time as a double +double +get_monotonic(void) +{ + struct timespec ts; + int ret = clock_gettime(CLOCK_MONOTONIC, &ts); + if (ret) { + report_errno("clock_gettime", ret); + return 0.; + } + return (double)ts.tv_sec + (double)ts.tv_nsec * .000000001; +} + +// Fill a 'struct timespec' with a system time stored in a double +struct timespec +fill_time(double time) +{ + time_t t = time; + return (struct timespec) {t, (time - t)*1000000000. }; +} + +static void +default_logger(const char *msg) +{ + fprintf(stderr, "%s\n", msg); +} + +static void (*python_logging_callback)(const char *msg) = default_logger; + +void +set_python_logging_callback(void (*func)(const char *)) +{ + python_logging_callback = func; +} + +// Log an error message +void +errorf(const char *fmt, ...) +{ + char buf[512]; + va_list args; + va_start(args, fmt); + vsnprintf(buf, sizeof(buf), fmt, args); + va_end(args); + buf[sizeof(buf)-1] = '\0'; + python_logging_callback(buf); +} + +// Report 'errno' in a message written to stderr +void +report_errno(char *where, int rc) +{ + int e = errno; + errorf("Got error %d in %s: (%d)%s", rc, where, e, strerror(e)); +} + +// Return a hex character for a given number +#define GETHEX(x) ((x) < 10 ? '0' + (x) : 'a' + (x) - 10) + +// Translate a binary string into an ASCII string with escape sequences +char * +dump_string(char *outbuf, int outbuf_size, char *inbuf, int inbuf_size) +{ + char *outend = &outbuf[outbuf_size-5], *o = outbuf; + uint8_t *inend = (void*)&inbuf[inbuf_size], *p = (void*)inbuf; + while (p < inend && o < outend) { + uint8_t c = *p++; + if (c > 31 && c < 127 && c != '\\') { + *o++ = c; + continue; + } + *o++ = '\\'; + *o++ = 'x'; + *o++ = GETHEX(c >> 4); + *o++ = GETHEX(c & 0x0f); + } + *o = '\0'; + return outbuf; +} diff --git a/klippy/chelper/pyhelper.h b/klippy/chelper/pyhelper.h new file mode 100644 index 00000000..d564c78b --- /dev/null +++ b/klippy/chelper/pyhelper.h @@ -0,0 +1,14 @@ +#ifndef PYHELPER_H +#define PYHELPER_H + +#define likely(x) __builtin_expect(!!(x), 1) +#define unlikely(x) __builtin_expect(!!(x), 0) + +double get_monotonic(void); +struct timespec fill_time(double time); +void set_python_logging_callback(void (*func)(const char *)); +void errorf(const char *fmt, ...) __attribute__ ((format (printf, 1, 2))); +void report_errno(char *where, int rc); +char *dump_string(char *outbuf, int outbuf_size, char *inbuf, int inbuf_size); + +#endif // pyhelper.h diff --git a/klippy/chelper/serialqueue.c b/klippy/chelper/serialqueue.c new file mode 100644 index 00000000..2f7c25a5 --- /dev/null +++ b/klippy/chelper/serialqueue.c @@ -0,0 +1,1090 @@ +// Serial port command queuing +// +// Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. +// +// This goal of this code is to handle low-level serial port +// communications with a microcontroller (mcu). This code is written +// in C (instead of python) to reduce communication latencies and to +// reduce scheduling jitter. The code queues messages to be +// transmitted, schedules transmission of commands at specified mcu +// clock times, prioritizes commands, and handles retransmissions. A +// background thread is launched to do this work and minimize latency. + +#include <fcntl.h> // fcntl +#include <math.h> // ceil +#include <poll.h> // poll +#include <pthread.h> // pthread_mutex_lock +#include <stddef.h> // offsetof +#include <stdint.h> // uint64_t +#include <stdio.h> // snprintf +#include <stdlib.h> // malloc +#include <string.h> // memset +#include <termios.h> // tcflush +#include <unistd.h> // pipe +#include "list.h" // list_add_tail +#include "pyhelper.h" // get_monotonic +#include "serialqueue.h" // struct queue_message + + +/**************************************************************** + * Poll reactor + ****************************************************************/ + +// The 'poll reactor' code is a mechanism for dispatching timer and +// file descriptor events. + +#define PR_NOW 0. +#define PR_NEVER 9999999999999999. + +struct pollreactor_timer { + double waketime; + double (*callback)(void *data, double eventtime); +}; + +struct pollreactor { + int num_fds, num_timers, must_exit; + void *callback_data; + double next_timer; + struct pollfd *fds; + void (**fd_callbacks)(void *data, double eventtime); + struct pollreactor_timer *timers; +}; + +// Allocate a new 'struct pollreactor' object +static void +pollreactor_setup(struct pollreactor *pr, int num_fds, int num_timers + , void *callback_data) +{ + pr->num_fds = num_fds; + pr->num_timers = num_timers; + pr->must_exit = 0; + pr->callback_data = callback_data; + pr->next_timer = PR_NEVER; + pr->fds = malloc(num_fds * sizeof(*pr->fds)); + memset(pr->fds, 0, num_fds * sizeof(*pr->fds)); + pr->fd_callbacks = malloc(num_fds * sizeof(*pr->fd_callbacks)); + memset(pr->fd_callbacks, 0, num_fds * sizeof(*pr->fd_callbacks)); + pr->timers = malloc(num_timers * sizeof(*pr->timers)); + memset(pr->timers, 0, num_timers * sizeof(*pr->timers)); + int i; + for (i=0; i<num_timers; i++) + pr->timers[i].waketime = PR_NEVER; +} + +// Free resources associated with a 'struct pollreactor' object +static void +pollreactor_free(struct pollreactor *pr) +{ + free(pr->fds); + pr->fds = NULL; + free(pr->fd_callbacks); + pr->fd_callbacks = NULL; + free(pr->timers); + pr->timers = NULL; +} + +// Add a callback for when a file descriptor (fd) becomes readable +static void +pollreactor_add_fd(struct pollreactor *pr, int pos, int fd, void *callback) +{ + pr->fds[pos].fd = fd; + pr->fds[pos].events = POLLIN|POLLHUP; + pr->fds[pos].revents = 0; + pr->fd_callbacks[pos] = callback; +} + +// Add a timer callback +static void +pollreactor_add_timer(struct pollreactor *pr, int pos, void *callback) +{ + pr->timers[pos].callback = callback; + pr->timers[pos].waketime = PR_NEVER; +} + +// Return the last schedule wake-up time for a timer +static double +pollreactor_get_timer(struct pollreactor *pr, int pos) +{ + return pr->timers[pos].waketime; +} + +// Set the wake-up time for a given timer +static void +pollreactor_update_timer(struct pollreactor *pr, int pos, double waketime) +{ + pr->timers[pos].waketime = waketime; + if (waketime < pr->next_timer) + pr->next_timer = waketime; +} + +// Internal code to invoke timer callbacks +static int +pollreactor_check_timers(struct pollreactor *pr, double eventtime) +{ + if (eventtime >= pr->next_timer) { + pr->next_timer = PR_NEVER; + int i; + for (i=0; i<pr->num_timers; i++) { + struct pollreactor_timer *timer = &pr->timers[i]; + double t = timer->waketime; + if (eventtime >= t) { + t = timer->callback(pr->callback_data, eventtime); + timer->waketime = t; + } + if (t < pr->next_timer) + pr->next_timer = t; + } + if (eventtime >= pr->next_timer) + return 0; + } + double timeout = ceil((pr->next_timer - eventtime) * 1000.); + return timeout < 1. ? 1 : (timeout > 1000. ? 1000 : (int)timeout); +} + +// Repeatedly check for timer and fd events and invoke their callbacks +static void +pollreactor_run(struct pollreactor *pr) +{ + double eventtime = get_monotonic(); + while (! pr->must_exit) { + int timeout = pollreactor_check_timers(pr, eventtime); + int ret = poll(pr->fds, pr->num_fds, timeout); + eventtime = get_monotonic(); + if (ret > 0) { + int i; + for (i=0; i<pr->num_fds; i++) + if (pr->fds[i].revents) + pr->fd_callbacks[i](pr->callback_data, eventtime); + } else if (ret < 0) { + report_errno("poll", ret); + pr->must_exit = 1; + } + } +} + +// Request that a currently running pollreactor_run() loop exit +static void +pollreactor_do_exit(struct pollreactor *pr) +{ + pr->must_exit = 1; +} + +// Check if a pollreactor_run() loop has been requested to exit +static int +pollreactor_is_exit(struct pollreactor *pr) +{ + return pr->must_exit; +} + +static int +set_non_blocking(int fd) +{ + int flags = fcntl(fd, F_GETFL); + if (flags < 0) { + report_errno("fcntl getfl", flags); + return -1; + } + int ret = fcntl(fd, F_SETFL, flags | O_NONBLOCK); + if (ret < 0) { + report_errno("fcntl setfl", flags); + return -1; + } + return 0; +} + + +/**************************************************************** + * Serial protocol helpers + ****************************************************************/ + +// Implement the standard crc "ccitt" algorithm on the given buffer +static uint16_t +crc16_ccitt(uint8_t *buf, uint8_t len) +{ + uint16_t crc = 0xffff; + while (len--) { + uint8_t data = *buf++; + data ^= crc & 0xff; + data ^= data << 4; + crc = ((((uint16_t)data << 8) | (crc >> 8)) ^ (uint8_t)(data >> 4) + ^ ((uint16_t)data << 3)); + } + return crc; +} + +// Verify a buffer starts with a valid mcu message +static int +check_message(uint8_t *need_sync, uint8_t *buf, int buf_len) +{ + if (buf_len < MESSAGE_MIN) + // Need more data + return 0; + if (*need_sync) + goto error; + uint8_t msglen = buf[MESSAGE_POS_LEN]; + if (msglen < MESSAGE_MIN || msglen > MESSAGE_MAX) + goto error; + uint8_t msgseq = buf[MESSAGE_POS_SEQ]; + if ((msgseq & ~MESSAGE_SEQ_MASK) != MESSAGE_DEST) + goto error; + if (buf_len < msglen) + // Need more data + return 0; + if (buf[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC) + goto error; + uint16_t msgcrc = ((buf[msglen-MESSAGE_TRAILER_CRC] << 8) + | (uint8_t)buf[msglen-MESSAGE_TRAILER_CRC+1]); + uint16_t crc = crc16_ccitt(buf, msglen-MESSAGE_TRAILER_SIZE); + if (crc != msgcrc) + goto error; + return msglen; + +error: ; + // Discard bytes until next SYNC found + uint8_t *next_sync = memchr(buf, MESSAGE_SYNC, buf_len); + if (next_sync) { + *need_sync = 0; + return -(next_sync - buf + 1); + } + *need_sync = 1; + return -buf_len; +} + +// Encode an integer as a variable length quantity (vlq) +static uint8_t * +encode_int(uint8_t *p, uint32_t v) +{ + int32_t sv = v; + if (sv < (3L<<5) && sv >= -(1L<<5)) goto f4; + if (sv < (3L<<12) && sv >= -(1L<<12)) goto f3; + if (sv < (3L<<19) && sv >= -(1L<<19)) goto f2; + if (sv < (3L<<26) && sv >= -(1L<<26)) goto f1; + *p++ = (v>>28) | 0x80; +f1: *p++ = ((v>>21) & 0x7f) | 0x80; +f2: *p++ = ((v>>14) & 0x7f) | 0x80; +f3: *p++ = ((v>>7) & 0x7f) | 0x80; +f4: *p++ = v & 0x7f; + return p; +} + + +/**************************************************************** + * Command queues + ****************************************************************/ + +struct command_queue { + struct list_head stalled_queue, ready_queue; + struct list_node node; +}; + +// Allocate a 'struct queue_message' object +static struct queue_message * +message_alloc(void) +{ + struct queue_message *qm = malloc(sizeof(*qm)); + memset(qm, 0, sizeof(*qm)); + return qm; +} + +// Allocate a queue_message and fill it with the specified data +static struct queue_message * +message_fill(uint8_t *data, int len) +{ + struct queue_message *qm = message_alloc(); + memcpy(qm->msg, data, len); + qm->len = len; + return qm; +} + +// Allocate a queue_message and fill it with a series of encoded vlq integers +struct queue_message * +message_alloc_and_encode(uint32_t *data, int len) +{ + struct queue_message *qm = message_alloc(); + int i; + uint8_t *p = qm->msg; + for (i=0; i<len; i++) { + p = encode_int(p, data[i]); + if (p > &qm->msg[MESSAGE_PAYLOAD_MAX]) + goto fail; + } + qm->len = p - qm->msg; + return qm; + +fail: + errorf("Encode error"); + qm->len = 0; + return qm; +} + +// Free the storage from a previous message_alloc() call +static void +message_free(struct queue_message *qm) +{ + free(qm); +} + +// Free all the messages on a queue +void +message_queue_free(struct list_head *root) +{ + while (!list_empty(root)) { + struct queue_message *qm = list_first_entry( + root, struct queue_message, node); + list_del(&qm->node); + message_free(qm); + } +} + + +/**************************************************************** + * Serialqueue interface + ****************************************************************/ + +struct serialqueue { + // Input reading + struct pollreactor pr; + int serial_fd; + int pipe_fds[2]; + uint8_t input_buf[4096]; + uint8_t need_sync; + int input_pos; + // Threading + pthread_t tid; + pthread_mutex_t lock; // protects variables below + pthread_cond_t cond; + int receive_waiting; + // Baud / clock tracking + double baud_adjust, idle_time; + double est_freq, last_clock_time; + uint64_t last_clock; + double last_receive_sent_time; + // Retransmit support + uint64_t send_seq, receive_seq; + uint64_t ignore_nak_seq, retransmit_seq, rtt_sample_seq; + struct list_head sent_queue; + double srtt, rttvar, rto; + // Pending transmission message queues + struct list_head pending_queues; + int ready_bytes, stalled_bytes, need_ack_bytes; + uint64_t need_kick_clock; + // Received messages + struct list_head receive_queue; + // Debugging + struct list_head old_sent, old_receive; + // Stats + uint32_t bytes_write, bytes_read, bytes_retransmit, bytes_invalid; +}; + +#define SQPF_SERIAL 0 +#define SQPF_PIPE 1 +#define SQPF_NUM 2 + +#define SQPT_RETRANSMIT 0 +#define SQPT_COMMAND 1 +#define SQPT_NUM 2 + +#define MIN_RTO 0.025 +#define MAX_RTO 5.000 +#define MIN_REQTIME_DELTA 0.250 +#define MIN_BACKGROUND_DELTA 0.005 +#define IDLE_QUERY_TIME 1.0 + +#define DEBUG_QUEUE_SENT 100 +#define DEBUG_QUEUE_RECEIVE 20 + +// Create a series of empty messages and add them to a list +static void +debug_queue_alloc(struct list_head *root, int count) +{ + int i; + for (i=0; i<count; i++) { + struct queue_message *qm = message_alloc(); + list_add_head(&qm->node, root); + } +} + +// Copy a message to a debug queue and free old debug messages +static void +debug_queue_add(struct list_head *root, struct queue_message *qm) +{ + list_add_tail(&qm->node, root); + struct queue_message *old = list_first_entry( + root, struct queue_message, node); + list_del(&old->node); + message_free(old); +} + +// Wake up the receiver thread if it is waiting +static void +check_wake_receive(struct serialqueue *sq) +{ + if (sq->receive_waiting) { + sq->receive_waiting = 0; + pthread_cond_signal(&sq->cond); + } +} + +// Write to the internal pipe to wake the background thread if in poll +static void +kick_bg_thread(struct serialqueue *sq) +{ + int ret = write(sq->pipe_fds[1], ".", 1); + if (ret < 0) + report_errno("pipe write", ret); +} + +// Update internal state when the receive sequence increases +static void +update_receive_seq(struct serialqueue *sq, double eventtime, uint64_t rseq) +{ + // Remove from sent queue + uint64_t sent_seq = sq->receive_seq; + for (;;) { + struct queue_message *sent = list_first_entry( + &sq->sent_queue, struct queue_message, node); + if (list_empty(&sq->sent_queue)) { + // Got an ack for a message not sent; must be connection init + sq->send_seq = rseq; + sq->last_receive_sent_time = 0.; + break; + } + sq->need_ack_bytes -= sent->len; + list_del(&sent->node); + debug_queue_add(&sq->old_sent, sent); + sent_seq++; + if (rseq == sent_seq) { + // Found sent message corresponding with the received sequence + sq->last_receive_sent_time = sent->receive_time; + break; + } + } + sq->receive_seq = rseq; + pollreactor_update_timer(&sq->pr, SQPT_COMMAND, PR_NOW); + + // Update retransmit info + if (sq->rtt_sample_seq && rseq > sq->rtt_sample_seq + && sq->last_receive_sent_time) { + // RFC6298 rtt calculations + double delta = eventtime - sq->last_receive_sent_time; + if (!sq->srtt) { + sq->rttvar = delta / 2.0; + sq->srtt = delta * 10.0; // use a higher start default + } else { + sq->rttvar = (3.0 * sq->rttvar + fabs(sq->srtt - delta)) / 4.0; + sq->srtt = (7.0 * sq->srtt + delta) / 8.0; + } + double rttvar4 = sq->rttvar * 4.0; + if (rttvar4 < 0.001) + rttvar4 = 0.001; + sq->rto = sq->srtt + rttvar4; + if (sq->rto < MIN_RTO) + sq->rto = MIN_RTO; + else if (sq->rto > MAX_RTO) + sq->rto = MAX_RTO; + sq->rtt_sample_seq = 0; + } + if (list_empty(&sq->sent_queue)) { + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NEVER); + } else { + struct queue_message *sent = list_first_entry( + &sq->sent_queue, struct queue_message, node); + double nr = eventtime + sq->rto + sent->len * sq->baud_adjust; + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, nr); + } +} + +// Process a well formed input message +static void +handle_message(struct serialqueue *sq, double eventtime, int len) +{ + // Calculate receive sequence number + uint64_t rseq = ((sq->receive_seq & ~MESSAGE_SEQ_MASK) + | (sq->input_buf[MESSAGE_POS_SEQ] & MESSAGE_SEQ_MASK)); + if (rseq < sq->receive_seq) + rseq += MESSAGE_SEQ_MASK+1; + + if (rseq != sq->receive_seq) + // New sequence number + update_receive_seq(sq, eventtime, rseq); + else if (len == MESSAGE_MIN && rseq > sq->ignore_nak_seq + && !list_empty(&sq->sent_queue)) + // Duplicate sequence number in an empty message is a nak + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT, PR_NOW); + + if (len > MESSAGE_MIN) { + // Add message to receive queue + struct queue_message *qm = message_fill(sq->input_buf, len); + qm->sent_time = (rseq > sq->retransmit_seq + ? sq->last_receive_sent_time : 0.); + qm->receive_time = get_monotonic(); // must be time post read() + qm->receive_time -= sq->baud_adjust * len; + list_add_tail(&qm->node, &sq->receive_queue); + check_wake_receive(sq); + } +} + +// Callback for input activity on the serial fd +static void +input_event(struct serialqueue *sq, double eventtime) +{ + int ret = read(sq->serial_fd, &sq->input_buf[sq->input_pos] + , sizeof(sq->input_buf) - sq->input_pos); + if (ret <= 0) { + report_errno("read", ret); + pollreactor_do_exit(&sq->pr); + return; + } + sq->input_pos += ret; + for (;;) { + ret = check_message(&sq->need_sync, sq->input_buf, sq->input_pos); + if (!ret) + // Need more data + return; + if (ret > 0) { + // Received a valid message + pthread_mutex_lock(&sq->lock); + handle_message(sq, eventtime, ret); + sq->bytes_read += ret; + pthread_mutex_unlock(&sq->lock); + } else { + // Skip bad data at beginning of input + ret = -ret; + pthread_mutex_lock(&sq->lock); + sq->bytes_invalid += ret; + pthread_mutex_unlock(&sq->lock); + } + sq->input_pos -= ret; + if (sq->input_pos) + memmove(sq->input_buf, &sq->input_buf[ret], sq->input_pos); + } +} + +// Callback for input activity on the pipe fd (wakes command_event) +static void +kick_event(struct serialqueue *sq, double eventtime) +{ + char dummy[4096]; + int ret = read(sq->pipe_fds[0], dummy, sizeof(dummy)); + if (ret < 0) + report_errno("pipe read", ret); + pollreactor_update_timer(&sq->pr, SQPT_COMMAND, PR_NOW); +} + +// Callback timer for when a retransmit should be done +static double +retransmit_event(struct serialqueue *sq, double eventtime) +{ + int ret = tcflush(sq->serial_fd, TCOFLUSH); + if (ret < 0) + report_errno("tcflush", ret); + + pthread_mutex_lock(&sq->lock); + + // Retransmit all pending messages + uint8_t buf[MESSAGE_MAX * MESSAGE_SEQ_MASK + 1]; + int buflen = 0, first_buflen = 0; + buf[buflen++] = MESSAGE_SYNC; + struct queue_message *qm; + list_for_each_entry(qm, &sq->sent_queue, node) { + memcpy(&buf[buflen], qm->msg, qm->len); + buflen += qm->len; + if (!first_buflen) + first_buflen = qm->len + 1; + } + ret = write(sq->serial_fd, buf, buflen); + if (ret < 0) + report_errno("retransmit write", ret); + sq->bytes_retransmit += buflen; + + // Update rto + if (pollreactor_get_timer(&sq->pr, SQPT_RETRANSMIT) == PR_NOW) { + // Retransmit due to nak + sq->ignore_nak_seq = sq->receive_seq; + if (sq->receive_seq < sq->retransmit_seq) + // Second nak for this retransmit - don't allow third + sq->ignore_nak_seq = sq->retransmit_seq; + } else { + // Retransmit due to timeout + sq->rto *= 2.0; + if (sq->rto > MAX_RTO) + sq->rto = MAX_RTO; + sq->ignore_nak_seq = sq->send_seq; + } + sq->retransmit_seq = sq->send_seq; + sq->rtt_sample_seq = 0; + sq->idle_time = eventtime + buflen * sq->baud_adjust; + double waketime = eventtime + first_buflen * sq->baud_adjust + sq->rto; + + pthread_mutex_unlock(&sq->lock); + return waketime; +} + +// Construct a block of data and send to the serial port +static void +build_and_send_command(struct serialqueue *sq, double eventtime) +{ + struct queue_message *out = message_alloc(); + out->len = MESSAGE_HEADER_SIZE; + + while (sq->ready_bytes) { + // Find highest priority message (message with lowest req_clock) + uint64_t min_clock = MAX_CLOCK; + struct command_queue *q, *cq = NULL; + struct queue_message *qm = NULL; + list_for_each_entry(q, &sq->pending_queues, node) { + if (!list_empty(&q->ready_queue)) { + struct queue_message *m = list_first_entry( + &q->ready_queue, struct queue_message, node); + if (m->req_clock < min_clock) { + min_clock = m->req_clock; + cq = q; + qm = m; + } + } + } + // Append message to outgoing command + if (out->len + qm->len > sizeof(out->msg) - MESSAGE_TRAILER_SIZE) + break; + list_del(&qm->node); + if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue)) + list_del(&cq->node); + memcpy(&out->msg[out->len], qm->msg, qm->len); + out->len += qm->len; + sq->ready_bytes -= qm->len; + message_free(qm); + } + + // Fill header / trailer + out->len += MESSAGE_TRAILER_SIZE; + out->msg[MESSAGE_POS_LEN] = out->len; + out->msg[MESSAGE_POS_SEQ] = MESSAGE_DEST | (sq->send_seq & MESSAGE_SEQ_MASK); + uint16_t crc = crc16_ccitt(out->msg, out->len - MESSAGE_TRAILER_SIZE); + out->msg[out->len - MESSAGE_TRAILER_CRC] = crc >> 8; + out->msg[out->len - MESSAGE_TRAILER_CRC+1] = crc & 0xff; + out->msg[out->len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC; + + // Send message + int ret = write(sq->serial_fd, out->msg, out->len); + if (ret < 0) + report_errno("write", ret); + sq->bytes_write += out->len; + if (eventtime > sq->idle_time) + sq->idle_time = eventtime; + sq->idle_time += out->len * sq->baud_adjust; + out->sent_time = eventtime; + out->receive_time = sq->idle_time; + if (list_empty(&sq->sent_queue)) + pollreactor_update_timer(&sq->pr, SQPT_RETRANSMIT + , sq->idle_time + sq->rto); + if (!sq->rtt_sample_seq) + sq->rtt_sample_seq = sq->send_seq; + sq->send_seq++; + sq->need_ack_bytes += out->len; + list_add_tail(&out->node, &sq->sent_queue); +} + +// Determine the time the next serial data should be sent +static double +check_send_command(struct serialqueue *sq, double eventtime) +{ + if ((sq->send_seq - sq->receive_seq >= MESSAGE_SEQ_MASK + || (sq->need_ack_bytes - 2*MESSAGE_MAX) * sq->baud_adjust > sq->srtt) + && sq->receive_seq != (uint64_t)-1) + // Need an ack before more messages can be sent + return PR_NEVER; + + // Check for stalled messages now ready + double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time; + idletime += MESSAGE_MIN * sq->baud_adjust; + double timedelta = idletime - sq->last_clock_time; + uint64_t ack_clock = ((uint64_t)(timedelta * sq->est_freq) + + sq->last_clock); + uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK; + struct command_queue *cq; + list_for_each_entry(cq, &sq->pending_queues, node) { + // Move messages from the stalled_queue to the ready_queue + while (!list_empty(&cq->stalled_queue)) { + struct queue_message *qm = list_first_entry( + &cq->stalled_queue, struct queue_message, node); + if (ack_clock < qm->min_clock) { + if (qm->min_clock < min_stalled_clock) + min_stalled_clock = qm->min_clock; + break; + } + list_del(&qm->node); + list_add_tail(&qm->node, &cq->ready_queue); + sq->stalled_bytes -= qm->len; + sq->ready_bytes += qm->len; + } + // Update min_ready_clock + if (!list_empty(&cq->ready_queue)) { + struct queue_message *qm = list_first_entry( + &cq->ready_queue, struct queue_message, node); + uint64_t req_clock = qm->req_clock; + if (req_clock == BACKGROUND_PRIORITY_CLOCK) + req_clock = (uint64_t)( + (sq->idle_time - sq->last_clock_time + MIN_BACKGROUND_DELTA) + * sq->est_freq) + sq->last_clock; + if (req_clock < min_ready_clock) + min_ready_clock = req_clock; + } + } + + // Check for messages to send + if (sq->ready_bytes >= MESSAGE_PAYLOAD_MAX) + return PR_NOW; + if (! sq->est_freq) { + if (sq->ready_bytes) + return PR_NOW; + sq->need_kick_clock = MAX_CLOCK; + return PR_NEVER; + } + uint64_t reqclock_delta = MIN_REQTIME_DELTA * sq->est_freq; + if (min_ready_clock <= ack_clock + reqclock_delta) + return PR_NOW; + uint64_t wantclock = min_ready_clock - reqclock_delta; + if (min_stalled_clock < wantclock) + wantclock = min_stalled_clock; + sq->need_kick_clock = wantclock; + return idletime + (wantclock - ack_clock) / sq->est_freq; +} + +// Callback timer to send data to the serial port +static double +command_event(struct serialqueue *sq, double eventtime) +{ + pthread_mutex_lock(&sq->lock); + double waketime; + for (;;) { + waketime = check_send_command(sq, eventtime); + if (waketime != PR_NOW) + break; + build_and_send_command(sq, eventtime); + } + pthread_mutex_unlock(&sq->lock); + return waketime; +} + +// Main background thread for reading/writing to serial port +static void * +background_thread(void *data) +{ + struct serialqueue *sq = data; + pollreactor_run(&sq->pr); + + pthread_mutex_lock(&sq->lock); + check_wake_receive(sq); + pthread_mutex_unlock(&sq->lock); + + return NULL; +} + +// Create a new 'struct serialqueue' object +struct serialqueue * +serialqueue_alloc(int serial_fd, int write_only) +{ + struct serialqueue *sq = malloc(sizeof(*sq)); + memset(sq, 0, sizeof(*sq)); + + // Reactor setup + sq->serial_fd = serial_fd; + int ret = pipe(sq->pipe_fds); + if (ret) + goto fail; + pollreactor_setup(&sq->pr, SQPF_NUM, SQPT_NUM, sq); + if (!write_only) + pollreactor_add_fd(&sq->pr, SQPF_SERIAL, serial_fd, input_event); + pollreactor_add_fd(&sq->pr, SQPF_PIPE, sq->pipe_fds[0], kick_event); + pollreactor_add_timer(&sq->pr, SQPT_RETRANSMIT, retransmit_event); + pollreactor_add_timer(&sq->pr, SQPT_COMMAND, command_event); + set_non_blocking(serial_fd); + set_non_blocking(sq->pipe_fds[0]); + set_non_blocking(sq->pipe_fds[1]); + + // Retransmit setup + sq->send_seq = 1; + if (write_only) { + sq->receive_seq = -1; + sq->rto = PR_NEVER; + } else { + sq->receive_seq = 1; + sq->rto = MIN_RTO; + } + + // Queues + sq->need_kick_clock = MAX_CLOCK; + list_init(&sq->pending_queues); + list_init(&sq->sent_queue); + list_init(&sq->receive_queue); + + // Debugging + list_init(&sq->old_sent); + list_init(&sq->old_receive); + debug_queue_alloc(&sq->old_sent, DEBUG_QUEUE_SENT); + debug_queue_alloc(&sq->old_receive, DEBUG_QUEUE_RECEIVE); + + // Thread setup + ret = pthread_mutex_init(&sq->lock, NULL); + if (ret) + goto fail; + ret = pthread_cond_init(&sq->cond, NULL); + if (ret) + goto fail; + ret = pthread_create(&sq->tid, NULL, background_thread, sq); + if (ret) + goto fail; + + return sq; + +fail: + report_errno("init", ret); + return NULL; +} + +// Request that the background thread exit +void +serialqueue_exit(struct serialqueue *sq) +{ + pollreactor_do_exit(&sq->pr); + kick_bg_thread(sq); + int ret = pthread_join(sq->tid, NULL); + if (ret) + report_errno("pthread_join", ret); +} + +// Free all resources associated with a serialqueue +void +serialqueue_free(struct serialqueue *sq) +{ + if (!sq) + return; + if (!pollreactor_is_exit(&sq->pr)) + serialqueue_exit(sq); + pthread_mutex_lock(&sq->lock); + message_queue_free(&sq->sent_queue); + message_queue_free(&sq->receive_queue); + message_queue_free(&sq->old_sent); + message_queue_free(&sq->old_receive); + while (!list_empty(&sq->pending_queues)) { + struct command_queue *cq = list_first_entry( + &sq->pending_queues, struct command_queue, node); + list_del(&cq->node); + message_queue_free(&cq->ready_queue); + message_queue_free(&cq->stalled_queue); + } + pthread_mutex_unlock(&sq->lock); + pollreactor_free(&sq->pr); + free(sq); +} + +// Allocate a 'struct command_queue' +struct command_queue * +serialqueue_alloc_commandqueue(void) +{ + struct command_queue *cq = malloc(sizeof(*cq)); + memset(cq, 0, sizeof(*cq)); + list_init(&cq->ready_queue); + list_init(&cq->stalled_queue); + return cq; +} + +// Free a 'struct command_queue' +void +serialqueue_free_commandqueue(struct command_queue *cq) +{ + if (!cq) + return; + if (!list_empty(&cq->ready_queue) || !list_empty(&cq->stalled_queue)) { + errorf("Memory leak! Can't free non-empty commandqueue"); + return; + } + free(cq); +} + +// Add a batch of messages to the given command_queue +void +serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq + , struct list_head *msgs) +{ + // Make sure min_clock is set in list and calculate total bytes + int len = 0; + struct queue_message *qm; + list_for_each_entry(qm, msgs, node) { + if (qm->min_clock + (1LL<<31) < qm->req_clock + && qm->req_clock != BACKGROUND_PRIORITY_CLOCK) + qm->min_clock = qm->req_clock - (1LL<<31); + len += qm->len; + } + if (! len) + return; + qm = list_first_entry(msgs, struct queue_message, node); + + // Add list to cq->stalled_queue + pthread_mutex_lock(&sq->lock); + if (list_empty(&cq->ready_queue) && list_empty(&cq->stalled_queue)) + list_add_tail(&cq->node, &sq->pending_queues); + list_join_tail(msgs, &cq->stalled_queue); + sq->stalled_bytes += len; + int mustwake = 0; + if (qm->min_clock < sq->need_kick_clock) { + sq->need_kick_clock = 0; + mustwake = 1; + } + pthread_mutex_unlock(&sq->lock); + + // Wake the background thread if necessary + if (mustwake) + kick_bg_thread(sq); +} + +// Schedule the transmission of a message on the serial port at a +// given time and priority. +void +serialqueue_send(struct serialqueue *sq, struct command_queue *cq + , uint8_t *msg, int len, uint64_t min_clock, uint64_t req_clock) +{ + struct queue_message *qm = message_fill(msg, len); + qm->min_clock = min_clock; + qm->req_clock = req_clock; + + struct list_head msgs; + list_init(&msgs); + list_add_tail(&qm->node, &msgs); + serialqueue_send_batch(sq, cq, &msgs); +} + +// Like serialqueue_send() but also builds the message to be sent +void +serialqueue_encode_and_send(struct serialqueue *sq, struct command_queue *cq + , uint32_t *data, int len + , uint64_t min_clock, uint64_t req_clock) +{ + struct queue_message *qm = message_alloc_and_encode(data, len); + qm->min_clock = min_clock; + qm->req_clock = req_clock; + + struct list_head msgs; + list_init(&msgs); + list_add_tail(&qm->node, &msgs); + serialqueue_send_batch(sq, cq, &msgs); +} + +// Return a message read from the serial port (or wait for one if none +// available) +void +serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm) +{ + pthread_mutex_lock(&sq->lock); + // Wait for message to be available + while (list_empty(&sq->receive_queue)) { + if (pollreactor_is_exit(&sq->pr)) + goto exit; + sq->receive_waiting = 1; + int ret = pthread_cond_wait(&sq->cond, &sq->lock); + if (ret) + report_errno("pthread_cond_wait", ret); + } + + // Remove message from queue + struct queue_message *qm = list_first_entry( + &sq->receive_queue, struct queue_message, node); + list_del(&qm->node); + + // Copy message + memcpy(pqm->msg, qm->msg, qm->len); + pqm->len = qm->len; + pqm->sent_time = qm->sent_time; + pqm->receive_time = qm->receive_time; + debug_queue_add(&sq->old_receive, qm); + + pthread_mutex_unlock(&sq->lock); + return; + +exit: + pqm->len = -1; + pthread_mutex_unlock(&sq->lock); +} + +void +serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust) +{ + pthread_mutex_lock(&sq->lock); + sq->baud_adjust = baud_adjust; + pthread_mutex_unlock(&sq->lock); +} + +// Set the estimated clock rate of the mcu on the other end of the +// serial port +void +serialqueue_set_clock_est(struct serialqueue *sq, double est_freq + , double last_clock_time, uint64_t last_clock) +{ + pthread_mutex_lock(&sq->lock); + sq->est_freq = est_freq; + sq->last_clock_time = last_clock_time; + sq->last_clock = last_clock; + pthread_mutex_unlock(&sq->lock); +} + +// Return a string buffer containing statistics for the serial port +void +serialqueue_get_stats(struct serialqueue *sq, char *buf, int len) +{ + struct serialqueue stats; + pthread_mutex_lock(&sq->lock); + memcpy(&stats, sq, sizeof(stats)); + pthread_mutex_unlock(&sq->lock); + + snprintf(buf, len, "bytes_write=%u bytes_read=%u" + " bytes_retransmit=%u bytes_invalid=%u" + " send_seq=%u receive_seq=%u retransmit_seq=%u" + " srtt=%.3f rttvar=%.3f rto=%.3f" + " ready_bytes=%u stalled_bytes=%u" + , stats.bytes_write, stats.bytes_read + , stats.bytes_retransmit, stats.bytes_invalid + , (int)stats.send_seq, (int)stats.receive_seq + , (int)stats.retransmit_seq + , stats.srtt, stats.rttvar, stats.rto + , stats.ready_bytes, stats.stalled_bytes); +} + +// Extract old messages stored in the debug queues +int +serialqueue_extract_old(struct serialqueue *sq, int sentq + , struct pull_queue_message *q, int max) +{ + int count = sentq ? DEBUG_QUEUE_SENT : DEBUG_QUEUE_RECEIVE; + struct list_head *rootp = sentq ? &sq->old_sent : &sq->old_receive; + struct list_head replacement, current; + list_init(&replacement); + debug_queue_alloc(&replacement, count); + list_init(¤t); + + // Atomically replace existing debug list with new zero'd list + pthread_mutex_lock(&sq->lock); + list_join_tail(rootp, ¤t); + list_init(rootp); + list_join_tail(&replacement, rootp); + pthread_mutex_unlock(&sq->lock); + + // Walk the debug list + int pos = 0; + while (!list_empty(¤t)) { + struct queue_message *qm = list_first_entry( + ¤t, struct queue_message, node); + if (qm->len && pos < max) { + struct pull_queue_message *pqm = q++; + pos++; + memcpy(pqm->msg, qm->msg, qm->len); + pqm->len = qm->len; + pqm->sent_time = qm->sent_time; + pqm->receive_time = qm->receive_time; + } + list_del(&qm->node); + message_free(qm); + } + return pos; +} diff --git a/klippy/chelper/serialqueue.h b/klippy/chelper/serialqueue.h new file mode 100644 index 00000000..80ab686e --- /dev/null +++ b/klippy/chelper/serialqueue.h @@ -0,0 +1,69 @@ +#ifndef SERIALQUEUE_H +#define SERIALQUEUE_H + +#include "list.h" // struct list_head + +#define MAX_CLOCK 0x7fffffffffffffffLL +#define BACKGROUND_PRIORITY_CLOCK 0x7fffffff00000000LL + +#define MESSAGE_MIN 5 +#define MESSAGE_MAX 64 +#define MESSAGE_HEADER_SIZE 2 +#define MESSAGE_TRAILER_SIZE 3 +#define MESSAGE_POS_LEN 0 +#define MESSAGE_POS_SEQ 1 +#define MESSAGE_TRAILER_CRC 3 +#define MESSAGE_TRAILER_SYNC 1 +#define MESSAGE_PAYLOAD_MAX (MESSAGE_MAX - MESSAGE_MIN) +#define MESSAGE_SEQ_MASK 0x0f +#define MESSAGE_DEST 0x10 +#define MESSAGE_SYNC 0x7E + +struct queue_message { + int len; + uint8_t msg[MESSAGE_MAX]; + union { + // Filled when on a command queue + struct { + uint64_t min_clock, req_clock; + }; + // Filled when in sent/receive queues + struct { + double sent_time, receive_time; + }; + }; + struct list_node node; +}; + +struct queue_message *message_alloc_and_encode(uint32_t *data, int len); +void message_queue_free(struct list_head *root); + +struct pull_queue_message { + uint8_t msg[MESSAGE_MAX]; + int len; + double sent_time, receive_time; +}; + +struct serialqueue; +struct serialqueue *serialqueue_alloc(int serial_fd, int write_only); +void serialqueue_exit(struct serialqueue *sq); +void serialqueue_free(struct serialqueue *sq); +struct command_queue *serialqueue_alloc_commandqueue(void); +void serialqueue_free_commandqueue(struct command_queue *cq); +void serialqueue_send_batch(struct serialqueue *sq, struct command_queue *cq + , struct list_head *msgs); +void serialqueue_send(struct serialqueue *sq, struct command_queue *cq + , uint8_t *msg, int len + , uint64_t min_clock, uint64_t req_clock); +void serialqueue_encode_and_send(struct serialqueue *sq, struct command_queue *cq + , uint32_t *data, int len + , uint64_t min_clock, uint64_t req_clock); +void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm); +void serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust); +void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq + , double last_clock_time, uint64_t last_clock); +void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len); +int serialqueue_extract_old(struct serialqueue *sq, int sentq + , struct pull_queue_message *q, int max); + +#endif // serialqueue.h diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c new file mode 100644 index 00000000..6c5f766f --- /dev/null +++ b/klippy/chelper/stepcompress.c @@ -0,0 +1,852 @@ +// Stepper pulse schedule compression +// +// Copyright (C) 2016,2017 Kevin O'Connor <kevin@koconnor.net> +// +// This file may be distributed under the terms of the GNU GPLv3 license. +// +// The goal of this code is to take a series of scheduled stepper +// pulse times and compress them into a handful of commands that can +// be efficiently transmitted and executed on a microcontroller (mcu). +// The mcu accepts step pulse commands that take interval, count, and +// add parameters such that 'count' pulses occur, with each step event +// calculating the next step event time using: +// next_wake_time = last_wake_time + interval; interval += add +// This code is writtin in C (instead of python) for processing +// efficiency - the repetitive integer math is vastly faster in C. + +#include <math.h> // sqrt +#include <stddef.h> // offsetof +#include <stdint.h> // uint32_t +#include <stdio.h> // fprintf +#include <stdlib.h> // malloc +#include <string.h> // memset +#include "pyhelper.h" // errorf +#include "serialqueue.h" // struct queue_message + +#define CHECK_LINES 1 +#define QUEUE_START_SIZE 1024 + +struct stepcompress { + // Buffer management + uint32_t *queue, *queue_end, *queue_pos, *queue_next; + // Internal tracking + uint32_t max_error; + double mcu_time_offset, mcu_freq; + // Message generation + uint64_t last_step_clock, homing_clock; + struct list_head msg_queue; + uint32_t queue_step_msgid, set_next_step_dir_msgid, oid; + int sdir, invert_sdir; +}; + + +/**************************************************************** + * Step compression + ****************************************************************/ + +#define DIV_UP(n,d) (((n) + (d) - 1) / (d)) + +static inline int32_t +idiv_up(int32_t n, int32_t d) +{ + return (n>=0) ? DIV_UP(n,d) : (n/d); +} + +static inline int32_t +idiv_down(int32_t n, int32_t d) +{ + return (n>=0) ? (n/d) : (n - d + 1) / d; +} + +struct points { + int32_t minp, maxp; +}; + +// Given a requested step time, return the minimum and maximum +// acceptable times +static inline struct points +minmax_point(struct stepcompress *sc, uint32_t *pos) +{ + uint32_t lsc = sc->last_step_clock, point = *pos - lsc; + uint32_t prevpoint = pos > sc->queue_pos ? *(pos-1) - lsc : 0; + uint32_t max_error = (point - prevpoint) / 2; + if (max_error > sc->max_error) + max_error = sc->max_error; + return (struct points){ point - max_error, point }; +} + +// The maximum add delta between two valid quadratic sequences of the +// form "add*count*(count-1)/2 + interval*count" is "(6 + 4*sqrt(2)) * +// maxerror / (count*count)". The "6 + 4*sqrt(2)" is 11.65685, but +// using 11 works well in practice. +#define QUADRATIC_DEV 11 + +struct step_move { + uint32_t interval; + uint16_t count; + int16_t add; +}; + +// Find a 'step_move' that covers a series of step times +static struct step_move +compress_bisect_add(struct stepcompress *sc) +{ + uint32_t *qlast = sc->queue_next; + if (qlast > sc->queue_pos + 65535) + qlast = sc->queue_pos + 65535; + struct points point = minmax_point(sc, sc->queue_pos); + int32_t outer_mininterval = point.minp, outer_maxinterval = point.maxp; + int32_t add = 0, minadd = -0x8000, maxadd = 0x7fff; + int32_t bestinterval = 0, bestcount = 1, bestadd = 1, bestreach = INT32_MIN; + int32_t zerointerval = 0, zerocount = 0; + + for (;;) { + // Find longest valid sequence with the given 'add' + struct points nextpoint; + int32_t nextmininterval = outer_mininterval; + int32_t nextmaxinterval = outer_maxinterval, interval = nextmaxinterval; + int32_t nextcount = 1; + for (;;) { + nextcount++; + if (&sc->queue_pos[nextcount-1] >= qlast) { + int32_t count = nextcount - 1; + return (struct step_move){ interval, count, add }; + } + nextpoint = minmax_point(sc, sc->queue_pos + nextcount - 1); + int32_t nextaddfactor = nextcount*(nextcount-1)/2; + int32_t c = add*nextaddfactor; + if (nextmininterval*nextcount < nextpoint.minp - c) + nextmininterval = DIV_UP(nextpoint.minp - c, nextcount); + if (nextmaxinterval*nextcount > nextpoint.maxp - c) + nextmaxinterval = (nextpoint.maxp - c) / nextcount; + if (nextmininterval > nextmaxinterval) + break; + interval = nextmaxinterval; + } + + // Check if this is the best sequence found so far + int32_t count = nextcount - 1, addfactor = count*(count-1)/2; + int32_t reach = add*addfactor + interval*count; + if (reach > bestreach + || (reach == bestreach && interval > bestinterval)) { + bestinterval = interval; + bestcount = count; + bestadd = add; + bestreach = reach; + if (!add) { + zerointerval = interval; + zerocount = count; + } + if (count > 0x200) + // No 'add' will improve sequence; avoid integer overflow + break; + } + + // Check if a greater or lesser add could extend the sequence + int32_t nextaddfactor = nextcount*(nextcount-1)/2; + int32_t nextreach = add*nextaddfactor + interval*nextcount; + if (nextreach < nextpoint.minp) { + minadd = add + 1; + outer_maxinterval = nextmaxinterval; + } else { + maxadd = add - 1; + outer_mininterval = nextmininterval; + } + + // The maximum valid deviation between two quadratic sequences + // can be calculated and used to further limit the add range. + if (count > 1) { + int32_t errdelta = sc->max_error*QUADRATIC_DEV / (count*count); + if (minadd < add - errdelta) + minadd = add - errdelta; + if (maxadd > add + errdelta) + maxadd = add + errdelta; + } + + // See if next point would further limit the add range + int32_t c = outer_maxinterval * nextcount; + if (minadd*nextaddfactor < nextpoint.minp - c) + minadd = idiv_up(nextpoint.minp - c, nextaddfactor); + c = outer_mininterval * nextcount; + if (maxadd*nextaddfactor > nextpoint.maxp - c) + maxadd = idiv_down(nextpoint.maxp - c, nextaddfactor); + + // Bisect valid add range and try again with new 'add' + if (minadd > maxadd) + break; + add = maxadd - (maxadd - minadd) / 4; + } + if (zerocount + zerocount/16 >= bestcount) + // Prefer add=0 if it's similar to the best found sequence + return (struct step_move){ zerointerval, zerocount, 0 }; + return (struct step_move){ bestinterval, bestcount, bestadd }; +} + + +/**************************************************************** + * Step compress checking + ****************************************************************/ + +#define ERROR_RET -989898989 + +// Verify that a given 'step_move' matches the actual step times +static int +check_line(struct stepcompress *sc, struct step_move move) +{ + if (!CHECK_LINES) + return 0; + if (!move.count || (!move.interval && !move.add && move.count > 1) + || move.interval >= 0x80000000) { + errorf("stepcompress o=%d i=%d c=%d a=%d: Invalid sequence" + , sc->oid, move.interval, move.count, move.add); + return ERROR_RET; + } + uint32_t interval = move.interval, p = 0; + uint16_t i; + for (i=0; i<move.count; i++) { + struct points point = minmax_point(sc, sc->queue_pos + i); + p += interval; + if (p < point.minp || p > point.maxp) { + errorf("stepcompress o=%d i=%d c=%d a=%d: Point %d: %d not in %d:%d" + , sc->oid, move.interval, move.count, move.add + , i+1, p, point.minp, point.maxp); + return ERROR_RET; + } + if (interval >= 0x80000000) { + errorf("stepcompress o=%d i=%d c=%d a=%d:" + " Point %d: interval overflow %d" + , sc->oid, move.interval, move.count, move.add + , i+1, interval); + return ERROR_RET; + } + interval += move.add; + } + return 0; +} + + +/**************************************************************** + * Step compress interface + ****************************************************************/ + +// Allocate a new 'stepcompress' object +struct stepcompress * +stepcompress_alloc(uint32_t max_error, uint32_t queue_step_msgid + , uint32_t set_next_step_dir_msgid, uint32_t invert_sdir + , uint32_t oid) +{ + struct stepcompress *sc = malloc(sizeof(*sc)); + memset(sc, 0, sizeof(*sc)); + sc->max_error = max_error; + list_init(&sc->msg_queue); + sc->queue_step_msgid = queue_step_msgid; + sc->set_next_step_dir_msgid = set_next_step_dir_msgid; + sc->oid = oid; + sc->sdir = -1; + sc->invert_sdir = !!invert_sdir; + return sc; +} + +// Free memory associated with a 'stepcompress' object +void +stepcompress_free(struct stepcompress *sc) +{ + if (!sc) + return; + free(sc->queue); + message_queue_free(&sc->msg_queue); + free(sc); +} + +// Convert previously scheduled steps into commands for the mcu +static int +stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) +{ + if (sc->queue_pos >= sc->queue_next) + return 0; + while (sc->last_step_clock < move_clock) { + struct step_move move = compress_bisect_add(sc); + int ret = check_line(sc, move); + if (ret) + return ret; + + uint32_t msg[5] = { + sc->queue_step_msgid, sc->oid, move.interval, move.count, move.add + }; + struct queue_message *qm = message_alloc_and_encode(msg, 5); + qm->min_clock = qm->req_clock = sc->last_step_clock; + int32_t addfactor = move.count*(move.count-1)/2; + uint32_t ticks = move.add*addfactor + move.interval*move.count; + sc->last_step_clock += ticks; + if (sc->homing_clock) + // When homing, all steps should be sent prior to homing_clock + qm->min_clock = qm->req_clock = sc->homing_clock; + list_add_tail(&qm->node, &sc->msg_queue); + + if (sc->queue_pos + move.count >= sc->queue_next) { + sc->queue_pos = sc->queue_next = sc->queue; + break; + } + sc->queue_pos += move.count; + } + return 0; +} + +// Generate a queue_step for a step far in the future from the last step +static int +stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock) +{ + uint32_t msg[5] = { + sc->queue_step_msgid, sc->oid, abs_step_clock - sc->last_step_clock, 1, 0 + }; + struct queue_message *qm = message_alloc_and_encode(msg, 5); + qm->min_clock = sc->last_step_clock; + sc->last_step_clock = qm->req_clock = abs_step_clock; + if (sc->homing_clock) + // When homing, all steps should be sent prior to homing_clock + qm->min_clock = qm->req_clock = sc->homing_clock; + list_add_tail(&qm->node, &sc->msg_queue); + return 0; +} + +// Send the set_next_step_dir command +static int +set_next_step_dir(struct stepcompress *sc, int sdir) +{ + if (sc->sdir == sdir) + return 0; + sc->sdir = sdir; + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; + uint32_t msg[3] = { + sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir + }; + struct queue_message *qm = message_alloc_and_encode(msg, 3); + qm->req_clock = sc->homing_clock ?: sc->last_step_clock; + list_add_tail(&qm->node, &sc->msg_queue); + return 0; +} + +// Reset the internal state of the stepcompress object +int +stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) +{ + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; + sc->last_step_clock = last_step_clock; + sc->sdir = -1; + return 0; +} + +// Indicate the stepper is in homing mode (or done homing if zero) +int +stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock) +{ + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; + sc->homing_clock = homing_clock; + return 0; +} + +// Queue an mcu command to go out in order with stepper commands +int +stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len) +{ + int ret = stepcompress_flush(sc, UINT64_MAX); + if (ret) + return ret; + + struct queue_message *qm = message_alloc_and_encode(data, len); + qm->req_clock = sc->homing_clock ?: sc->last_step_clock; + list_add_tail(&qm->node, &sc->msg_queue); + return 0; +} + +// Set the conversion rate of 'print_time' to mcu clock +static void +stepcompress_set_time(struct stepcompress *sc + , double time_offset, double mcu_freq) +{ + sc->mcu_time_offset = time_offset; + sc->mcu_freq = mcu_freq; +} + + +/**************************************************************** + * Queue management + ****************************************************************/ + +struct queue_append { + struct stepcompress *sc; + uint32_t *qnext, *qend, last_step_clock_32; + double clock_offset; +}; + +// Maximium clock delta between messages in the queue +#define CLOCK_DIFF_MAX (3<<28) + +// Create a cursor for inserting clock times into the queue +static inline struct queue_append +queue_append_start(struct stepcompress *sc, double print_time, double adjust) +{ + double print_clock = (print_time - sc->mcu_time_offset) * sc->mcu_freq; + return (struct queue_append) { + .sc = sc, .qnext = sc->queue_next, .qend = sc->queue_end, + .last_step_clock_32 = sc->last_step_clock, + .clock_offset = (print_clock - (double)sc->last_step_clock) + adjust }; +} + +// Finalize a cursor created with queue_append_start() +static inline void +queue_append_finish(struct queue_append qa) +{ + qa.sc->queue_next = qa.qnext; +} + +// Slow path for queue_append() +static int +queue_append_slow(struct stepcompress *sc, double rel_sc) +{ + uint64_t abs_step_clock = (uint64_t)rel_sc + sc->last_step_clock; + if (abs_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) { + // Avoid integer overflow on steps far in the future + int ret = stepcompress_flush(sc, abs_step_clock - CLOCK_DIFF_MAX + 1); + if (ret) + return ret; + + if (abs_step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) + return stepcompress_flush_far(sc, abs_step_clock); + } + + if (sc->queue_next - sc->queue_pos > 65535 + 2000) { + // No point in keeping more than 64K steps in memory + uint32_t flush = *(sc->queue_next-65535) - (uint32_t)sc->last_step_clock; + int ret = stepcompress_flush(sc, sc->last_step_clock + flush); + if (ret) + return ret; + } + + if (sc->queue_next >= sc->queue_end) { + // Make room in the queue + int in_use = sc->queue_next - sc->queue_pos; + if (sc->queue_pos > sc->queue) { + // Shuffle the internal queue to avoid having to allocate more ram + memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue)); + } else { + // Expand the internal queue of step times + int alloc = sc->queue_end - sc->queue; + if (!alloc) + alloc = QUEUE_START_SIZE; + while (in_use >= alloc) + alloc *= 2; + sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue)); + sc->queue_end = sc->queue + alloc; + } + sc->queue_pos = sc->queue; + sc->queue_next = sc->queue + in_use; + } + + *sc->queue_next++ = abs_step_clock; + return 0; +} + +// Add a clock time to the queue (flushing the queue if needed) +static inline int +queue_append(struct queue_append *qa, double step_clock) +{ + double rel_sc = step_clock + qa->clock_offset; + if (likely(!(qa->qnext >= qa->qend || rel_sc >= (double)CLOCK_DIFF_MAX))) { + *qa->qnext++ = qa->last_step_clock_32 + (uint32_t)rel_sc; + return 0; + } + // Call queue_append_slow() to handle queue expansion and integer overflow + struct stepcompress *sc = qa->sc; + uint64_t old_last_step_clock = sc->last_step_clock; + sc->queue_next = qa->qnext; + int ret = queue_append_slow(sc, rel_sc); + if (ret) + return ret; + qa->qnext = sc->queue_next; + qa->qend = sc->queue_end; + qa->last_step_clock_32 = sc->last_step_clock; + qa->clock_offset -= sc->last_step_clock - old_last_step_clock; + return 0; +} + + +/**************************************************************** + * Motion to step conversions + ****************************************************************/ + +// Common suffixes: _sd is step distance (a unit length the same +// distance the stepper moves on each step), _sv is step velocity (in +// units of step distance per time), _sd2 is step distance squared, _r +// is ratio (scalar usually between 0.0 and 1.0). Times are in +// seconds and acceleration is in units of step distance per second +// squared. + +// Wrapper around sqrt() to handle small negative numbers +static double +_safe_sqrt(double v) +{ + // Due to floating point truncation, it's possible to get a small + // negative number - treat it as zero. + if (v < -0.001) + errorf("safe_sqrt of %.9f", v); + return 0.; +} +static inline double safe_sqrt(double v) { + return likely(v >= 0.) ? sqrt(v) : _safe_sqrt(v); +} + +// Schedule a step event at the specified step_clock time +int32_t +stepcompress_push(struct stepcompress *sc, double print_time, int32_t sdir) +{ + int ret = set_next_step_dir(sc, !!sdir); + if (ret) + return ret; + struct queue_append qa = queue_append_start(sc, print_time, 0.5); + ret = queue_append(&qa, 0.); + if (ret) + return ret; + queue_append_finish(qa); + return sdir ? 1 : -1; +} + +// Schedule 'steps' number of steps at constant acceleration. If +// acceleration is zero (ie, constant velocity) it uses the formula: +// step_time = print_time + step_num/start_sv +// Otherwise it uses the formula: +// step_time = (print_time + sqrt(2*step_num/accel + (start_sv/accel)**2) +// - start_sv/accel) +int32_t +stepcompress_push_const( + struct stepcompress *sc, double print_time + , double step_offset, double steps, double start_sv, double accel) +{ + // Calculate number of steps to take + int sdir = 1; + if (steps < 0) { + sdir = 0; + steps = -steps; + step_offset = -step_offset; + } + int count = steps + .5 - step_offset; + if (count <= 0 || count > 10000000) { + if (count && steps) { + errorf("push_const invalid count %d %f %f %f %f %f" + , sc->oid, print_time, step_offset, steps + , start_sv, accel); + return ERROR_RET; + } + return 0; + } + int ret = set_next_step_dir(sc, sdir); + if (ret) + return ret; + int res = sdir ? count : -count; + + // Calculate each step time + if (!accel) { + // Move at constant velocity (zero acceleration) + struct queue_append qa = queue_append_start(sc, print_time, .5); + double inv_cruise_sv = sc->mcu_freq / start_sv; + double pos = (step_offset + .5) * inv_cruise_sv; + while (count--) { + ret = queue_append(&qa, pos); + if (ret) + return ret; + pos += inv_cruise_sv; + } + queue_append_finish(qa); + } else { + // Move with constant acceleration + double inv_accel = 1. / accel; + double accel_time = start_sv * inv_accel * sc->mcu_freq; + struct queue_append qa = queue_append_start( + sc, print_time, 0.5 - accel_time); + double accel_multiplier = 2. * inv_accel * sc->mcu_freq * sc->mcu_freq; + double pos = (step_offset + .5)*accel_multiplier + accel_time*accel_time; + while (count--) { + double v = safe_sqrt(pos); + int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v); + if (ret) + return ret; + pos += accel_multiplier; + } + queue_append_finish(qa); + } + return res; +} + +// Schedule steps using delta kinematics +static int32_t +_stepcompress_push_delta( + struct stepcompress *sc, int sdir + , double print_time, double move_sd, double start_sv, double accel + , double height, double startxy_sd, double arm_sd, double movez_r) +{ + // Calculate number of steps to take + double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; + double arm_sd2 = arm_sd * arm_sd; + double endxy_sd = startxy_sd - movexy_r*move_sd; + double end_height = safe_sqrt(arm_sd2 - endxy_sd*endxy_sd); + int count = (end_height + movez_r*move_sd - height) * (sdir ? 1. : -1.) + .5; + if (count <= 0 || count > 10000000) { + if (count) { + errorf("push_delta invalid count %d %d %f %f %f %f %f %f %f %f" + , sc->oid, count, print_time, move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + return ERROR_RET; + } + return 0; + } + int ret = set_next_step_dir(sc, sdir); + if (ret) + return ret; + int res = sdir ? count : -count; + + // Calculate each step time + height += (sdir ? .5 : -.5); + if (!accel) { + // Move at constant velocity (zero acceleration) + struct queue_append qa = queue_append_start(sc, print_time, .5); + double inv_cruise_sv = sc->mcu_freq / start_sv; + if (!movez_r) { + // Optimized case for common XY only moves (no Z movement) + while (count--) { + double v = safe_sqrt(arm_sd2 - height*height); + double pos = startxy_sd + (sdir ? -v : v); + int ret = queue_append(&qa, pos * inv_cruise_sv); + if (ret) + return ret; + height += (sdir ? 1. : -1.); + } + } else if (!movexy_r) { + // Optimized case for Z only moves + double pos = ((sdir ? height-end_height : end_height-height) + * inv_cruise_sv); + while (count--) { + int ret = queue_append(&qa, pos); + if (ret) + return ret; + pos += inv_cruise_sv; + } + } else { + // General case (handles XY+Z moves) + double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd; + while (count--) { + double relheight = movexy_r*height - zoffset; + double v = safe_sqrt(arm_sd2 - relheight*relheight); + double pos = start_pos + movez_r*height + (sdir ? -v : v); + int ret = queue_append(&qa, pos * inv_cruise_sv); + if (ret) + return ret; + height += (sdir ? 1. : -1.); + } + } + queue_append_finish(qa); + } else { + // Move with constant acceleration + double start_pos = movexy_r*startxy_sd, zoffset = movez_r*startxy_sd; + double inv_accel = 1. / accel; + start_pos += 0.5 * start_sv*start_sv * inv_accel; + struct queue_append qa = queue_append_start( + sc, print_time, 0.5 - start_sv * inv_accel * sc->mcu_freq); + double accel_multiplier = 2. * inv_accel * sc->mcu_freq * sc->mcu_freq; + while (count--) { + double relheight = movexy_r*height - zoffset; + double v = safe_sqrt(arm_sd2 - relheight*relheight); + double pos = start_pos + movez_r*height + (sdir ? -v : v); + v = safe_sqrt(pos * accel_multiplier); + int ret = queue_append(&qa, accel_multiplier >= 0. ? v : -v); + if (ret) + return ret; + height += (sdir ? 1. : -1.); + } + queue_append_finish(qa); + } + return res; +} + +int32_t +stepcompress_push_delta( + struct stepcompress *sc, double print_time, double move_sd + , double start_sv, double accel + , double height, double startxy_sd, double arm_sd, double movez_r) +{ + double reversexy_sd = startxy_sd + arm_sd*movez_r; + if (reversexy_sd <= 0.) + // All steps are in down direction + return _stepcompress_push_delta( + sc, 0, print_time, move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.; + if (reversexy_sd >= move_sd * movexy_r) + // All steps are in up direction + return _stepcompress_push_delta( + sc, 1, print_time, move_sd, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + // Steps in both up and down direction + int res1 = _stepcompress_push_delta( + sc, 1, print_time, reversexy_sd / movexy_r, start_sv, accel + , height, startxy_sd, arm_sd, movez_r); + if (res1 == ERROR_RET) + return res1; + int res2 = _stepcompress_push_delta( + sc, 0, print_time, move_sd, start_sv, accel + , height + res1, startxy_sd, arm_sd, movez_r); + if (res2 == ERROR_RET) + return res2; + return res1 + res2; +} + + +/**************************************************************** + * 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 * +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 +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 +steppersync_set_time(struct steppersync *ss, double time_offset, double mcu_freq) +{ + int i; + for (i=0; i<ss->sc_num; i++) { + struct stepcompress *sc = ss->sc_list[i]; + stepcompress_set_time(sc, time_offset, mcu_freq); + } +} + +// 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 +steppersync_flush(struct steppersync *ss, uint64_t move_clock) +{ + // Flush each stepcompress to the specified move_clock + int i; + for (i=0; i<ss->sc_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; i<ss->sc_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; +} |