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>

src/command.c

@@ -10,10 +10,13 @@
 #include "byteorder.h" // cpu_to_le32
 #include "command.h" // send_ack
 
-// Input Tracking
+uint_fast8_t
-#define CMD_BUF_SIZE (CONFIG_BLOCK_SIZE + 64)
+command_encode_and_frame(uint8_t *buf, const struct command_encoder *ce
-static uint8_t cmd_buf[CMD_BUF_SIZE];
+                         , va_list args)
-static uint8_t cmd_pos = 0;
+{
+    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
+// Dispatch all the commands found in a message block
-process_command(uint8_t cmd, uint32_t *data, uint8_t data_len)
+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
+enum { CF_NEED_SYNC=1<<0, CF_NEED_VALID=1<<1 };
-decode_command(void)
+
+// 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;
+    static uint8_t sync_state;
-    uint8_t *tmpbuf = cmd_buf;
+    if (buf_len && sync_state & CF_NEED_SYNC)
-    while (remaining) {
+        goto need_sync;
-        if (tmpbuf[0] == 0x01) {
+    if (buf_len < MESSAGE_MIN)
-            // potential match
+        goto need_more_data;
-            if (remaining >= PROTO_SIZE) {
+    if (buf[MESSAGE_POS_STX1] != MESSAGE_STX1
-                uint16_t header = tmpbuf[0] << 8 | tmpbuf[1];
+        || buf[MESSAGE_POS_STX2] != MESSAGE_STX2)
-                uint8_t cmd = tmpbuf[2];
+        goto error;
-                uint8_t length = tmpbuf[3];
+    uint_fast8_t msglen = buf[MESSAGE_POS_LEN] * 4 + 8;
-                uint16_t full_length = PROTO_SIZE * 2 + length * 4;
+    if (msglen < MESSAGE_MIN || msglen > MESSAGE_MAX)
-                if (header == CMD_HEADER) {
+        goto error;
-                    if (full_length > CMD_BUF_SIZE) {
+    if (buf_len < msglen)
-                        // packet too large, nack it and move on
+        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 (sync_state & CF_NEED_VALID)
+        return -1;
+    sync_state |= CF_NEED_VALID;
     command_respond_nack();
-                    } else if (remaining >= full_length) {
+    return -1;
-                        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++;
-    }
-    if (remaining) {
-        // move the buffer
-        uint8_t rpos = cmd_pos - remaining;
-        memmove(&cmd_buf[0], &cmd_buf[rpos], remaining);
-    }
-    cmd_pos = remaining;
 }
 
+// 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;
+// Find a message block and then dispatch all the commands in it
-    else if (cmd_pos + len > CMD_BUF_SIZE)
+int_fast8_t
-        len = CMD_BUF_SIZE - cmd_pos;
+command_find_and_dispatch(uint8_t *buf, uint_fast8_t buf_len
-    memcpy(&cmd_buf[cmd_pos], data, len);
+                          , uint_fast8_t *pop_count)
-    cmd_pos += len;
+{
-    if (cmd_pos > PROTO_SIZE)
+    int_fast8_t ret = command_find_block(buf, buf_len, pop_count);
-        decode_command();
+    if (ret > 0) {
+        command_dispatch(buf, *pop_count);
+        command_send_ack();
+    }
+    return ret;
 }

src/command.h

@@ -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 MESSAGE_MIN 8
-#define CMD_TRAILER     0x9903
+#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

src/generic/canbus.c

@@ -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;
-
+    uint32_t max_size = ce->max_size;
-    if (tmax + size > sizeof(transmit_buf)) {
+    if (tmax + max_size > sizeof(transmit_buf)) {
-        if (tmax + size - tpos > 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);
 

src/generic/misc.h

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