driver: can: add new filter to match CAN-FD frames

drivers/can/can_loopback.c

@@ -173,7 +173,12 @@ static int can_loopback_add_rx_filter(const struct device *dev, can_rx_callback_
 
 	LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->id, filter->mask);
 
+#ifdef CONFIG_CAN_FD_MODE
+	if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA |
+							CAN_FILTER_RTR | CAN_FILTER_FDF)) != 0) {
+#else
 	if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA | CAN_FILTER_RTR)) != 0) {
+#endif
 		LOG_ERR("unsupported CAN filter flags 0x%02x", filter->flags);
 		return -ENOTSUP;
 	}

drivers/can/can_mcan.c

@@ -618,6 +618,7 @@ static void can_mcan_get_message(const struct device *dev,
 	struct can_mcan_rx_fifo_hdr hdr;
 	bool rtr_filter_mask;
 	bool rtr_filter;
+	bool fd_frame_filter;
 
 	while ((*fifo_status_reg & CAN_MCAN_RXF0S_F0FL)) {
 		get_idx = (*fifo_status_reg & CAN_MCAN_RXF0S_F0GI) >>
@@ -653,16 +654,22 @@ static void can_mcan_get_message(const struct device *dev,
 			frame.flags |= CAN_FRAME_IDE;
 			rtr_filter_mask = (data->ext_filt_rtr_mask & BIT(filt_idx)) != 0;
 			rtr_filter = (data->ext_filt_rtr & BIT(filt_idx)) != 0;
+			fd_frame_filter = (data->ext_filt_fd_frame & BIT(filt_idx)) != 0;
 		} else {
 			frame.id = hdr.std_id;
 			rtr_filter_mask = (data->std_filt_rtr_mask & BIT(filt_idx)) != 0;
 			rtr_filter = (data->std_filt_rtr & BIT(filt_idx)) != 0;
+			fd_frame_filter = (data->std_filt_fd_frame & BIT(filt_idx)) != 0;
 		}
 
 		if (rtr_filter_mask && (rtr_filter != ((frame.flags & CAN_FRAME_RTR) != 0))) {
 			/* RTR bit does not match filter RTR mask, drop frame */
 			*fifo_ack_reg = get_idx;
 			continue;
+		} else if (fd_frame_filter != ((frame.flags & CAN_FRAME_FDF) != 0)) {
+			/* FD bit does not match filter FD frame, drop frame */
+			*fifo_ack_reg = get_idx;
+			continue;
 		}
 
 		data_length = can_dlc_to_bytes(frame.dlc);
@@ -949,11 +956,6 @@ int can_mcan_add_rx_filter_std(const struct device *dev,
 	};
 	int filter_id;
 
-	if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA | CAN_FILTER_RTR)) != 0) {
-		LOG_ERR("unsupported CAN filter flags 0x%02x", filter->flags);
-		return -ENOTSUP;
-	}
-
 	k_mutex_lock(&data->inst_mutex, K_FOREVER);
 	filter_id = can_mcan_get_free_std(msg_ram->std_filt);
 
@@ -988,6 +990,12 @@ int can_mcan_add_rx_filter_std(const struct device *dev,
 		data->std_filt_rtr_mask &= ~(1U << filter_id);
 	}
 
+	if ((filter->flags & CAN_FILTER_FDF) != 0) {
+		data->std_filt_fd_frame |= (1U << filter_id);
+	} else {
+		data->std_filt_fd_frame &= ~(1U << filter_id);
+	}
+
 	data->rx_cb_std[filter_id] = callback;
 	data->cb_arg_std[filter_id] = user_data;
 
@@ -1052,6 +1060,12 @@ static int can_mcan_add_rx_filter_ext(const struct device *dev,
 		data->ext_filt_rtr_mask &= ~(1U << filter_id);
 	}
 
