diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c index 35b436a491e1..13d6b178777c 100644 --- a/drivers/net/dsa/ocelot/felix.c +++ b/drivers/net/dsa/ocelot/felix.c @@ -1799,6 +1799,44 @@ felix_mrp_del_ring_role(struct dsa_switch *ds, int port, return ocelot_mrp_del_ring_role(ocelot, port, mrp); } +static int felix_port_get_default_prio(struct dsa_switch *ds, int port) +{ + struct ocelot *ocelot = ds->priv; + + return ocelot_port_get_default_prio(ocelot, port); +} + +static int felix_port_set_default_prio(struct dsa_switch *ds, int port, + u8 prio) +{ + struct ocelot *ocelot = ds->priv; + + return ocelot_port_set_default_prio(ocelot, port, prio); +} + +static int felix_port_get_dscp_prio(struct dsa_switch *ds, int port, u8 dscp) +{ + struct ocelot *ocelot = ds->priv; + + return ocelot_port_get_dscp_prio(ocelot, port, dscp); +} + +static int felix_port_add_dscp_prio(struct dsa_switch *ds, int port, u8 dscp, + u8 prio) +{ + struct ocelot *ocelot = ds->priv; + + return ocelot_port_add_dscp_prio(ocelot, port, dscp, prio); +} + +static int felix_port_del_dscp_prio(struct dsa_switch *ds, int port, u8 dscp, + u8 prio) +{ + struct ocelot *ocelot = ds->priv; + + return ocelot_port_del_dscp_prio(ocelot, port, dscp, prio); +} + const struct dsa_switch_ops felix_switch_ops = { .get_tag_protocol = felix_get_tag_protocol, .change_tag_protocol = felix_change_tag_protocol, @@ -1862,6 +1900,11 @@ const struct dsa_switch_ops felix_switch_ops = { .port_mrp_del_ring_role = felix_mrp_del_ring_role, .tag_8021q_vlan_add = felix_tag_8021q_vlan_add, .tag_8021q_vlan_del = felix_tag_8021q_vlan_del, + .port_get_default_prio = felix_port_get_default_prio, + .port_set_default_prio = felix_port_set_default_prio, + .port_get_dscp_prio = felix_port_get_dscp_prio, + .port_add_dscp_prio = felix_port_add_dscp_prio, + .port_del_dscp_prio = felix_port_del_dscp_prio, }; struct net_device *felix_port_to_netdev(struct ocelot *ocelot, int port) diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c index 21134125a6e4..41dbb1e326c4 100644 --- a/drivers/net/ethernet/mscc/ocelot.c +++ b/drivers/net/ethernet/mscc/ocelot.c @@ -2907,6 +2907,122 @@ void ocelot_port_bridge_flags(struct ocelot *ocelot, int port, } EXPORT_SYMBOL(ocelot_port_bridge_flags); +int ocelot_port_get_default_prio(struct ocelot *ocelot, int port) +{ + int val = ocelot_read_gix(ocelot, ANA_PORT_QOS_CFG, port); + + return ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL_X(val); +} +EXPORT_SYMBOL_GPL(ocelot_port_get_default_prio); + +int ocelot_port_set_default_prio(struct ocelot *ocelot, int port, u8 prio) +{ + if (prio >= IEEE_8021QAZ_MAX_TCS) + return -ERANGE; + + ocelot_rmw_gix(ocelot, + ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL(prio), + ANA_PORT_QOS_CFG_QOS_DEFAULT_VAL_M, + ANA_PORT_QOS_CFG, + port); + + return 0; +} +EXPORT_SYMBOL_GPL(ocelot_port_set_default_prio); + +int ocelot_port_get_dscp_prio(struct ocelot *ocelot, int port, u8 dscp) +{ + int qos_cfg = ocelot_read_gix(ocelot, ANA_PORT_QOS_CFG, port); + int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp); + + /* Return error if DSCP prioritization isn't enabled */ + if (!(qos_cfg & ANA_PORT_QOS_CFG_QOS_DSCP_ENA)) + return -EOPNOTSUPP; + + if (qos_cfg & ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA) { + dscp = ANA_DSCP_CFG_DSCP_TRANSLATE_VAL_X(dscp_cfg); + /* Re-read ANA_DSCP_CFG for the translated DSCP */ + dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp); + } + + /* If the DSCP value is not trusted, the QoS classification falls back + * to VLAN PCP or port-based default. + */ + if (!(dscp_cfg & ANA_DSCP_CFG_DSCP_TRUST_ENA)) + return -EOPNOTSUPP; + + return ANA_DSCP_CFG_QOS_DSCP_VAL_X(dscp_cfg); +} +EXPORT_SYMBOL_GPL(ocelot_port_get_dscp_prio); + +int ocelot_port_add_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio) +{ + int mask, val; + + if (prio >= IEEE_8021QAZ_MAX_TCS) + return -ERANGE; + + /* There is at least one app table priority (this one), so we need to + * make sure DSCP prioritization is enabled on the port. + * Also make sure DSCP translation is disabled + * (dcbnl doesn't support it). + */ + mask = ANA_PORT_QOS_CFG_QOS_DSCP_ENA | + ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA; + + ocelot_rmw_gix(ocelot, ANA_PORT_QOS_CFG_QOS_DSCP_ENA, mask, + ANA_PORT_QOS_CFG, port); + + /* Trust this DSCP value and map it to the given QoS class */ + val = ANA_DSCP_CFG_DSCP_TRUST_ENA | ANA_DSCP_CFG_QOS_DSCP_VAL(prio); + + ocelot_write_rix(ocelot, val, ANA_DSCP_CFG, dscp); + + return 0; +} +EXPORT_SYMBOL_GPL(ocelot_port_add_dscp_prio); + +int ocelot_port_del_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio) +{ + int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, dscp); + int mask, i; + + /* During a "dcb app replace" command, the new app table entry will be + * added first, then the old one will be deleted. But the hardware only + * supports one QoS class per DSCP value (duh), so if we blindly delete + * the app table entry for this DSCP value, we end up deleting the + * entry with the new priority. Avoid that by checking whether user + * space wants to delete the priority which is currently configured, or + * something else which is no longer current. + */ + if (ANA_DSCP_CFG_QOS_DSCP_VAL_X(dscp_cfg) != prio) + return 0; + + /* Untrust this DSCP value */ + ocelot_write_rix(ocelot, 0, ANA_DSCP_CFG, dscp); + + for (i = 0; i < 64; i++) { + int dscp_cfg = ocelot_read_rix(ocelot, ANA_DSCP_CFG, i); + + /* There are still app table entries on the port, so we need to + * keep DSCP enabled, nothing to do. + */ + if (dscp_cfg & ANA_DSCP_CFG_DSCP_TRUST_ENA) + return 0; + } + + /* Disable DSCP QoS classification if there isn't any trusted + * DSCP value left. + */ + mask = ANA_PORT_QOS_CFG_QOS_DSCP_ENA | + ANA_PORT_QOS_CFG_DSCP_TRANSLATE_ENA; + + ocelot_rmw_gix(ocelot, 0, mask, ANA_PORT_QOS_CFG, port); + + return 0; +} +EXPORT_SYMBOL_GPL(ocelot_port_del_dscp_prio); + void ocelot_init_port(struct ocelot *ocelot, int port) { struct ocelot_port *ocelot_port = ocelot->ports[port]; diff --git a/include/net/dsa.h b/include/net/dsa.h index 9d16505fc0e2..9bfe984fcdbf 100644 --- a/include/net/dsa.h +++ b/include/net/dsa.h @@ -892,6 +892,18 @@ struct dsa_switch_ops { int (*get_ts_info)(struct dsa_switch *ds, int port, struct ethtool_ts_info *ts); + /* + * DCB ops + */ + int (*port_get_default_prio)(struct dsa_switch *ds, int port); + int (*port_set_default_prio)(struct dsa_switch *ds, int port, + u8 prio); + int (*port_get_dscp_prio)(struct dsa_switch *ds, int port, u8 dscp); + int (*port_add_dscp_prio)(struct dsa_switch *ds, int port, u8 dscp, + u8 prio); + int (*port_del_dscp_prio)(struct dsa_switch *ds, int port, u8 dscp, + u8 prio); + /* * Suspend and resume */ diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h index ee3c59639d70..4d51e2a7120f 100644 --- a/include/soc/mscc/ocelot.h +++ b/include/soc/mscc/ocelot.h @@ -869,6 +869,11 @@ int ocelot_port_pre_bridge_flags(struct ocelot *ocelot, int port, struct switchdev_brport_flags val); void ocelot_port_bridge_flags(struct ocelot *ocelot, int port, struct switchdev_brport_flags val); +int ocelot_port_get_default_prio(struct ocelot *ocelot, int port); +int ocelot_port_set_default_prio(struct ocelot *ocelot, int port, u8 prio); +int ocelot_port_get_dscp_prio(struct ocelot *ocelot, int port, u8 dscp); +int ocelot_port_add_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio); +int ocelot_port_del_dscp_prio(struct ocelot *ocelot, int port, u8 dscp, u8 prio); int ocelot_port_bridge_join(struct ocelot *ocelot, int port, struct net_device *bridge, int bridge_num, struct netlink_ext_ack *extack); diff --git a/net/dsa/slave.c b/net/dsa/slave.c index a61a7c54af20..f9cecda791d5 100644 --- a/net/dsa/slave.c +++ b/net/dsa/slave.c @@ -19,6 +19,7 @@ #include #include #include +#include #include #include "dsa_priv.h" @@ -1852,6 +1853,209 @@ out_master_failed: return err; } +static int __maybe_unused +dsa_slave_dcbnl_set_default_prio(struct net_device *dev, struct dcb_app *app) +{ + struct dsa_port *dp = dsa_slave_to_port(dev); + struct dsa_switch *ds = dp->ds; + unsigned long mask, new_prio; + int err, port = dp->index; + + if (!ds->ops->port_set_default_prio) + return -EOPNOTSUPP; + + err = dcb_ieee_setapp(dev, app); + if (err) + return err; + + mask = dcb_ieee_getapp_mask(dev, app); + new_prio = __fls(mask); + + err = ds->ops->port_set_default_prio(ds, port, new_prio); + if (err) { + dcb_ieee_delapp(dev, app); + return err; + } + + return 0; +} + +static int __maybe_unused +dsa_slave_dcbnl_add_dscp_prio(struct net_device *dev, struct dcb_app *app) +{ + struct dsa_port *dp = dsa_slave_to_port(dev); + struct dsa_switch *ds = dp->ds; + unsigned long mask, new_prio; + int err, port = dp->index; + u8 dscp = app->protocol; + + if (!ds->ops->port_add_dscp_prio) + return -EOPNOTSUPP; + + if (dscp >= 64) { + netdev_err(dev, "DSCP APP entry with protocol value %u is invalid\n", + dscp); + return -EINVAL; + } + + err = dcb_ieee_setapp(dev, app); + if (err) + return err; + + mask = dcb_ieee_getapp_mask(dev, app); + new_prio = __fls(mask); + + err = ds->ops->port_add_dscp_prio(ds, port, dscp, new_prio); + if (err) { + dcb_ieee_delapp(dev, app); + return err; + } + + return 0; +} + +static int __maybe_unused dsa_slave_dcbnl_ieee_setapp(struct net_device *dev, + struct dcb_app *app) +{ + switch (app->selector) { + case IEEE_8021QAZ_APP_SEL_ETHERTYPE: + switch (app->protocol) { + case 0: + return dsa_slave_dcbnl_set_default_prio(dev, app); + default: + return -EOPNOTSUPP; + } + break; + case IEEE_8021QAZ_APP_SEL_DSCP: + return dsa_slave_dcbnl_add_dscp_prio(dev, app); + default: + return -EOPNOTSUPP; + } +} + +static int __maybe_unused +dsa_slave_dcbnl_del_default_prio(struct net_device *dev, struct dcb_app *app) +{ + struct dsa_port *dp = dsa_slave_to_port(dev); + struct dsa_switch *ds = dp->ds; + unsigned long mask, new_prio; + int err, port = dp->index; + + if (!ds->ops->port_set_default_prio) + return -EOPNOTSUPP; + + err = dcb_ieee_delapp(dev, app); + if (err) + return err; + + mask = dcb_ieee_getapp_mask(dev, app); + new_prio = mask ? __fls(mask) : 0; + + err = ds->ops->port_set_default_prio(ds, port, new_prio); + if (err) { + dcb_ieee_setapp(dev, app); + return err; + } + + return 0; +} + +static int __maybe_unused +dsa_slave_dcbnl_del_dscp_prio(struct net_device *dev, struct dcb_app *app) +{ + struct dsa_port *dp = dsa_slave_to_port(dev); + struct dsa_switch *ds = dp->ds; + int err, port = dp->index; + u8 dscp = app->protocol; + + if (!ds->ops->port_del_dscp_prio) + return -EOPNOTSUPP; + + err = dcb_ieee_delapp(dev, app); + if (err) + return err; + + err = ds->ops->port_del_dscp_prio(ds, port, dscp, app->priority); + if (err) { + dcb_ieee_setapp(dev, app); + return err; + } + + return 0; +} + +static int __maybe_unused dsa_slave_dcbnl_ieee_delapp(struct net_device *dev, + struct dcb_app *app) +{ + switch (app->selector) { + case IEEE_8021QAZ_APP_SEL_ETHERTYPE: + switch (app->protocol) { + case 0: + return dsa_slave_dcbnl_del_default_prio(dev, app); + default: + return -EOPNOTSUPP; + } + break; + case IEEE_8021QAZ_APP_SEL_DSCP: + return dsa_slave_dcbnl_del_dscp_prio(dev, app); + default: + return -EOPNOTSUPP; + } +} + +/* Pre-populate the DCB application priority table with the priorities + * configured during switch setup, which we read from hardware here. + */ +static int dsa_slave_dcbnl_init(struct net_device *dev) +{ + struct dsa_port *dp = dsa_slave_to_port(dev); + struct dsa_switch *ds = dp->ds; + int port = dp->index; + int err; + + if (ds->ops->port_get_default_prio) { + int prio = ds->ops->port_get_default_prio(ds, port); + struct dcb_app app = { + .selector = IEEE_8021QAZ_APP_SEL_ETHERTYPE, + .protocol = 0, + .priority = prio, + }; + + if (prio < 0) + return prio; + + err = dcb_ieee_setapp(dev, &app); + if (err) + return err; + } + + if (ds->ops->port_get_dscp_prio) { + int protocol; + + for (protocol = 0; protocol < 64; protocol++) { + struct dcb_app app = { + .selector = IEEE_8021QAZ_APP_SEL_DSCP, + .protocol = protocol, + }; + int prio; + + prio = ds->ops->port_get_dscp_prio(ds, port, protocol); + if (prio == -EOPNOTSUPP) + continue; + if (prio < 0) + return prio; + + app.priority = prio; + + err = dcb_ieee_setapp(dev, &app); + if (err) + return err; + } + } + + return 0; +} + static const struct ethtool_ops dsa_slave_ethtool_ops = { .get_drvinfo = dsa_slave_get_drvinfo, .get_regs_len = dsa_slave_get_regs_len, @@ -1881,6 +2085,11 @@ static const struct ethtool_ops dsa_slave_ethtool_ops = { .self_test = dsa_slave_net_selftest, }; +static const struct dcbnl_rtnl_ops __maybe_unused dsa_slave_dcbnl_ops = { + .ieee_setapp = dsa_slave_dcbnl_ieee_setapp, + .ieee_delapp = dsa_slave_dcbnl_ieee_delapp, +}; + static struct devlink_port *dsa_slave_get_devlink_port(struct net_device *dev) { struct dsa_port *dp = dsa_slave_to_port(dev); @@ -2105,6 +2314,9 @@ int dsa_slave_create(struct dsa_port *port) return -ENOMEM; slave_dev->ethtool_ops = &dsa_slave_ethtool_ops; +#if IS_ENABLED(CONFIG_DCB) + slave_dev->dcbnl_ops = &dsa_slave_dcbnl_ops; +#endif if (!is_zero_ether_addr(port->mac)) eth_hw_addr_set(slave_dev, port->mac); else @@ -2162,6 +2374,17 @@ int dsa_slave_create(struct dsa_port *port) goto out_phy; } + if (IS_ENABLED(CONFIG_DCB)) { + ret = dsa_slave_dcbnl_init(slave_dev); + if (ret) { + netdev_err(slave_dev, + "failed to initialize DCB: %pe\n", + ERR_PTR(ret)); + rtnl_unlock(); + goto out_unregister; + } + } + ret = netdev_upper_dev_link(master, slave_dev, NULL); rtnl_unlock();