[PATCH v6 3/3] net: dsa: ocelot: Add support for QinQ Operation

From: hongbo . wang
Date: Wed Sep 16 2020 - 05:46:55 EST


From: "hongbo.wang" <hongbo.wang@xxxxxxx>

This feature can be test in the following case:
Customer <-----> swp0 <-----> swp1 <-----> ISP

Customer will send and receive packets with single VLAN tag(CTAG),
ISP will send and receive packets with double VLAN tag(STAG and CTAG).
This refers to "4.3.3 Provider Bridges and Q-in-Q Operation" in
VSC99599_1_00_TS.pdf.

The related test commands:
1.
devlink dev param set pci/0000:00:00.5 name qinq_port_bitmap \
value 2 cmode runtime
2.
ip link add dev br0 type bridge vlan_protocol 802.1ad
ip link set dev swp0 master br0
ip link set dev swp1 master br0
ip link set dev br0 type bridge vlan_filtering 1
3.
bridge vlan del dev swp0 vid 1 pvid
bridge vlan add dev swp0 vid 100 pvid untagged
bridge vlan add dev swp1 vid 100
Result:
Customer(tpid:8100 vid:111) -> swp0 -> swp1 -> ISP(STAG \
tpid:88A8 vid:100, CTAG tpid:8100 vid:111)
ISP(tpid:88A8 vid:100 tpid:8100 vid:222) -> swp1 -> swp0 ->\
Customer(tpid:8100 vid:222)

Signed-off-by: hongbo.wang <hongbo.wang@xxxxxxx>
---
drivers/net/dsa/ocelot/felix.c | 123 +++++++++++++++++++++++++++++
drivers/net/ethernet/mscc/ocelot.c | 39 +++++++--
include/soc/mscc/ocelot.h | 4 +
3 files changed, 160 insertions(+), 6 deletions(-)

diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index a1e1d3824110..5888b0fa5669 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -148,9 +148,26 @@ static void felix_vlan_add(struct dsa_switch *ds, int port,
vid, port, err);
return;
}
+
+ if (vlan->proto == ETH_P_8021AD) {
+ if (!ocelot->qinq_enable) {
+ ocelot->qinq_enable = true;
+ kref_init(&ocelot->qinq_refcount);
+ } else {
+ kref_get(&ocelot->qinq_refcount);
+ }
+ }
}
}

+static void felix_vlan_qinq_release(struct kref *ref)
+{
+ struct ocelot *ocelot;
+
+ ocelot = container_of(ref, struct ocelot, qinq_refcount);
+ ocelot->qinq_enable = false;
+}
+
static int felix_vlan_del(struct dsa_switch *ds, int port,
const struct switchdev_obj_port_vlan *vlan)
{
@@ -165,6 +182,9 @@ static int felix_vlan_del(struct dsa_switch *ds, int port,
vid, port, err);
return err;
}
+
+ if (ocelot->qinq_enable && vlan->proto == ETH_P_8021AD)
+ kref_put(&ocelot->qinq_refcount, felix_vlan_qinq_release);
}
return 0;
}
@@ -173,9 +193,13 @@ static int felix_port_enable(struct dsa_switch *ds, int port,
struct phy_device *phy)
{
struct ocelot *ocelot = ds->priv;
+ struct net_device *slave;

ocelot_port_enable(ocelot, port, phy);

+ slave = dsa_to_port(ds, port)->slave;
+ slave->features |= NETIF_F_HW_VLAN_STAG_FILTER;
+
return 0;
}

@@ -555,6 +579,97 @@ static struct ptp_clock_info ocelot_ptp_clock_info = {
.enable = ocelot_ptp_enable,
};

