diff mbox series

[v4,2/2] net: dsa: ocelot: Add support for QinQ Operation

Message ID 20200730102505.27039-3-hongbo.wang@nxp.com
State Changes Requested
Delegated to: David Miller
Headers show
Series Add 802.1AD protocol support for dsa switch and ocelot driver | expand

Commit Message

Hongbo Wang July 30, 2020, 10:25 a.m. UTC
From: "hongbo.wang" <hongbo.wang@nxp.com>

This featue can be test using network test tools
TX-tool -----> swp0  -----> swp1 -----> RX-tool

TX-tool simulates Customer that will send and receive packets with single
VLAN tag(CTAG), RX-tool simulates Service-Provider that 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.
ip link add dev br0 type bridge
ip link set dev swp1 master br0
ip link set br0 type bridge vlan_protocol 802.1ad
ip link set dev swp0 master br0

2.
ip link set dev br0 type bridge vlan_filtering 1
bridge vlan add dev swp0 vid 100 pvid
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)

3.
bridge vlan del dev swp0 vid 1 pvid
bridge vlan add dev swp0 vid 100 pvid untagged
Result:
ISP(tpid:88A8 vid:100 tpid:8100 vid:222) -> swp1 -> swp0 ->\
		Customer(tpid:8100 vid:222)

Signed-off-by: hongbo.wang <hongbo.wang@nxp.com>
---
 drivers/net/dsa/ocelot/felix.c     | 12 +++++++
 drivers/net/ethernet/mscc/ocelot.c | 53 +++++++++++++++++++++++++-----
 include/soc/mscc/ocelot.h          |  2 ++
 3 files changed, 59 insertions(+), 8 deletions(-)

Comments

David Miller Aug. 3, 2020, 9:58 p.m. UTC | #1
From: hongbo.wang@nxp.com
Date: Thu, 30 Jul 2020 18:25:05 +0800

> +	if (vlan->proto == ETH_P_8021AD) {
> +		ocelot->enable_qinq = true;
> +		ocelot_port->qinq_mode = true;
> +	}
 ...
> +	if (vlan->proto == ETH_P_8021AD) {
> +		ocelot->enable_qinq = false;
> +		ocelot_port->qinq_mode = false;
> +	}
> +

I don't understand how this can work just by using a boolean to track
the state.

This won't work properly if you are handling multiple QinQ VLAN entries.

Also, I need Andrew and Florian to review and ACK the DSA layer changes
that add the protocol value to the device notifier block.
Florian Fainelli Aug. 3, 2020, 10:47 p.m. UTC | #2
On 7/30/2020 3:25 AM, hongbo.wang@nxp.com wrote:
> From: "hongbo.wang" <hongbo.wang@nxp.com>
> 
> This featue can be test using network test tools

mispelled: feature, can be used to test network test tools? or can be
used to exercise network test tool?

> TX-tool -----> swp0  -----> swp1 -----> RX-tool
> 
> TX-tool simulates Customer that will send and receive packets with single
> VLAN tag(CTAG), RX-tool simulates Service-Provider that 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.
> ip link add dev br0 type bridge
> ip link set dev swp1 master br0
> ip link set br0 type bridge vlan_protocol 802.1ad
> ip link set dev swp0 master br0
> 
> 2.
> ip link set dev br0 type bridge vlan_filtering 1
> bridge vlan add dev swp0 vid 100 pvid
> 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)
> 
> 3.
> bridge vlan del dev swp0 vid 1 pvid
> bridge vlan add dev swp0 vid 100 pvid untagged
> Result:
> ISP(tpid:88A8 vid:100 tpid:8100 vid:222) -> swp1 -> swp0 ->\
> 		Customer(tpid:8100 vid:222)
> 
> Signed-off-by: hongbo.wang <hongbo.wang@nxp.com>
> ---
>  drivers/net/dsa/ocelot/felix.c     | 12 +++++++
>  drivers/net/ethernet/mscc/ocelot.c | 53 +++++++++++++++++++++++++-----
>  include/soc/mscc/ocelot.h          |  2 ++
>  3 files changed, 59 insertions(+), 8 deletions(-)
> 
> diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
> index c69d9592a2b7..72a27b61080e 100644
> --- a/drivers/net/dsa/ocelot/felix.c
> +++ b/drivers/net/dsa/ocelot/felix.c
> @@ -131,10 +131,16 @@ static void felix_vlan_add(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 flags = vlan->flags;
>  	u16 vid;
>  	int err;
>  
> +	if (vlan->proto == ETH_P_8021AD) {
> +		ocelot->enable_qinq = true;
> +		ocelot_port->qinq_mode = true;
> +	}
> +
>  	if (dsa_is_cpu_port(ds, port))
>  		flags &= ~BRIDGE_VLAN_INFO_UNTAGGED;
>  
> @@ -154,9 +160,15 @@ 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->enable_qinq = false;
> +		ocelot_port->qinq_mode = false;
> +	}

