Merge branch 'net-lan966x-add-support-for-pcp-dei-dscp'

Horatiu Vultur says:

====================
net: lan966x: Add support for PCP, DEI, DSCP

This patch series extends lan966x to offload to the hardware the
following features:
- PCP: this configuration is per port both at ingress and egress.
- App trust: which allows to specify a trust order of app selectors.
  This can be PCP or DSCP or DSCP/PCP.
- default priority
- DSCP: this configuration is shared between the ports both at ingress
  and egress.
====================

Link: https://lore.kernel.org/r/20230516201408.3172428-1-horatiu.vultur@microchip.comSigned-off-by: Paolo Abeni <pabeni@redhat.com>

drivers/net/ethernet/microchip/lan966x/Kconfig

@@ -10,3 +10,14 @@ config LAN966X_SWITCH
 	select VCAP
 	help
 	  This driver supports the Lan966x network switch device.
+
+config LAN966X_DCB
+	bool "Data Center Bridging (DCB) support"
+	depends on LAN966X_SWITCH && DCB
+	default y
+	help
+	  Say Y here if you want to use Data Center Bridging (DCB) in the
+	  driver. This can be used to assign priority to traffic, based on
+	  DSCP and PCP.
+
+	  If unsure, set to Y.

drivers/net/ethernet/microchip/lan966x/Makefile

@@ -15,6 +15,7 @@ lan966x-switch-objs  := lan966x_main.o lan966x_phylink.o lan966x_port.o \
 			lan966x_xdp.o lan966x_vcap_impl.o lan966x_vcap_ag_api.o \
 			lan966x_tc_flower.o lan966x_goto.o
 
+lan966x-switch-$(CONFIG_LAN966X_DCB) += lan966x_dcb.o
 lan966x-switch-$(CONFIG_DEBUG_FS) += lan966x_vcap_debugfs.o
 
 # Provide include files

drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c

