msgblock: Add msgblock_decode()

Add function that can parse a simple VLQ encoded message to an array
of integers.

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

2 files changed, 34 insertions, 0 deletions

diff --git a/klippy/chelper/msgblock.c b/klippy/chelper/msgblock.c
index cc91d455..3c848b13 100644
--- a/klippy/chelper/msgblock.c
+++ b/klippy/chelper/msgblock.c
@@ -85,6 +85,39 @@ f4: *p++ = v & 0x7f;
     return p;
 }
 
+// Parse an integer that was encoded as a "variable length quantity"
+static uint32_t
+parse_int(uint8_t **pp)
+{
+    uint8_t *p = *pp, c = *p++;
+    uint32_t v = c & 0x7f;
+    if ((c & 0x60) == 0x60)
+        v |= -0x20;
+    while (c & 0x80) {
+        c = *p++;
+        v = (v<<7) | (c & 0x7f);
+    }
+    *pp = p;
+    return v;
+}
+
+// Parse the VLQ contents of a message
+int
+msgblock_decode(uint32_t *data, int data_len, uint8_t *msg, int msg_len)
+{
+    uint8_t *p = &msg[MESSAGE_HEADER_SIZE];
+    uint8_t *end = &msg[msg_len - MESSAGE_TRAILER_SIZE];
+    while (data_len--) {
+        if (p >= end)
+            return -1;
+        *data++ = parse_int(&p);
+    }
+    if (p != end)
+        // Invalid message
+        return -1;
+    return 0;
+}
+
 
 /****************************************************************
  * Command queues
diff --git a/klippy/chelper/msgblock.h b/klippy/chelper/msgblock.h
index cce1b2ee..2d2967eb 100644
--- a/klippy/chelper/msgblock.h
+++ b/klippy/chelper/msgblock.h
@@ -36,6 +36,7 @@ struct queue_message {
 
 uint16_t msgblock_crc16_ccitt(uint8_t *buf, uint8_t len);
 int msgblock_check(uint8_t *need_sync, uint8_t *buf, int buf_len);
+int msgblock_decode(uint32_t *data, int data_len, uint8_t *msg, int msg_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);