You need the delete part to be reference counted, otherwise the first
802.1AD VLAN delete request that comes in, regardless of whether other
802.1AD VLAN entries are installed will disable qinq_mode and
enable_qinq for the entire port and switch, that does not sound like
what you want.

Is not ocelot->enable_qinq the logical or of all ocelo_port instances's
qinq_mode as well?

> +
>  	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
>  		err = ocelot_vlan_del(ocelot, port, vid);
>  		if (err) {
> diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
> index f2d94b026d88..b5fec6855afd 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_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);

This is nearly the same branch as the one above, can you merge the
conditions vlan_aware || qinq_mode and just set an appropriate TAG_CFG()
register value based on either booleans?

Are you also not possibly missing a if (untaged || qinq_mode) check in
ocelot_vlan_add() to call into ocelot_port_set_native_vlan()?
Hongbo Wang Aug. 4, 2020, 6:36 a.m. UTC | #3
> > +     if (vlan->proto == ETH_P_8021AD) {
> > +             ocelot->enable_qinq = true;
> > +             ocelot_port->qinq_mode = true;
> > +     }
>  ...
> > +     if (vlan->proto == ETH_P_8021AD) {
> > +             ocelot->enable_qinq = false;
> > +             ocelot_port->qinq_mode = false;
> > +     }
> > +
> 
> I don't understand how this can work just by using a boolean to track the
> state.
> 
> This won't work properly if you are handling multiple QinQ VLAN entries.
> 
> Also, I need Andrew and Florian to review and ACK the DSA layer changes that
> add the protocol value to the device notifier block.

Hi David,
Thanks for reply.

When setting bridge's VLAN protocol to 802.1AD by the command "ip link set br0 type bridge vlan_protocol 802.1ad", it will call dsa_slave_vlan_rx_add(dev, proto, vid) for every port in the bridge, the parameter vid is port's pvid 1,
if pvid's proto is 802.1AD, I will enable switch's enable_qinq, and the related port's qinq_mode,

When there are multiple QinQ VLAN entries, If one VLAN's proto is 802.1AD, I will enable switch and the related port into QinQ mode.

Thanks,
hongbo
Hongbo Wang Aug. 4, 2020, 8:13 a.m. UTC | #4
> > This featue can be test using network test tools
> 
> mispelled: feature, can be used to test network test tools? or can be used to
> exercise network test tool?

when testing this feature, I need network tool to send packet with VLAN tag(pcp proto and vid), I will change it to avoid ambiguity.

> 
> >       struct ocelot *ocelot = ds->priv;
> > +     struct ocelot_port *ocelot_port = ocelot->ports[port];
> >       u16 vid;
> >       int err;
> >
> > +     if (vlan->proto == ETH_P_8021AD) {
> > +             ocelot->enable_qinq = false;
> > +             ocelot_port->qinq_mode = false;
> > +     }
> 
> You need the delete part to be reference counted, otherwise the first 802.1AD
> VLAN delete request that comes in, regardless of whether other 802.1AD VLAN
> entries are installed will disable qinq_mode and enable_qinq for the entire
> port and switch, that does not sound like what you want.

I will add reference count in next version.
maybe I can disable qinq_mode and enable_qinq only when deleting pvid 1, I will test and change it.

> 
> Is not ocelot->enable_qinq the logical or of all ocelo_port instances's
> qinq_mode as well?

enable_qinq is flag of switch, and qinq_mode is flag of single port,
if switch is in working in QinQ mode, some ports that linked to ISP networking should enable qinq_mode,
other ports that linked to customer networking don't need set qinq_mode. these two types of port have different action.

> >               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);
> 
> This is nearly the same branch as the one above, can you merge the conditions
> vlan_aware || qinq_mode and just set an appropriate TAG_CFG() register value
> based on either booleans?

this feature needs vlan_filter=1, so the branch if (vlan_filter == 0 && qinq_mode) can be deleted now.
I will optimize the related code.

> 
> Are you also not possibly missing a if (untaged || qinq_mode) check in
> ocelot_vlan_add() to call into ocelot_port_set_native_vlan()?

The qinq_mode action can be triggered by ocelot_port_vlan_filtering, so don't need if (untagged || qinq_mode).

