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

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
klippy/chelper/__init__.py
View File

@ -18,7 +18,7 @@ COMPILE_ARGS = ("-Wall -g -O2 -shared -fPIC"
SSE_FLAGS = "-mfpmath=sse -msse2" SSE_FLAGS = "-mfpmath=sse -msse2"
SOURCE_FILES = [ SOURCE_FILES = [
'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c', 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'itersolve.c', 'trapq.c',
'pollreactor.c', 'pollreactor.c', 'msgblock.c',
'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c', 'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c',
'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', 'kin_extruder.c',
'kin_shaper.c', 'kin_shaper.c',
@ -26,7 +26,7 @@ SOURCE_FILES = [
DEST_LIB = "c_helper.so" DEST_LIB = "c_helper.so"
OTHER_FILES = [ OTHER_FILES = [
'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h', 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h',
'trapq.h', 'pollreactor.h', 'trapq.h', 'pollreactor.h', 'msgblock.h'
] ]
defs_stepcompress = """ defs_stepcompress = """

150
klippy/chelper/msgblock.c Normal file
View File

@ -0,0 +1,150 @@
// Helper code for the Klipper mcu protocol "message blocks"
//
// Copyright (C) 2016-2021 Kevin O'Connor <kevin@koconnor.net>
//
// This file may be distributed under the terms of the GNU GPLv3 license.
#include <stddef.h> // offsetof
#include <stdlib.h> // malloc
#include <string.h> // memset
#include "msgblock.h" // message_alloc
#include "pyhelper.h" // errorf
/****************************************************************
* Serial protocol helpers
****************************************************************/
// Implement the standard crc "ccitt" algorithm on the given buffer
uint16_t
msgblock_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
int
msgblock_check(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 = msgblock_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
****************************************************************/
// Allocate a 'struct queue_message' object
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
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
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);
}
}

45
klippy/chelper/msgblock.h Normal file
View File

@ -0,0 +1,45 @@
#ifndef MSGBLOCK_H
#define MSGBLOCK_H
#include <stdint.h> // uint8_t
#include "list.h" // struct list_node
#define MESSAGE_MIN 5
#define MESSAGE_MAX 64
#define MESSAGE_HEADER_SIZE 2
#define MESSAGE_TRAILER_SIZE 3
#define MESSAGE_POS_LEN 0
#define MESSAGE_POS_SEQ 1
#define MESSAGE_TRAILER_CRC 3
#define MESSAGE_TRAILER_SYNC 1
#define MESSAGE_PAYLOAD_MAX (MESSAGE_MAX - MESSAGE_MIN)
#define MESSAGE_SEQ_MASK 0x0f
#define MESSAGE_DEST 0x10
#define MESSAGE_SYNC 0x7E
struct queue_message {
int len;
uint8_t msg[MESSAGE_MAX];
union {
// Filled when on a command queue
struct {
uint64_t min_clock, req_clock;
};
// Filled when in sent/receive queues
struct {
double sent_time, receive_time;
};
};
uint64_t notify_id;
struct list_node node;
};
uint16_t msgblock_crc16_ccitt(uint8_t *buf, uint8_t len);
int msgblock_check(uint8_t *need_sync, uint8_t *buf, int buf_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);
void message_free(struct queue_message *qm);
void message_queue_free(struct list_head *root);
#endif // msgblock.h

149
klippy/chelper/serialqueue.c
View File

@ -24,159 +24,16 @@
#include <unistd.h> // pipe #include <unistd.h> // pipe
#include "compiler.h" // __visible #include "compiler.h" // __visible
#include "list.h" // list_add_tail #include "list.h" // list_add_tail
#include "msgblock.h" // message_alloc
#include "pollreactor.h" // pollreactor_alloc #include "pollreactor.h" // pollreactor_alloc
#include "pyhelper.h" // get_monotonic #include "pyhelper.h" // get_monotonic
#include "serialqueue.h" // struct queue_message #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 command_queue {
struct list_head stalled_queue, ready_queue; struct list_head stalled_queue, ready_queue;
struct list_node node; 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 { struct serialqueue {
// Input reading // Input reading
struct pollreactor *pr; struct pollreactor *pr;
@ -426,7 +283,7 @@ input_event(struct serialqueue *sq, double eventtime)
sq->input_pos += ret; sq->input_pos += ret;
} }
for (;;) { 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) if (!len)
// Need more data // Need more data
return; return;
@ -581,7 +438,7 @@ build_and_send_command(struct serialqueue *sq, uint8_t *buf, double eventtime)
len += MESSAGE_TRAILER_SIZE; len += MESSAGE_TRAILER_SIZE;
buf[MESSAGE_POS_LEN] = len; buf[MESSAGE_POS_LEN] = len;
buf[MESSAGE_POS_SEQ] = MESSAGE_DEST | (sq->send_seq & MESSAGE_SEQ_MASK); 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] = crc >> 8;
buf[len - MESSAGE_TRAILER_CRC+1] = crc & 0xff; buf[len - MESSAGE_TRAILER_CRC+1] = crc & 0xff;
buf[len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC; buf[len - MESSAGE_TRAILER_SYNC] = MESSAGE_SYNC;

34
klippy/chelper/serialqueue.h
View File

@ -2,43 +2,11 @@
#define SERIALQUEUE_H #define SERIALQUEUE_H
#include "list.h" // struct list_head #include "list.h" // struct list_head
#include "msgblock.h" // MESSAGE_MAX
#define MAX_CLOCK 0x7fffffffffffffffLL #define MAX_CLOCK 0x7fffffffffffffffLL
#define BACKGROUND_PRIORITY_CLOCK 0x7fffffff00000000LL #define BACKGROUND_PRIORITY_CLOCK 0x7fffffff00000000LL
#define MESSAGE_MIN 5
#define MESSAGE_MAX 64
#define MESSAGE_HEADER_SIZE 2
#define MESSAGE_TRAILER_SIZE 3
#define MESSAGE_POS_LEN 0
#define MESSAGE_POS_SEQ 1
#define MESSAGE_TRAILER_CRC 3
#define MESSAGE_TRAILER_SYNC 1
#define MESSAGE_PAYLOAD_MAX (MESSAGE_MAX - MESSAGE_MIN)
#define MESSAGE_SEQ_MASK 0x0f
#define MESSAGE_DEST 0x10
#define MESSAGE_SYNC 0x7E
struct queue_message {
int len;
uint8_t msg[MESSAGE_MAX];
union {
// Filled when on a command queue
struct {
uint64_t min_clock, req_clock;
};
// Filled when in sent/receive queues
struct {
double sent_time, receive_time;
};
};
uint64_t notify_id;
struct list_node node;
};
struct queue_message *message_alloc_and_encode(uint32_t *data, int len);
void message_queue_free(struct list_head *root);
struct pull_queue_message { struct pull_queue_message {
uint8_t msg[MESSAGE_MAX]; uint8_t msg[MESSAGE_MAX];
int len; int len;