+	if ((filter->flags & CAN_FILTER_FDF) != 0) {
+		data->ext_filt_fd_frame |= (1U << filter_id);
+	} else {
+		data->ext_filt_fd_frame &= ~(1U << filter_id);
+	}
+
 	data->rx_cb_ext[filter_id] = callback;
 	data->cb_arg_ext[filter_id] = user_data;
 
@@ -1068,6 +1082,17 @@ int can_mcan_add_rx_filter(const struct device *dev,
 		return -EINVAL;
 	}
 
+
+#ifdef CONFIG_CAN_FD_MODE
+	if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA |
+							CAN_FILTER_RTR | CAN_FILTER_FDF)) != 0) {
+#else
+	if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA | CAN_FILTER_RTR)) != 0) {
+#endif
+		LOG_ERR("unsupported CAN filter flags 0x%02x", filter->flags);
+		return -ENOTSUP;
+	}
+
 	if ((filter->flags & CAN_FILTER_IDE) != 0) {
 		filter_id = can_mcan_add_rx_filter_ext(dev, callback, user_data, filter);
 		if (filter_id >= 0) {

drivers/can/can_mcan.h

@@ -178,8 +178,10 @@ struct can_mcan_data {
 	void *cb_arg_ext[NUM_EXT_FILTER_DATA];
 	can_state_change_callback_t state_change_cb;
 	void *state_change_cb_data;
+	uint32_t std_filt_fd_frame;
 	uint32_t std_filt_rtr;
 	uint32_t std_filt_rtr_mask;
+	uint16_t ext_filt_fd_frame;
 	uint16_t ext_filt_rtr;
 	uint16_t ext_filt_rtr_mask;
 	struct can_mcan_mm mm;

drivers/can/can_native_posix_linux.c

@@ -199,7 +199,12 @@ static int can_npl_add_rx_filter(const struct device *dev, can_rx_callback_t cb,
 	LOG_DBG("Setting filter ID: 0x%x, mask: 0x%x", filter->id,
 		filter->mask);
 
+#ifdef CONFIG_CAN_FD_MODE
+	if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA |
+							CAN_FILTER_RTR | CAN_FILTER_FDF)) != 0) {
+#else
 	if ((filter->flags & ~(CAN_FILTER_IDE | CAN_FILTER_DATA | CAN_FILTER_RTR)) != 0) {
+#endif
 		LOG_ERR("unsupported CAN filter flags 0x%02x", filter->flags);
 		return -ENOTSUP;
 	}

drivers/can/can_utils.h

@@ -33,6 +33,10 @@ static inline bool can_utils_filter_match(const struct can_frame *frame,
 		return false;
 	}
 
+	if (((frame->flags & CAN_FRAME_FDF) != 0) && (filter->flags & CAN_FILTER_FDF) == 0) {
+		return false;
+	}
+
 	if ((frame->id ^ filter->id) & filter->mask) {
 		return false;
 	}

include/zephyr/drivers/can.h

@@ -196,6 +196,9 @@ struct can_frame {
 /** Filter matches data frames */
 #define CAN_FILTER_DATA BIT(2)
 
+/** Filter matches CAN-FD frames (FDF) */
+#define CAN_FILTER_FDF BIT(3)
+
 /** @} */
 
 /**
@@ -212,7 +215,7 @@ struct can_filter {
 	 */
 	uint32_t mask         : 29;
 	/** Flags. @see @ref CAN_FILTER_FLAGS. */
-	uint8_t flags         : 3;
+	uint8_t flags;
 };
 
 /**

include/zephyr/net/socketcan.h

@@ -124,6 +124,8 @@ struct socketcan_filter {
 	socketcan_id_t can_id;
 	/** The mask applied to @a can_id for matching. */
 	socketcan_id_t can_mask;
+	/** Additional flags for FD frame filter. */
+	uint8_t flags;
 };
 
 /** @} */

include/zephyr/net/socketcan_utils.h

@@ -88,6 +88,7 @@ static inline void socketcan_to_can_filter(const struct socketcan_filter *sfilte
 	zfilter->flags |= (sfilter->can_id & BIT(31)) != 0 ? CAN_FILTER_IDE : 0;
 	zfilter->id = sfilter->can_id & BIT_MASK(29);
 	zfilter->mask = sfilter->can_mask & BIT_MASK(29);
+	zfilter->flags |= (sfilter->flags & CANFD_FDF) != 0 ? CAN_FILTER_FDF : 0;
 
 	if ((sfilter->can_mask & BIT(30)) == 0) {
 		zfilter->flags |= CAN_FILTER_DATA | CAN_FILTER_RTR;
@@ -120,6 +121,10 @@ static inline void socketcan_from_can_filter(const struct can_filter *zfilter,
 		(CAN_FILTER_DATA | CAN_FILTER_RTR)) {
 		sfilter->can_mask |= BIT(30);
 	}
+
+	if ((zfilter->flags & CAN_FILTER_FDF) != 0) {
+		sfilter->flags |= CANFD_FDF;
+	}
 }
 
 /**

tests/drivers/can/canfd/src/main.c

@@ -113,6 +113,24 @@ const struct can_filter test_std_filter_2 = {
 	.mask = CAN_STD_ID_MASK
 };
 
+/**
+ * @brief Standard (11-bit) CAN-FD ID filter 1.
+ */
+const struct can_filter test_std_filter_fd_1 = {
+	.flags = CAN_FILTER_DATA | CAN_FILTER_FDF,
+	.id = TEST_CAN_STD_ID_1,
+	.mask = CAN_STD_ID_MASK
+};
+
+/**
+ * @brief Standard (11-bit) CAN-FD ID filter 2.
+ */
+const struct can_filter test_std_filter_fd_2 = {
+	.flags = CAN_FILTER_DATA | CAN_FILTER_FDF,
+	.id = TEST_CAN_STD_ID_2,
+	.mask = CAN_STD_ID_MASK
+};
+
 /**
  * @brief Assert that two CAN frames are equal.
  *
@@ -177,7 +195,7 @@ static void rx_std_callback_fd_1(const struct device *dev, struct can_frame *fra
 
 	assert_frame_equal(frame, &test_std_frame_fd_1);
 	zassert_equal(dev, can_dev, "CAN device does not match");
-	zassert_equal_ptr(filter, &test_std_filter_1, "filter does not match");
+	zassert_equal_ptr(filter, &test_std_filter_fd_1, "filter does not match");
 
 	k_sem_give(&rx_callback_sem);
 }
@@ -189,7 +207,7 @@ static void rx_std_callback_fd_2(const struct device *dev, struct can_frame *fra
 
 	assert_frame_equal(frame, &test_std_frame_fd_2);
 	zassert_equal(dev, can_dev, "CAN device does not match");
-	zassert_equal_ptr(filter, &test_std_filter_2, "filter does not match");
+	zassert_equal_ptr(filter, &test_std_filter_fd_2, "filter does not match");
 
 	k_sem_give(&rx_callback_sem);
 }
@@ -363,7 +381,7 @@ ZTEST(canfd, test_send_receive_classic)
  */
 ZTEST(canfd, test_send_receive_fd)
 {
-	send_receive(&test_std_filter_1, &test_std_filter_2,
+	send_receive(&test_std_filter_fd_1, &test_std_filter_fd_2,
 		     &test_std_frame_fd_1, &test_std_frame_fd_2);
 }
 
@@ -372,7 +390,7 @@ ZTEST(canfd, test_send_receive_fd)
  */
 ZTEST(canfd, test_send_receive_mixed)
 {
-	send_receive(&test_std_filter_1, &test_std_filter_2,
+	send_receive(&test_std_filter_fd_1, &test_std_filter_2,
 		     &test_std_frame_fd_1, &test_std_frame_2);
 }