Thanks!
Hongbo
Florian Fainelli Aug. 4, 2020, 4:38 p.m. UTC | #5
On 8/3/2020 11:36 PM, Hongbo Wang wrote:
>>> +     if (vlan->proto == ETH_P_8021AD) {
>>> +             ocelot->enable_qinq = true;
>>> +             ocelot_port->qinq_mode = true;
>>> +     }
>>  ...
>>> +     if (vlan->proto == ETH_P_8021AD) {
>>> +             ocelot->enable_qinq = false;
>>> +             ocelot_port->qinq_mode = false;
>>> +     }
>>> +
>>
>> I don't understand how this can work just by using a boolean to track the
>> state.
>>
>> This won't work properly if you are handling multiple QinQ VLAN entries.
>>
>> Also, I need Andrew and Florian to review and ACK the DSA layer changes that
>> add the protocol value to the device notifier block.
> 
> Hi David,
> Thanks for reply.
> 
> When setting bridge's VLAN protocol to 802.1AD by the command "ip link set br0 type bridge vlan_protocol 802.1ad", it will call dsa_slave_vlan_rx_add(dev, proto, vid) for every port in the bridge, the parameter vid is port's pvid 1,
> if pvid's proto is 802.1AD, I will enable switch's enable_qinq, and the related port's qinq_mode,
> 
> When there are multiple QinQ VLAN entries, If one VLAN's proto is 802.1AD, I will enable switch and the related port into QinQ mode.

The enabling appears fine, the problem is the disabling, the first
802.1AD VLAN entry that gets deleted will lead to the port and switch no
longer being in QinQ mode, and this does not look intended.
Hongbo Wang Aug. 5, 2020, 2:32 a.m. UTC | #6
> Caution: EXT Email
> 
> On 8/3/2020 11:36 PM, Hongbo Wang wrote:
> >>> +     if (vlan->proto == ETH_P_8021AD) {
> >>> +             ocelot->enable_qinq = true;
> >>> +             ocelot_port->qinq_mode = true;
> >>> +     }
> >>  ...
> >>> +     if (vlan->proto == ETH_P_8021AD) {
> >>> +             ocelot->enable_qinq = false;
> >>> +             ocelot_port->qinq_mode = false;
> >>> +     }
> >>> +
> >>
> >> I don't understand how this can work just by using a boolean to track
> >> the state.
> >>
> >> This won't work properly if you are handling multiple QinQ VLAN entries.
> >>
> >> Also, I need Andrew and Florian to review and ACK the DSA layer
> >> changes that add the protocol value to the device notifier block.
> >
> > Hi David,
> > Thanks for reply.
> >
> > When setting bridge's VLAN protocol to 802.1AD by the command "ip link
> > set br0 type bridge vlan_protocol 802.1ad", it will call
> > dsa_slave_vlan_rx_add(dev, proto, vid) for every port in the bridge,
> > the parameter vid is port's pvid 1, if pvid's proto is 802.1AD, I will
> > enable switch's enable_qinq, and the related port's qinq_mode,
> >
> > When there are multiple QinQ VLAN entries, If one VLAN's proto is 802.1AD,
> I will enable switch and the related port into QinQ mode.
> 
> The enabling appears fine, the problem is the disabling, the first 802.1AD VLAN
> entry that gets deleted will lead to the port and switch no longer being in QinQ
> mode, and this does not look intended.
> --
> Florian

Thanks, Florian

I will add reference counter.
When deleting VLAN entry, only if the counter is zero, I will set the switch to exit QinQ mode.

hongbo
Hongbo Wang Aug. 6, 2020, 4:04 a.m. UTC | #7
> On 8/3/2020 11:36 PM, Hongbo Wang wrote:
> >>> +     if (vlan->proto == ETH_P_8021AD) {
> >>> +             ocelot->enable_qinq = true;
> >>> +             ocelot_port->qinq_mode = true;
> >>> +     }
> >>  ...
> >>> +     if (vlan->proto == ETH_P_8021AD) {
> >>> +             ocelot->enable_qinq = false;
> >>> +             ocelot_port->qinq_mode = false;
> >>> +     }
> >>> +
> >>
> >> I don't understand how this can work just by using a boolean to track
> >> the state.
> >>
> >> This won't work properly if you are handling multiple QinQ VLAN entries.
> >>
> >> Also, I need Andrew and Florian to review and ACK the DSA layer
> >> changes that add the protocol value to the device notifier block.
> >
> > Hi David,
> > Thanks for reply.
> >
> > When setting bridge's VLAN protocol to 802.1AD by the command "ip link
> > set br0 type bridge vlan_protocol 802.1ad", it will call
> > dsa_slave_vlan_rx_add(dev, proto, vid) for every port in the bridge,
> > the parameter vid is port's pvid 1, if pvid's proto is 802.1AD, I will
> > enable switch's enable_qinq, and the related port's qinq_mode,
> >
> > When there are multiple QinQ VLAN entries, If one VLAN's proto is 802.1AD,
> I will enable switch and the related port into QinQ mode.
> 
> The enabling appears fine, the problem is the disabling, the first 802.1AD VLAN
> entry that gets deleted will lead to the port and switch no longer being in QinQ
> mode, and this does not look intended.
> --
> Florian

