From ebca842041d737b7441748a17ffd535aab851fce Mon Sep 17 00:00:00 2001
From: Phil Sutter <phil@nwl.cc>
Date: Tue, 13 May 2014 01:32:11 +0200
Subject: [PATCH] net: allow PHY drivers to insert packet mangle hooks

---
 include/linux/netdevice.h |  8 ++++++++
 include/linux/skbuff.h    | 14 ++++----------
 include/uapi/linux/if.h   |  1 +
 net/Kconfig               |  6 ++++++
 net/core/dev.c            | 36 ++++++++++++++++++++++++++++--------
 net/core/skbuff.c         | 17 +++++++++++++++++
 net/ethernet/eth.c        |  6 ++++++
 7 files changed, 70 insertions(+), 18 deletions(-)

diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h
index 911718f..8e8dd46 100644
--- a/include/linux/netdevice.h
+++ b/include/linux/netdevice.h
@@ -1245,6 +1245,11 @@ struct net_device {
 	const struct ethtool_ops *ethtool_ops;
 	const struct forwarding_accel_ops *fwd_ops;
 
+#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
+
 	/* Hardware header description */
 	const struct header_ops *header_ops;
 
@@ -1313,6 +1318,9 @@ struct net_device {
 	void			*ax25_ptr;	/* AX.25 specific data */
 	struct wireless_dev	*ieee80211_ptr;	/* IEEE 802.11 specific data,
 						   assign before registering */
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+	void			*phy_ptr; /* PHY device specific data */
+#endif
 
 /*
  * Cache lines mostly used on receive path (including eth_type_trans())
diff --git a/include/linux/skbuff.h b/include/linux/skbuff.h
index 15ede6a..5530766 100644
--- a/include/linux/skbuff.h
+++ b/include/linux/skbuff.h
@@ -1858,6 +1858,10 @@ static inline int pskb_trim(struct sk_buff *skb, unsigned int len)
 	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
@@ -1966,16 +1970,6 @@ static inline struct sk_buff *dev_alloc_skb(unsigned int length)
 }
 
 
-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)
 {
diff --git a/include/uapi/linux/if.h b/include/uapi/linux/if.h
index d758163..7ffa548 100644
--- a/include/uapi/linux/if.h
+++ b/include/uapi/linux/if.h
@@ -84,6 +84,7 @@
 #define IFF_LIVE_ADDR_CHANGE 0x100000	/* device supports hardware address
 					 * change when it's running */
 #define IFF_MACVLAN 0x200000		/* Macvlan device */
+#define IFF_NO_IP_ALIGN	0x400000	/* do not ip-align allocated rx pkts */
 
 
 #define IF_GET_IFACE	0x0001		/* for querying only */
diff --git a/net/Kconfig b/net/Kconfig
index e411046..970c52a 100644
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -24,6 +24,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
diff --git a/net/core/dev.c b/net/core/dev.c
index fccc195..2e0ba23 100644
--- a/net/core/dev.c
+++ b/net/core/dev.c
@@ -2607,10 +2607,20 @@ int dev_hard_start_xmit(struct sk_buff *skb, struct net_device *dev,
 		if (!list_empty(&ptype_all))
 			dev_queue_xmit_nit(skb, dev);
 
-		skb_len = skb->len;
-		trace_net_dev_start_xmit(skb, dev);
-		rc = ops->ndo_start_xmit(skb, dev);
-		trace_net_dev_xmit(skb, rc, dev, skb_len);
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+		if (!dev->eth_mangle_tx ||
+		    (skb = dev->eth_mangle_tx(dev, skb)) != NULL)
+#else
+		if (1)
+#endif
+		{
+			skb_len = skb->len;
+			trace_net_dev_start_xmit(skb, dev);
+			rc = ops->ndo_start_xmit(skb, dev);
+			trace_net_dev_xmit(skb, rc, dev, skb_len);
+		} else {
+			rc = NETDEV_TX_OK;
+		}
 		if (rc == NETDEV_TX_OK)
 			txq_trans_update(txq);
 		return rc;
@@ -2626,10 +2636,20 @@ gso:
 		if (!list_empty(&ptype_all))
 			dev_queue_xmit_nit(nskb, dev);
 
-		skb_len = nskb->len;
-		trace_net_dev_start_xmit(nskb, dev);
-		rc = ops->ndo_start_xmit(nskb, dev);
-		trace_net_dev_xmit(nskb, rc, dev, skb_len);
+#ifdef CONFIG_ETHERNET_PACKET_MANGLE
+		if (!dev->eth_mangle_tx ||
+		    (nskb = dev->eth_mangle_tx(dev, nskb)) != NULL)
+#else
+		if (1)
+#endif
+		{
+			skb_len = nskb->len;
+			trace_net_dev_start_xmit(nskb, dev);
+			rc = ops->ndo_start_xmit(nskb, dev);
+			trace_net_dev_xmit(nskb, rc, dev, skb_len);
+		} else {
+			rc = NETDEV_TX_OK;
+		}
 		if (unlikely(rc != NETDEV_TX_OK)) {
 			if (rc & ~NETDEV_TX_MASK)
 				goto out_kfree_gso_skb;
diff --git a/net/core/skbuff.c b/net/core/skbuff.c
index e5ae776e..400ff2a 100644
--- a/net/core/skbuff.c
+++ b/net/core/skbuff.c
@@ -62,6 +62,7 @@
 #include <linux/scatterlist.h>
 #include <linux/errqueue.h>
 #include <linux/prefetch.h>
+#include <uapi/linux/if.h>
 
 #include <net/protocol.h>
 #include <net/dst.h>
@@ -439,6 +440,22 @@ struct sk_buff *__netdev_alloc_skb(struct net_device *dev,
 }
 EXPORT_SYMBOL(__netdev_alloc_skb);
 
+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 skb_add_rx_frag(struct sk_buff *skb, int i, struct page *page, int off,
 		     int size, unsigned int truesize)
 {
diff --git a/net/ethernet/eth.c b/net/ethernet/eth.c
index 5dc638c..f4fd124 100644
--- a/net/ethernet/eth.c
+++ b/net/ethernet/eth.c
@@ -161,6 +161,12 @@ __be16 eth_type_trans(struct sk_buff *skb, struct net_device *dev)
 	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);
 	skb_pull_inline(skb, ETH_HLEN);
 	eth = eth_hdr(skb);
-- 
1.8.5.3

