aboutsummaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
Diffstat
-rw-r--r--klippy/chelper/__init__.py4
-rw-r--r--klippy/chelper/msgblock.c150
-rw-r--r--klippy/chelper/msgblock.h45
-rw-r--r--klippy/chelper/serialqueue.c149
-rw-r--r--klippy/chelper/serialqueue.h34
5 files changed, 201 insertions, 181 deletions
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 <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <stddef.h> // offsetof
+#include <stdlib.h> // malloc
+#include <string.h> // 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<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
+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 <stdint.h> // 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 <unistd.h> // 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<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;
@@ -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;
generated by cgit v1.2.3-70-g09d2 (git 2.50.1) at 2025-09-02 03:02:34 +0000