From 799fe5313b0b5b47b5de2f05418355a8d1ce8bda Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Wed, 11 May 2022 12:34:00 -0400 Subject: [PATCH] command: Rework command encoding and processing to be more similar to Klipper Introduce the Klipper command_encode() and command_dispatch() style functions. This makes it easier to import additional Klipper low-level message handling code. Signed-off-by: Kevin O'Connor --- src/command.c | 148 +++++++++++++++++++++++-------------------- src/command.h | 35 ++++++++-- src/generic/canbus.c | 62 +++++++++++++++--- src/generic/misc.h | 15 +++-- 4 files changed, 171 insertions(+), 89 deletions(-) diff --git a/src/command.c b/src/command.c index 97a56d5..ff149ef 100644 --- a/src/command.c +++ b/src/command.c @@ -10,10 +10,13 @@ #include "byteorder.h" // cpu_to_le32 #include "command.h" // send_ack -// Input Tracking -#define CMD_BUF_SIZE (CONFIG_BLOCK_SIZE + 64) -static uint8_t cmd_buf[CMD_BUF_SIZE]; -static uint8_t cmd_pos = 0; +uint_fast8_t +command_encode_and_frame(uint8_t *buf, const struct command_encoder *ce + , va_list args) +{ + memcpy(buf, ce->data, ce->max_size); + return ce->max_size; +} static void command_respond(uint32_t *data, uint32_t cmdid, uint32_t data_len) @@ -23,7 +26,9 @@ command_respond(uint32_t *data, uint32_t cmdid, uint32_t data_len) // calculate the CRC uint16_t crc = crc16_ccitt((uint8_t *)data + 2, (data_len - 2) * 4 + 2); data[data_len - 1] = cpu_to_le32(0x0399 << 16 | crc); - console_process_tx((uint8_t *)data, data_len * 4); + + struct command_encoder ce = { .data = data, .max_size = data_len * 4 }; + console_sendf(&ce, (va_list){}); } void @@ -53,9 +58,13 @@ command_get_arg_count(uint32_t *data) return le32_to_cpu(data[0]) >> 24; } -static void -process_command(uint8_t cmd, uint32_t *data, uint8_t data_len) +// Dispatch all the commands found in a message block +void +command_dispatch(uint8_t *buf, uint_fast8_t msglen) { + uint32_t data[DIV_ROUND_UP(MESSAGE_MAX, 4)]; + memcpy(data, buf, msglen); + uint32_t cmd = (le32_to_cpu(data[0]) >> 16) & 0xff; switch (cmd) { case CMD_CONNECT: command_connect(data); @@ -81,72 +90,73 @@ process_command(uint8_t cmd, uint32_t *data, uint8_t data_len) } } -static void -decode_command(void) +enum { CF_NEED_SYNC=1<<0, CF_NEED_VALID=1<<1 }; + +// Find the next complete message block +int_fast8_t +command_find_block(uint8_t *buf, uint_fast8_t buf_len, uint_fast8_t *pop_count) { - uint8_t remaining = cmd_pos; - uint8_t *tmpbuf = cmd_buf; - while (remaining) { - if (tmpbuf[0] == 0x01) { - // potential match - if (remaining >= PROTO_SIZE) { - uint16_t header = tmpbuf[0] << 8 | tmpbuf[1]; - uint8_t cmd = tmpbuf[2]; - uint8_t length = tmpbuf[3]; - uint16_t full_length = PROTO_SIZE * 2 + length * 4; - if (header == CMD_HEADER) { - if (full_length > CMD_BUF_SIZE) { - // packet too large, nack it and move on - command_respond_nack(); - } else if (remaining >= full_length) { - remaining -= full_length; - uint16_t fpos = full_length - 4; - uint16_t trailer = tmpbuf[fpos + 2] << 8 | tmpbuf[fpos + 3]; - if (trailer != CMD_TRAILER) { - command_respond_nack(); - } else { - uint16_t crc = le16_to_cpu(*(uint16_t *)(&tmpbuf[fpos])); - uint16_t calc_crc = crc16_ccitt(&tmpbuf[2], full_length - 6); - if (crc != calc_crc) { - command_respond_nack(); - } else { - // valid command, process - process_command(cmd, (uint32_t *)tmpbuf, length); - } - } - if (!remaining) - break; - } else { - // Header is valid, haven't received full packet - break; - } - } - } else { - // Not enough data, check again after the next read - break; - } - } - remaining--; - tmpbuf++; + static uint8_t sync_state; + if (buf_len && sync_state & CF_NEED_SYNC) + goto need_sync; + if (buf_len < MESSAGE_MIN) + goto need_more_data; + if (buf[MESSAGE_POS_STX1] != MESSAGE_STX1 + || buf[MESSAGE_POS_STX2] != MESSAGE_STX2) + goto error; + uint_fast8_t msglen = buf[MESSAGE_POS_LEN] * 4 + 8; + if (msglen < MESSAGE_MIN || msglen > MESSAGE_MAX) + goto error; + if (buf_len < msglen) + goto need_more_data; + if (buf[msglen-MESSAGE_TRAILER_SYNC2] != MESSAGE_SYNC2 + || buf[msglen-MESSAGE_TRAILER_SYNC] != MESSAGE_SYNC) + goto error; + uint16_t msgcrc = (buf[msglen-MESSAGE_TRAILER_CRC] + | (buf[msglen-MESSAGE_TRAILER_CRC+1] << 8)); + uint16_t crc = crc16_ccitt(buf+2, msglen-MESSAGE_TRAILER_SIZE-2); + if (crc != msgcrc) + goto error; + sync_state &= ~CF_NEED_VALID; + *pop_count = msglen; + return 1; + +need_more_data: + *pop_count = 0; + return 0; +error: + sync_state |= CF_NEED_SYNC; +need_sync: ; + // Discard bytes until next SYNC found + uint8_t *next_sync = memchr(buf, MESSAGE_STX1, buf_len); + if (next_sync) { + sync_state &= ~CF_NEED_SYNC; + *pop_count = next_sync - buf; + } else { + *pop_count = buf_len; } - if (remaining) { - // move the buffer - uint8_t rpos = cmd_pos - remaining; - memmove(&cmd_buf[0], &cmd_buf[rpos], remaining); - } - cmd_pos = remaining; + if (sync_state & CF_NEED_VALID) + return -1; + sync_state |= CF_NEED_VALID; + command_respond_nack(); + return -1; } +// Compat wrapper for klipper low-level code void -console_process_rx(uint8_t *data, uint32_t len) +command_send_ack(void) { - // read into the command buffer - if (cmd_pos >= CMD_BUF_SIZE) - return; - else if (cmd_pos + len > CMD_BUF_SIZE) - len = CMD_BUF_SIZE - cmd_pos; - memcpy(&cmd_buf[cmd_pos], data, len); - cmd_pos += len; - if (cmd_pos > PROTO_SIZE) - decode_command(); +} + +// Find a message block and then dispatch all the commands in it +int_fast8_t +command_find_and_dispatch(uint8_t *buf, uint_fast8_t buf_len + , uint_fast8_t *pop_count) +{ + int_fast8_t ret = command_find_block(buf, buf_len, pop_count); + if (ret > 0) { + command_dispatch(buf, *pop_count); + command_send_ack(); + } + return ret; } diff --git a/src/command.h b/src/command.h index b461024..d0b5821 100644 --- a/src/command.h +++ b/src/command.h @@ -1,6 +1,7 @@ #ifndef __COMMAND_H #define __COMMAND_H +#include // va_list #include // uint32_t #include "ctr.h" // DECL_CTR @@ -18,7 +19,6 @@ 2, CTR_INT(VALUE), CTR_INT(COUNT)) #define PROTO_VERSION 0x00010000 // Version 1.0.0 -#define PROTO_SIZE 4 #define CMD_CONNECT 0x11 #define CMD_RX_BLOCK 0x12 #define CMD_RX_EOF 0x13 @@ -31,8 +31,20 @@ // Command Format: // <2 byte header> <1 byte cmd> <1 byte data word count> <2 byte crc> <2 byte trailer> -#define CMD_HEADER 0x0188 -#define CMD_TRAILER 0x9903 +#define MESSAGE_MIN 8 +#define MESSAGE_MAX 128 +#define MESSAGE_HEADER_SIZE 4 +#define MESSAGE_TRAILER_SIZE 4 +#define MESSAGE_POS_STX1 0 +#define MESSAGE_POS_STX2 1 +#define MESSAGE_POS_LEN 3 +#define MESSAGE_TRAILER_CRC 4 +#define MESSAGE_TRAILER_SYNC2 2 +#define MESSAGE_TRAILER_SYNC 1 +#define MESSAGE_STX1 0x01 +#define MESSAGE_STX2 0x88 +#define MESSAGE_SYNC2 0x99 +#define MESSAGE_SYNC 0x03 // command handlers void command_connect(uint32_t *data); @@ -42,13 +54,22 @@ void command_eof(uint32_t *data); void command_complete(uint32_t *data); void command_get_canbus_id(uint32_t *data); -// board specific code -void console_process_tx(uint8_t *data, uint32_t size); - // command.c void command_respond_ack(uint32_t acked_cmd, uint32_t *out, uint32_t out_len); void command_respond_command_error(void); int command_get_arg_count(uint32_t *data); -void console_process_rx(uint8_t *data, uint32_t len); + +struct command_encoder { + uint32_t *data; + uint_fast8_t max_size; +}; +uint_fast8_t command_encode_and_frame( + uint8_t *buf, const struct command_encoder *ce, va_list args); +int_fast8_t command_find_block(uint8_t *buf, uint_fast8_t buf_len + , uint_fast8_t *pop_count); +void command_dispatch(uint8_t *buf, uint_fast8_t msglen); +void command_send_ack(void); +int_fast8_t command_find_and_dispatch(uint8_t *buf, uint_fast8_t buf_len + , uint_fast8_t *pop_count); #endif // command.h diff --git a/src/generic/canbus.c b/src/generic/canbus.c index a8899e7..efc48ce 100644 --- a/src/generic/canbus.c +++ b/src/generic/canbus.c @@ -8,7 +8,9 @@ #include // memcpy #include "canbus.h" // canbus_set_uuid -#include "command.h" // DECL_TASK +#include "command.h" // DECL_CONSTANT +#include "generic/io.h" // readb +#include "generic/irq.h" // irq_disable #include "sched.h" // sched_wake_task static uint32_t canbus_assigned_id; @@ -54,15 +56,15 @@ DECL_TASK(canbus_tx_task); // Encode and transmit a "response" message void -console_process_tx(uint8_t *data, uint32_t size) +console_sendf(const struct command_encoder *ce, va_list args) { // Verify space for message uint32_t tpos = transmit_pos, tmax = transmit_max; if (tpos >= tmax) transmit_pos = transmit_max = tpos = tmax = 0; - - if (tmax + size > sizeof(transmit_buf)) { - if (tmax + size - tpos > sizeof(transmit_buf)) + uint32_t max_size = ce->max_size; + if (tmax + max_size > sizeof(transmit_buf)) { + if (tmax + max_size - tpos > sizeof(transmit_buf)) // Not enough space for message return; // Move buffer @@ -73,10 +75,10 @@ console_process_tx(uint8_t *data, uint32_t size) } // Generate message - memcpy(&transmit_buf[tmax], data, size); + uint32_t msglen = command_encode_and_frame(&transmit_buf[tmax], ce, args); // Start message transmit - transmit_max = tmax + size; + transmit_max = tmax + msglen; canbus_notify_tx(); } @@ -184,16 +186,49 @@ canbus_notify_rx(void) sched_wake_task(&canbus_rx_wake); } +static uint8_t receive_buf[192], receive_pos; +DECL_CONSTANT("RECEIVE_WINDOW", ARRAY_SIZE(receive_buf)); + // Handle incoming data (called from IRQ handler) void canbus_process_data(uint32_t id, uint32_t len, uint8_t *data) { if (!id || id != canbus_assigned_id) return; - console_process_rx(data, len); + int rpos = receive_pos; + if (len > sizeof(receive_buf) - rpos) + len = sizeof(receive_buf) - rpos; + memcpy(&receive_buf[rpos], data, len); + receive_pos = rpos + len; canbus_notify_rx(); } +// Remove from the receive buffer the given number of bytes +static void +console_pop_input(int len) +{ + int copied = 0; + for (;;) { + int rpos = readb(&receive_pos); + int needcopy = rpos - len; + if (needcopy) { + memmove(&receive_buf[copied], &receive_buf[copied + len] + , needcopy - copied); + copied = needcopy; + canbus_notify_rx(); + } + irqstatus_t flag = irq_save(); + if (rpos != readb(&receive_pos)) { + // Raced with irq handler - retry + irq_restore(flag); + continue; + } + receive_pos = needcopy; + irq_restore(flag); + break; + } +} + // Task to process incoming commands and admin messages void canbus_rx_task(void) @@ -213,6 +248,17 @@ canbus_rx_task(void) else if (id == CANBUS_ID_ADMIN) can_process(id, ret, data); } + + // Check for a complete message block and process it + uint_fast8_t rpos = readb(&receive_pos), pop_count; + int ret = command_find_block(receive_buf, rpos, &pop_count); + if (ret > 0) + command_dispatch(receive_buf, pop_count); + if (ret) { + console_pop_input(pop_count); + if (ret > 0) + command_send_ack(); + } } DECL_TASK(canbus_rx_task); diff --git a/src/generic/misc.h b/src/generic/misc.h index a19df60..f5ba829 100644 --- a/src/generic/misc.h +++ b/src/generic/misc.h @@ -3,20 +3,25 @@ #include // va_list #include // uint8_t -#include "autoconf.h" // CONFIG_MACH_STM32F0 + +struct command_encoder; +void console_sendf(const struct command_encoder *ce, va_list args); +void *console_receive_buffer(void); uint64_t get_bootup_code(void); void set_bootup_code(uint64_t code); void jump_to_application(void); -// Timer Functions -#if CONFIG_MACH_STM32F0 void timer_init(void); -#endif +void udelay(uint32_t usecs); + uint32_t timer_from_us(uint32_t us); uint8_t timer_is_before(uint32_t time1, uint32_t time2); uint32_t timer_read_time(void); -void udelay(uint32_t usecs); +void timer_kick(void); + +void *dynmem_start(void); +void *dynmem_end(void); uint16_t crc16_ccitt(uint8_t *buf, uint_fast8_t len);