1 files changed, 109 insertions, 12 deletions

diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_dcb.c b/drivers/net/ethernet/microchip/sparx5/sparx5_dcb.c
index 74abb946b2a3..871a3e62f852 100644
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_dcb.c
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_dcb.c
@@ -133,12 +133,17 @@ static bool sparx5_dcb_apptrust_contains(int portno, u8 selector)
 
 static int sparx5_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 sparx5_port *port = netdev_priv(dev);
 	struct sparx5_port_qos_dscp_map *dscp_map;
 	struct sparx5_port_qos_pcp_map *pcp_map;
 	struct sparx5_port_qos qos = {0};
 	struct dcb_app app_itr = {0};
 	int portno = port->portno;
+	bool dscp_rewr = false;
+	bool pcp_rewr = false;
+	u16 dscp;
 	int i;
 
 	dscp_map = &qos.dscp.map;
@@ -163,31 +168,72 @@ static int sparx5_dcb_app_update(struct net_device *dev)
 		pcp_map->map[i] = dcb_getapp(dev, &app_itr);
 	}
 
+	/* Get pcp rewrite mapping */
+	dcb_getrewr_prio_pcp_mask_map(dev, &pcp_rewr_map);
+	for (i = 0; i < ARRAY_SIZE(pcp_rewr_map.map); i++) {
+		if (!pcp_rewr_map.map[i])
+			continue;
+		pcp_rewr = true;
+		qos.pcp_rewr.map.map[i] = fls(pcp_rewr_map.map[i]) - 1;
+	}
+
+	/* Get dscp rewrite mapping */
+	dcb_getrewr_prio_dscp_mask_map(dev, &dscp_rewr_map);
+	for (i = 0; i < ARRAY_SIZE(dscp_rewr_map.map); i++) {
+		if (!dscp_rewr_map.map[i])
+			continue;
+
+		/* The rewrite table of the switch has 32 entries; one for each
+		 * priority for each DP level. Currently, the rewrite map does
+		 * not indicate DP level, so we map classified QoS class to
+		 * classified DSCP, for each classified DP level. Rewrite of
+		 * DSCP is only enabled, if we have active mappings.
+		 */
+		dscp_rewr = true;
+		dscp = fls64(dscp_rewr_map.map[i]) - 1;
+		qos.dscp_rewr.map.map[i] = dscp;      /* DP 0 */
+		qos.dscp_rewr.map.map[i + 8] = dscp;  /* DP 1 */
+		qos.dscp_rewr.map.map[i + 16] = dscp; /* DP 2 */
+		qos.dscp_rewr.map.map[i + 24] = dscp; /* DP 3 */
+	}
+
 	/* Enable use of pcp for queue classification ? */
 	if (sparx5_dcb_apptrust_contains(portno, DCB_APP_SEL_PCP)) {
 		qos.pcp.qos_enable = true;
 		qos.pcp.dp_enable = qos.pcp.qos_enable;
+		/* Enable rewrite of PCP and DEI if PCP is trusted *and* rewrite
+		 * table is not empty.
+		 */
+		if (pcp_rewr)
+			qos.pcp_rewr.enable = true;
 	}
 
 	/* Enable use of dscp for queue classification ? */
 	if (sparx5_dcb_apptrust_contains(portno, IEEE_8021QAZ_APP_SEL_DSCP)) {
 		qos.dscp.qos_enable = true;
 		qos.dscp.dp_enable = qos.dscp.qos_enable;
+		if (dscp_rewr)
+			/* Do not enable rewrite if no mappings are active, as
+			 * classified DSCP will then be zero for all classified
+			 * QoS class and DP combinations.
+			 */
+			qos.dscp_rewr.enable = true;
 	}
 
 	return sparx5_port_qos_set(port, &qos);
 }
 
-/* Set or delete dscp app entry.
+/* Set or delete DSCP app entry.
  *
- * Dscp mapping is global for all ports, so set and delete app entries are
+ * DSCP mapping is global for all ports, so set and delete app entries are
  * replicated for each port.
  */
