diff --git a/drivers/net/can/m_can/m_can.c b/drivers/net/can/m_can/m_can.c
index 16ecc11c7f62..7c8110076256 100644
--- a/drivers/net/can/m_can/m_can.c
+++ b/drivers/net/can/m_can/m_can.c
@@ -223,6 +223,52 @@ enum m_can_reg {
#define ILE_EINT1 BIT(1)
#define ILE_EINT0 BIT(0)
+/* Standard ID message filters (SIDF) */
+#define SIDF_SFT_MASK GENMASK(31, 30)
+#define SIDF_SFEC_MASK GENMASK(29, 27)
+#define SIDF_SFID1_MASK GENMASK(26, 16)
+#define SIDF_SFID2_MASK GENMASK(10, 0)
+
+/* Extended ID message filters (XIDF) */
+#define XIDF_EFT_MASK GENMASK_ULL(63, 62)
+#define XIDF_EFID2_MASK GENMASK_ULL(60, 32)
+#define XIDF_EFEC_MASK GENMASK_ULL(31, 29)
+#define XIDF_EFID1_MASK GENMASK_ULL(28, 0)
+
+/* Standard Filter Type */
+#define SFT_RANGE 0x0
+#define SFT_DUAL 0x1
+#define SFT_CLASSIC 0x2
+#define SFT_DISABLED 0x3
+
+/* Standard Filter Element Configuration */
+#define SFEC_DISABLE 0x0
+#define SFEC_RXF0 0x1
+#define SFEC_RXF1 0x2
+#define SFEC_REJECT 0x3
+#define SFEC_PRIO 0x4
+#define SFEC_PRIO_RXF0 0x5
+#define SFEC_PRIO_RXF1 0x6
+#define SFEC_DEBUG 0x7
+
+/* Global Filter Configuration Field */
+#define GFC_ANFS GENMASK(5, 4)
+#define GFC_ANFE GENMASK(3, 2)
+#define GFC_RRFS BIT(1)
+#define GFC_RRFE BIT(0)
+
+#define ANFS_ACCEPT_RXF0 0x0
+#define ANFS_ACCEPT_RXF1 0x1
+#define ANFS_REJECT 0x2
+
+/* Standard ID Filter Configuration */
+#define SIDFC_LSS GENMASK(23, 16)
+#define SIDFC_FLSSA GENMASK(15, 0)
+
+/* Extended ID Filter Configuration */
+#define XIDFC_LSE GENMASK(22, 16)
+#define XIDFC_FLSAE GENMASK(15, 0)
+
/* Rx FIFO 0/1 Configuration (RXF0C/RXF1C) */
#define RXFC_FWM_MASK GENMASK(30, 24)
#define RXFC_FS_MASK GENMASK(22, 16)@@ -382,6 +428,56 @@ static inline bool m_can_tx_fifo_full(struct m_can_classdev *cdev)
return _m_can_tx_fifo_full(m_can_read(cdev, M_CAN_TXFQS));
}
+static int m_can_validate_hw_filter(struct net_device *dev,
+ struct can_filter *hwf,
+ unsigned int hwf_cnt)
+{
+ struct m_can_classdev *cdev = netdev_priv(dev);
+ size_t sff_filter_cnt = 0;
+ size_t eff_filter_cnt = 0;
+ int i;
+
+ for (i = 0; i < hwf_cnt; i++)
+ if (hwf[i].can_id <= CAN_SFF_MASK || hwf[i].can_mask <= CAN_SFF_MASK)
+ sff_filter_cnt++;
+ else
+ eff_filter_cnt++;
+
+ if (sff_filter_cnt > cdev->mcfg[MRAM_SIDF].num)
+ return -EINVAL;
+
+ if (eff_filter_cnt > cdev->mcfg[MRAM_XIDF].num)
+ return -EINVAL;
+
+ return 0;
+}
+
+static int m_can_sid_filter_write(struct m_can_classdev *cdev,
+ struct can_filter *filter,
+ size_t index)
+{
+ u32 addr_offset = cdev->mcfg[MRAM_SIDF].off + SIDF_ELEMENT_SIZE * index;
+ u32 sidf = FIELD_PREP(SIDF_SFT_MASK, SFT_CLASSIC) |
+ FIELD_PREP(SIDF_SFEC_MASK, SFEC_RXF0) |
+ FIELD_PREP(SIDF_SFID1_MASK, filter->can_id) |
+ FIELD_PREP(SIDF_SFID2_MASK, filter->can_mask);
+
+ return cdev->ops->write_fifo(cdev, addr_offset, &sidf, sizeof(sidf));
+}
+
+static int m_can_xid_filter_write(struct m_can_classdev *cdev,
+ struct can_filter *filter,
+ size_t index)
+{
+ u32 addr_offset = cdev->mcfg[MRAM_XIDF].off + XIDF_ELEMENT_SIZE * index;
+ u64 xidf = FIELD_PREP(XIDF_EFT_MASK, SFT_CLASSIC) |
+ FIELD_PREP(XIDF_EFEC_MASK, SFEC_RXF0) |
+ FIELD_PREP(XIDF_EFID1_MASK, filter->can_id) |
+ FIELD_PREP(XIDF_EFID2_MASK, filter->can_mask);
+
+ return cdev->ops->write_fifo(cdev, addr_offset, &xidf, sizeof(xidf));
+}
+
static void m_can_config_endisable(struct m_can_classdev *cdev, bool enable)
{
u32 cccr = m_can_read(cdev, M_CAN_CCCR);@@ -1266,8 +1362,10 @@ static int m_can_chip_config(struct net_device *dev)
{
struct m_can_classdev *cdev = netdev_priv(dev);
u32 interrupts = IR_ALL_INT;
- u32 cccr, test;
- int err;
+ size_t sff_filter_cnt = 0;
+ size_t eff_filter_cnt = 0;
+ u32 cccr, test, anfs;
+ int err, i;
err = m_can_init_ram(cdev);
if (err) {@@ -1288,8 +1386,38 @@ static int m_can_chip_config(struct net_device *dev)
FIELD_PREP(RXESC_F1DS_MASK, RXESC_64B) |
FIELD_PREP(RXESC_F0DS_MASK, RXESC_64B));
- /* Accept Non-matching Frames Into FIFO 0 */
- m_can_write(cdev, M_CAN_GFC, 0x0);
+ /* Configure HW filters and count standard vs extended id filters */
+ for (i = 0; i < cdev->can.hw_filter_cnt; i++) {
+ struct can_filter *filter = &cdev->can.hw_filter[i];
+
+ if (filter->can_id <= CAN_SFF_MASK ||
+ filter->can_mask <= CAN_SFF_MASK)
+ err = m_can_sid_filter_write(cdev, filter,
+ sff_filter_cnt++);
+ else
+ err = m_can_xid_filter_write(cdev, filter,
+ eff_filter_cnt++);
+
+ if (err)
+ return err;
+ }
+
+ /* Configure offset to and number of standard id filters in MRAM */
+ m_can_write(cdev, M_CAN_SIDFC,
+ FIELD_PREP(SIDFC_FLSSA, cdev->mcfg[MRAM_SIDF].off) |
+ FIELD_PREP(SIDFC_LSS, sff_filter_cnt));
+ /* Configure offset to and number of extended id filters in MRAM */
+ m_can_write(cdev, M_CAN_XIDFC,
+ FIELD_PREP(XIDFC_FLSAE, cdev->mcfg[MRAM_XIDF].off) |
+ FIELD_PREP(XIDFC_LSE, sff_filter_cnt));
+
+ /* If any filter is configured, the Global Filter Configuration is set
+ * to reject non-matching frames.
+ */
+ anfs = cdev->can.hw_filter_cnt ? ANFS_REJECT : ANFS_ACCEPT_RXF0;
+ m_can_write(cdev, M_CAN_GFC,
+ FIELD_PREP(GFC_ANFS, anfs) | FIELD_PREP(GFC_ANFE, anfs));
+
if (cdev->version == 30) {
/* only support one Tx Buffer currently */@@ -1524,6 +1652,7 @@ static int m_can_dev_setup(struct m_can_classdev *cdev)
/* Shared properties of all M_CAN versions */
cdev->version = m_can_version;
cdev->can.do_set_mode = m_can_set_mode;
+ cdev->can.validate_hw_filter = m_can_validate_hw_filter;
cdev->can.do_get_berr_counter = m_can_get_berr_counter;
/* Set M_CAN supported operations */
--
2.41.0