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

Add support FD frame filter to configure type frame for
each Rx msg to receive corresponding frames (classic, FD frames).

The Bosch M_CAN driver does not support FD frame filter,
so inmplement driver to handle it in software.

Signed-off-by: Cong Nguyen Huu <cong.nguyenhuu@nxp.com>

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;
+	}
 }
 
 /**