From ffe387740bbe88dd88bbe04d6375902708003d6e Mon Sep 17 00:00:00 2001
From: Felix Fietkau <nbd@nbd.name>
Date: Fri, 7 Jul 2017 17:25:00 +0200
Subject: net: add packet mangeling

ar8216 switches have a hardware bug, which renders normal 802.1q support
unusable. Packet mangling is required to fix up the vlan for incoming
packets.

Signed-off-by: Felix Fietkau <nbd@nbd.name>
---
 include/linux/netdevice.h | 10 ++++++++++
 include/linux/skbuff.h    | 14 ++++----------
 net/Kconfig               |  6 ++++++
 net/core/dev.c            | 20 +++++++++++++++-----
 net/core/skbuff.c         | 17 +++++++++++++++++
 net/ethernet/eth.c        |  6 ++++++
 6 files changed, 58 insertions(+), 15 deletions(-)

--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -1687,6 +1687,7 @@ enum netdev_priv_flags {
 	IFF_L3MDEV_RX_HANDLER		= 1<<29,
 	IFF_NO_ADDRCONF			= BIT_ULL(30),
 	IFF_TX_SKB_NO_LINEAR		= BIT_ULL(31),
+	IFF_NO_IP_ALIGN			= BIT_ULL(32),
 };
 
 /* Specifies the type of the struct net_device::ml_priv pointer */
@@ -2168,6 +2169,11 @@ struct net_device {
 	const struct tlsdev_ops *tlsdev_ops;
 #endif
 
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+	void (*eth_mangle_rx)(struct net_device *dev, struct sk_buff *skb);
+	struct sk_buff *(*eth_mangle_tx)(struct net_device *dev, struct sk_buff *skb);
+#endif
+
 	unsigned int		operstate;
 	unsigned char		link_mode;
 
@@ -2237,6 +2243,10 @@ struct net_device {
 	struct mctp_dev __rcu	*mctp_ptr;
 #endif
 
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+	void			*phy_ptr; /* PHY device specific data */
+#endif
+
 /*
  * Cache lines mostly used on receive path (including eth_type_trans())
  */
--- a/include/linux/skbuff.h
+++ b/include/linux/skbuff.h
@@ -3242,6 +3242,10 @@ static inline int pskb_trim(struct sk_bu
 	return (len < skb->len) ? __pskb_trim(skb, len) : 0;
 }
 
+extern struct sk_buff *__netdev_alloc_skb_ip_align(struct net_device *dev,
+		unsigned int length, gfp_t gfp);
+
+
 /**
  *	pskb_trim_unique - remove end from a paged unique (not cloned) buffer
  *	@skb: buffer to alter
@@ -3407,16 +3411,6 @@ static inline struct sk_buff *dev_alloc_
 }
 
 
-static inline struct sk_buff *__netdev_alloc_skb_ip_align(struct net_device *dev,
-		unsigned int length, gfp_t gfp)
-{
-	struct sk_buff *skb = __netdev_alloc_skb(dev, length + NET_IP_ALIGN, gfp);
-
-	if (NET_IP_ALIGN && skb)
-		skb_reserve(skb, NET_IP_ALIGN);
-	return skb;
-}
-
 static inline struct sk_buff *netdev_alloc_skb_ip_align(struct net_device *dev,
 		unsigned int length)
 {
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -26,6 +26,12 @@ menuconfig NET
 
 if NET
 
+config ETHERNET_PACKET_MANGLE
+	bool
+	help
+	  This option can be selected by phy drivers that need to mangle
+	  packets going in or out of an ethernet device.
+
 config WANT_COMPAT_NETLINK_MESSAGES
 	bool
 	help
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -3671,6 +3671,11 @@ static int xmit_one(struct sk_buff *skb,
 	if (dev_nit_active(dev))
 		dev_queue_xmit_nit(skb, dev);
 
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+	if (dev->eth_mangle_tx && !(skb = dev->eth_mangle_tx(dev, skb)))
+		return NETDEV_TX_OK;
+#endif
+
 	len = skb->len;
 	trace_net_dev_start_xmit(skb, dev);
 	rc = netdev_start_xmit(skb, dev, txq, more);
--- a/net/core/skbuff.c
+++ b/net/core/skbuff.c
@@ -64,6 +64,7 @@
 #include <linux/mpls.h>
 #include <linux/kcov.h>
 #include <linux/iov_iter.h>
+#include <linux/if.h>
 
 #include <net/protocol.h>
 #include <net/dst.h>
@@ -326,6 +327,22 @@ void *__napi_alloc_frag_align(unsigned i
 }
 EXPORT_SYMBOL(__napi_alloc_frag_align);
 
+struct sk_buff *__netdev_alloc_skb_ip_align(struct net_device *dev,
+		unsigned int length, gfp_t gfp)
+{
+	struct sk_buff *skb = __netdev_alloc_skb(dev, length + NET_IP_ALIGN, gfp);
+
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+	if (dev && (dev->priv_flags & IFF_NO_IP_ALIGN))
+		return skb;
+#endif
+
+	if (NET_IP_ALIGN && skb)
+		skb_reserve(skb, NET_IP_ALIGN);
+	return skb;
+}
+EXPORT_SYMBOL(__netdev_alloc_skb_ip_align);
+
 void *__netdev_alloc_frag_align(unsigned int fragsz, unsigned int align_mask)
 {
 	void *data;
--- a/net/ethernet/eth.c
+++ b/net/ethernet/eth.c
@@ -159,6 +159,12 @@ __be16 eth_type_trans(struct sk_buff *sk
 	const struct ethhdr *eth;
 
 	skb->dev = dev;
+
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+	if (dev->eth_mangle_rx)
+		dev->eth_mangle_rx(dev, skb);
+#endif
+
 	skb_reset_mac_header(skb);
 
 	eth = eth_skb_pull_mac(skb);
