diff mbox series

[net-next,2/7] net: lan966x: Add support for offloading pcp table

Message ID 20230514201029.1867738-3-horatiu.vultur@microchip.com (mailing list archive)
State Superseded
Delegated to: Netdev Maintainers
Headers show
Series net: lan966x: Add support for PCP, DEI, DSCP | expand

Checks

Context Check Description
netdev/series_format success Posting correctly formatted
netdev/tree_selection success Clearly marked for net-next
netdev/fixes_present success Fixes tag not required for -next series
netdev/header_inline success No static functions without inline keyword in header files
netdev/build_32bit success Errors and warnings before: 8 this patch: 8
netdev/cc_maintainers success CCed 7 of 7 maintainers
netdev/build_clang success Errors and warnings before: 8 this patch: 8
netdev/verify_signedoff success Signed-off-by tag matches author and committer
netdev/deprecated_api success None detected
netdev/check_selftest success No net selftest shell script
netdev/verify_fixes success No Fixes tag
netdev/build_allmodconfig_warn success Errors and warnings before: 8 this patch: 8
netdev/checkpatch warning WARNING: added, moved or deleted file(s), does MAINTAINERS need updating?
netdev/kdoc success Errors and warnings before: 0 this patch: 0
netdev/source_inline success Was 0 now: 0

Commit Message

Horatiu Vultur May 14, 2023, 8:10 p.m. UTC 
Add support for offloading pcp app entries. Lan966x has 8 priority
queues per port and for each priority it also has a drop precedence.

Signed-off-by: Horatiu Vultur <horatiu.vultur@microchip.com>
---
 .../net/ethernet/microchip/lan966x/Kconfig    |  11 ++
 .../net/ethernet/microchip/lan966x/Makefile   |   1 +
 .../ethernet/microchip/lan966x/lan966x_dcb.c  | 104 ++++++++++++++++++
 .../ethernet/microchip/lan966x/lan966x_main.c |   2 +
 .../ethernet/microchip/lan966x/lan966x_main.h |  25 +++++
 .../ethernet/microchip/lan966x/lan966x_port.c |  30 +++++
 6 files changed, 173 insertions(+)
 create mode 100644 drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c

Comments

