src: add updated can source from Klipper

Signed-off-by:  Eric Callahan <arksine.code@gmail.com>

src/generic/canbus.c

@@ -1,291 +1,32 @@
-// Generic handling of serial over CAN support
+// Wrapper functions connecting canserial.c to low-level can hardware
 //
-// Copyright (C) 2019 Eug Krashtan <eug.krashtan@gmail.com>
+// Copyright (C) 2022  Kevin O'Connor <kevin@koconnor.net>
-// Copyright (C) 2020 Pontus Borg <glpontus@gmail.com>
-// Copyright (C) 2021  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
-#include <string.h> // memcpy
+#include "canbus.h" // canbus_send
-#include "canbus.h" // canbus_set_uuid
+#include "canserial.h" // canserial_send
-#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;
+int
-static uint8_t canbus_uuid[CANBUS_UUID_LEN];
+canserial_send(struct canbus_msg *msg)
+{
+    return canbus_send(msg);
+}
 
-
+void
-/****************************************************************
+canserial_set_filter(uint32_t id)
- * Data transmission over CAN
+{
- ****************************************************************/
+    canbus_set_filter(id);
-
+}
-static struct task_wake canbus_tx_wake;
-static uint8_t transmit_buf[96], transmit_pos, transmit_max;
 
 void
 canbus_notify_tx(void)
 {
-    sched_wake_task(&canbus_tx_wake);
+    canserial_notify_tx();
 }
 
 void
