msgblock: Move message manipulation code from serialqueue.c to new msgblock.c

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 150 insertions, 0 deletions

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);
+    }
+}