-static int sparx5_dcb_ieee_dscp_setdel_app(struct net_device *dev,
-					   struct dcb_app *app, bool del)
+static int sparx5_dcb_ieee_dscp_setdel(struct net_device *dev,
+				       struct dcb_app *app,
+				       int (*setdel)(struct net_device *,
+						     struct dcb_app *))
 {
 	struct sparx5_port *port = netdev_priv(dev);
-	struct dcb_app apps[SPX5_PORTS];
 	struct sparx5_port *port_itr;
 	int err, i;
 
@@ -195,11 +241,7 @@ static int sparx5_dcb_ieee_dscp_setdel_app(struct net_device *dev,
 		port_itr = port->sparx5->ports[i];
 		if (!port_itr)
 			continue;
-		memcpy(&apps[i], app, sizeof(struct dcb_app));
-		if (del)
-			err = dcb_ieee_delapp(port_itr->ndev, &apps[i]);
-		else
-			err = dcb_ieee_setapp(port_itr->ndev, &apps[i]);
+		err = setdel(port_itr->ndev, app);
 		if (err)
 			return err;
 	}
@@ -226,7 +268,7 @@ static int sparx5_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app)
 	}
 
 	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
-		err = sparx5_dcb_ieee_dscp_setdel_app(dev, app, false);
+		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_setapp);
 	else
 		err = dcb_ieee_setapp(dev, app);
 
@@ -244,7 +286,7 @@ static int sparx5_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app)
 	int err;
 
 	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
-		err = sparx5_dcb_ieee_dscp_setdel_app(dev, app, true);
+		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_delapp);
 	else
 		err = dcb_ieee_delapp(dev, app);
 
@@ -283,11 +325,60 @@ static int sparx5_dcb_getapptrust(struct net_device *dev, u8 *selectors,
 	return 0;
 }
 
+static int sparx5_dcb_delrewr(struct net_device *dev, struct dcb_app *app)
+{
+	int err;
+
+	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
+		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_delrewr);
+	else
+		err = dcb_delrewr(dev, app);
+
+	if (err < 0)
+		return err;
+
+	return sparx5_dcb_app_update(dev);
+}
+
+static int sparx5_dcb_setrewr(struct net_device *dev, struct dcb_app *app)
+{
+	struct dcb_app app_itr;
+	int err = 0;
+	u16 proto;
+
+	err = sparx5_dcb_app_validate(dev, app);
+	if (err)
+		goto out;
+
+	/* Delete current mapping, if it exists. */
+	proto = dcb_getrewr(dev, app);
+	if (proto) {
+		app_itr = *app;
+		app_itr.protocol = proto;
+		sparx5_dcb_delrewr(dev, &app_itr);
+	}
+
+	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
+		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_setrewr);
+	else
+		err = dcb_setrewr(dev, app);
+
+	if (err)
+		goto out;
+
+	sparx5_dcb_app_update(dev);
+
+out:
+	return err;
+}
+
 const struct dcbnl_rtnl_ops sparx5_dcbnl_ops = {
 	.ieee_setapp = sparx5_dcb_ieee_setapp,
 	.ieee_delapp = sparx5_dcb_ieee_delapp,
 	.dcbnl_setapptrust = sparx5_dcb_setapptrust,
 	.dcbnl_getapptrust = sparx5_dcb_getapptrust,
+	.dcbnl_setrewr = sparx5_dcb_setrewr,
+	.dcbnl_delrewr = sparx5_dcb_delrewr,
 };
 
 int sparx5_dcb_init(struct sparx5 *sparx5)
@@ -304,6 +395,12 @@ int sparx5_dcb_init(struct sparx5 *sparx5)
 		sparx5_port_apptrust[port->portno] =
 			&sparx5_dcb_apptrust_policies
 				[SPARX5_DCB_APPTRUST_DSCP_PCP];
+
+		/* Enable DSCP classification based on classified QoS class and
+		 * DP, for all DSCP values, for all ports.
+		 */
+		sparx5_port_qos_dscp_rewr_mode_set(port,
+						   SPARX5_PORT_REW_DSCP_ALL);
 	}
 
 	return 0;