Piotr Raczynski May 15, 2023, 10:16 a.m. UTC | #1 
On Sun, May 14, 2023 at 10:10:24PM +0200, Horatiu Vultur wrote:
> Add support for offloading pcp app entries. Lan966x has 8 priority
> queues per port and for each priority it also has a drop precedence.
> 
> Signed-off-by: Horatiu Vultur <horatiu.vultur@microchip.com>
> ---
>  .../net/ethernet/microchip/lan966x/Kconfig    |  11 ++
>  .../net/ethernet/microchip/lan966x/Makefile   |   1 +
>  .../ethernet/microchip/lan966x/lan966x_dcb.c  | 104 ++++++++++++++++++
>  .../ethernet/microchip/lan966x/lan966x_main.c |   2 +
>  .../ethernet/microchip/lan966x/lan966x_main.h |  25 +++++
>  .../ethernet/microchip/lan966x/lan966x_port.c |  30 +++++
>  6 files changed, 173 insertions(+)
>  create mode 100644 drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
> 
> diff --git a/drivers/net/ethernet/microchip/lan966x/Kconfig b/drivers/net/ethernet/microchip/lan966x/Kconfig
> index 571e6d4da1e9d..f9ebffc04eb85 100644
> --- a/drivers/net/ethernet/microchip/lan966x/Kconfig
> +++ b/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.
> diff --git a/drivers/net/ethernet/microchip/lan966x/Makefile b/drivers/net/ethernet/microchip/lan966x/Makefile
> index 7b0cda4ffa6b5..3b6ac331691d0 100644
> --- a/drivers/net/ethernet/microchip/lan966x/Makefile
> +++ b/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
> diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c b/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
> new file mode 100644
> index 0000000000000..8ec64336abd5f
> --- /dev/null
> +++ b/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
> @@ -0,0 +1,104 @@
> +// SPDX-License-Identifier: GPL-2.0+
> +
> +#include "lan966x_main.h"
> +
> +static void lan966x_dcb_app_update(struct net_device *dev, bool enable)
> +{
> +	struct lan966x_port *port = netdev_priv(dev);
> +	struct lan966x_port_qos qos = {0};
> +	struct dcb_app app_itr;
> +
> +	/* 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);
> +	}
> +
> +	qos.pcp.enable = enable;
> +	lan966x_port_qos_set(port, &qos);
> +}
> +
> +static int lan966x_dcb_app_validate(struct net_device *dev,
> +				    const struct dcb_app *app)
> +{
> +	int err = 0;
> +
> +	switch (app->selector) {
> +	/* 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;
> +
> +	err = dcb_ieee_delapp(dev, app);
> +	if (err < 0)
> +		return err;
> +
> +	lan966x_dcb_app_update(dev, false);
> +
> +	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)
> +		goto out;
> +
> +	/* Delete current mapping, if it exists */
> +	prio = dcb_getapp(dev, app);
> +	if (prio) {
> +		app_itr = *app;
> +		app_itr .priority = prio;
Compiles OK, still looks little weird :).

> +		dcb_ieee_delapp(dev, &app_itr);
> +	}
> +
> +	err = dcb_ieee_setapp(dev, app);
> +	if (err)
> +		goto out;
> +
> +	lan966x_dcb_app_update(dev, true);
> +
> +out:
> +	return err;
> +}
> +
> +static const struct dcbnl_rtnl_ops lan966x_dcbnl_ops = {
> +	.ieee_setapp = lan966x_dcb_ieee_setapp,
> +	.ieee_delapp = lan966x_dcb_ieee_delapp,
> +};
> +
> +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;
> +	}
> +}
> diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_main.c b/drivers/net/ethernet/microchip/lan966x/lan966x_main.c
> index ee2698698d71a..f6931dfb3e68e 100644
> --- a/drivers/net/ethernet/microchip/lan966x/lan966x_main.c
> +++ b/drivers/net/ethernet/microchip/lan966x/lan966x_main.c
> @@ -1223,6 +1223,8 @@ static int lan966x_probe(struct platform_device *pdev)
>  	if (err)
>  		goto cleanup_fdma;
>  
> +	lan966x_dcb_init(lan966x);
> +
>  	return 0;
>  
>  cleanup_fdma:
> diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
> index 882d5a08e7d51..b9ca47ab6e8be 100644
> --- a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
> +++ b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
> @@ -104,6 +104,11 @@
>  #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)
> +
>  /* MAC table entry types.
>   * ENTRYTYPE_NORMAL is subject to aging.
>   * ENTRYTYPE_LOCKED is not subject to aging.
> @@ -392,6 +397,15 @@ 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 {
> +	struct lan966x_port_qos_pcp pcp;
> +};
> +
>  struct lan966x_port {
>  	struct net_device *dev;
>  	struct lan966x *lan966x;
> @@ -456,6 +470,9 @@ 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);
> +
>  int lan966x_mac_ip_learn(struct lan966x *lan966x,
>  			 bool cpu_copy,
>  			 const unsigned char mac[ETH_ALEN],
> @@ -680,6 +697,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,
> diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
> index 0050fcb988b75..0cee8127c48eb 100644
> --- a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
> +++ b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
> @@ -394,6 +394,36 @@ 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));
> +	}
> +}
> +
> +void lan966x_port_qos_set(struct lan966x_port *port,
> +			  struct lan966x_port_qos *qos)
> +{
> +	lan966x_port_qos_pcp_set(port, &qos->pcp);
> +}
> +
>  void lan966x_port_init(struct lan966x_port *port)
>  {
>  	struct lan966x_port_config *config = &port->config;
> -- 
> 2.38.0
> 
>
Piotr Raczynski May 15, 2023, 10:24 a.m. UTC | #2 
On Mon, May 15, 2023 at 12:16:49PM +0200, Piotr Raczynski wrote:

[...]

> > +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)
> > +		goto out;
> > +
> > +	/* Delete current mapping, if it exists */
> > +	prio = dcb_getapp(dev, app);
> > +	if (prio) {
> > +		app_itr = *app;
> > +		app_itr .priority = prio;
> Compiles OK, still looks little weird :).
> 

Other than that, looks OK.
Reviewed-by: Piotr Raczynski <piotr.raczynski@intel.com>

[...]
Paolo Abeni May 16, 2023, 7:07 a.m. UTC | #3 
On Mon, 2023-05-15 at 12:16 +0200, Piotr Raczynski wrote:
> On Sun, May 14, 2023 at 10:10:24PM +0200, Horatiu Vultur wrote:

[...]