-canbus_tx_task(void)
+canbus_process_data(struct canbus_msg *msg)
 {
-    if (!sched_check_wake(&canbus_tx_wake))
+    canserial_process_data(msg);
-        return;
-    uint32_t id = canbus_assigned_id;
-    if (!id) {
-        transmit_pos = transmit_max = 0;
-        return;
 }
-    uint32_t tpos = transmit_pos, tmax = transmit_max;
-    for (;;) {
-        int avail = tmax - tpos, now = avail > 8 ? 8 : avail;
-        if (avail <= 0)
-            break;
-        int ret = canbus_send(id + 1, now, &transmit_buf[tpos]);
-        if (ret <= 0)
-            break;
-        tpos += now;
-    }
-    transmit_pos = tpos;
-}
-DECL_TASK(canbus_tx_task);
-
-// Encode and transmit a "response" message
-void
-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 + max_size > sizeof(transmit_buf)) {
-        if (tmax + max_size - tpos > sizeof(transmit_buf))
-            // Not enough space for message
-            return;
-        // Move buffer
-        tmax -= tpos;
-        memmove(&transmit_buf[0], &transmit_buf[tpos], tmax);
-        transmit_pos = tpos = 0;
-        transmit_max = tmax;
-    }
-
-    // Generate message
-    uint32_t msglen = command_encode_and_frame(&transmit_buf[tmax], ce, args);
-
-    // Start message transmit
-    transmit_max = tmax + msglen;
-    canbus_notify_tx();
-}
-
-
-/****************************************************************
- * CAN "admin" command handling
- ****************************************************************/
-
-// Available commands and responses
-#define CANBUS_CMD_QUERY_UNASSIGNED 0x00
-#define CANBUS_CMD_SET_CANBOOT_NODEID 0x11
-#define CANBUS_CMD_CLEAR_CANBOOT_NODEID 0x12
-#define CANBUS_RESP_NEED_NODEID 0x20
-
-// 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)
-{
-    return len >= 7 && memcmp(&data[1], canbus_uuid, sizeof(canbus_uuid)) == 0;
-}
-
-static uint32_t
-can_decode_nodeid(int nodeid)
-{
-    return (nodeid << 1) + 0x100;
-}
-
-static void
-can_process_query_unassigned(uint32_t id, uint32_t len, uint8_t *data)
-{
-    if (canbus_assigned_id)
-        return;
-    uint8_t send[8];
-    send[0] = CANBUS_RESP_NEED_NODEID;
-    memcpy(&send[1], canbus_uuid, sizeof(canbus_uuid));
-    send[7] = CANBUS_CMD_SET_CANBOOT_NODEID;
-    // Send with retry
-    for (;;) {
-        int ret = canbus_send(CANBUS_ID_ADMIN_RESP, 8, send);
-        if (ret >= 0)
-            return;
-    }
-}
-
-static void
-can_process_clear_canboot_nodeid(void)
-{
-    canbus_assigned_id = 0;
-    canbus_set_filter(canbus_assigned_id);
-}
-
-static void
-can_id_conflict(void)
-{
-    canbus_assigned_id = 0;
-    canbus_set_filter(canbus_assigned_id);
-    // TODO: We should likely do something here, such as report back
-}
-
-static void
-can_process_set_canboot_nodeid(uint32_t id, uint32_t len, uint8_t *data)
-{
-    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 (!len)
-        return;
-    switch (data[0]) {
-    case CANBUS_CMD_QUERY_UNASSIGNED:
-        can_process_query_unassigned(id, len, data);
-        break;
-    case CANBUS_CMD_SET_CANBOOT_NODEID:
-        can_process_set_canboot_nodeid(id, len, data);
-        break;
-    case CANBUS_CMD_CLEAR_CANBOOT_NODEID:
-        can_process_clear_canboot_nodeid();
-        break;
-    }
-}
-
-
-/****************************************************************
- * CAN packet reading
- ****************************************************************/
-
-static struct task_wake canbus_rx_wake;
-
-void
-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;
-    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)
-{
-    if (!sched_check_wake(&canbus_rx_wake))
-        return;
-
-    // Read any pending CAN packets
-    for (;;) {
-        uint8_t data[8];
-        uint32_t id;
-        int ret = canbus_read(&id, data);
-        if (ret < 0)
-            break;
-        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
-    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);
-
-
-/****************************************************************
- * Setup and shutdown
- ****************************************************************/
-
-void
-command_get_canbus_id(uint32_t *data)
-{
-    uint32_t out[5] = {};
-    memcpy(&out[2], canbus_uuid, 6);
-    command_respond_ack(CMD_GET_CANBUS_ID, out, ARRAY_SIZE(out));
-}
-
-void
-canbus_set_uuid(void *uuid)
-{
-    memcpy(canbus_uuid, uuid, sizeof(canbus_uuid));
-    canbus_notify_rx();
-}
-
-void
-canbus_shutdown(void)
-{
-    canbus_notify_tx();
-    canbus_notify_rx();
-}
-DECL_SHUTDOWN(canbus_shutdown);

src/generic/canbus.h

@@ -3,19 +3,26 @@
 
 #include <stdint.h> // uint32_t
 
-#define CANBUS_ID_ADMIN 0x3f0
+struct canbus_msg {
-#define CANBUS_ID_ADMIN_RESP 0x3f1
+    uint32_t id;
-#define CANBUS_UUID_LEN 6
+    uint32_t dlc;
+    union {
+        uint8_t data[8];
+        uint32_t data32[2];
+    };
+};
+
+#define CANMSG_ID_RTR (1<<30)
+#define CANMSG_ID_EFF (1<<31)
+
+#define CANMSG_DATA_LEN(msg) ((msg)->dlc > 8 ? 8 : (msg)->dlc)
 
 // callbacks provided by board specific code
-int canbus_read(uint32_t *id, uint8_t *data);
+int canbus_send(struct canbus_msg *msg);
-int canbus_send(uint32_t id, uint32_t len, uint8_t *data);
 void canbus_set_filter(uint32_t id);
 
 // canbus.c
 void canbus_notify_tx(void);
-void canbus_notify_rx(void);
+void canbus_process_data(struct canbus_msg *msg);
-void canbus_process_data(uint32_t id, uint32_t len, uint8_t *data);
-void canbus_set_uuid(void *data);
 
 #endif // canbus.h

src/generic/canserial.c

@@ -0,0 +1,342 @@
+// Generic handling of serial over CAN support
+//
+// Copyright (C) 2019 Eug Krashtan <eug.krashtan@gmail.com>
+// Copyright (C) 2020 Pontus Borg <glpontus@gmail.com>
+// Copyright (C) 2021  Kevin O'Connor <kevin@koconnor.net>
+//
+// This file may be distributed under the terms of the GNU GPLv3 license.
+
+#include <string.h> // memcpy
+#include "board/io.h" // readb
+#include "board/irq.h" // irq_save
+#include "board/misc.h" // console_sendf
+#include "canbus.h" // canbus_set_uuid
+#include "canserial.h" // canserial_notify_tx
+#include "command.h" // DECL_CONSTANT
+#include "fasthash.h" // fasthash64
+#include "sched.h" // sched_wake_task
+
+#define CANBUS_UUID_LEN 6
+
+// Global storage
+static struct canbus_data {
+    uint32_t assigned_id;
+    uint8_t uuid[CANBUS_UUID_LEN];
+
+    // Tx data
+    struct task_wake tx_wake;
+    uint8_t transmit_pos, transmit_max;
+
+    // Rx data
+    struct task_wake rx_wake;
+    uint8_t receive_pos;
+    uint32_t admin_pull_pos, admin_push_pos;
+
+    // Transfer buffers
+    struct canbus_msg admin_queue[8];
+    uint8_t transmit_buf[96];
+    uint8_t receive_buf[192];
+} CanData;
+
+
+/****************************************************************
+ * Data transmission over CAN
+ ****************************************************************/
+
+void
+canserial_notify_tx(void)
+{
+    sched_wake_task(&CanData.tx_wake);
+}
+
+void
+canserial_tx_task(void)
+{
+    if (!sched_check_wake(&CanData.tx_wake))
+        return;
+    uint32_t id = CanData.assigned_id;
+    if (!id) {
+        CanData.transmit_pos = CanData.transmit_max = 0;
+        return;
+    }
+    struct canbus_msg msg;
+    msg.id = id + 1;
+    uint32_t tpos = CanData.transmit_pos, tmax = CanData.transmit_max;
+    for (;;) {
+        int avail = tmax - tpos, now = avail > 8 ? 8 : avail;
+        if (avail <= 0)
+            break;
+        msg.dlc = now;
+        memcpy(msg.data, &CanData.transmit_buf[tpos], now);
+        int ret = canserial_send(&msg);
+        if (ret <= 0)
+            break;
+        tpos += now;
+    }
+    CanData.transmit_pos = tpos;
+}
+DECL_TASK(canserial_tx_task);
+
+// Encode and transmit a "response" message
+void
+console_sendf(const struct command_encoder *ce, va_list args)
+{
+    // Verify space for message
+    uint32_t tpos = CanData.transmit_pos, tmax = CanData.transmit_max;
+    if (tpos >= tmax)
+        CanData.transmit_pos = CanData.transmit_max = tpos = tmax = 0;
+    uint32_t max_size = ce->max_size;
+    if (tmax + max_size > sizeof(CanData.transmit_buf)) {
+        if (tmax + max_size - tpos > sizeof(CanData.transmit_buf))
+            // Not enough space for message
+            return;
+        // Move buffer
+        tmax -= tpos;
+        memmove(&CanData.transmit_buf[0], &CanData.transmit_buf[tpos], tmax);
+        CanData.transmit_pos = tpos = 0;
+        CanData.transmit_max = tmax;
+    }
+
+    // Generate message
+    uint32_t msglen = command_encode_and_frame(&CanData.transmit_buf[tmax]
+                                               , ce, args);
+
+    // Start message transmit
+    CanData.transmit_max = tmax + msglen;
+    canserial_notify_tx();
+}
+
+
+/****************************************************************
+ * CAN "admin" command handling
+ ****************************************************************/
+
+// Available commands and responses
+#define CANBUS_CMD_QUERY_UNASSIGNED 0x00
+#define CANBUS_CMD_SET_CANBOOT_NODEID 0x11
+#define CANBUS_CMD_CLEAR_CANBOOT_NODEID 0x12
+#define CANBUS_RESP_NEED_NODEID 0x20
+
+// Helper to verify a UUID in a command matches this chip's UUID
+static int
+can_check_uuid(struct canbus_msg *msg)
+{
+    return (msg->dlc >= 7
+            && memcmp(&msg->data[1], CanData.uuid, sizeof(CanData.uuid)) == 0);
+}
+
+// Helpers to encode/decode a CAN identifier to a 1-byte "nodeid"
+/*static int
+can_get_nodeid(void)
+{
+    if (!CanData.assigned_id)
+        return 0;
+    return (CanData.assigned_id - 0x100) >> 1;
+}*/
+
+static uint32_t
+can_decode_nodeid(int nodeid)
+{
+    return (nodeid << 1) + 0x100;
+}
+
+static void
+can_process_query_unassigned(struct canbus_msg *msg)
+{
+    if (CanData.assigned_id)
+        return;
+    struct canbus_msg send;
+    send.id = CANBUS_ID_ADMIN_RESP;
+    send.dlc = 8;
+    send.data[0] = CANBUS_RESP_NEED_NODEID;
+    memcpy(&send.data[1], CanData.uuid, sizeof(CanData.uuid));
+    send.data[7] = CANBUS_CMD_SET_CANBOOT_NODEID;
+    // Send with retry
+    for (;;) {
+        int ret = canserial_send(&send);
+        if (ret >= 0)
+            return;
+    }
+}
+
+static void
+can_process_clear_canboot_nodeid(void)
+{
+    CanData.assigned_id = 0;
+    canbus_set_filter(CanData.assigned_id);
+}
+
+static void
+can_id_conflict(void)
+{
+    CanData.assigned_id = 0;
+    canserial_set_filter(CanData.assigned_id);
+}
+
+static void
+can_process_set_canboot_nodeid(struct canbus_msg *msg)
+{
+    if (msg->dlc < 8)
+        return;
+    uint32_t newid = can_decode_nodeid(msg->data[7]);
+    if (can_check_uuid(msg)) {
+        if (newid != CanData.assigned_id) {
+            CanData.assigned_id = newid;
+            canserial_set_filter(CanData.assigned_id);
+        }
+    } else if (newid == CanData.assigned_id) {
+        can_id_conflict();
+    }
+}
+
+// Handle an "admin" command
+static void
+can_process_admin(struct canbus_msg *msg)
+{
+    if (!msg->dlc)
+        return;
+    switch (msg->data[0]) {
+    case CANBUS_CMD_QUERY_UNASSIGNED:
+        can_process_query_unassigned(msg);
+        break;
+    case CANBUS_CMD_SET_CANBOOT_NODEID:
+        can_process_set_canboot_nodeid(msg);
+        break;
+    case CANBUS_CMD_CLEAR_CANBOOT_NODEID:
+        can_process_clear_canboot_nodeid();
+        break;
+    }
+}
+
+
+/****************************************************************
+ * CAN packet reading
+ ****************************************************************/
+
+static void
+canserial_notify_rx(void)
+{
+    sched_wake_task(&CanData.rx_wake);
+}
+
+DECL_CONSTANT("RECEIVE_WINDOW", ARRAY_SIZE(CanData.receive_buf));
+
+// Handle incoming data (called from IRQ handler)
+int
+canserial_process_data(struct canbus_msg *msg)
+{
+    uint32_t id = msg->id;
+    if (CanData.assigned_id && id == CanData.assigned_id) {
+        // Add to incoming data buffer
+        int rpos = CanData.receive_pos;
+        uint32_t len = CANMSG_DATA_LEN(msg);
+        if (len > sizeof(CanData.receive_buf) - rpos)
+            return -1;
+        memcpy(&CanData.receive_buf[rpos], msg->data, len);
+        CanData.receive_pos = rpos + len;
+        canserial_notify_rx();
+    } else if (id == CANBUS_ID_ADMIN
+               || (CanData.assigned_id && id == CanData.assigned_id + 1)) {
+        // Add to admin command queue
+        uint32_t pushp = CanData.admin_push_pos;
+        if (pushp >= CanData.admin_pull_pos + ARRAY_SIZE(CanData.admin_queue))
+            // No space - drop message
+            return -1;
+        uint32_t pos = pushp % ARRAY_SIZE(CanData.admin_queue);
+        memcpy(&CanData.admin_queue[pos], msg, sizeof(*msg));
+        CanData.admin_push_pos = pushp + 1;
+        canserial_notify_rx();
+    }
+    return 0;
+}
+
+// Remove from the receive buffer the given number of bytes
+static void
+console_pop_input(int len)
+{
+    int copied = 0;
+    for (;;) {
+        int rpos = readb(&CanData.receive_pos);
+        int needcopy = rpos - len;
+        if (needcopy) {
+            memmove(&CanData.receive_buf[copied]
+                    , &CanData.receive_buf[copied + len], needcopy - copied);
+            copied = needcopy;
+            canserial_notify_rx();
+        }
+        irqstatus_t flag = irq_save();
+        if (rpos != readb(&CanData.receive_pos)) {
+            // Raced with irq handler - retry
+            irq_restore(flag);
+            continue;
+        }
+        CanData.receive_pos = needcopy;
+        irq_restore(flag);
+        break;
+    }
+}
+
+// Task to process incoming commands and admin messages
+void
+canserial_rx_task(void)
+{
+    if (!sched_check_wake(&CanData.rx_wake))
+        return;
+
+    // Process pending admin messages
+    for (;;) {
+        uint32_t pushp = readl(&CanData.admin_push_pos);
+        uint32_t pullp = CanData.admin_pull_pos;
+        if (pushp == pullp)
+            break;
+        uint32_t pos = pullp % ARRAY_SIZE(CanData.admin_queue);
+        struct canbus_msg *msg = &CanData.admin_queue[pos];
+        uint32_t id = msg->id;
+        if (CanData.assigned_id && id == CanData.assigned_id + 1)
+            can_id_conflict();
+        else if (id == CANBUS_ID_ADMIN)
+            can_process_admin(msg);
+        CanData.admin_pull_pos = pullp + 1;
+    }
+
+    // Check for a complete message block and process it
+    uint_fast8_t rpos = readb(&CanData.receive_pos), pop_count;
+    int ret = command_find_block(CanData.receive_buf, rpos, &pop_count);
+    if (ret > 0)
+        command_dispatch(CanData.receive_buf, pop_count);
+    if (ret) {
+        console_pop_input(pop_count);
+        if (ret > 0)
+            command_send_ack();
+    }
+}
+DECL_TASK(canserial_rx_task);
+
+
+/****************************************************************
+ * Setup and shutdown
+ ****************************************************************/
+
+void
+command_get_canbus_id(uint32_t *args)
+{
+    uint32_t out[5] = {};
+    memcpy(&out[2], CanData.uuid, 6);
+    command_respond_ack(CMD_GET_CANBUS_ID, out, ARRAY_SIZE(out));
+}
+
+void
+canserial_set_uuid(uint8_t *raw_uuid, uint32_t raw_uuid_len)
+{
+    uint64_t hash = fasthash64(raw_uuid, raw_uuid_len, 0xA16231A7);
+    memcpy(CanData.uuid, &hash, sizeof(CanData.uuid));
+    canserial_notify_rx();
+}
+
+void
+canserial_shutdown(void)
+{
+    canserial_notify_tx();
+    canserial_notify_rx();
+}
+DECL_SHUTDOWN(canserial_shutdown);

src/generic/canserial.h

@@ -0,0 +1,19 @@
+#ifndef __CANSERIAL_H__
+#define __CANSERIAL_H__
+
+#include <stdint.h> // uint32_t
+
+#define CANBUS_ID_ADMIN 0x3f0
+#define CANBUS_ID_ADMIN_RESP 0x3f1
+
+// callbacks provided by board specific code
+struct canbus_msg;
+int canserial_send(struct canbus_msg *msg);
+void canserial_set_filter(uint32_t id);
+
+// canserial.c
+void canserial_notify_tx(void);
+int canserial_process_data(struct canbus_msg *msg);
+void canserial_set_uuid(uint8_t *raw_uuid, uint32_t raw_uuid_len);
+
+#endif // canserial.h

src/stm32/can.c

@@ -10,9 +10,9 @@
 #include "autoconf.h" // CONFIG_MACH_STM32F1
 #include "board/irq.h" // irq_disable
 #include "command.h" // DECL_CONSTANT_STR
-#include "fasthash.h" // fasthash64
 #include "generic/armcm_boot.h" // armcm_enable_irq
 #include "generic/canbus.h" // canbus_notify_tx
+#include "generic/canserial.h" // CANBUS_ID_ADMIN
 #include "internal.h" // enable_pclock
 #include "sched.h" // DECL_INIT
 
@@ -91,41 +91,9 @@
  #error No known CAN device for configured MCU
 #endif
 
-// Read the next CAN packet
-int
-canbus_read(uint32_t *id, uint8_t *data)
-{
-    if (!(SOC_CAN->RF0R & CAN_RF0R_FMP0)) {
-        // All rx mboxes empty, enable wake on rx IRQ
-        irq_disable();
-        SOC_CAN->IER |= CAN_IER_FMPIE0;
-        irq_enable();
-        return -1;
-    }
-
-    // Read and ack packet
-    CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[0];
-    uint32_t rir_id = (mb->RIR >> CAN_RI0R_STID_Pos) & 0x7FF;
-    uint32_t dlc = mb->RDTR & CAN_RDT0R_DLC;
-    uint32_t rdlr = mb->RDLR, rdhr = mb->RDHR;
-    SOC_CAN->RF0R = CAN_RF0R_RFOM0;
-
-    // Return packet
-    *id = rir_id;
-    data[0] = (rdlr >>  0) & 0xff;
-    data[1] = (rdlr >>  8) & 0xff;
-    data[2] = (rdlr >> 16) & 0xff;
-    data[3] = (rdlr >> 24) & 0xff;
-    data[4] = (rdhr >>  0) & 0xff;
-    data[5] = (rdhr >>  8) & 0xff;
-    data[6] = (rdhr >> 16) & 0xff;
-    data[7] = (rdhr >> 24) & 0xff;
-    return dlc;
-}
-
 // Transmit a packet
 int
-canbus_send(uint32_t id, uint32_t len, uint8_t *data)
+canbus_send(struct canbus_msg *msg)
 {
     uint32_t tsr = SOC_CAN->TSR;
     if (!(tsr & (CAN_TSR_TME0|CAN_TSR_TME1|CAN_TSR_TME2))) {
@@ -143,23 +111,21 @@ canbus_send(uint32_t id, uint32_t len, uint8_t *data)
     CAN_TxMailBox_TypeDef *mb = &SOC_CAN->sTxMailBox[mbox];
 
     /* Set up the DLC */
-    mb->TDTR = (mb->TDTR & 0xFFFFFFF0) | (len & 0x0F);
+    mb->TDTR = (mb->TDTR & 0xFFFFFFF0) | (msg->dlc & 0x0F);
 
     /* Set up the data field */
-    if (len) {
+    mb->TDLR = msg->data32[0];
-        mb->TDLR = (((uint32_t)data[3] << 24)
+    mb->TDHR = msg->data32[1];
-                    | ((uint32_t)data[2] << 16)
-                    | ((uint32_t)data[1] << 8)
-                    | ((uint32_t)data[0] << 0));
-        mb->TDHR = (((uint32_t)data[7] << 24)
-                    | ((uint32_t)data[6] << 16)
-                    | ((uint32_t)data[5] << 8)
-                    | ((uint32_t)data[4] << 0));
-    }
 
     /* Request transmission */
-    mb->TIR = (id << CAN_TI0R_STID_Pos) | CAN_TI0R_TXRQ;
+    uint32_t tir;
-    return len;
+    if (msg->id & CANMSG_ID_EFF)
+        tir = ((msg->id & 0x1fffffff) << CAN_TI0R_EXID_Pos) | CAN_TI0R_IDE;
+    else
+        tir = (msg->id & 0x7ff) << CAN_TI0R_STID_Pos;
+    tir |= msg->id & CANMSG_ID_RTR ? CAN_TI0R_RTR : 0;
+    mb->TIR = (msg->id << CAN_TI0R_STID_Pos) | CAN_TI0R_TXRQ;
+    return CANMSG_DATA_LEN(msg);
 }
 
 // Setup the receive packet filter
@@ -171,20 +137,23 @@ canbus_set_filter(uint32_t id)
     /* Initialisation mode for the filter */
     SOC_CAN->FA1R = 0;
 
-    uint32_t mask = CAN_RI0R_STID | CAN_TI0R_IDE | CAN_TI0R_RTR;
+    if (CONFIG_CANBUS_FILTER) {
+        uint32_t mask = CAN_TI0R_STID | CAN_TI0R_IDE | CAN_TI0R_RTR;
         SOC_CAN->sFilterRegister[0].FR1 = CANBUS_ID_ADMIN << CAN_RI0R_STID_Pos;
         SOC_CAN->sFilterRegister[0].FR2 = mask;
         SOC_CAN->sFilterRegister[1].FR1 = (id + 1) << CAN_RI0R_STID_Pos;
         SOC_CAN->sFilterRegister[1].FR2 = mask;
         SOC_CAN->sFilterRegister[2].FR1 = id << CAN_RI0R_STID_Pos;
         SOC_CAN->sFilterRegister[2].FR2 = mask;
+    } else {
+        SOC_CAN->sFilterRegister[0].FR1 = 0;
+        SOC_CAN->sFilterRegister[0].FR2 = 0;
+        id = 0;
+    }
 
     /* 32-bit scale for the filter */
     SOC_CAN->FS1R = (1<<0) | (1<<1) | (1<<2);
 
-    /* FIFO 1 assigned to 'id' */
-    SOC_CAN->FFA1R = (1<<2);
-
     /* Filter activation */
     SOC_CAN->FA1R = (1<<0) | (id ? (1<<1) | (1<<2) : 0);
     /* Leave the initialisation mode for the filter */
@@ -195,27 +164,25 @@ canbus_set_filter(uint32_t id)
 void
 CAN_IRQHandler(void)
 {
-    if (SOC_CAN->RF1R & CAN_RF1R_FMP1) {
+    if (SOC_CAN->RF0R & CAN_RF0R_FMP0) {
         // Read and ack data packet
-        CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[1];
+        CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[0];
-        uint32_t rir_id = (mb->RIR >> CAN_RI0R_STID_Pos) & 0x7FF;
+        uint32_t rir = mb->RIR;
-        uint32_t dlc = mb->RDTR & CAN_RDT0R_DLC;
+        struct canbus_msg msg;
-        uint32_t rdlr = mb->RDLR, rdhr = mb->RDHR;
+        if (rir & CAN_RI0R_IDE)
-        SOC_CAN->RF1R = CAN_RF1R_RFOM1;
+            msg.id = ((rir >> CAN_RI0R_EXID_Pos) & 0x1fffffff) | CANMSG_ID_EFF;
+        else
+            msg.id = (rir >> CAN_RI0R_STID_Pos) & 0x7ff;
+        msg.id |= rir & CAN_RI0R_RTR ? CANMSG_ID_RTR : 0;
+        msg.dlc = mb->RDTR & CAN_RDT0R_DLC;
+        msg.data32[0] = mb->RDLR;
+        msg.data32[1] = mb->RDHR;
+        SOC_CAN->RF0R = CAN_RF0R_RFOM0;
 
         // Process packet
-        union {
+        canbus_process_data(&msg);
-            struct { uint32_t rdlr, rdhr; };
-            uint8_t data[8];
-        } rdata = { .rdlr = rdlr, .rdhr = rdhr };
-        canbus_process_data(rir_id, dlc, rdata.data);
     }
     uint32_t ier = SOC_CAN->IER;
-    if (ier & CAN_IER_FMPIE0 && SOC_CAN->RF0R & CAN_RF0R_FMP0) {
-        // Admin Rx
-        SOC_CAN->IER = ier = ier & ~CAN_IER_FMPIE0;
-        canbus_notify_rx();
-    }
     if (ier & CAN_IER_TMEIE
         && SOC_CAN->TSR & (CAN_TSR_RQCP0|CAN_TSR_RQCP1|CAN_TSR_RQCP2)) {
         // Tx
@@ -310,10 +277,6 @@ can_init(void)
         armcm_enable_irq(CAN_IRQHandler, CAN_RX1_IRQn, 0);
     if (CAN_RX0_IRQn != CAN_TX_IRQn)
         armcm_enable_irq(CAN_IRQHandler, CAN_TX_IRQn, 0);
-    SOC_CAN->IER = CAN_IER_FMPIE1;
+    SOC_CAN->IER = CAN_IER_FMPIE0;
-
-    // Convert unique 96-bit chip id into 48 bit representation
-    uint64_t hash = fasthash64((uint8_t*)UID_BASE, 12, 0xA16231A7);
-    canbus_set_uuid(&hash);
 }
 DECL_INIT(can_init);

src/stm32/chipid.c

@@ -4,6 +4,7 @@
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
+#include "generic/canserial.h" // canserial_set_uuid
 #include "generic/usb_cdc.h" // usb_fill_serial
 #include "generic/usbstd.h" // usb_string_descriptor
 #include "internal.h" // UID_BASE
@@ -25,9 +26,10 @@ usbserial_get_serialid(void)
 void
 chipid_init(void)
 {
-    if (!CONFIG_USB_SERIAL_NUMBER_CHIPID)
+    if (CONFIG_USB_SERIAL_NUMBER_CHIPID)
-        return;
         usb_fill_serial(&cdc_chipid.desc, ARRAY_SIZE(cdc_chipid.data)
                         , (void*)UID_BASE);
+    if (CONFIG_CANBUS)
+        canserial_set_uuid((void*)UID_BASE, CHIP_UID_LEN);
 }
 DECL_INIT(chipid_init);