@@ -132,9 +132,13 @@ static void felix_vlan_add(struct dsa_switch *ds, int port,
{
struct ocelot *ocelot = ds->priv;
u16 flags = vlan->flags;
+ struct ocelot_port *ocelot_port = ocelot->ports[port];
u16 vid;
int err;
+ if (vlan->proto == ETH_P_8021AD)
+ ocelot_port->qinq_mode = true;
+
if (dsa_is_cpu_port(ds, port))
flags &= ~BRIDGE_VLAN_INFO_UNTAGGED;
@@ -154,9 +158,13 @@ static int felix_vlan_del(struct dsa_switch *ds, int port,
const struct switchdev_obj_port_vlan *vlan)
{
struct ocelot *ocelot = ds->priv;
+ struct ocelot_port *ocelot_port = ocelot->ports[port];
u16 vid;
int err;
+ if (vlan->proto == ETH_P_8021AD)
+ ocelot_port->qinq_mode = false;
+
for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
err = ocelot_vlan_del(ocelot, port, vid);
if (err) {
@@ -144,6 +144,8 @@ static int ocelot_port_set_native_vlan(struct ocelot *ocelot, int port,
{
struct ocelot_port *ocelot_port = ocelot->ports[port];
u32 val = 0;
+ u32 tag_tpid = 0;
+ u32 port_tpid = 0;
if (ocelot_port->vid != vid) {
/* Always permit deleting the native VLAN (vid = 0) */
@@ -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_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,28 @@ 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_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);
+ if (ocelot_port->qinq_mode) {
+ if (ocelot_port->vid)
+ val = REW_TAG_CFG_TAG_CFG(1);
+ else
+ val = REW_TAG_CFG_TAG_CFG(3);
+
+ tag_tpid = REW_TAG_CFG_TAG_TPID_CFG(1);
+ } 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;
@@ -216,11 +240,15 @@ EXPORT_SYMBOL(ocelot_port_vlan_filtering);
/* Default vlan to clasify for untagged frames (may be zero) */
static void ocelot_port_set_pvid(struct ocelot *ocelot, int port, u16 pvid)
{
+ u32 tag_type = 0;
struct ocelot_port *ocelot_port = ocelot->ports[port];
+ if (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;
@@ -556,6 +556,7 @@ struct ocelot_port {
struct regmap *target;
bool vlan_aware;
+ bool qinq_mode;
/* Ingress default VLAN (pvid) */
u16 pvid;