diff options
| -rw-r--r-- | src/generic/canbus.c | 227 | ||||
| -rw-r--r-- | src/generic/canbus.h | 22 | ||||
| -rw-r--r-- | src/stm32/Kconfig | 4 | ||||
| -rw-r--r-- | src/stm32/Makefile | 4 | ||||
| -rw-r--r-- | src/stm32/can.c | 245 | ||||
| -rw-r--r-- | src/stm32/can.h | 13 |
6 files changed, 323 insertions, 192 deletions
diff --git a/src/generic/canbus.c b/src/generic/canbus.c new file mode 100644 index 00000000..b3e1bada --- /dev/null +++ b/src/generic/canbus.c @@ -0,0 +1,227 @@ +// 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 "canbus.h" // canbus_set_uuid +#include "command.h" // DECL_CONSTANT +#include "sched.h" // sched_wake_task + +static uint32_t canbus_assigned_id; +static uint8_t canbus_uuid[CANBUS_UUID_LEN]; + + +/**************************************************************** + * Data transmission over CAN + ****************************************************************/ + +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); +} + +void +canbus_tx_task(void) +{ + if (!sched_check_wake(&canbus_tx_wake)) + 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 command handling + ****************************************************************/ + +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; +} + +// Helper to retry sending until successful +static void +canbus_send_blocking(uint32_t id, uint32_t len, uint8_t *data) +{ + for (;;) { + int ret = canbus_send(id, len, data); + if (ret >= 0) + return; + } +} + +static void +can_process_ping(uint32_t id, uint32_t len, uint8_t *data) +{ + canbus_send_blocking(canbus_assigned_id + 1, 0, NULL); +} + +static void +can_process_reset(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(); +} + +static void +can_process_uuid(uint32_t id, uint32_t len, uint8_t *data) +{ + if (canbus_assigned_id) + return; + canbus_send_blocking(CANBUS_ID_UUID_RESP, sizeof(canbus_uuid), canbus_uuid); +} + +static void +can_process_set_id(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_dataport(canbus_assigned_id); + } +} + +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); + } +} + + +/**************************************************************** + * CAN packet reading + ****************************************************************/ + +static struct task_wake canbus_rx_wake; + +void +canbus_notify_rx(void) +{ + sched_wake_task(&canbus_rx_wake); +} + +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; + can_process(id, ret, data); + } + + // Check for a complete message block and process it + uint_fast8_t rpos = receive_pos, pop_count; + int ret = command_find_and_dispatch(receive_buf, rpos, &pop_count); + if (ret) { + // Move buffer + int needcopy = rpos - pop_count; + if (needcopy) { + memmove(receive_buf, &receive_buf[pop_count], needcopy); + canbus_notify_rx(); + } + rpos = needcopy; + } + receive_pos = rpos; +} +DECL_TASK(canbus_rx_task); + + +/**************************************************************** + * Setup and shutdown + ****************************************************************/ + +void +canbus_set_uuid(void *uuid) +{ + memcpy(canbus_uuid, uuid, sizeof(canbus_uuid)); + + // Send initial message + can_process_uuid(0, 0, NULL); +} + +void +canbus_shutdown(void) +{ + canbus_notify_tx(); + canbus_notify_rx(); +} +DECL_SHUTDOWN(canbus_shutdown); diff --git a/src/generic/canbus.h b/src/generic/canbus.h new file mode 100644 index 00000000..d797dc53 --- /dev/null +++ b/src/generic/canbus.h @@ -0,0 +1,22 @@ +#ifndef __CANBUS_H__ +#define __CANBUS_H__ + +#include <stdint.h> // uint32_t + +#define CANBUS_ID_UUID 0x321 +#define CANBUS_ID_SET 0x322 +#define CANBUS_ID_UUID_RESP 0x323 +#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_dataport(uint32_t id); +void canbus_reboot(void); + +// canbus.c +void canbus_notify_tx(void); +void canbus_notify_rx(void); +void canbus_set_uuid(void *data); + +#endif // canbus.h diff --git a/src/stm32/Kconfig b/src/stm32/Kconfig index fd6276cd..19da0e22 100644 --- a/src/stm32/Kconfig +++ b/src/stm32/Kconfig @@ -176,9 +176,9 @@ config CANSERIAL bool "Use CAN for communication (instead of serial)" depends on !USBSERIAL default n -config SERIAL_BAUD +config CANBUS_FREQUENCY int "CAN bus speed" if LOW_LEVEL_OPTIONS && CANSERIAL - default 500000 if CANSERIAL + default 500000 choice depends on CANSERIAL prompt "CAN pins" diff --git a/src/stm32/Makefile b/src/stm32/Makefile index bd5017f2..1587d937 100644 --- a/src/stm32/Makefile +++ b/src/stm32/Makefile @@ -46,8 +46,8 @@ src-$(CONFIG_USBSERIAL) += $(usb-src-y) stm32/chipid.c generic/usb_cdc.c serial-src-y := stm32/serial.c serial-src-$(CONFIG_MACH_STM32F0) := stm32/stm32f0_serial.c src-$(CONFIG_SERIAL) += $(serial-src-y) generic/serial_irq.c -can-src-$(CONFIG_CANSERIAL) += stm32/can.c ../lib/fast-hash/fasthash.c -src-$(CONFIG_CANSERIAL) += $(can-src-y) generic/serial_irq.c +src-$(CONFIG_CANSERIAL) += stm32/can.c ../lib/fast-hash/fasthash.c +src-$(CONFIG_CANSERIAL) += generic/canbus.c dirs-$(CONFIG_CANSERIAL) += lib/fast-hash diff --git a/src/stm32/can.c b/src/stm32/can.c index 20162f14..987a8190 100644 --- a/src/stm32/can.c +++ b/src/stm32/can.c @@ -9,10 +9,10 @@ #include <string.h> // memcpy #include "autoconf.h" // CONFIG_MACH_STM32F1 #include "board/irq.h" // irq_disable -#include "can.h" // SHORT_UUID_LEN #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/serial_irq.h" // serial_rx_byte #include "internal.h" // enable_pclock #include "sched.h" // DECL_INIT @@ -87,64 +87,76 @@ #error No known CAN device for configured MCU #endif -static uint16_t MyCanId = 0; - -static int -can_find_empty_tx_mbox(void) +// Read the next CAN packet +int +canbus_read(uint32_t *id, uint8_t *data) { - uint32_t tsr = SOC_CAN->TSR; - if (tsr & (CAN_TSR_TME0|CAN_TSR_TME1|CAN_TSR_TME2)) - return (tsr & CAN_TSR_CODE) >> CAN_TSR_CODE_Pos; - return -1; + 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; } -static void -can_transmit_mbox(uint32_t id, int mbox, uint32_t dlc, uint8_t *pkt) +// Transmit a packet +int +canbus_send(uint32_t id, uint32_t len, uint8_t *data) { + uint32_t tsr = SOC_CAN->TSR; + if (!(tsr & (CAN_TSR_TME0|CAN_TSR_TME1|CAN_TSR_TME2))) { + // No space in transmit fifo - enable tx irq + irq_disable(); + SOC_CAN->IER |= CAN_IER_TMEIE; + irq_enable(); + return -1; + } + int mbox = (tsr & CAN_TSR_CODE) >> CAN_TSR_CODE_Pos; CAN_TxMailBox_TypeDef *mb = &SOC_CAN->sTxMailBox[mbox]; /* Set up the DLC */ - mb->TDTR = (mb->TDTR & 0xFFFFFFF0) | (dlc & 0x0F); + mb->TDTR = (mb->TDTR & 0xFFFFFFF0) | (len & 0x0F); /* Set up the data field */ - if (pkt) { - mb->TDLR = (((uint32_t)pkt[3] << 24) - | ((uint32_t)pkt[2] << 16) - | ((uint32_t)pkt[1] << 8) - | ((uint32_t)pkt[0] << 0)); - mb->TDHR = (((uint32_t)pkt[7] << 24) - | ((uint32_t)pkt[6] << 16) - | ((uint32_t)pkt[5] << 8) - | ((uint32_t)pkt[4] << 0)); + if (len) { + mb->TDLR = (((uint32_t)data[3] << 24) + | ((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; -} - -// Blocking transmit function -static void -can_transmit(uint32_t id, uint32_t dlc, uint8_t *pkt) -{ - int mbox = -1; - - do { - mbox = can_find_empty_tx_mbox(); - } while (mbox < 0); - - can_transmit_mbox(id, mbox, dlc, pkt); -} - -// Convert Unique 96-bit value into 48 bit representation -static void -pack_uuid(uint8_t *u) -{ - uint64_t hash = fasthash64((uint8_t*)UID_BASE, 12, 0xA16231A7); - memcpy(u, &hash, SHORT_UUID_LEN); + return len; } #define CAN_FILTER_NUMBER 0 +// Setup the receive packet filter static void can_set_filter(uint32_t id1, uint32_t id2) { @@ -172,145 +184,33 @@ can_set_filter(uint32_t id1, uint32_t id2) SOC_CAN->FMR &= ~CAN_FMR_FINIT; } -static void -can_process_data(uint32_t id, uint32_t dlc, uint8_t *data) -{ - int i; - for (i=0; i < dlc; i++) - serial_rx_byte(data[i]); -} - -static void -can_process_ping(uint32_t id, uint32_t dlc, uint8_t *data) -{ - can_transmit(MyCanId+1, 0, NULL); -} - -static void -can_process_reset(uint32_t id, uint32_t dlc, uint8_t *data) -{ - uint32_t reset_id = data[0] | (data[1] << 8); - if (reset_id == MyCanId) - NVIC_SystemReset(); -} - -static void -can_process_uuid(uint32_t id, uint32_t dlc, uint8_t *data) -{ - if (MyCanId) - return; - uint8_t short_uuid[SHORT_UUID_LEN]; - pack_uuid(short_uuid); - can_transmit(PKT_ID_UUID_RESP, SHORT_UUID_LEN, short_uuid); -} - -static void -can_process_set_id(uint32_t id, uint32_t dlc, uint8_t *data) -{ - uint8_t short_uuid[SHORT_UUID_LEN]; - pack_uuid(short_uuid); - - // compare my UUID with packet to check if this packet mine - if (memcmp(&data[2], short_uuid, SHORT_UUID_LEN) == 0) { - MyCanId = data[0] | (data[1] << 8); - can_set_filter(MyCanId, PKT_ID_UUID); - } -} - -static void -can_process(uint32_t id, uint32_t dlc, uint8_t *data) +void +canbus_set_dataport(uint32_t id) { - if (id == MyCanId) { - if (dlc) - can_process_data(id, dlc, data); - else - can_process_ping(id, dlc, data); - } else if (id == PKT_ID_UUID) { - if (dlc) - can_process_reset(id, dlc, data); - else - can_process_uuid(id, dlc, data); - } else if (id==PKT_ID_SET) { - can_process_set_id(id, dlc, data); - } + can_set_filter(CANBUS_ID_UUID, id); } -static struct task_wake canbus_wake; - void -can_dispatch_task(void) +canbus_reboot(void) { - if (!sched_check_wake(&canbus_wake)) - return; - - // Check for rx - for (;;) { - 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(); - break; - } - - // Read and ack packet - CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[0]; - uint32_t 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; - - // Process packet - uint8_t data[8]; - 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; - can_process(id, dlc, data); - } - - // Check for tx data - for (;;) { - int mbox = can_find_empty_tx_mbox(); - if (mbox < 0) { - // All tx mboxes full, enable wake on tx IRQ - irq_disable(); - SOC_CAN->IER |= CAN_IER_TMEIE; - irq_enable(); - break; - } - int i; - uint8_t databuf[8]; - for (i=0; i<8; i++) { - if (serial_get_tx_byte(&(databuf[i])) == -1) - break; - } - if (!i) - break; - can_transmit_mbox(MyCanId+1, mbox, i, databuf); - } + NVIC_SystemReset(); } -DECL_TASK(can_dispatch_task); // This function handles CAN global interrupts void CAN_IRQHandler(void) { - if (SOC_CAN->RF0R & CAN_RF0R_FMP0) { + uint32_t ier = SOC_CAN->IER; + if (ier & CAN_IER_FMPIE0 && SOC_CAN->RF0R & CAN_RF0R_FMP0) { // Rx - SOC_CAN->IER &= ~CAN_IER_FMPIE0; - sched_wake_task(&canbus_wake); + SOC_CAN->IER = ier = ier & ~CAN_IER_FMPIE0; + canbus_notify_rx(); } - uint32_t ier = SOC_CAN->IER; if (ier & CAN_IER_TMEIE && SOC_CAN->TSR & (CAN_TSR_RQCP0|CAN_TSR_RQCP1|CAN_TSR_RQCP2)) { // Tx - SOC_CAN->IER &= ~CAN_IER_TMEIE; - sched_wake_task(&canbus_wake); + SOC_CAN->IER = ier & ~CAN_IER_TMEIE; + canbus_notify_tx(); } } @@ -372,7 +272,7 @@ can_init(void) uint32_t pclock = get_pclock_frequency((uint32_t)SOC_CAN); - uint32_t btr = compute_btr(pclock, CONFIG_SERIAL_BAUD); + uint32_t btr = compute_btr(pclock, CONFIG_CANBUS_FREQUENCY); /*##-1- Configure the CAN #######################################*/ @@ -392,7 +292,7 @@ can_init(void) ; /*##-2- Configure the CAN Filter #######################################*/ - can_set_filter(PKT_ID_UUID, PKT_ID_SET); + can_set_filter(CANBUS_ID_UUID, CANBUS_ID_SET); /*##-3- Configure Interrupts #################################*/ @@ -404,13 +304,8 @@ can_init(void) if (CAN_RX0_IRQn != CAN_TX_IRQn) armcm_enable_irq(CAN_IRQHandler, CAN_TX_IRQn, 0); - /*##-4- Say Hello #################################*/ - can_process_uuid(0, 0, NULL); + // 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); - -void -serial_enable_tx_irq(void) -{ - sched_wake_task(&canbus_wake); -} diff --git a/src/stm32/can.h b/src/stm32/can.h deleted file mode 100644 index 92429a26..00000000 --- a/src/stm32/can.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef __STM32_CAN_H__ -#define __STM32_CAN_H__ - -// Read UUID (6bytes) -#define PKT_ID_UUID (0x321) -// Set address (2bytes) to UUID (6b) -#define PKT_ID_SET (0x322) -// UUID response from slave (6bytes) -#define PKT_ID_UUID_RESP (0x323) - -#define SHORT_UUID_LEN (6) - -#endif /* __STM32_CAN_H__*/ |