net: dsa: tag_sja1105: implement sub-VLAN decoding

Create a subvlan_map as part of each port's tagger private structure.
This keeps reverse mappings of bridge-to-dsa_8021q VLAN retagging rules.

Note that as of this patch, this piece of code is never engaged, due to
the fact that the driver hasn't installed any retagging rule, so we'll
always see packets with a subvlan code of 0 (untagged).

Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Reviewed-by: Florian Fainelli <f.fainelli@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/dsa/sja1105/sja1105_main.c b/drivers/net/dsa/sja1105/sja1105_main.c
index b7e4a85caaded451a95e981533ef57beb2f03d92..fd15a18596eac05d59210d1c16023d4b1977cc5a 100644 (file)
--- a/drivers/net/dsa/sja1105/sja1105_main.c
+++ b/drivers/net/dsa/sja1105/sja1105_main.c
@@ -2856,6 +2856,7 @@ static int sja1105_probe(struct spi_device *spi)
                struct sja1105_port *sp = &priv->ports[port];
                struct dsa_port *dp = dsa_to_port(ds, port);
                struct net_device *slave;
+               int subvlan;
 
                if (!dsa_is_user_port(ds, port))
                        continue;
@@ -2876,6 +2877,9 @@ static int sja1105_probe(struct spi_device *spi)
                }
                skb_queue_head_init(&sp->xmit_queue);
                sp->xmit_tpid = ETH_P_SJA1105;
+
+               for (subvlan = 0; subvlan < DSA_8021Q_N_SUBVLAN; subvlan++)
+                       sp->subvlan_map[subvlan] = VLAN_N_VID;
        }
 
        return 0;

diff --git a/include/linux/dsa/sja1105.h b/include/linux/dsa/sja1105.h
index f821d08b1b5fefac523411503ac21fbd7b7dda89..dd93735ae2282d2653277c86df05c15ee1e9e448 100644 (file)
--- a/include/linux/dsa/sja1105.h
+++ b/include/linux/dsa/sja1105.h
@@ -9,6 +9,7 @@
 
 #include <linux/skbuff.h>
 #include <linux/etherdevice.h>
+#include <linux/dsa/8021q.h>
 #include <net/dsa.h>
 
 #define ETH_P_SJA1105                          ETH_P_DSA_8021Q
@@ -53,6 +54,7 @@ struct sja1105_skb_cb {
        ((struct sja1105_skb_cb *)DSA_SKB_CB_PRIV(skb))
 
 struct sja1105_port {
+       u16 subvlan_map[DSA_8021Q_N_SUBVLAN];
        struct kthread_worker *xmit_worker;
        struct kthread_work xmit_work;
        struct sk_buff_head xmit_queue;

diff --git a/net/dsa/tag_sja1105.c b/net/dsa/tag_sja1105.c
index 398e2b9a1b965773a129b54b4263a1bc750ed3ef..ad105550b1455a1e0aa2dcdac1810112efa66afe 100644 (file)
--- a/net/dsa/tag_sja1105.c
+++ b/net/dsa/tag_sja1105.c
@@ -254,6 +254,20 @@ static struct sk_buff
        return skb;
 }
 
+static void sja1105_decode_subvlan(struct sk_buff *skb, u16 subvlan)
+{
+       struct dsa_port *dp = dsa_slave_to_port(skb->dev);
+       struct sja1105_port *sp = dp->priv;
+       u16 vid = sp->subvlan_map[subvlan];
+       u16 vlan_tci;
+
+       if (vid == VLAN_N_VID)
+               return;
+
+       vlan_tci = (skb->priority << VLAN_PRIO_SHIFT) | vid;
+       __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci);
+}
+
 static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
                                   struct net_device *netdev,
                                   struct packet_type *pt)
@@ -263,6 +277,7 @@ static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
        struct ethhdr *hdr;
        u16 tpid, vid, tci;
        bool is_link_local;
+       u16 subvlan = 0;
        bool is_tagged;
        bool is_meta;
 
@@ -286,6 +301,7 @@ static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
                source_port = dsa_8021q_rx_source_port(vid);
                switch_id = dsa_8021q_rx_switch_id(vid);
                skb->priority = (tci & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
+               subvlan = dsa_8021q_rx_subvlan(vid);
        } else if (is_link_local) {
                /* Management traffic path. Switch embeds the switch ID and
                 * port ID into bytes of the destination MAC, courtesy of
@@ -310,6 +326,9 @@ static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
                return NULL;
        }
 
+       if (subvlan)
+               sja1105_decode_subvlan(skb, subvlan);
+
        return sja1105_rcv_meta_state_machine(skb, &meta, is_link_local,
                                              is_meta);
 }