shinys000114/klipper
1
0
Fork 0
mirror of git://soft.sys114.com/klipper synced 2026-02-11 01:58:06 +09:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
2021-02-17 22:20:47 -05:00
parent 2559a2dd5a
commit 1865080a07
5 changed files with 201 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files

149
klippy/chelper/serialqueue.c
View File

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