+// SPDX-License-Identifier: GPL-2.0+
+
+#include "lan966x_main.h"
+
+enum lan966x_dcb_apptrust_values {
+	LAN966X_DCB_APPTRUST_EMPTY,
+	LAN966X_DCB_APPTRUST_DSCP,
+	LAN966X_DCB_APPTRUST_PCP,
+	LAN966X_DCB_APPTRUST_DSCP_PCP,
+	__LAN966X_DCB_APPTRUST_MAX
+};
+
+static const struct lan966x_dcb_apptrust {
+	u8 selectors[IEEE_8021QAZ_APP_SEL_MAX + 1];
+	int nselectors;
+} *lan966x_port_apptrust[NUM_PHYS_PORTS];
+
+static const char *lan966x_dcb_apptrust_names[__LAN966X_DCB_APPTRUST_MAX] = {
+	[LAN966X_DCB_APPTRUST_EMPTY]    = "empty",
+	[LAN966X_DCB_APPTRUST_DSCP]     = "dscp",
+	[LAN966X_DCB_APPTRUST_PCP]      = "pcp",
+	[LAN966X_DCB_APPTRUST_DSCP_PCP] = "dscp pcp"
+};
+
+/* Lan966x supported apptrust policies */
+static const struct lan966x_dcb_apptrust
+	lan966x_dcb_apptrust_policies[__LAN966X_DCB_APPTRUST_MAX] = {
+	/* Empty *must* be first */
+	[LAN966X_DCB_APPTRUST_EMPTY]    = { { 0 }, 0 },
+	[LAN966X_DCB_APPTRUST_DSCP]     = { { IEEE_8021QAZ_APP_SEL_DSCP }, 1 },
+	[LAN966X_DCB_APPTRUST_PCP]      = { { DCB_APP_SEL_PCP }, 1 },
+	[LAN966X_DCB_APPTRUST_DSCP_PCP] = { { IEEE_8021QAZ_APP_SEL_DSCP,
+					      DCB_APP_SEL_PCP }, 2 },
+};
+
+static bool lan966x_dcb_apptrust_contains(int portno, u8 selector)
+{
+	const struct lan966x_dcb_apptrust *conf = lan966x_port_apptrust[portno];
+
+	for (int i = 0; i < conf->nselectors; i++)
+		if (conf->selectors[i] == selector)
+			return true;
+
+	return false;
+}
+
+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 */
+	for (int i = 0; i < ARRAY_SIZE(qos.pcp.map); i++) {
+		app_itr.selector = DCB_APP_SEL_PCP;
+		app_itr.protocol = i;
+		qos.pcp.map[i] = dcb_getapp(dev, &app_itr);
+	}
+
+	/* Get dscp ingress mapping */
+	for (int i = 0; i < ARRAY_SIZE(qos.dscp.map); i++) {
+		app_itr.selector = IEEE_8021QAZ_APP_SEL_DSCP;
+		app_itr.protocol = i;
+		qos.dscp.map[i] = dcb_getapp(dev, &app_itr);
+	}
+
+	/* Get default prio */
+	qos.default_prio = dcb_ieee_getapp_default_prio_mask(dev);
+	if (qos.default_prio)
+		qos.default_prio = fls(qos.default_prio) - 1;
+
+	/* Get pcp rewrite mapping */
+	dcb_getrewr_prio_pcp_mask_map(dev, &pcp_rewr_map);
+	for (int i = 0; i < ARRAY_SIZE(pcp_rewr_map.map); i++) {
+		if (!pcp_rewr_map.map[i])
+			continue;
+
+		pcp_rewr = true;
+		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;
+
+		if (pcp_rewr)
+			qos.pcp_rewr.enable = true;
+	}
+
+	/* Enable use of dscp for queue classification */
+	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);
+}
+
+/* DSCP mapping is global for all ports, so set and delete app entries are
+ * replicated for each port.
+ */
+static int lan966x_dcb_ieee_dscp_setdel(struct net_device *dev,
+					struct dcb_app *app,
+					int (*setdel)(struct net_device *,
+						      struct dcb_app *))
+{
+	struct lan966x_port *port = netdev_priv(dev);
+	struct lan966x *lan966x = port->lan966x;
+	int err;
+
+	for (int i = 0; i < NUM_PHYS_PORTS; i++) {
+		port = lan966x->ports[i];
+		if (!port)
+			continue;
+
+		err = setdel(port->dev, app);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int lan966x_dcb_app_validate(struct net_device *dev,
+				    const struct dcb_app *app)
+{
+	int err = 0;
+
+	switch (app->selector) {
+	/* Default priority checks */
+	case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
+		if (app->protocol)
+			err = -EINVAL;
+		else if (app->priority >= NUM_PRIO_QUEUES)
+			err = -ERANGE;
+		break;
+	/* Dscp checks */
+	case IEEE_8021QAZ_APP_SEL_DSCP:
+		if (app->protocol >= LAN966X_PORT_QOS_DSCP_COUNT)
+			err = -EINVAL;
+		else if (app->priority >= NUM_PRIO_QUEUES)
+			err = -ERANGE;
+		break;
+	/* Pcp checks */
+	case DCB_APP_SEL_PCP:
+		if (app->protocol >= LAN966X_PORT_QOS_PCP_DEI_COUNT)
+			err = -EINVAL;
+		else if (app->priority >= NUM_PRIO_QUEUES)
+			err = -ERANGE;
+		break;
+	default:
+		err = -EINVAL;
+		break;
+	}
+
+	if (err)
+		netdev_err(dev, "Invalid entry: %d:%d\n", app->protocol,
+			   app->priority);
+
+	return err;
+}
+
+static int lan966x_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app)
+{
+	int err;
+
+	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
+		err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_delapp);
+	else
+		err = dcb_ieee_delapp(dev, app);
+
+	if (err)
+		return err;
+
+	lan966x_dcb_app_update(dev);
+
+	return 0;
+}
+
+static int lan966x_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app)
+{
+	struct dcb_app app_itr;
+	int err;
+	u8 prio;
+
+	err = lan966x_dcb_app_validate(dev, app);
+	if (err)
+		return err;
+
+	/* Delete current mapping, if it exists */
+	prio = dcb_getapp(dev, app);
+	if (prio) {
+		app_itr = *app;
+		app_itr.priority = prio;
+		lan966x_dcb_ieee_delapp(dev, &app_itr);
+	}
+
+	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
+		err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_setapp);
+	else
+		err = dcb_ieee_setapp(dev, app);
+
+	if (err)
+		return err;
+
+	lan966x_dcb_app_update(dev);
+
+	return 0;
+}
+
+static int lan966x_dcb_apptrust_validate(struct net_device *dev,
+					 u8 *selectors,
+					 int nselectors)
+{
+	for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_policies); i++) {
+		bool match;
+
+		if (lan966x_dcb_apptrust_policies[i].nselectors != nselectors)
+			continue;
+
+		match = true;
+		for (int j = 0; j < nselectors; j++) {
+			if (lan966x_dcb_apptrust_policies[i].selectors[j] !=
+			    *(selectors + j)) {
+				match = false;
+				break;
+			}
+		}
+		if (match)
+			return i;
+	}
+
+	netdev_err(dev, "Valid apptrust configurations are:\n");
+	for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_names); i++)
+		pr_info("order: %s\n", lan966x_dcb_apptrust_names[i]);
+
+	return -EOPNOTSUPP;
+}
+
+static int lan966x_dcb_setapptrust(struct net_device *dev,
+				   u8 *selectors,
+				   int nselectors)
+{
+	struct lan966x_port *port = netdev_priv(dev);
+	int idx;
+
+	idx = lan966x_dcb_apptrust_validate(dev, selectors, nselectors);
+	if (idx < 0)
+		return idx;
+
+	lan966x_port_apptrust[port->chip_port] = &lan966x_dcb_apptrust_policies[idx];
+	lan966x_dcb_app_update(dev);
+
+	return 0;
+}
+
+static int lan966x_dcb_getapptrust(struct net_device *dev, u8 *selectors,
+				   int *nselectors)
+{
+	struct lan966x_port *port = netdev_priv(dev);
+	const struct lan966x_dcb_apptrust *trust;
+
+	trust = lan966x_port_apptrust[port->chip_port];
+
+	memcpy(selectors, trust->selectors, trust->nselectors);
+	*nselectors = trust->nselectors;
+
+	return 0;
+}
+
+static int lan966x_dcb_delrewr(struct net_device *dev, struct dcb_app *app)
+{
+	int err;
+
+	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;
+
+	lan966x_dcb_app_update(dev);
+
+	return 0;
+}
+
+static int lan966x_dcb_setrewr(struct net_device *dev, struct dcb_app *app)
+{
+	struct dcb_app app_itr;
+	u16 proto;
+	int err;
+
+	err = lan966x_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;
+		lan966x_dcb_delrewr(dev, &app_itr);
+	}
+
+	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;
+
+	lan966x_dcb_app_update(dev);
+
+out:
+	return err;
+}
+
+static const struct dcbnl_rtnl_ops lan966x_dcbnl_ops = {
+	.ieee_setapp = lan966x_dcb_ieee_setapp,
+	.ieee_delapp = lan966x_dcb_ieee_delapp,
+	.dcbnl_setapptrust = lan966x_dcb_setapptrust,
+	.dcbnl_getapptrust = lan966x_dcb_getapptrust,
+	.dcbnl_setrewr = lan966x_dcb_setrewr,
+	.dcbnl_delrewr = lan966x_dcb_delrewr,
+};
+
+void lan966x_dcb_init(struct lan966x *lan966x)
+{
+	for (int p = 0; p < lan966x->num_phys_ports; ++p) {
+		struct lan966x_port *port;
+
+		port = lan966x->ports[p];
+		if (!port)
+			continue;
+
+		port->dev->dcbnl_ops = &lan966x_dcbnl_ops;
+
+		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);
+	}
+}

