diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c b/drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c
index 0ea6509436531..ed2d96d7908eb 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 16b0149ac2b5d..27f272831ea5c 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 6887746d081f6..92108d354051c 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)