From 1865080a0759b318a6bd9141aa7695de943736d5 Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 17 Feb 2021 22:20:47 -0500 Subject: [PATCH] msgblock: Move message manipulation code from serialqueue.c to new msgblock.c Signed-off-by: Kevin O'Connor --- klippy/chelper/__init__.py | 4 +- klippy/chelper/msgblock.c | 150 +++++++++++++++++++++++++++++++++++ klippy/chelper/msgblock.h | 45 +++++++++++ klippy/chelper/serialqueue.c | 149 +--------------------------------- klippy/chelper/serialqueue.h | 34 +------- 5 files changed, 201 insertions(+), 181 deletions(-) create mode 100644 klippy/chelper/msgblock.c create mode 100644 klippy/chelper/msgblock.h diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 07ee190d..55f394ef 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -18,7 +18,7 @@ COMPILE_ARGS = ("-Wall -g -O2 -shared -fPIC" SSE_FLAGS = "-mfpmath=sse -msse2" SOURCE_FILES = [ 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c', - 'pollreactor.c', + 'pollreactor.c', 'msgblock.c', 'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c', 'kin_shaper.c', @@ -26,7 +26,7 @@ SOURCE_FILES = [ DEST_LIB = "c_helper.so" OTHER_FILES = [ 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h', - 'trapq.h', 'pollreactor.h', + 'trapq.h', 'pollreactor.h', 'msgblock.h' ] defs_stepcompress = """ diff --git a/klippy/chelper/msgblock.c b/klippy/chelper/msgblock.c new file mode 100644 index 00000000..cc91d455 --- /dev/null +++ b/klippy/chelper/msgblock.c @@ -0,0 +1,150 @@ +// Helper code for the Klipper mcu protocol "message blocks" +// +// Copyright (C) 2016-2021 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // offsetof +#include // malloc +#include // memset +#include "msgblock.h" // message_alloc +#include "pyhelper.h" // errorf + + +/**************************************************************** + * Serial protocol helpers + ****************************************************************/ + +// Implement the standard crc "ccitt" algorithm on the given buffer +uint16_t +msgblock_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 +int +msgblock_check(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 = msgblock_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 + ****************************************************************/ + +// Allocate a 'struct queue_message' object +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 +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 &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 +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); + } +} diff --git a/klippy/chelper/msgblock.h b/klippy/chelper/msgblock.h new file mode 100644 index 00000000..cce1b2ee --- /dev/null +++ b/klippy/chelper/msgblock.h @@ -0,0 +1,45 @@ +#ifndef MSGBLOCK_H +#define MSGBLOCK_H + +#include // uint8_t +#include "list.h" // struct list_node + +#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; + }; + }; + uint64_t notify_id; + struct list_node node; +}; + +uint16_t msgblock_crc16_ccitt(uint8_t *buf, uint8_t len); +int msgblock_check(uint8_t *need_sync, uint8_t *buf, int buf_len); +struct queue_message *message_alloc(void); +struct queue_message *message_fill(uint8_t *data, int len); +struct queue_message *message_alloc_and_encode(uint32_t *data, int len); +void message_free(struct queue_message *qm); +void message_queue_free(struct list_head *root); + +#endif // msgblock.h diff --git a/klippy/chelper/serialqueue.c b/klippy/chelper/serialqueue.c index 38f65199..517a435c 100644 --- a/klippy/chelper/serialqueue.c +++ b/klippy/chelper/serialqueue.c @@ -24,159 +24,16 @@ #include // pipe #include "compiler.h" // __visible #include "list.h" // list_add_tail +#include "msgblock.h" // message_alloc #include "pollreactor.h" // pollreactor_alloc #include "pyhelper.h" // get_monotonic #include "serialqueue.h" // struct queue_message - -/**************************************************************** - * 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 &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; @@ -426,7 +283,7 @@ input_event(struct serialqueue *sq, double eventtime) sq->input_pos += ret; } for (;;) { - int len = check_message(&sq->need_sync, sq->input_buf, sq->input_pos); + int len = msgblock_check(&sq->need_sync, sq->input_buf, sq->input_pos); if (!len) // Need more data return; @@ -581,7 +438,7 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime) len += MESSAGE_TRAILER_SIZE; buf[MESSAGE_POS_LEN] = len; buf[MESSAGE_POS_SEQ] = MESSAGE_DEST | (sq->send_seq & MESSAGE_SEQ_MASK); - uint16_t crc = crc16_ccitt(buf, len - MESSAGE_TRAILER_SIZE); + uint16_t crc = msgblock_crc16_ccitt(buf, len - MESSAGE_TRAILER_SIZE); buf[len - MESSAGE_TRAILER_CRC] = crc >> 8; buf[len - MESSAGE_TRAILER_CRC+1] = crc & 0xff; buf[len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC; diff --git a/klippy/chelper/serialqueue.h b/klippy/chelper/serialqueue.h index 62af9aaf..17e14316 100644 --- a/klippy/chelper/serialqueue.h +++ b/klippy/chelper/serialqueue.h @@ -2,43 +2,11 @@ #define SERIALQUEUE_H #include "list.h" // struct list_head +#include "msgblock.h" // MESSAGE_MAX #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; - }; - }; - uint64_t notify_id; - 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;