net: ethernet: ti: am65-cpsw: enable DSCP to priority map for RX

AM65 CPSW hardware can map the 6-bit DSCP/TOS field to
appropriate priority queue via DSCP to Priority mapping registers
(CPSW_PN_RX_PRI_MAP_REG).

Use a default DSCP to User Priority (UP) mapping as per
https://datatracker.ietf.org/doc/html/rfc8325#section-4.3
and
https://datatracker.ietf.org/doc/html/rfc8622#section-11

Signed-off-by: Roger Quadros <rogerq@kernel.org>
Reviewed-by: Guillaume Nault <gnault@redhat.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/ti/am65-cpsw-nuss.c b/drivers/net/ethernet/ti/am65-cpsw-nuss.c
index 47fb5af1679681b0a0634625fdcf163e7d36c377..14e1df721f2e802e9eddcbb52ac8812217219f5d 100644 (file)
--- a/drivers/net/ethernet/ti/am65-cpsw-nuss.c
+++ b/drivers/net/ethernet/ti/am65-cpsw-nuss.c
@@ -71,6 +71,8 @@
 #define AM65_CPSW_PORT_REG_RX_PRI_MAP          0x020
 #define AM65_CPSW_PORT_REG_RX_MAXLEN           0x024
 
+#define AM65_CPSW_PORTN_REG_CTL                        0x004
+#define AM65_CPSW_PORTN_REG_DSCP_MAP           0x120
 #define AM65_CPSW_PORTN_REG_SA_L               0x308
 #define AM65_CPSW_PORTN_REG_SA_H               0x30c
 #define AM65_CPSW_PORTN_REG_TS_CTL              0x310
@@ -94,6 +96,10 @@
 /* AM65_CPSW_PORT_REG_PRI_CTL */
 #define AM65_CPSW_PORT_REG_PRI_CTL_RX_PTYPE_RROBIN     BIT(8)
 
+/* AM65_CPSW_PN_REG_CTL */
+#define AM65_CPSW_PN_REG_CTL_DSCP_IPV4_EN      BIT(1)
+#define AM65_CPSW_PN_REG_CTL_DSCP_IPV6_EN      BIT(2)
+
 /* AM65_CPSW_PN_TS_CTL register fields */
 #define AM65_CPSW_PN_TS_CTL_TX_ANX_F_EN                BIT(4)
 #define AM65_CPSW_PN_TS_CTL_TX_VLAN_LT1_EN     BIT(5)
@@ -176,6 +182,99 @@ static void am65_cpsw_port_set_sl_mac(struct am65_cpsw_port *slave,
        writel(mac_lo, slave->port_base + AM65_CPSW_PORTN_REG_SA_L);
 }
 
+#define AM65_CPSW_DSCP_MAX     GENMASK(5, 0)
+#define AM65_CPSW_PRI_MAX      GENMASK(2, 0)
+#define AM65_CPSW_DSCP_PRI_PER_REG     8
+#define AM65_CPSW_DSCP_PRI_SIZE                4       /* in bits */
+static int am65_cpsw_port_set_dscp_map(struct am65_cpsw_port *slave, u8 dscp, u8 pri)
+{
+       int reg_ofs;
+       int bit_ofs;
+       u32 val;
+
+       if (dscp > AM65_CPSW_DSCP_MAX)
+               return -EINVAL;
+
+       if (pri > AM65_CPSW_PRI_MAX)
+               return -EINVAL;
+
+       /* 32-bit register offset to this dscp */
+       reg_ofs = (dscp / AM65_CPSW_DSCP_PRI_PER_REG) * 4;
+       /* bit field offset to this dscp */
+       bit_ofs = AM65_CPSW_DSCP_PRI_SIZE * (dscp % AM65_CPSW_DSCP_PRI_PER_REG);
+
+       val = readl(slave->port_base + AM65_CPSW_PORTN_REG_DSCP_MAP + reg_ofs);
+       val &= ~(AM65_CPSW_PRI_MAX << bit_ofs); /* clear */
+       val |= pri << bit_ofs;                  /* set */
+       writel(val, slave->port_base + AM65_CPSW_PORTN_REG_DSCP_MAP + reg_ofs);
+
+       return 0;
+}
+
+static void am65_cpsw_port_enable_dscp_map(struct am65_cpsw_port *slave)
+{
+       int dscp, pri;
+       u32 val;
+
+       /* Default DSCP to User Priority mapping as per:
+        * https://datatracker.ietf.org/doc/html/rfc8325#section-4.3
+        * and
+        * https://datatracker.ietf.org/doc/html/rfc8622#section-11
+        */
+       for (dscp = 0; dscp <= AM65_CPSW_DSCP_MAX; dscp++) {
+               switch (dscp) {
+               case 56:        /* CS7 */
+               case 48:        /* CS6 */
+                       pri = 7;
+                       break;
+               case 46:        /* EF */
+               case 44:        /* VA */
+                       pri = 6;
+                       break;
+               case 40:        /* CS5 */
+                       pri = 5;
+                       break;
+               case 34:        /* AF41 */
+               case 36:        /* AF42 */
+               case 38:        /* AF43 */
+               case 32:        /* CS4 */
+               case 26:        /* AF31 */
+               case 28:        /* AF32 */
+               case 30:        /* AF33 */
+               case 24:        /* CS3 */
+                       pri = 4;
+                       break;
+               case 18:        /* AF21 */
+               case 20:        /* AF22 */
+               case 22:        /* AF23 */
+                       pri = 3;
+                       break;
+               case 16:        /* CS2 */
+               case 10:        /* AF11 */
+               case 12:        /* AF12 */
+               case 14:        /* AF13 */
+               case 0:         /* DF */
+                       pri = 0;
+                       break;
+               case 8:         /* CS1 */
+               case 1:         /* LE */
+                       pri = 1;
+                       break;
+               default:
+                       pri = 0;
+                       break;
+               }
+
+               am65_cpsw_port_set_dscp_map(slave, dscp, pri);
+       }
+
+       /* enable port IPV4 and IPV6 DSCP for this port */
+       val = readl(slave->port_base + AM65_CPSW_PORTN_REG_CTL);
+       val |= AM65_CPSW_PN_REG_CTL_DSCP_IPV4_EN |
+               AM65_CPSW_PN_REG_CTL_DSCP_IPV6_EN;
+       writel(val, slave->port_base + AM65_CPSW_PORTN_REG_CTL);
+}
+
 static void am65_cpsw_sl_ctl_reset(struct am65_cpsw_port *port)
 {
        cpsw_sl_reset(port->slave.mac_sl, 100);
@@ -916,6 +1015,7 @@ static int am65_cpsw_nuss_ndo_slave_open(struct net_device *ndev)
        common->usage_count++;
 
        am65_cpsw_port_set_sl_mac(port, ndev->dev_addr);
+       am65_cpsw_port_enable_dscp_map(port);
 
        if (common->is_emac_mode)
                am65_cpsw_init_port_emac_ale(port);