Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

canbus: Use single method for reading canbus messages 

Previously the code had canbus_read() which was called from task
context (for admin messages), and canbus_process_data() which was
called from irq context (used for data messages).  Change that to a
single canbus_process_data() function that is called from irq context
(used for all messages).  This simplifies the low-level hardware
specific canbus code and should make it easier to support other
hardware implementations.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2022-06-10 14:13:39 -04:00
parent da755c3c1b
commit 84d798f516
4 changed files with 125 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
src/generic/canbus.c
View File

@ -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

17
src/generic/canbus.h
View File

@ -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

82
src/stm32/can.c
View File

@ -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);

122
src/stm32/fdcan.c
View File

@ -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);