net: enetc: fix incorrect TPID when receiving 802.1ad tagged packets

When the enetc ports have rx-vlan-offload enabled, they report a TPID of
ETH_P_8021Q regardless of what was actually in the packet. When
rx-vlan-offload is disabled, packets have the proper TPID. Fix this
inconsistency by finishing the TODO left in the code.

Fixes: d4fd0404c1c9 ("enetc: Introduce basic PF and VF ENETC ethernet drivers")
Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/freescale/enetc/enetc.c b/drivers/net/ethernet/freescale/enetc/enetc.c
index 9bcceb7..8ddf0cd 100644 (file)
--- a/drivers/net/ethernet/freescale/enetc/enetc.c
+++ b/drivers/net/ethernet/freescale/enetc/enetc.c
@@ -523,9 +523,8 @@ static void enetc_get_rx_tstamp(struct net_device *ndev,
 static void enetc_get_offloads(struct enetc_bdr *rx_ring,
                               union enetc_rx_bd *rxbd, struct sk_buff *skb)
 {
-#ifdef CONFIG_FSL_ENETC_PTP_CLOCK
        struct enetc_ndev_priv *priv = netdev_priv(rx_ring->ndev);
-#endif
+
        /* TODO: hashing */
        if (rx_ring->ndev->features & NETIF_F_RXCSUM) {
                u16 inet_csum = le16_to_cpu(rxbd->r.inet_csum);
@@ -534,12 +533,31 @@ static void enetc_get_offloads(struct enetc_bdr *rx_ring,
                skb->ip_summed = CHECKSUM_COMPLETE;
        }
 
-       /* copy VLAN to skb, if one is extracted, for now we assume it's a
-        * standard TPID, but HW also supports custom values
-        */
-       if (le16_to_cpu(rxbd->r.flags) & ENETC_RXBD_FLAG_VLAN)
-               __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
-                                      le16_to_cpu(rxbd->r.vlan_opt));
+       if (le16_to_cpu(rxbd->r.flags) & ENETC_RXBD_FLAG_VLAN) {
+               __be16 tpid = 0;
+
+               switch (le16_to_cpu(rxbd->r.flags) & ENETC_RXBD_FLAG_TPID) {
+               case 0:
+                       tpid = htons(ETH_P_8021Q);
+                       break;
+               case 1:
+                       tpid = htons(ETH_P_8021AD);
+                       break;
+               case 2:
+                       tpid = htons(enetc_port_rd(&priv->si->hw,
+                                                  ENETC_PCVLANR1));
+                       break;
+               case 3:
+                       tpid = htons(enetc_port_rd(&priv->si->hw,
+                                                  ENETC_PCVLANR2));
+                       break;
+               default:
+                       break;
+               }
+
+               __vlan_hwaccel_put_tag(skb, tpid, le16_to_cpu(rxbd->r.vlan_opt));
+       }
+
 #ifdef CONFIG_FSL_ENETC_PTP_CLOCK
        if (priv->active_offloads & ENETC_F_RX_TSTAMP)
                enetc_get_rx_tstamp(rx_ring->ndev, rxbd, skb);

diff --git a/drivers/net/ethernet/freescale/enetc/enetc_hw.h b/drivers/net/ethernet/freescale/enetc/enetc_hw.h
index 8b54562..a62604a 100644 (file)
--- a/drivers/net/ethernet/freescale/enetc/enetc_hw.h
+++ b/drivers/net/ethernet/freescale/enetc/enetc_hw.h
@@ -172,6 +172,8 @@ enum enetc_bdr_type {TX, RX};
 #define ENETC_PSIPMAR0(n)      (0x0100 + (n) * 0x8) /* n = SI index */
 #define ENETC_PSIPMAR1(n)      (0x0104 + (n) * 0x8)
 #define ENETC_PVCLCTR          0x0208
+#define ENETC_PCVLANR1         0x0210
+#define ENETC_PCVLANR2         0x0214
 #define ENETC_VLAN_TYPE_C      BIT(0)
 #define ENETC_VLAN_TYPE_S      BIT(1)
 #define ENETC_PVCLCTR_OVTPIDL(bmp)     ((bmp) & 0xff) /* VLAN_TYPE */
@@ -570,6 +572,7 @@ union enetc_rx_bd {
 #define ENETC_RXBD_LSTATUS(flags)      ((flags) << 16)
 #define ENETC_RXBD_FLAG_VLAN   BIT(9)
 #define ENETC_RXBD_FLAG_TSTMP  BIT(10)
+#define ENETC_RXBD_FLAG_TPID   GENMASK(1, 0)
 
 #define ENETC_MAC_ADDR_FILT_CNT        8 /* # of supported entries per port */
 #define EMETC_MAC_ADDR_FILT_RES        3 /* # of reserved entries at the beginning */