net: lan966x: Add support for DSCP rewrite

Add support for DSCP rewrite in lan966x driver. On egress DSCP is
rewritten from either classified DSCP, or frame DSCP. Classified DSCP is
determined by the Analyzer Classifier on ingress, and is mapped from
classified QoS class and DP level. Classification of DSCP is by default
enabled for all ports.

It is required that DSCP is trusted for the egress port *and* rewrite
table is not empty, in order to rewrite DSCP based on classified DSCP,
otherwise DSCP is always rewritten from frame DSCP.

Reviewed-by: Daniel Machon <daniel.machon@microchip.com>
Signed-off-by: Horatiu Vultur <horatiu.vultur@microchip.com>
Signed-off-by: Paolo Abeni <pabeni@redhat.com>

3 files changed, 81 insertions, 3 deletions

diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c b/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
index 0ea650943653..ed2d96d7908e 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
@@ -46,10 +46,12 @@ static bool lan966x_dcb_apptrust_contains(int portno, u8 selector)
 
 static void lan966x_dcb_app_update(struct net_device *dev)
 {
+	struct dcb_ieee_app_prio_map dscp_rewr_map = {0};
 	struct dcb_rewr_prio_pcp_map pcp_rewr_map = {0};
 	struct lan966x_port *port = netdev_priv(dev);
 	struct lan966x_port_qos qos = {0};
 	struct dcb_app app_itr;
+	bool dscp_rewr = false;
 	bool pcp_rewr = false;
 
 	/* Get pcp ingress mapping */
@@ -81,6 +83,16 @@ static void lan966x_dcb_app_update(struct net_device *dev)
 		qos.pcp_rewr.map[i] = fls(pcp_rewr_map.map[i]) - 1;
 	}
 
+	/* Get dscp rewrite mapping */
+	dcb_getrewr_prio_dscp_mask_map(dev, &dscp_rewr_map);
+	for (int i = 0; i < ARRAY_SIZE(dscp_rewr_map.map); i++) {
+		if (!dscp_rewr_map.map[i])
+			continue;
+
+		dscp_rewr = true;
+		qos.dscp_rewr.map[i] = fls64(dscp_rewr_map.map[i]) - 1;
+	}
+
 	/* Enable use of pcp for queue classification */
 	if (lan966x_dcb_apptrust_contains(port->chip_port, DCB_APP_SEL_PCP)) {
 		qos.pcp.enable = true;
@@ -90,9 +102,13 @@ static void lan966x_dcb_app_update(struct net_device *dev)
 	}
 
 	/* Enable use of dscp for queue classification */
-	if (lan966x_dcb_apptrust_contains(port->chip_port, IEEE_8021QAZ_APP_SEL_DSCP))
+	if (lan966x_dcb_apptrust_contains(port->chip_port, IEEE_8021QAZ_APP_SEL_DSCP)) {
 		qos.dscp.enable = true;
 
+		if (dscp_rewr)
+			qos.dscp_rewr.enable = true;
+	}
+
 	lan966x_port_qos_set(port, &qos);
 }
 
@@ -272,7 +288,11 @@ static int lan966x_dcb_delrewr(struct net_device *dev, struct dcb_app *app)
 {
 	int err;
 
-	err = dcb_delrewr(dev, app);
+	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
+		err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_delrewr);
+	else
+		err = dcb_delrewr(dev, app);
+
 	if (err < 0)
 		return err;
 
@@ -299,7 +319,11 @@ static int lan966x_dcb_setrewr(struct net_device *dev, struct dcb_app *app)
 		lan966x_dcb_delrewr(dev, &app_itr);
 	}
 
-	err = dcb_setrewr(dev, app);
+	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
+		err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_setrewr);
+	else
+		err = dcb_setrewr(dev, app);
+
 	if (err)
 		goto out;
 
@@ -331,5 +355,11 @@ void lan966x_dcb_init(struct lan966x *lan966x)
 
 		lan966x_port_apptrust[port->chip_port] =
 			&lan966x_dcb_apptrust_policies[LAN966X_DCB_APPTRUST_DSCP_PCP];
