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:
parent
1865080a07
commit
f938caa0d2
|
@ -85,6 +85,39 @@ f4: *p++ = v & 0x7f;
|
||||||
return p;
|
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
|
* Command queues
|
||||||
|
|
|
@ -36,6 +36,7 @@ struct queue_message {
|
||||||
|
|
||||||
uint16_t msgblock_crc16_ccitt(uint8_t *buf, uint8_t len);
|
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_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_alloc(void);
|
||||||
struct queue_message *message_fill(uint8_t *data, int len);
|
struct queue_message *message_fill(uint8_t *data, int len);
|
||||||
struct queue_message *message_alloc_and_encode(uint32_t *data, int len);
|
struct queue_message *message_alloc_and_encode(uint32_t *data, int len);
|
||||||
|
|
Loading…
Reference in New Issue