When I try to add reference counter, I found that:
1.
the command "ip link set br0 type bridge vlan_protocol 802.1ad" call path is:
br_changelink -> __br_vlan_set_proto -> vlan_vid_add -> ... -> ndo_vlan_rx_add_vid -> dsa_slave_vlan_rx_add_vid(dev, proto, vid) -> felix_vlan_add

dsa_slave_vlan_rx_add_vid can pass correct protocol and vid(1) to ocelot driver.

vlan_vid_add is in net/8021q/vlan_core.c, it maintains a vid_list that stores the map of vid and protocol,
the function vlan_vid_info_get can read the map.

but when deleting bridge using "ip link del dev br0 type bridge", the call path is:
br_dev_delete -> ... -> br_switchdev_port_vlan_del -> ... -> dsa_slave_port_obj_del -> dsa_slave_vlan_del -> ... -> felix_vlan_del

br_switchdev_port_vlan_del is in net/bridge/br_switchdev.c, it didn't have the list for map vid and protocol,
so it can't pass correct protocol that corresponding with vid to ocelot driver.

2.
For ocelot QinQ case, the switch port linked to customer has different actions with the port for ISP,

uplink: Customer LAN(CTAG) -> swp0(vlan_aware:0 pop_cnt:0) -> swp1(add STAG) -> ISP MAN(STAG + CTAG)
downlink: ISP MAN(STAG + CTAG) -> swp1(vlan_aware:1 pop_cnt:1, pop STAG) -> swp0(only CTAG) -> Customer LAN

the different action is descripted in "4.3.3 Provider Bridges and Q-in-Q Operation" in VSC99599_1_00_TS.pdf

so I need a standard command to set swp0 and swp1 for different mode, 
but "ip link set br0 type bridge vlan_protocol 802.1ad" will set all ports into the same mode, it's not my intent.

3.
I thought some ways to resovle the above issue:
a. br_switchdev_port_vlan_del will pass default value ETH_P_8021Q, but don't care it in felix_vlan_del.
b. In felix_vlan_add and felix_vlan_del, only when vid is ocelot_port's pvid, it enable or disable switch's enable_qinq.
c. Maybe I can use devlink to set swp0 and swp1 into different mode.
d. let br_switchdev_port_vlan_del call vlan_vid_info_get to get protocol for vid, but vlan_vid_info_get is static in vlan_core.c, so this need to add related functions in br_switchdev.c.

Any comments is welcome!

Thanks
Hongbo
diff mbox series

Patch

diff --git a/drivers/net/dsa/ocelot/felix.c b/drivers/net/dsa/ocelot/felix.c
index c69d9592a2b7..72a27b61080e 100644
--- a/drivers/net/dsa/ocelot/felix.c
+++ b/drivers/net/dsa/ocelot/felix.c
@@ -131,10 +131,16 @@  static void felix_vlan_add(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 flags = vlan->flags;
 	u16 vid;
 	int err;
 
+	if (vlan->proto == ETH_P_8021AD) {
+		ocelot->enable_qinq = true;
+		ocelot_port->qinq_mode = true;
+	}
+
 	if (dsa_is_cpu_port(ds, port))
 		flags &= ~BRIDGE_VLAN_INFO_UNTAGGED;
 
@@ -154,9 +160,15 @@  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->enable_qinq = false;
+		ocelot_port->qinq_mode = false;
+	}
+
 	for (vid = vlan->vid_begin; vid <= vlan->vid_end; vid++) {
 		err = ocelot_vlan_del(ocelot, port, vid);
 		if (err) {
diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c
index f2d94b026d88..b5fec6855afd 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_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;
@@ -204,6 +228,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->enable_qinq && !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 +250,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_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..59020bb8fe68 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,7 @@  struct ocelot {
 	/* Protects the PTP clock */
 	spinlock_t			ptp_clock_lock;
 	struct ptp_pin_desc		ptp_pins[OCELOT_PTP_PINS_NUM];
+	bool				enable_qinq;
 };
 
 struct ocelot_policer {