diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/generic/canbus.c | 133 | ||||
| -rw-r--r-- | src/generic/canbus.h | 6 | ||||
| -rw-r--r-- | src/stm32/can.c | 28 |
3 files changed, 94 insertions, 73 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 diff --git a/src/generic/canbus.h b/src/generic/canbus.h index 3a3283ca..d9c330b3 100644 --- a/src/generic/canbus.h +++ b/src/generic/canbus.h @@ -3,16 +3,14 @@ #include <stdint.h> // uint32_t -#define CANBUS_ID_UUID 0x321 -#define CANBUS_ID_SET 0x322 -#define CANBUS_ID_UUID_RESP 0x323 +#define CANBUS_ID_ADMIN 0x3f0 +#define CANBUS_ID_ADMIN_RESP 0x3f1 #define CANBUS_UUID_LEN 6 // callbacks provided by board specific code int canbus_read(uint32_t *id, uint8_t *data); int canbus_send(uint32_t id, uint32_t len, uint8_t *data); void canbus_set_filter(uint32_t id); -void canbus_reboot(void); // canbus.c void canbus_notify_tx(void); diff --git a/src/stm32/can.c b/src/stm32/can.c index c4d45f9c..867bbfd7 100644 --- a/src/stm32/can.c +++ b/src/stm32/can.c @@ -159,43 +159,35 @@ canbus_send(uint32_t id, uint32_t len, uint8_t *data) return len; } -#define CAN_FILTER_NUMBER 0 - // Setup the receive packet filter void canbus_set_filter(uint32_t id) { - uint32_t filternbrbitpos = 1 << CAN_FILTER_NUMBER; - /* Select the start slave bank */ SOC_CAN->FMR |= CAN_FMR_FINIT; /* Initialisation mode for the filter */ SOC_CAN->FA1R = 0; - uint32_t idadmin = CANBUS_ID_UUID; - SOC_CAN->sFilterRegister[CAN_FILTER_NUMBER].FR1 = idadmin << (5 + 16); - SOC_CAN->sFilterRegister[CAN_FILTER_NUMBER].FR2 = id << (5 + 16); + uint32_t mask = CAN_RI0R_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; - /* Identifier list mode for the filter */ - SOC_CAN->FM1R = filternbrbitpos; /* 32-bit scale for the filter */ - SOC_CAN->FS1R = filternbrbitpos; + SOC_CAN->FS1R = (1<<0) | (1<<1) | (1<<2); /* FIFO 0 assigned for the filter */ SOC_CAN->FFA1R = 0; /* Filter activation */ - SOC_CAN->FA1R = filternbrbitpos; + SOC_CAN->FA1R = (1<<0) | (id ? (1<<1) | (1<<2) : 0); /* Leave the initialisation mode for the filter */ SOC_CAN->FMR &= ~CAN_FMR_FINIT; } -void -canbus_reboot(void) -{ - NVIC_SystemReset(); -} - // This function handles CAN global interrupts void CAN_IRQHandler(void) @@ -292,7 +284,7 @@ can_init(void) ; /*##-2- Configure the CAN Filter #######################################*/ - canbus_set_filter(CANBUS_ID_SET); + canbus_set_filter(0); /*##-3- Configure Interrupts #################################*/ armcm_enable_irq(CAN_IRQHandler, CAN_RX0_IRQn, 0); |