> > +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)
> > +		goto out;
> > +
> > +	/* Delete current mapping, if it exists */
> > +	prio = dcb_getapp(dev, app);
> > +	if (prio) {
> > +		app_itr = *app;
> > +		app_itr .priority = prio;
> Compiles OK, still looks little weird :).

Since it looks like a v2 is required to update later patches, please
additionally remove the unneeded whitespace above:

		app_itr.priority = prio;

Thanks!

Paolo
diff mbox series

Patch

diff --git a/drivers/net/ethernet/microchip/lan966x/Kconfig b/drivers/net/ethernet/microchip/lan966x/Kconfig
index 571e6d4da1e9d..f9ebffc04eb85 100644
--- a/drivers/net/ethernet/microchip/lan966x/Kconfig
+++ b/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.
diff --git a/drivers/net/ethernet/microchip/lan966x/Makefile b/drivers/net/ethernet/microchip/lan966x/Makefile
index 7b0cda4ffa6b5..3b6ac331691d0 100644
--- a/drivers/net/ethernet/microchip/lan966x/Makefile
+++ b/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
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c b/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
new file mode 100644
index 0000000000000..8ec64336abd5f
--- /dev/null
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
@@ -0,0 +1,104 @@ 
+// SPDX-License-Identifier: GPL-2.0+
+
+#include "lan966x_main.h"
+
+static void lan966x_dcb_app_update(struct net_device *dev, bool enable)
+{
+	struct lan966x_port *port = netdev_priv(dev);
+	struct lan966x_port_qos qos = {0};
+	struct dcb_app app_itr;
+
+	/* 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);
+	}
+
+	qos.pcp.enable = enable;
+	lan966x_port_qos_set(port, &qos);
+}
+
+static int lan966x_dcb_app_validate(struct net_device *dev,
+				    const struct dcb_app *app)
+{
+	int err = 0;
+
+	switch (app->selector) {
+	/* 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;
+
+	err = dcb_ieee_delapp(dev, app);
+	if (err < 0)
+		return err;
+
+	lan966x_dcb_app_update(dev, false);
+
+	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)
+		goto out;
+
+	/* Delete current mapping, if it exists */
+	prio = dcb_getapp(dev, app);
+	if (prio) {
+		app_itr = *app;
+		app_itr .priority = prio;
+		dcb_ieee_delapp(dev, &app_itr);
+	}
+
+	err = dcb_ieee_setapp(dev, app);
+	if (err)
+		goto out;
+
+	lan966x_dcb_app_update(dev, true);
+
+out:
+	return err;
+}
+
+static const struct dcbnl_rtnl_ops lan966x_dcbnl_ops = {
+	.ieee_setapp = lan966x_dcb_ieee_setapp,
+	.ieee_delapp = lan966x_dcb_ieee_delapp,
+};
+
+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;
+	}
+}
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_main.c b/drivers/net/ethernet/microchip/lan966x/lan966x_main.c
index ee2698698d71a..f6931dfb3e68e 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_main.c
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_main.c
@@ -1223,6 +1223,8 @@  static int lan966x_probe(struct platform_device *pdev)
 	if (err)
 		goto cleanup_fdma;
 
+	lan966x_dcb_init(lan966x);
+
 	return 0;
 
 cleanup_fdma:
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
index 882d5a08e7d51..b9ca47ab6e8be 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
@@ -104,6 +104,11 @@ 
 #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)
+
 /* MAC table entry types.
  * ENTRYTYPE_NORMAL is subject to aging.
  * ENTRYTYPE_LOCKED is not subject to aging.
@@ -392,6 +397,15 @@  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 {
+	struct lan966x_port_qos_pcp pcp;
+};
+
 struct lan966x_port {
 	struct net_device *dev;
 	struct lan966x *lan966x;
@@ -456,6 +470,9 @@  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);
+
 int lan966x_mac_ip_learn(struct lan966x *lan966x,
 			 bool cpu_copy,
 			 const unsigned char mac[ETH_ALEN],
@@ -680,6 +697,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,
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
index 0050fcb988b75..0cee8127c48eb 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
@@ -394,6 +394,36 @@  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));
+	}
+}
+
+void lan966x_port_qos_set(struct lan966x_port *port,
+			  struct lan966x_port_qos *qos)
+{
+	lan966x_port_qos_pcp_set(port, &qos->pcp);
+}
+
 void lan966x_port_init(struct lan966x_port *port)
 {
 	struct lan966x_port_config *config = &port->config;