+static int felix_qinq_port_bitmap_get(struct dsa_switch *ds, u32 *bitmap)
+{
+ struct ocelot *ocelot = ds->priv;
+ struct ocelot_port *ocelot_port;
+ int port;
+
+ *bitmap = 0;
+ for (port = 0; port < ds->num_ports; port++) {
+ ocelot_port = ocelot->ports[port];
+ if (ocelot_port->qinq_mode)
+ *bitmap |= 0x01 << port;
+ }
+
+ return 0;
+}
+
+static int felix_qinq_port_bitmap_set(struct dsa_switch *ds, u32 bitmap)
+{
+ struct ocelot *ocelot = ds->priv;
+ struct ocelot_port *ocelot_port;
+ int port;
+
+ for (port = 0; port < ds->num_ports; port++) {
+ ocelot_port = ocelot->ports[port];
+ if (bitmap & (0x01 << port))
+ ocelot_port->qinq_mode = true;
+ else
+ ocelot_port->qinq_mode = false;
+ }
+
+ return 0;
+}
+
+enum felix_devlink_param_id {
+ FELIX_DEVLINK_PARAM_ID_BASE = DEVLINK_PARAM_GENERIC_ID_MAX,
+ FELIX_DEVLINK_PARAM_ID_QINQ_PORT_BITMAP,
+};
+
+static int felix_devlink_param_get(struct dsa_switch *ds, u32 id,
+ struct devlink_param_gset_ctx *ctx)
+{
+ int err;
+
+ switch (id) {
+ case FELIX_DEVLINK_PARAM_ID_QINQ_PORT_BITMAP:
+ err = felix_qinq_port_bitmap_get(ds, &ctx->val.vu32);
+ break;
+ default:
+ err = -EOPNOTSUPP;
+ break;
+ }
+
+ return err;
+}
+
+static int felix_devlink_param_set(struct dsa_switch *ds, u32 id,
+ struct devlink_param_gset_ctx *ctx)
+{
+ int err;
+
+ switch (id) {
+ case FELIX_DEVLINK_PARAM_ID_QINQ_PORT_BITMAP:
+ err = felix_qinq_port_bitmap_set(ds, ctx->val.vu32);
+ break;
+ default:
+ err = -EOPNOTSUPP;
+ break;
+ }
+
+ return err;
+}
+
+static const struct devlink_param felix_devlink_params[] = {
+ DSA_DEVLINK_PARAM_DRIVER(FELIX_DEVLINK_PARAM_ID_QINQ_PORT_BITMAP,
+ "qinq_port_bitmap",
+ DEVLINK_PARAM_TYPE_U32,
+ BIT(DEVLINK_PARAM_CMODE_RUNTIME)),
+};
+
+static int felix_setup_devlink_params(struct dsa_switch *ds)
+{
+ return dsa_devlink_params_register(ds, felix_devlink_params,
+ ARRAY_SIZE(felix_devlink_params));
+}
+
+static void felix_teardown_devlink_params(struct dsa_switch *ds)
+{
+ dsa_devlink_params_unregister(ds, felix_devlink_params,
+ ARRAY_SIZE(felix_devlink_params));
+}
+
/* Hardware initialization done here so that we can allocate structures with
* devm without fear of dsa_register_switch returning -EPROBE_DEFER and causing
* us to allocate structures twice (leak memory) and map PCI memory twice
@@ -614,6 +729,10 @@ static int felix_setup(struct dsa_switch *ds)
ds->mtu_enforcement_ingress = true;
ds->configure_vlan_while_not_filtering = true;

+ err = felix_setup_devlink_params(ds);
+ if (err < 0)
+ return err;
+
return 0;
}

@@ -625,6 +744,8 @@ static void felix_teardown(struct dsa_switch *ds)
if (felix->info->mdio_bus_free)
felix->info->mdio_bus_free(ocelot);

+ felix_teardown_devlink_params(ds);
+
ocelot_deinit_timestamp(ocelot);
/* stop workqueue thread */
ocelot_deinit(ocelot);
@@ -798,6 +919,8 @@ const struct dsa_switch_ops felix_switch_ops = {
.cls_flower_del = felix_cls_flower_del,
.cls_flower_stats = felix_cls_flower_stats,
.port_setup_tc = felix_port_setup_tc,
+ .devlink_param_get = felix_devlink_param_get,
+ .devlink_param_set = felix_devlink_param_set,
};

static int __init felix_init(void)
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index 5abb7d2b0a9e..9186501cef03 100644
--- a/drivers/net/ethernet/mscc/ocelot.c
+++ b/drivers/net/ethernet/mscc/ocelot.c
@@ -143,6 +143,8 @@ static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
u16 vid)
{
struct ocelot_port *ocelot_port = ocelot->ports[port];
+ u32 port_tpid = 0;
+ u32 tag_tpid = 0;
u32 val = 0;

if (ocelot_port->vid != vid) {
@@ -156,8 +158,14 @@ static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
ocelot_port->vid = vid;
}

- ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_VID(vid),
- REW_PORT_VLAN_CFG_PORT_VID_M,
+ if (ocelot->qinq_enable && ocelot_port->qinq_mode)
+ port_tpid = REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021AD);
+ else
+ port_tpid = REW_PORT_VLAN_CFG_PORT_TPID(ETH_P_8021Q);
+
+ ocelot_rmw_gix(ocelot, REW_PORT_VLAN_CFG_PORT_VID(vid) | port_tpid,
+ REW_PORT_VLAN_CFG_PORT_VID_M |
+ REW_PORT_VLAN_CFG_PORT_TPID_M,
REW_PORT_VLAN_CFG, port);