+
+		/* Enable DSCP classification based on classified QoS class and
+		 * DP, for all DSCP values, for all ports.
+		 */
+		lan966x_port_qos_dscp_rewr_mode_set(port,
+						    LAN966X_PORT_QOS_REWR_DSCP_ALL);
 	}
 }
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
index 16b0149ac2b5..27f272831ea5 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
@@ -115,6 +115,11 @@
 #define LAN966X_PORT_REW_TAG_CTRL_CLASSIFIED	0
 #define LAN966X_PORT_REW_TAG_CTRL_MAPPED	2
 
+/* Port DSCP rewrite mode */
+#define LAN966X_PORT_REW_DSCP_FRAME		0
+#define LAN966X_PORT_REW_DSCP_ANALIZER		1
+#define LAN966X_PORT_QOS_REWR_DSCP_ALL		3
+
 /* MAC table entry types.
  * ENTRYTYPE_NORMAL is subject to aging.
  * ENTRYTYPE_LOCKED is not subject to aging.
@@ -418,10 +423,16 @@ struct lan966x_port_qos_pcp_rewr {
 	bool enable;
 };
 
+struct lan966x_port_qos_dscp_rewr {
+	u16 map[LAN966X_PORT_QOS_DSCP_COUNT];
+	bool enable;
+};
+
 struct lan966x_port_qos {
 	struct lan966x_port_qos_pcp pcp;
 	struct lan966x_port_qos_dscp dscp;
 	struct lan966x_port_qos_pcp_rewr pcp_rewr;
+	struct lan966x_port_qos_dscp_rewr dscp_rewr;
 	u8 default_prio;
 };
 
@@ -491,6 +502,8 @@ void lan966x_port_init(struct lan966x_port *port);
 
 void lan966x_port_qos_set(struct lan966x_port *port,
 			  struct lan966x_port_qos *qos);
+void lan966x_port_qos_dscp_rewr_mode_set(struct lan966x_port *port,
+					 int mode);
 
 int lan966x_mac_ip_learn(struct lan966x *lan966x,
 			 bool cpu_copy,
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
index 6887746d081f..92108d354051 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
@@ -499,6 +499,40 @@ static void lan966x_port_qos_pcp_rewr_set(struct lan966x_port *port,
 	}
 }
 
+static void lan966x_port_qos_dscp_rewr_set(struct lan966x_port *port,
+					   struct lan966x_port_qos_dscp_rewr *qos)
+{
+	u16 dscp;
+	u8 mode;
+
+	if (qos->enable)
+		mode = LAN966X_PORT_REW_DSCP_ANALIZER;
+	else
+		mode = LAN966X_PORT_REW_DSCP_FRAME;
+
+	/* Enable the rewrite otherwise will use the values from the frame */
+	lan_rmw(REW_DSCP_CFG_DSCP_REWR_CFG_SET(mode),
+		REW_DSCP_CFG_DSCP_REWR_CFG,
+		port->lan966x, REW_DSCP_CFG(port->chip_port));
+
+	/* Map each classified Qos class and DP to classified DSCP value */
+	for (int i = 0; i < ARRAY_SIZE(qos->map); i++) {
+		dscp = qos->map[i];
+
+		lan_rmw(ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL_SET(dscp),
+			ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL,
+			port->lan966x, ANA_DSCP_REWR_CFG(i));
+	}
+}
+
+void lan966x_port_qos_dscp_rewr_mode_set(struct lan966x_port *port,
+					 int mode)
+{
+	lan_rmw(ANA_QOS_CFG_DSCP_REWR_CFG_SET(mode),
+		ANA_QOS_CFG_DSCP_REWR_CFG,
+		port->lan966x, ANA_QOS_CFG(port->chip_port));
+}
+
 void lan966x_port_qos_set(struct lan966x_port *port,
 			  struct lan966x_port_qos *qos)
 {
@@ -506,6 +540,7 @@ void lan966x_port_qos_set(struct lan966x_port *port,
 	lan966x_port_qos_dscp_set(port, &qos->dscp);
 	lan966x_port_qos_default_set(port, qos);
 	lan966x_port_qos_pcp_rewr_set(port, &qos->pcp_rewr);
+	lan966x_port_qos_dscp_rewr_set(port, &qos->dscp_rewr);
 }
 
 void lan966x_port_init(struct lan966x_port *port)