mirror of
https://github.com/andreili/katapult.git
synced 2025-08-23 19:34:06 +02:00
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 <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor
Eric Callahan
parent
4a798a2e3b
commit
799fe5313b
148
src/command.c
148
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;
|
||||
}
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#ifndef __COMMAND_H
|
||||
#define __COMMAND_H
|
||||
|
||||
#include <stdarg.h> // va_list
|
||||
#include <stdint.h> // 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> <data> <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
|
||||
|
||||
@ -8,7 +8,9 @@
|
||||
|
||||
#include <string.h> // 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);
|
||||
|
||||
|
||||
@ -3,20 +3,25 @@
|
||||
|
||||
#include <stdarg.h> // va_list
|
||||
#include <stdint.h> // 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);
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user