michaelh/zephyr
1
0
Fork 0
Code Releases Activity

modules: canopennode: process rx buffers in priority order

Browse source 
The CANopenNode stack expects registered RX buffers to be processed and
matched in priority order. The priority corresponds to the index of each
each registered RX buffer with lower indexes having higher priority.

Depending on the CANopen COB-ID network configuration used, it may result
in overlapping CAN RX filters. In the case of overlaps, the priorities of
the registered RX buffers matter.

When receiving a CAN frame, process the RX buffers in priority order and
only dispatch the callback for the matching object with the highest
priority.

Fixes: #54364

Signed-off-by: Henrik Brix Andersen <hebad@vestas.com>
This commit is contained in:
Henrik Brix Andersen 2023-02-13 11:59:31 +01:00 • committed by Anas Nashif
commit bc4b49c149
2 changed files with 23 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

33
modules/canopennode/CO_driver.c
View file

@ -78,22 +78,31 @@ static void canopen_detach_all_rx_filters(CO_CANmodule_t *CANmodule)
}
}
static void canopen_rx_callback(const struct device *dev, struct can_frame *frame, void *arg)
static void canopen_rx_callback(const struct device *dev, struct can_frame *frame, void *user_data)
{
CO_CANrx_t *buffer = (CO_CANrx_t *)arg;
CO_CANmodule_t *CANmodule = (CO_CANmodule_t *)user_data;
CO_CANrxMsg_t rxMsg;
CO_CANrx_t *buffer;
int i;
ARG_UNUSED(dev);
if (!buffer || !buffer->pFunct) {
LOG_ERR("failed to process CAN rx callback");
return;
}
/* Loop through registered rx buffers in priority order */
for (i = 0; i < CANmodule->rx_size; i++) {
buffer = &CANmodule->rx_array[i];
rxMsg.ident = frame->id;
rxMsg.DLC = frame->dlc;
memcpy(rxMsg.data, frame->data, frame->dlc);
buffer->pFunct(buffer->object, &rxMsg);
if (buffer->filter_id == -ENOSPC || buffer->pFunct == NULL) {
continue;
}
if (((frame->id ^ buffer->ident) & buffer->mask) == 0U) {
rxMsg.ident = frame->id;
rxMsg.DLC = frame->dlc;
memcpy(rxMsg.data, frame->data, frame->dlc);
buffer->pFunct(buffer->object, &rxMsg);
break;
}
}
}
static void canopen_tx_callback(const struct device *dev, int error, void *arg)
@ -298,6 +307,8 @@ CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index,
buffer = &CANmodule->rx_array[index];
buffer->object = object;
buffer->pFunct = pFunct;
buffer->ident = ident;
buffer->mask = mask;
filter.flags = (rtr ? CAN_FILTER_RTR : CAN_FILTER_DATA);
filter.id = ident;
@ -309,7 +320,7 @@ CO_ReturnError_t CO_CANrxBufferInit(CO_CANmodule_t *CANmodule, uint16_t index,
buffer->filter_id = can_add_rx_filter(CANmodule->dev,
canopen_rx_callback,
buffer, &filter);
CANmodule, &filter);
if (buffer->filter_id == -ENOSPC) {
LOG_ERR("failed to add CAN rx callback, no free filter");
CO_errorReport(CANmodule->em, CO_EM_MEMORY_ALLOCATION_ERROR,

1
modules/canopennode/CO_driver_target.h
View file

@ -66,6 +66,7 @@ typedef struct canopen_rx {
void *object;
CO_CANrxBufferCallback_t pFunct;
uint16_t ident;
uint16_t mask;
} CO_CANrx_t;
typedef struct canopen_tx {