modules: canopennode: process rx buffers in priority order

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 <[email protected]>

Author: henrikbrixandersen

modules/canopennode/CO_driver.c

@@ -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];
+
+		if (buffer->filter_id == -ENOSPC || buffer->pFunct == NULL) {
+			continue;
+		}
 
-	rxMsg.ident = frame->id;
-	rxMsg.DLC = frame->dlc;
-	memcpy(rxMsg.data, frame->data, frame->dlc);
-	buffer->pFunct(buffer->object, &rxMsg);
+		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,

modules/canopennode/CO_driver_target.h

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