3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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>
This commit is contained in:
Kevin O'Connor 2021-02-17 22:29:38 -05:00
parent 1865080a07
commit f938caa0d2
2 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
klippy/chelper/msgblock.c
View File

@ -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

1
klippy/chelper/msgblock.h
View File

@ -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);