canbus: Rework CAN command protocol

Rework the micro-controller command protocol so that it supports
direct communication with the serialqueue.c code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 82 insertions, 51 deletions

diff --git a/src/generic/canbus.c b/src/generic/canbus.c
index 80d0e462..9b8e0e54 100644
--- a/src/generic/canbus.c
+++ b/src/generic/canbus.c
@@ -82,80 +82,88 @@ console_sendf(const struct command_encoder *ce, va_list args)
 
 
 /****************************************************************
- * CAN command handling
+ * CAN "admin" command handling
  ****************************************************************/
 
-static uint8_t receive_buf[192], receive_pos;
-DECL_CONSTANT("RECEIVE_WINDOW", ARRAY_SIZE(receive_buf));
+// Available commands and responses
+#define CANBUS_CMD_QUERY_UNASSIGNED 0x00
+#define CANBUS_CMD_SET_NODEID 0x01
+#define CANBUS_RESP_NEED_NODEID 0x20
 
-static void
-can_process_data(uint32_t id, uint32_t len, uint8_t *data)
+// Helper to verify a UUID in a command matches this chip's UUID
+static int
+can_check_uuid(uint32_t id, uint32_t len, uint8_t *data)
 {
-    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;
+    return len >= 7 && memcmp(&data[1], canbus_uuid, sizeof(canbus_uuid)) == 0;
 }
 
-// Helper to retry sending until successful
-static void
-canbus_send_blocking(uint32_t id, uint32_t len, uint8_t *data)
+// Helpers to encode/decode a CAN identifier to a 1-byte "nodeid"
+static int
+can_get_nodeid(void)
 {
-    for (;;) {
-        int ret = canbus_send(id, len, data);
-        if (ret >= 0)
-            return;
-    }
+    if (!canbus_assigned_id)
+        return 0;
+    return (canbus_assigned_id - 0x100) >> 1;
 }
-
-static void
-can_process_ping(uint32_t id, uint32_t len, uint8_t *data)
+static uint32_t
+can_decode_nodeid(int nodeid)
 {
-    canbus_send_blocking(canbus_assigned_id + 1, 0, NULL);
+    return (nodeid << 1) + 0x100;
 }
 
 static void
-can_process_reset(uint32_t id, uint32_t len, uint8_t *data)
+can_process_query_unassigned(uint32_t id, uint32_t len, uint8_t *data)
 {
-    uint32_t reset_id = data[0] | (data[1] << 8);
-    if (reset_id == canbus_assigned_id)
-        canbus_reboot();
+    if (canbus_assigned_id)
+        return;
+    uint8_t send[8];
+    send[0] = CANBUS_RESP_NEED_NODEID;
+    memcpy(&send[1], canbus_uuid, sizeof(canbus_uuid));
+    // Send with retry
+    for (;;) {
+        int ret = canbus_send(CANBUS_ID_ADMIN_RESP, 7, send);
+        if (ret >= 0)
+            return;
+    }
 }
 
 static void
-can_process_uuid(uint32_t id, uint32_t len, uint8_t *data)
+can_id_conflict(void)
 {
-    if (canbus_assigned_id)
-        return;
-    canbus_send_blocking(CANBUS_ID_UUID_RESP, sizeof(canbus_uuid), canbus_uuid);
+    canbus_assigned_id = 0;
+    canbus_set_filter(canbus_assigned_id);
+    shutdown("Another CAN node assigned this ID");
 }
 
 static void
-can_process_set_id(uint32_t id, uint32_t len, uint8_t *data)
+can_process_set_nodeid(uint32_t id, uint32_t len, uint8_t *data)
 {
-    // compare my UUID with packet to check if this packet mine
-    if (memcmp(&data[2], canbus_uuid, sizeof(canbus_uuid)) == 0) {
-        canbus_assigned_id = data[0] | (data[1] << 8);
-        canbus_set_filter(canbus_assigned_id);
+    if (len < 8)
+        return;
+    uint32_t newid = can_decode_nodeid(data[7]);
+    if (can_check_uuid(id, len, data)) {
+        if (newid != canbus_assigned_id) {
+            canbus_assigned_id = newid;
+            canbus_set_filter(canbus_assigned_id);
+        }
+    } else if (newid == canbus_assigned_id) {
+        can_id_conflict();
     }
 }
 
+// Handle an "admin" command
 static void
 can_process(uint32_t id, uint32_t len, uint8_t *data)
 {
-    if (id == canbus_assigned_id) {
-        if (len)
-            can_process_data(id, len, data);
-        else
-            can_process_ping(id, len, data);
-    } else if (id == CANBUS_ID_UUID) {
-        if (len)
-            can_process_reset(id, len, data);
-        else
-            can_process_uuid(id, len, data);
-    } else if (id==CANBUS_ID_SET) {
-        can_process_set_id(id, len, data);
+    if (!len)
+        return;
+    switch (data[0]) {
+    case CANBUS_CMD_QUERY_UNASSIGNED:
+        can_process_query_unassigned(id, len, data);
+        break;
+    case CANBUS_CMD_SET_NODEID:
+        can_process_set_nodeid(id, len, data);
+        break;
     }
 }
 
@@ -172,6 +180,19 @@ 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));
+
+static void
+can_process_data(uint32_t id, uint32_t len, uint8_t *data)
+{
+    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;
+}
+
 void
 canbus_rx_task(void)
 {
@@ -185,7 +206,12 @@ canbus_rx_task(void)
         int ret = canbus_read(&id, data);
         if (ret < 0)
             break;
-        can_process(id, ret, data);
+        if (id && id == canbus_assigned_id)
+            can_process_data(id, ret, data);
+        if (id && id == canbus_assigned_id + 1)
+            can_id_conflict();
+        else if (id == CANBUS_ID_ADMIN)
+            can_process(id, ret, data);
     }
 
     // Check for a complete message block and process it
@@ -210,13 +236,18 @@ DECL_TASK(canbus_rx_task);
  ****************************************************************/
 
 void
+command_get_canbus_id(uint32_t *args)
+{
+    sendf("canbus_id canbus_uuid=%.*s canbus_nodeid=%u"
+          , sizeof(canbus_uuid), canbus_uuid, can_get_nodeid());
+}
+DECL_COMMAND_FLAGS(command_get_canbus_id, HF_IN_SHUTDOWN, "get_canbus_id");
+
+void
 canbus_set_uuid(void *uuid)
 {
     memcpy(canbus_uuid, uuid, sizeof(canbus_uuid));
     canbus_notify_rx();
-
-    // Send initial message
-    can_process_uuid(0, 0, NULL);
 }
 
 void