diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/generic/canbus.c | 102 | ||||
| -rw-r--r-- | src/generic/canbus.h | 17 | ||||
| -rw-r--r-- | src/stm32/can.c | 82 | ||||
| -rwxr-xr-x | src/stm32/fdcan.c | 122 |
4 files changed, 125 insertions, 198 deletions
diff --git a/src/generic/canbus.c b/src/generic/canbus.c index de7952cb..8526c5c1 100644 --- a/src/generic/canbus.c +++ b/src/generic/canbus.c @@ -27,8 +27,10 @@ static struct canbus_data { // 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; @@ -54,12 +56,16 @@ canbus_tx_task(void) 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; - int ret = canbus_send(id + 1, now, &CanData.transmit_buf[tpos]); + msg.dlc = now; + memcpy(msg.data, &CanData.transmit_buf[tpos], now); + int ret = canbus_send(&msg); if (ret <= 0) break; tpos += now; @@ -110,9 +116,10 @@ console_sendf(const struct command_encoder *ce, va_list args) // 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) +can_check_uuid(struct canbus_msg *msg) { - return len >= 7 && memcmp(&data[1], CanData.uuid, sizeof(CanData.uuid))==0; + 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" @@ -130,17 +137,19 @@ can_decode_nodeid(int nodeid) } static void -can_process_query_unassigned(uint32_t id, uint32_t len, uint8_t *data) +can_process_query_unassigned(struct canbus_msg *msg) { if (CanData.assigned_id) return; - uint8_t send[8]; - send[0] = CANBUS_RESP_NEED_NODEID; - memcpy(&send[1], CanData.uuid, sizeof(CanData.uuid)); - send[7] = CANBUS_CMD_SET_KLIPPER_NODEID; + 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_KLIPPER_NODEID; // Send with retry for (;;) { - int ret = canbus_send(CANBUS_ID_ADMIN_RESP, 8, send); + int ret = canbus_send(&send); if (ret >= 0) return; } @@ -155,12 +164,12 @@ can_id_conflict(void) } static void -can_process_set_klipper_nodeid(uint32_t id, uint32_t len, uint8_t *data) +can_process_set_klipper_nodeid(struct canbus_msg *msg) { - if (len < 8) + if (msg->dlc < 8) return; - uint32_t newid = can_decode_nodeid(data[7]); - if (can_check_uuid(id, len, data)) { + uint32_t newid = can_decode_nodeid(msg->data[7]); + if (can_check_uuid(msg)) { if (newid != CanData.assigned_id) { CanData.assigned_id = newid; canbus_set_filter(CanData.assigned_id); @@ -171,28 +180,28 @@ can_process_set_klipper_nodeid(uint32_t id, uint32_t len, uint8_t *data) } static void -can_process_request_bootloader(uint32_t id, uint32_t len, uint8_t *data) +can_process_request_bootloader(struct canbus_msg *msg) { - if (!can_check_uuid(id, len, data)) + if (!can_check_uuid(msg)) return; try_request_canboot(); } // Handle an "admin" command static void -can_process(uint32_t id, uint32_t len, uint8_t *data) +can_process_admin(struct canbus_msg *msg) { - if (!len) + if (!msg->dlc) return; - switch (data[0]) { + switch (msg->data[0]) { case CANBUS_CMD_QUERY_UNASSIGNED: - can_process_query_unassigned(id, len, data); + can_process_query_unassigned(msg); break; case CANBUS_CMD_SET_KLIPPER_NODEID: - can_process_set_klipper_nodeid(id, len, data); + can_process_set_klipper_nodeid(msg); break; case CANBUS_CMD_REQUEST_BOOTLOADER: - can_process_request_bootloader(id, len, data); + can_process_request_bootloader(msg); break; } } @@ -202,7 +211,7 @@ can_process(uint32_t id, uint32_t len, uint8_t *data) * CAN packet reading ****************************************************************/ -void +static void canbus_notify_rx(void) { sched_wake_task(&CanData.rx_wake); @@ -212,16 +221,30 @@ DECL_CONSTANT("RECEIVE_WINDOW", ARRAY_SIZE(CanData.receive_buf)); // Handle incoming data (called from IRQ handler) void -canbus_process_data(uint32_t id, uint32_t len, uint8_t *data) +canbus_process_data(struct canbus_msg *msg) { - if (!id || id != CanData.assigned_id) - return; - int rpos = CanData.receive_pos; - if (len > sizeof(CanData.receive_buf) - rpos) - len = sizeof(CanData.receive_buf) - rpos; - memcpy(&CanData.receive_buf[rpos], data, len); - CanData.receive_pos = rpos + len; - canbus_notify_rx(); + 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) + len = sizeof(CanData.receive_buf) - rpos; + memcpy(&CanData.receive_buf[rpos], msg->data, len); + CanData.receive_pos = rpos + len; + canbus_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; + uint32_t pos = pushp % ARRAY_SIZE(CanData.admin_queue); + memcpy(&CanData.admin_queue[pos], msg, sizeof(*msg)); + CanData.admin_push_pos = pushp + 1; + canbus_notify_rx(); + } } // Remove from the receive buffer the given number of bytes @@ -257,17 +280,20 @@ canbus_rx_task(void) if (!sched_check_wake(&CanData.rx_wake)) return; - // Read any pending CAN packets + // Process pending admin messages for (;;) { - uint8_t data[8]; - uint32_t id; - int ret = canbus_read(&id, data); - if (ret < 0) + uint32_t pushp = readl(&CanData.admin_push_pos); + uint32_t pullp = CanData.admin_pull_pos; + if (pushp == pullp) break; - if (id && id == CanData.assigned_id + 1) + 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(id, ret, data); + can_process_admin(msg); + CanData.admin_pull_pos = pullp + 1; } // Check for a complete message block and process it diff --git a/src/generic/canbus.h b/src/generic/canbus.h index a7df077d..a6463246 100644 --- a/src/generic/canbus.h +++ b/src/generic/canbus.h @@ -7,15 +7,24 @@ #define CANBUS_ID_ADMIN_RESP 0x3f1 #define CANBUS_UUID_LEN 6 +struct canbus_msg { + uint32_t id; + uint32_t dlc; + union { + uint8_t data[8]; + uint32_t data32[2]; + }; +}; + +#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(uint32_t id, uint32_t len, uint8_t *data); +int canbus_send(struct canbus_msg *msg); void canbus_set_filter(uint32_t id); // canbus.c void canbus_notify_tx(void); -void canbus_notify_rx(void); -void canbus_process_data(uint32_t id, uint32_t len, uint8_t *data); +void canbus_process_data(struct canbus_msg *msg); void canbus_set_uuid(void *data); #endif // canbus.h diff --git a/src/stm32/can.c b/src/stm32/can.c index a82ad688..99fe98dc 100644 --- a/src/stm32/can.c +++ b/src/stm32/can.c @@ -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,15 @@ 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 = (((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)); - } + mb->TDLR = msg->data32[0]; + mb->TDHR = msg->data32[1]; /* Request transmission */ - mb->TIR = (id << CAN_TI0R_STID_Pos) | CAN_TI0R_TXRQ; - return len; + mb->TIR = (msg->id << CAN_TI0R_STID_Pos) | CAN_TI0R_TXRQ; + return CANMSG_DATA_LEN(msg); } // Setup the receive packet filter @@ -182,9 +142,6 @@ canbus_set_filter(uint32_t id) /* 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 +152,20 @@ 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]; - 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->RF1R = CAN_RF1R_RFOM1; + CAN_FIFOMailBox_TypeDef *mb = &SOC_CAN->sFIFOMailBox[0]; + struct canbus_msg msg; + msg.id = (mb->RIR >> CAN_RI0R_STID_Pos) & 0x7FF; + 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 { - struct { uint32_t rdlr, rdhr; }; - uint8_t data[8]; - } rdata = { .rdlr = rdlr, .rdhr = rdhr }; - canbus_process_data(rir_id, dlc, rdata.data); + canbus_process_data(&msg); } 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,7 +260,7 @@ 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); diff --git a/src/stm32/fdcan.c b/src/stm32/fdcan.c index aeb6eced..99a76a02 100755 --- a/src/stm32/fdcan.c +++ b/src/stm32/fdcan.c @@ -17,13 +17,6 @@ #include "internal.h" // enable_pclock #include "sched.h" // DECL_INIT -/* - FDCAN max date length = 64bytes - data_len[] is the data length & DLC mapping table - Required when the data length exceeds 64bytes - */ -uint8_t data_len[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; - typedef struct { uint32_t RESERVED0 : 18; @@ -38,7 +31,7 @@ typedef struct uint32_t RESERVED1 : 2; __IO uint32_t FIDX : 7; __IO uint32_t ANMF : 1; - __IO uint8_t data[64]; + __IO uint32_t data[64 / 4]; }FDCAN_RX_FIFO_TypeDef; typedef struct @@ -66,8 +59,6 @@ typedef struct FDCAN_RAM_TypeDef *fdcan_ram = (FDCAN_RAM_TypeDef *)(SRAMCAN_BASE); -#define FDCAN_IE_RX_FIFO0 (FDCAN_IE_RF0NE | FDCAN_IE_RF0FE | FDCAN_IE_RF0LE) -#define FDCAN_IE_RX_FIFO1 (FDCAN_IE_RF1NE | FDCAN_IE_RF1FE | FDCAN_IE_RF1LE) #define FDCAN_IE_TC (FDCAN_IE_TCE | FDCAN_IE_TCFE | FDCAN_IE_TFEE) #if CONFIG_STM32_CANBUS_PB0_PB1 @@ -85,7 +76,6 @@ FDCAN_RAM_TypeDef *fdcan_ram = (FDCAN_RAM_TypeDef *)(SRAMCAN_BASE); #endif #define CAN_IT0_IRQn TIM16_FDCAN_IT0_IRQn - #define CAN_IT1_IRQn TIM17_FDCAN_IT1_IRQn #define CAN_FUNCTION GPIO_FUNCTION(3) // Alternative function mapping number #endif @@ -93,35 +83,9 @@ FDCAN_RAM_TypeDef *fdcan_ram = (FDCAN_RAM_TypeDef *)(SRAMCAN_BASE); #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->RXF0S & FDCAN_RXF0S_F0FL)) { - // All rx mboxes empty, enable wake on rx IRQ - irq_disable(); - SOC_CAN->IE |= FDCAN_IE_RF0NE; - irq_enable(); - return -1; - } - - // Read and ack packet - uint32_t r_index = ((SOC_CAN->RXF0S & FDCAN_RXF0S_F0GI) - >> FDCAN_RXF0S_F0GI_Pos); - FDCAN_RX_FIFO_TypeDef *rxf0 = &MSG_RAM.RXF0[r_index]; - uint32_t dlc = rxf0->DLC; - *id = rxf0->ID; - for (uint8_t i = 0; i < dlc; i++) { - data[i] = rxf0->data[i]; - } - SOC_CAN->RXF0A = r_index; - - 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 txfqs = SOC_CAN->TXFQS; if (txfqs & FDCAN_TXFQS_TFQF) { @@ -134,23 +98,16 @@ canbus_send(uint32_t id, uint32_t len, uint8_t *data) uint32_t w_index = ((txfqs & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos); FDCAN_TX_FIFO_TypeDef *txfifo = &MSG_RAM.TXFIFO[w_index]; - txfifo->id_section = id << 18; - txfifo->dlc_section = len << 16; - if (len) { - txfifo->data[0] = (((uint32_t)data[3] << 24) - | ((uint32_t)data[2] << 16) - | ((uint32_t)data[1] << 8) - | ((uint32_t)data[0] << 0)); - txfifo->data[1] = (((uint32_t)data[7] << 24) - | ((uint32_t)data[6] << 16) - | ((uint32_t)data[5] << 8) - | ((uint32_t)data[4] << 0)); - } + txfifo->id_section = (msg->id & 0x1fffffff) << 18; + txfifo->dlc_section = (msg->dlc & 0x0f) << 16; + txfifo->data[0] = msg->data32[0]; + txfifo->data[1] = msg->data32[1]; SOC_CAN->TXBAR = ((uint32_t)1 << w_index); - return len; + return CANMSG_DATA_LEN(msg); } -void can_filter(uint32_t id, uint8_t index) +static void +can_filter(uint32_t index, uint32_t id) { MSG_RAM.FLS[index] = ((0x2 << 30) // Classic filter | (0x1 << 27) // Store in Rx FIFO 0 if filter matches @@ -170,17 +127,12 @@ canbus_set_filter(uint32_t id) /* Enable configuration change */ SOC_CAN->CCCR |= FDCAN_CCCR_CCE; - can_filter(CANBUS_ID_ADMIN, 0); - - /* List size standard */ - SOC_CAN->RXGFC &= ~(FDCAN_RXGFC_LSS); - SOC_CAN->RXGFC |= 1 << FDCAN_RXGFC_LSS_Pos; - - /* Filter remote frames with 11-bit standard IDs - Non-matching frames standard reject or accept in Rx FIFO 1 */ - SOC_CAN->RXGFC &= ~(FDCAN_RXGFC_RRFS | FDCAN_RXGFC_ANFS); - SOC_CAN->RXGFC |= ((0 << FDCAN_RXGFC_RRFS_Pos) - | ((id ? 0x01 : 0x02) << FDCAN_RXGFC_ANFS_Pos)); + // Load filter + can_filter(0, CANBUS_ID_ADMIN); + can_filter(1, id); + can_filter(2, id + 1); + SOC_CAN->RXGFC = ((id ? 3 : 1) << FDCAN_RXGFC_LSS_Pos + | 0x02 << FDCAN_RXGFC_ANFS_Pos); /* Leave the initialisation mode for the filter */ SOC_CAN->CCCR &= ~FDCAN_CCCR_CCE; @@ -191,36 +143,28 @@ canbus_set_filter(uint32_t id) void CAN_IRQHandler(void) { - uint32_t ir = SOC_CAN->IR; - uint32_t ie = SOC_CAN->IE; + uint32_t ir = SOC_CAN->IR & SOC_CAN->IE; - if (ir & FDCAN_IE_RX_FIFO1 && ie & FDCAN_IE_RX_FIFO1) { - SOC_CAN->IR = FDCAN_IE_RX_FIFO1; + if (ir & FDCAN_IE_RF0NE) { + SOC_CAN->IR = FDCAN_IE_RF0NE; - if (SOC_CAN->RXF1S & FDCAN_RXF1S_F1FL) { + uint32_t rxf0s = SOC_CAN->RXF0S; + if (rxf0s & FDCAN_RXF0S_F0FL) { // Read and ack data packet - uint32_t r_index = ((SOC_CAN->RXF1S & FDCAN_RXF1S_F1GI) - >> FDCAN_RXF1S_F1GI_Pos); - FDCAN_RX_FIFO_TypeDef *rxf1 = &MSG_RAM.RXF1[r_index]; - - uint32_t rir_id = rxf1->ID; - uint32_t dlc = rxf1->DLC; - uint8_t data[8]; - for (uint8_t i = 0; i < dlc; i++) { - data[i] = rxf1->data[i]; - } - SOC_CAN->RXF1A = r_index; + uint32_t idx = (rxf0s & FDCAN_RXF0S_F0GI) >> FDCAN_RXF0S_F0GI_Pos; + FDCAN_RX_FIFO_TypeDef *rxf0 = &MSG_RAM.RXF0[idx]; + struct canbus_msg msg; + msg.id = rxf0->ID; + msg.dlc = rxf0->DLC; + msg.data32[0] = rxf0->data[0]; + msg.data32[1] = rxf0->data[1]; + SOC_CAN->RXF0A = idx; // Process packet - canbus_process_data(rir_id, dlc, data); + canbus_process_data(&msg); } } - if (ie & FDCAN_IE_RX_FIFO0 && ir & FDCAN_IE_RX_FIFO0) { - // Admin Rx - SOC_CAN->IR = FDCAN_IE_RX_FIFO0; - canbus_notify_rx(); - } - if (ie & FDCAN_IE_TC && ir & FDCAN_IE_TC) { + if (ir & FDCAN_IE_TC) { // Tx SOC_CAN->IR = FDCAN_IE_TC; canbus_notify_tx(); @@ -317,10 +261,8 @@ can_init(void) /*##-3- Configure Interrupts #################################*/ armcm_enable_irq(CAN_IRQHandler, CAN_IT0_IRQn, 0); - if (CAN_IT0_IRQn != CAN_IT1_IRQn) - armcm_enable_irq(CAN_IRQHandler, CAN_IT1_IRQn, 0); - SOC_CAN->ILE |= 0x03; - SOC_CAN->IE |= FDCAN_IE_RX_FIFO0 | FDCAN_IE_RX_FIFO1; + SOC_CAN->ILE = FDCAN_ILE_EINT0; + SOC_CAN->IE = FDCAN_IE_RF0NE; // Convert unique 96-bit chip id into 48 bit representation uint64_t hash = fasthash64((uint8_t*)UID_BASE, 12, 0xA16231A7); |