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

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

1 files changed, 3 insertions, 146 deletions

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;