drivers/net/ethernet/microchip/lan966x/lan966x_main.c

@@ -1213,6 +1213,8 @@ static int lan966x_probe(struct platform_device *pdev)
 	if (err)
 		goto cleanup_fdma;
 
+	lan966x_dcb_init(lan966x);
+
 	return 0;
 
 cleanup_fdma:

drivers/net/ethernet/microchip/lan966x/lan966x_main.h

@@ -104,6 +104,22 @@
 #define LAN966X_VCAP_CID_ES0_L0 VCAP_CID_EGRESS_L0 /* ES0 lookup 0 */
 #define LAN966X_VCAP_CID_ES0_MAX (VCAP_CID_EGRESS_L1 - 1) /* ES0 Max */
 
+#define LAN966X_PORT_QOS_PCP_COUNT	8
+#define LAN966X_PORT_QOS_DEI_COUNT	8
+#define LAN966X_PORT_QOS_PCP_DEI_COUNT \
+	(LAN966X_PORT_QOS_PCP_COUNT + LAN966X_PORT_QOS_DEI_COUNT)
+
+#define LAN966X_PORT_QOS_DSCP_COUNT	64
+
+/* Port PCP rewrite mode */
+#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.
@@ -392,6 +408,34 @@ struct lan966x_port_tc {
 	struct flow_stats mirror_stat;
 };
 