if (ocelot_port->vlan_aware && !ocelot_port->vid)
@@ -180,12 +188,18 @@ static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
else
/* Tag all frames */
val = REW_TAG_CFG_TAG_CFG(3);
+
+ if (ocelot->qinq_enable && ocelot_port->qinq_mode)
+ tag_tpid = REW_TAG_CFG_TAG_TPID_CFG(1);
+ else
+ tag_tpid = REW_TAG_CFG_TAG_TPID_CFG(0);
} else {
/* Port tagging disabled. */
val = REW_TAG_CFG_TAG_CFG(0);
+ tag_tpid = REW_TAG_CFG_TAG_TPID_CFG(0);
}
- ocelot_rmw_gix(ocelot, val,
- REW_TAG_CFG_TAG_CFG_M,
+ ocelot_rmw_gix(ocelot, val | tag_tpid,
+ REW_TAG_CFG_TAG_CFG_M | REW_TAG_CFG_TAG_TPID_CFG_M,
REW_TAG_CFG, port);

return 0;
@@ -204,6 +218,15 @@ void ocelot_port_vlan_filtering(struct ocelot *ocelot, int port,
ANA_PORT_VLAN_CFG_VLAN_POP_CNT(1);
else
val = 0;
+
+ /* if switch is enabled for QinQ, the port for LAN should set
+ * VLAN_CFG.VLAN_POP_CNT=0 && VLAN_CFG.VLAN_AWARE_ENA=0.
+ * the port for MAN should set VLAN_CFG.VLAN_POP_CNT=1 &&
+ * VLAN_CFG.VLAN_AWARE_ENA=1. referring to 4.3.3 in VSC9959_1_00_TS.pdf
+ */
+ if (ocelot->qinq_enable && !ocelot_port->qinq_mode)
+ val = 0;
+
ocelot_rmw_gix(ocelot, val,
ANA_PORT_VLAN_CFG_VLAN_AWARE_ENA |
ANA_PORT_VLAN_CFG_VLAN_POP_CNT_M,
@@ -217,10 +240,14 @@ EXPORT_SYMBOL(ocelot_port_vlan_filtering);
static void ocelot_port_set_pvid(struct ocelot *ocelot, int port, u16 pvid)
{
struct ocelot_port *ocelot_port = ocelot->ports[port];
+ u32 tag_type = 0;
+
+ if (ocelot->qinq_enable && ocelot_port->qinq_mode)
+ tag_type = ANA_PORT_VLAN_CFG_VLAN_TAG_TYPE;

ocelot_rmw_gix(ocelot,
- ANA_PORT_VLAN_CFG_VLAN_VID(pvid),
- ANA_PORT_VLAN_CFG_VLAN_VID_M,
+ ANA_PORT_VLAN_CFG_VLAN_VID(pvid) | tag_type,
+ ANA_PORT_VLAN_CFG_VLAN_VID_M | tag_type,
ANA_PORT_VLAN_CFG, port);

ocelot_port->pvid = pvid;
diff --git a/include/soc/mscc/ocelot.h b/include/soc/mscc/ocelot.h
index da369b12005f..8d0f9f9ec0b2 100644
--- a/include/soc/mscc/ocelot.h
+++ b/include/soc/mscc/ocelot.h
@@ -556,6 +556,7 @@ struct ocelot_port {
struct regmap *target;

bool vlan_aware;
+ bool qinq_mode;

/* Ingress default VLAN (pvid) */
u16 pvid;
@@ -632,6 +633,9 @@ struct ocelot {
/* Protects the PTP clock */
spinlock_t ptp_clock_lock;
struct ptp_pin_desc ptp_pins[OCELOT_PTP_PINS_NUM];
+
+ bool qinq_enable;
+ struct kref qinq_refcount;
};

struct ocelot_policer {
--
2.17.1