+struct lan966x_port_qos_pcp {
+	u8 map[LAN966X_PORT_QOS_PCP_DEI_COUNT];
+	bool enable;
+};
+
+struct lan966x_port_qos_dscp {
+	u8 map[LAN966X_PORT_QOS_DSCP_COUNT];
+	bool enable;
+};
+
+struct lan966x_port_qos_pcp_rewr {
+	u16 map[NUM_PRIO_QUEUES];
+	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;
+};
+
 struct lan966x_port {
 	struct net_device *dev;
 	struct lan966x *lan966x;
@@ -456,6 +500,11 @@ int lan966x_port_pcs_set(struct lan966x_port *port,
 			 struct lan966x_port_config *config);
 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,
 			 const unsigned char mac[ETH_ALEN],
@@ -680,6 +729,14 @@ int lan966x_goto_port_del(struct lan966x_port *port,
 			  unsigned long goto_id,
 			  struct netlink_ext_ack *extack);
 
+#ifdef CONFIG_LAN966X_DCB
+void lan966x_dcb_init(struct lan966x *lan966x);
+#else
+static inline void lan966x_dcb_init(struct lan966x *lan966x)
+{
+}
+#endif
+
 static inline void __iomem *lan_addr(void __iomem *base[],
 				     int id, int tinst, int tcnt,
 				     int gbase, int ginst,

drivers/net/ethernet/microchip/lan966x/lan966x_port.c

@@ -394,6 +394,155 @@ int lan966x_port_pcs_set(struct lan966x_port *port,
 	return 0;
 }
 
+static void lan966x_port_qos_pcp_set(struct lan966x_port *port,
+				     struct lan966x_port_qos_pcp *qos)
+{
+	u8 *pcp_itr = qos->map;
+	u8 pcp, dp;
+
+	lan_rmw(ANA_QOS_CFG_QOS_PCP_ENA_SET(qos->enable),
+		ANA_QOS_CFG_QOS_PCP_ENA,
+		port->lan966x, ANA_QOS_CFG(port->chip_port));
+
+	/* Map PCP and DEI to priority */
+	for (int i = 0; i < ARRAY_SIZE(qos->map); i++) {
+		pcp = *(pcp_itr + i);
+		dp = (i < LAN966X_PORT_QOS_PCP_COUNT) ? 0 : 1;
+
+		lan_rmw(ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL_SET(pcp) |
+			ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL_SET(dp),
+			ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL |
+			ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL,
+			port->lan966x,
+			ANA_PCP_DEI_CFG(port->chip_port, i));
+	}
+}
+
+static void lan966x_port_qos_dscp_set(struct lan966x_port *port,
+				      struct lan966x_port_qos_dscp *qos)
+{
+	struct lan966x *lan966x = port->lan966x;
+
+	/* Enable/disable dscp for qos classification. */
+	lan_rmw(ANA_QOS_CFG_QOS_DSCP_ENA_SET(qos->enable),
+		ANA_QOS_CFG_QOS_DSCP_ENA,
+		lan966x, ANA_QOS_CFG(port->chip_port));
+
+	/* Map each dscp value to priority and dp */
+	for (int i = 0; i < ARRAY_SIZE(qos->map); i++)
+		lan_rmw(ANA_DSCP_CFG_DP_DSCP_VAL_SET(0) |
+			ANA_DSCP_CFG_QOS_DSCP_VAL_SET(*(qos->map + i)),
+			ANA_DSCP_CFG_DP_DSCP_VAL |
+			ANA_DSCP_CFG_QOS_DSCP_VAL,
+			lan966x, ANA_DSCP_CFG(i));
+
+	/* Set per-dscp trust */
+	for (int i = 0; i <  ARRAY_SIZE(qos->map); i++)
+		lan_rmw(ANA_DSCP_CFG_DSCP_TRUST_ENA_SET(qos->enable),
+			ANA_DSCP_CFG_DSCP_TRUST_ENA,
+			lan966x, ANA_DSCP_CFG(i));
+}
+
+static int lan966x_port_qos_default_set(struct lan966x_port *port,
+					struct lan966x_port_qos *qos)
+{
+	/* Set default prio and dp level */
+	lan_rmw(ANA_QOS_CFG_DP_DEFAULT_VAL_SET(0) |
+		ANA_QOS_CFG_QOS_DEFAULT_VAL_SET(qos->default_prio),
+		ANA_QOS_CFG_DP_DEFAULT_VAL |
+		ANA_QOS_CFG_QOS_DEFAULT_VAL,
+		port->lan966x, ANA_QOS_CFG(port->chip_port));
+
+	/* Set default pcp and dei for untagged frames */
+	lan_rmw(ANA_VLAN_CFG_VLAN_DEI_SET(0) |
+		ANA_VLAN_CFG_VLAN_PCP_SET(0),
+		ANA_VLAN_CFG_VLAN_DEI |
+		ANA_VLAN_CFG_VLAN_PCP,
+		port->lan966x, ANA_VLAN_CFG(port->chip_port));
+
+	return 0;
+}
+
+static void lan966x_port_qos_pcp_rewr_set(struct lan966x_port *port,
+					  struct lan966x_port_qos_pcp_rewr *qos)
+{
+	u8 mode = LAN966X_PORT_REW_TAG_CTRL_CLASSIFIED;
+	u8 pcp, dei;
+
+	if (qos->enable)
+		mode = LAN966X_PORT_REW_TAG_CTRL_MAPPED;
+
+	/* Map the values only if it is enabled otherwise will be the classified
+	 * value
+	 */
+	lan_rmw(REW_TAG_CFG_TAG_PCP_CFG_SET(mode) |
+		REW_TAG_CFG_TAG_DEI_CFG_SET(mode),
+		REW_TAG_CFG_TAG_PCP_CFG |
+		REW_TAG_CFG_TAG_DEI_CFG,
+		port->lan966x, REW_TAG_CFG(port->chip_port));
+
+	/* Map each value to pcp and dei */
+	for (int i = 0; i < ARRAY_SIZE(qos->map); i++) {
+		pcp = qos->map[i];
+		if (pcp > LAN966X_PORT_QOS_PCP_COUNT)
+			dei = 1;
+		else
+			dei = 0;
+
+		lan_rmw(REW_PCP_DEI_CFG_DEI_QOS_VAL_SET(dei) |
+			REW_PCP_DEI_CFG_PCP_QOS_VAL_SET(pcp),
+			REW_PCP_DEI_CFG_DEI_QOS_VAL |
+			REW_PCP_DEI_CFG_PCP_QOS_VAL,
+			port->lan966x,
+			REW_PCP_DEI_CFG(port->chip_port,
+					i + dei * LAN966X_PORT_QOS_PCP_COUNT));
+	}
+}
+
+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)
+{
+	lan966x_port_qos_pcp_set(port, &qos->pcp);
+	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)
 {
 	struct lan966x_port_config *config = &port->config;

drivers/net/ethernet/microchip/lan966x/lan966x_regs.h

@@ -283,6 +283,18 @@ enum lan966x_target {
 #define ANA_VLAN_CFG_VLAN_POP_CNT_GET(x)\
 	FIELD_GET(ANA_VLAN_CFG_VLAN_POP_CNT, x)
 
+#define ANA_VLAN_CFG_VLAN_PCP                    GENMASK(15, 13)
+#define ANA_VLAN_CFG_VLAN_PCP_SET(x)\
+	FIELD_PREP(ANA_VLAN_CFG_VLAN_PCP, x)
+#define ANA_VLAN_CFG_VLAN_PCP_GET(x)\
+	FIELD_GET(ANA_VLAN_CFG_VLAN_PCP, x)
+
+#define ANA_VLAN_CFG_VLAN_DEI                    BIT(12)
+#define ANA_VLAN_CFG_VLAN_DEI_SET(x)\
+	FIELD_PREP(ANA_VLAN_CFG_VLAN_DEI, x)
+#define ANA_VLAN_CFG_VLAN_DEI_GET(x)\
+	FIELD_GET(ANA_VLAN_CFG_VLAN_DEI, x)
+
 #define ANA_VLAN_CFG_VLAN_VID                    GENMASK(11, 0)
 #define ANA_VLAN_CFG_VLAN_VID_SET(x)\
 	FIELD_PREP(ANA_VLAN_CFG_VLAN_VID, x)
@@ -316,6 +328,39 @@ enum lan966x_target {
 #define ANA_DROP_CFG_DROP_MC_SMAC_ENA_GET(x)\
 	FIELD_GET(ANA_DROP_CFG_DROP_MC_SMAC_ENA, x)
 
+/*      ANA:PORT:QOS_CFG */
+#define ANA_QOS_CFG(g)            __REG(TARGET_ANA, 0, 1, 28672, g, 9, 128, 8, 0, 1, 4)
+
+#define ANA_QOS_CFG_DP_DEFAULT_VAL               BIT(8)
+#define ANA_QOS_CFG_DP_DEFAULT_VAL_SET(x)\
+	FIELD_PREP(ANA_QOS_CFG_DP_DEFAULT_VAL, x)
+#define ANA_QOS_CFG_DP_DEFAULT_VAL_GET(x)\
+	FIELD_GET(ANA_QOS_CFG_DP_DEFAULT_VAL, x)
+
+#define ANA_QOS_CFG_QOS_DEFAULT_VAL              GENMASK(7, 5)
+#define ANA_QOS_CFG_QOS_DEFAULT_VAL_SET(x)\
+	FIELD_PREP(ANA_QOS_CFG_QOS_DEFAULT_VAL, x)
+#define ANA_QOS_CFG_QOS_DEFAULT_VAL_GET(x)\
+	FIELD_GET(ANA_QOS_CFG_QOS_DEFAULT_VAL, x)
+
+#define ANA_QOS_CFG_QOS_DSCP_ENA                 BIT(4)
+#define ANA_QOS_CFG_QOS_DSCP_ENA_SET(x)\
+	FIELD_PREP(ANA_QOS_CFG_QOS_DSCP_ENA, x)
+#define ANA_QOS_CFG_QOS_DSCP_ENA_GET(x)\
+	FIELD_GET(ANA_QOS_CFG_QOS_DSCP_ENA, x)
+
+#define ANA_QOS_CFG_QOS_PCP_ENA                  BIT(3)
+#define ANA_QOS_CFG_QOS_PCP_ENA_SET(x)\
+	FIELD_PREP(ANA_QOS_CFG_QOS_PCP_ENA, x)
+#define ANA_QOS_CFG_QOS_PCP_ENA_GET(x)\
+	FIELD_GET(ANA_QOS_CFG_QOS_PCP_ENA, x)
+
+#define ANA_QOS_CFG_DSCP_REWR_CFG                GENMASK(1, 0)
+#define ANA_QOS_CFG_DSCP_REWR_CFG_SET(x)\
+	FIELD_PREP(ANA_QOS_CFG_DSCP_REWR_CFG, x)
+#define ANA_QOS_CFG_DSCP_REWR_CFG_GET(x)\
+	FIELD_GET(ANA_QOS_CFG_DSCP_REWR_CFG, x)
+
 /*      ANA:PORT:VCAP_CFG */
 #define ANA_VCAP_CFG(g)           __REG(TARGET_ANA, 0, 1, 28672, g, 9, 128, 12, 0, 1, 4)
 
@@ -415,6 +460,21 @@ enum lan966x_target {
 #define ANA_VCAP_S2_CFG_OAM_DIS_GET(x)\
 	FIELD_GET(ANA_VCAP_S2_CFG_OAM_DIS, x)
 
+/*      ANA:PORT:QOS_PCP_DEI_MAP_CFG */
+#define ANA_PCP_DEI_CFG(g, r)     __REG(TARGET_ANA, 0, 1, 28672, g, 9, 128, 32, r, 16, 4)
+
+#define ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL           BIT(3)
+#define ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL_SET(x)\
+	FIELD_PREP(ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL, x)
+#define ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL_GET(x)\
+	FIELD_GET(ANA_PCP_DEI_CFG_DP_PCP_DEI_VAL, x)
+
+#define ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL          GENMASK(2, 0)
+#define ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL_SET(x)\
+	FIELD_PREP(ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL, x)
+#define ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL_GET(x)\
+	FIELD_GET(ANA_PCP_DEI_CFG_QOS_PCP_DEI_VAL, x)
+
 /*      ANA:PORT:CPU_FWD_CFG */
 #define ANA_CPU_FWD_CFG(g)        __REG(TARGET_ANA, 0, 1, 28672, g, 9, 128, 96, 0, 1, 4)
 
@@ -478,6 +538,15 @@ enum lan966x_target {
 #define ANA_PORT_CFG_PORTID_VAL_GET(x)\
 	FIELD_GET(ANA_PORT_CFG_PORTID_VAL, x)
 
+/*      ANA:COMMON:DSCP_REWR_CFG */
+#define ANA_DSCP_REWR_CFG(r)      __REG(TARGET_ANA, 0, 1, 31232, 0, 1, 552, 332, r, 16, 4)
+
+#define ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL      GENMASK(5, 0)
+#define ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL_SET(x)\
+	FIELD_PREP(ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL, x)
+#define ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL_GET(x)\
+	FIELD_GET(ANA_DSCP_REWR_CFG_DSCP_QOS_REWR_VAL, x)
+
 /*      ANA:PORT:POL_CFG */
 #define ANA_POL_CFG(g)            __REG(TARGET_ANA, 0, 1, 28672, g, 9, 128, 116, 0, 1, 4)
 
@@ -547,6 +616,33 @@ enum lan966x_target {
 #define ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA_GET(x)\
 	FIELD_GET(ANA_AGGR_CFG_AC_IP4_TCPUDP_ENA, x)
 
+/*      ANA:COMMON:DSCP_CFG */
+#define ANA_DSCP_CFG(r)           __REG(TARGET_ANA, 0, 1, 31232, 0, 1, 552, 76, r, 64, 4)
+
+#define ANA_DSCP_CFG_DP_DSCP_VAL                 BIT(11)
+#define ANA_DSCP_CFG_DP_DSCP_VAL_SET(x)\
+	FIELD_PREP(ANA_DSCP_CFG_DP_DSCP_VAL, x)
+#define ANA_DSCP_CFG_DP_DSCP_VAL_GET(x)\
+	FIELD_GET(ANA_DSCP_CFG_DP_DSCP_VAL, x)
+
+#define ANA_DSCP_CFG_QOS_DSCP_VAL                GENMASK(10, 8)
+#define ANA_DSCP_CFG_QOS_DSCP_VAL_SET(x)\
+	FIELD_PREP(ANA_DSCP_CFG_QOS_DSCP_VAL, x)
+#define ANA_DSCP_CFG_QOS_DSCP_VAL_GET(x)\
+	FIELD_GET(ANA_DSCP_CFG_QOS_DSCP_VAL, x)
+
+#define ANA_DSCP_CFG_DSCP_TRUST_ENA              BIT(1)
+#define ANA_DSCP_CFG_DSCP_TRUST_ENA_SET(x)\
+	FIELD_PREP(ANA_DSCP_CFG_DSCP_TRUST_ENA, x)
+#define ANA_DSCP_CFG_DSCP_TRUST_ENA_GET(x)\
+	FIELD_GET(ANA_DSCP_CFG_DSCP_TRUST_ENA, x)
+
+#define ANA_DSCP_CFG_DSCP_REWR_ENA               BIT(0)
+#define ANA_DSCP_CFG_DSCP_REWR_ENA_SET(x)\
+	FIELD_PREP(ANA_DSCP_CFG_DSCP_REWR_ENA, x)
+#define ANA_DSCP_CFG_DSCP_REWR_ENA_GET(x)\
+	FIELD_GET(ANA_DSCP_CFG_DSCP_REWR_ENA, x)
+
 /*      ANA:POL:POL_PIR_CFG */
 #define ANA_POL_PIR_CFG(g)        __REG(TARGET_ANA, 0, 1, 16384, g, 345, 32, 0, 0, 1, 4)
 
@@ -1468,6 +1564,18 @@ enum lan966x_target {
 #define REW_TAG_CFG_TAG_TPID_CFG_GET(x)\
 	FIELD_GET(REW_TAG_CFG_TAG_TPID_CFG, x)
 
+#define REW_TAG_CFG_TAG_PCP_CFG                  GENMASK(3, 2)
+#define REW_TAG_CFG_TAG_PCP_CFG_SET(x)\
+	FIELD_PREP(REW_TAG_CFG_TAG_PCP_CFG, x)
+#define REW_TAG_CFG_TAG_PCP_CFG_GET(x)\
+	FIELD_GET(REW_TAG_CFG_TAG_PCP_CFG, x)
+
+#define REW_TAG_CFG_TAG_DEI_CFG                  GENMASK(1, 0)
+#define REW_TAG_CFG_TAG_DEI_CFG_SET(x)\
+	FIELD_PREP(REW_TAG_CFG_TAG_DEI_CFG, x)
+#define REW_TAG_CFG_TAG_DEI_CFG_GET(x)\
+	FIELD_GET(REW_TAG_CFG_TAG_DEI_CFG, x)
+
 /*      REW:PORT:PORT_CFG */
 #define REW_PORT_CFG(g)           __REG(TARGET_REW, 0, 1, 0, g, 10, 128, 8, 0, 1, 4)
 
@@ -1483,6 +1591,30 @@ enum lan966x_target {
 #define REW_PORT_CFG_NO_REWRITE_GET(x)\
 	FIELD_GET(REW_PORT_CFG_NO_REWRITE, x)
 
+/*      REW:PORT:DSCP_CFG */
+#define REW_DSCP_CFG(g)           __REG(TARGET_REW, 0, 1, 0, g, 10, 128, 12, 0, 1, 4)
+
+#define REW_DSCP_CFG_DSCP_REWR_CFG               GENMASK(1, 0)
+#define REW_DSCP_CFG_DSCP_REWR_CFG_SET(x)\
+	FIELD_PREP(REW_DSCP_CFG_DSCP_REWR_CFG, x)
+#define REW_DSCP_CFG_DSCP_REWR_CFG_GET(x)\
+	FIELD_GET(REW_DSCP_CFG_DSCP_REWR_CFG, x)
+
+/*      REW:PORT:PCP_DEI_QOS_MAP_CFG */
+#define REW_PCP_DEI_CFG(g, r)     __REG(TARGET_REW, 0, 1, 0, g, 10, 128, 16, r, 16, 4)
+
+#define REW_PCP_DEI_CFG_DEI_QOS_VAL              BIT(3)
+#define REW_PCP_DEI_CFG_DEI_QOS_VAL_SET(x)\
+	FIELD_PREP(REW_PCP_DEI_CFG_DEI_QOS_VAL, x)
+#define REW_PCP_DEI_CFG_DEI_QOS_VAL_GET(x)\
+	FIELD_GET(REW_PCP_DEI_CFG_DEI_QOS_VAL, x)
+
+#define REW_PCP_DEI_CFG_PCP_QOS_VAL              GENMASK(2, 0)
+#define REW_PCP_DEI_CFG_PCP_QOS_VAL_SET(x)\
+	FIELD_PREP(REW_PCP_DEI_CFG_PCP_QOS_VAL, x)
+#define REW_PCP_DEI_CFG_PCP_QOS_VAL_GET(x)\
+	FIELD_GET(REW_PCP_DEI_CFG_PCP_QOS_VAL, x)
+
 /*      REW:COMMON:STAT_CFG */
 #define REW_STAT_CFG              __REG(TARGET_REW, 0, 1, 3072, 0, 1, 528, 520, 0, 1, 4)