10 files changed, 173 insertions, 96 deletions

diff --git a/drivers/net/ethernet/xscale/Kconfig b/drivers/net/ethernet/xscale/Kconfig
index 3f431019e615..b81bc9fca378 100644
--- a/drivers/net/ethernet/xscale/Kconfig
+++ b/drivers/net/ethernet/xscale/Kconfig
@@ -23,6 +23,7 @@ config IXP4XX_ETH
         tristate "Intel IXP4xx Ethernet support"
         depends on ARM && ARCH_IXP4XX && IXP4XX_NPE && IXP4XX_QMGR
         select PHYLIB
+        select NET_PTP_CLASSIFY
         ---help---
           Say Y here if you want to use built-in Ethernet ports
           on IXP4xx processor.
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index 352c5e45fe9c..6a999e6814a0 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -27,6 +27,7 @@
 #include <linux/module.h>
 #include <linux/net_tstamp.h>
 #include <linux/netdevice.h>
+#include <linux/if_vlan.h>
 #include <linux/phy.h>
 #include <linux/ptp_classify.h>
 #include <linux/ptp_clock_kernel.h>
diff --git a/drivers/ptp/Kconfig b/drivers/ptp/Kconfig
index 5a7910e61e17..6963bdf54175 100644
--- a/drivers/ptp/Kconfig
+++ b/drivers/ptp/Kconfig
@@ -7,6 +7,7 @@ menu "PTP clock support"
 config PTP_1588_CLOCK
         tristate "PTP clock support"
         select PPS
+        select NET_PTP_CLASSIFY
         help
           The IEEE 1588 standard defines a method to precisely
           synchronize distributed clocks over Ethernet networks. The
diff --git a/include/linux/ptp_classify.h b/include/linux/ptp_classify.h
index 6d3b0a2ef9ce..7dfed71d76a6 100644
--- a/include/linux/ptp_classify.h
+++ b/include/linux/ptp_classify.h
@@ -23,11 +23,8 @@
 #ifndef _PTP_CLASSIFY_H_
 #define _PTP_CLASSIFY_H_
 
-#include <linux/if_ether.h>
-#include <linux/if_vlan.h>
 #include <linux/ip.h>
-#include <linux/filter.h>
-#include <linux/in.h>
+#include <linux/skbuff.h>
 
 #define PTP_CLASS_NONE  0x00 /* not a PTP event message */
 #define PTP_CLASS_V1    0x01 /* protocol version 1 */
@@ -40,7 +37,7 @@
 #define PTP_CLASS_PMASK 0xf0 /* mask for the packet type field */
 
 #define PTP_CLASS_V1_IPV4 (PTP_CLASS_V1 | PTP_CLASS_IPV4)
-#define PTP_CLASS_V1_IPV6 (PTP_CLASS_V1 | PTP_CLASS_IPV6) /*probably DNE*/
+#define PTP_CLASS_V1_IPV6 (PTP_CLASS_V1 | PTP_CLASS_IPV6) /* probably DNE */
 #define PTP_CLASS_V2_IPV4 (PTP_CLASS_V2 | PTP_CLASS_IPV4)
 #define PTP_CLASS_V2_IPV6 (PTP_CLASS_V2 | PTP_CLASS_IPV6)
 #define PTP_CLASS_V2_L2   (PTP_CLASS_V2 | PTP_CLASS_L2)
@@ -49,82 +46,34 @@
 #define PTP_EV_PORT 319
 #define PTP_GEN_BIT 0x08 /* indicates general message, if set in message type */
 
-#define OFF_ETYPE       12
-#define OFF_IHL         14
-#define OFF_FRAG        20
-#define OFF_PROTO4      23
-#define OFF_NEXT        6
-#define OFF_UDP_DST     2
-
 #define OFF_PTP_SOURCE_UUID     22 /* PTPv1 only */
 #define OFF_PTP_SEQUENCE_ID     30
 #define OFF_PTP_CONTROL         32 /* PTPv1 only */
 
-#define IPV4_HLEN(data) (((struct iphdr *)(data + OFF_IHL))->ihl << 2)
-
+/* Below defines should actually be removed at some point in time. */
 #define IP6_HLEN        40
 #define UDP_HLEN        8
-
-#define RELOFF_DST4     (ETH_HLEN + OFF_UDP_DST)
-#define OFF_DST6        (ETH_HLEN + IP6_HLEN + OFF_UDP_DST)
+#define OFF_IHL         14
 #define OFF_PTP6        (ETH_HLEN + IP6_HLEN + UDP_HLEN)
+#define IPV4_HLEN(data) (((struct iphdr *)(data + OFF_IHL))->ihl << 2)
 
-#define OP_AND  (BPF_ALU | BPF_AND  | BPF_K)
-#define OP_JEQ  (BPF_JMP | BPF_JEQ  | BPF_K)
-#define OP_JSET (BPF_JMP | BPF_JSET | BPF_K)
-#define OP_LDB  (BPF_LD  | BPF_B    | BPF_ABS)
-#define OP_LDH  (BPF_LD  | BPF_H    | BPF_ABS)
-#define OP_LDHI (BPF_LD  | BPF_H    | BPF_IND)
-#define OP_LDX  (BPF_LDX | BPF_B    | BPF_MSH)
-#define OP_OR   (BPF_ALU | BPF_OR   | BPF_K)
-#define OP_RETA (BPF_RET | BPF_A)
-#define OP_RETK (BPF_RET | BPF_K)
-
-#define PTP_FILTER \
-        {OP_LDH,        0,   0, OFF_ETYPE               }, /*              */ \
-        {OP_JEQ,        0,  12, ETH_P_IP                }, /* f goto L20   */ \
-        {OP_LDB,        0,   0, OFF_PROTO4              }, /*              */ \
-        {OP_JEQ,        0,   9, IPPROTO_UDP             }, /* f goto L10   */ \
-        {OP_LDH,        0,   0, OFF_FRAG                }, /*              */ \
-        {OP_JSET,       7,   0, 0x1fff                  }, /* t goto L11   */ \
-        {OP_LDX,        0,   0, OFF_IHL                 }, /*              */ \
-        {OP_LDHI,       0,   0, RELOFF_DST4             }, /*              */ \
-        {OP_JEQ,        0,   4, PTP_EV_PORT             }, /* f goto L12   */ \
-        {OP_LDHI,       0,   0, ETH_HLEN + UDP_HLEN     }, /*              */ \
-        {OP_AND,        0,   0, PTP_CLASS_VMASK         }, /*              */ \
-        {OP_OR,         0,   0, PTP_CLASS_IPV4          }, /*              */ \
-        {OP_RETA,       0,   0, 0                       }, /*              */ \
-/*L1x*/ {OP_RETK,       0,   0, PTP_CLASS_NONE          }, /*              */ \
-/*L20*/ {OP_JEQ,        0,   9, ETH_P_IPV6              }, /* f goto L40   */ \
-        {OP_LDB,        0,   0, ETH_HLEN + OFF_NEXT     }, /*              */ \
-        {OP_JEQ,        0,   6, IPPROTO_UDP             }, /* f goto L30   */ \
-        {OP_LDH,        0,   0, OFF_DST6                }, /*              */ \
-        {OP_JEQ,        0,   4, PTP_EV_PORT             }, /* f goto L31   */ \
-        {OP_LDH,        0,   0, OFF_PTP6                }, /*              */ \
-        {OP_AND,        0,   0, PTP_CLASS_VMASK         }, /*              */ \
-        {OP_OR,         0,   0, PTP_CLASS_IPV6          }, /*              */ \
-        {OP_RETA,       0,   0, 0                       }, /*              */ \
-/*L3x*/ {OP_RETK,       0,   0, PTP_CLASS_NONE          }, /*              */ \
-/*L40*/ {OP_JEQ,        0,   9, ETH_P_8021Q             }, /* f goto L50   */ \
-        {OP_LDH,        0,   0, OFF_ETYPE + 4           }, /*              */ \
-        {OP_JEQ,        0,  15, ETH_P_1588              }, /* f goto L60   */ \
-        {OP_LDB,        0,   0, ETH_HLEN + VLAN_HLEN    }, /*              */ \
-        {OP_AND,        0,   0, PTP_GEN_BIT             }, /*              */ \
-        {OP_JEQ,        0,  12, 0                       }, /* f goto L6x   */ \
-        {OP_LDH,        0,   0, ETH_HLEN + VLAN_HLEN    }, /*              */ \
-        {OP_AND,        0,   0, PTP_CLASS_VMASK         }, /*              */ \
-        {OP_OR,         0,   0, PTP_CLASS_VLAN          }, /*              */ \
-        {OP_RETA,       0,   0, 0                       }, /*              */ \
-/*L50*/ {OP_JEQ,        0,   7, ETH_P_1588              }, /* f goto L61   */ \
-        {OP_LDB,        0,   0, ETH_HLEN                }, /*              */ \
-        {OP_AND,        0,   0, PTP_GEN_BIT             }, /*              */ \
-        {OP_JEQ,        0,   4, 0                       }, /* f goto L6x   */ \
-        {OP_LDH,        0,   0, ETH_HLEN                }, /*              */ \
-        {OP_AND,        0,   0, PTP_CLASS_VMASK         }, /*              */ \
-        {OP_OR,         0,   0, PTP_CLASS_L2            }, /*              */ \
-        {OP_RETA,       0,   0, 0                       }, /*              */ \
-/*L6x*/ {OP_RETK,       0,   0, PTP_CLASS_NONE          },
-
+#if defined(CONFIG_NET_PTP_CLASSIFY)
+/**
+ * ptp_classify_raw - classify a PTP packet
+ * @skb: buffer
+ *
+ * Runs a minimal BPF dissector to classify a network packet to
+ * determine the PTP class. In case the skb does not contain any
+ * PTP protocol data, PTP_CLASS_NONE will be returned, otherwise
+ * PTP_CLASS_V1_IPV{4,6}, PTP_CLASS_V2_IPV{4,6} or
+ * PTP_CLASS_V2_{L2,VLAN}, depending on the packet content.
+ */
 unsigned int ptp_classify_raw(const struct sk_buff *skb);
 
+void __init ptp_classifier_init(void);
+#else
+static inline void ptp_classifier_init(void)
+{
+}
 #endif
+#endif /* _PTP_CLASSIFY_H_ */
diff --git a/include/linux/skbuff.h b/include/linux/skbuff.h
index 18ef0224fb6a..31edf63937a1 100644
--- a/include/linux/skbuff.h
+++ b/include/linux/skbuff.h
@@ -2630,8 +2630,6 @@ static inline ktime_t net_invalid_timestamp(void)
         return ktime_set(0, 0);
 }
 
-void skb_timestamping_init(void);
-
 #ifdef CONFIG_NETWORK_PHY_TIMESTAMPING
 
 void skb_clone_tx_timestamp(struct sk_buff *skb);
diff --git a/net/Kconfig b/net/Kconfig
index e411046a62e3..d1f6f968fc09 100644
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -89,8 +89,12 @@ config NETWORK_SECMARK
           to nfmark, but designated for security purposes.
           If you are unsure how to answer this question, answer N.
 
+config NET_PTP_CLASSIFY
+        def_bool n
+
 config NETWORK_PHY_TIMESTAMPING
         bool "Timestamping in PHY devices"
+        select NET_PTP_CLASSIFY
         help
           This allows timestamping of network packets by PHYs with
           hardware timestamping capabilities. This option adds some
diff --git a/net/core/Makefile b/net/core/Makefile
index 9628c20acff6..826b925aa453 100644
--- a/net/core/Makefile
+++ b/net/core/Makefile
@@ -21,5 +21,6 @@ obj-$(CONFIG_FIB_RULES) += fib_rules.o
 obj-$(CONFIG_TRACEPOINTS) += net-traces.o
 obj-$(CONFIG_NET_DROP_MONITOR) += drop_monitor.o
 obj-$(CONFIG_NETWORK_PHY_TIMESTAMPING) += timestamping.o
+obj-$(CONFIG_NET_PTP_CLASSIFY) += ptp_classifier.o
 obj-$(CONFIG_CGROUP_NET_PRIO) += netprio_cgroup.o
 obj-$(CONFIG_CGROUP_NET_CLASSID) += netclassid_cgroup.o
diff --git a/net/core/ptp_classifier.c b/net/core/ptp_classifier.c
new file mode 100644
index 000000000000..eaba0f68f860
--- /dev/null
+++ b/net/core/ptp_classifier.c
@@ -0,0 +1,141 @@
+/* PTP classifier
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of version 2 of the GNU General Public
+ * License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ */
+
+/* The below program is the bpf_asm (tools/net/) representation of
+ * the opcode array in the ptp_filter structure.
+ *
+ * For convenience, this can easily be altered and reviewed with
+ * bpf_asm and bpf_dbg, e.g. `./bpf_asm -c prog` where prog is a
+ * simple file containing the below program:
+ *
+ * ldh [12]                        ; load ethertype
+ *
+ * ; PTP over UDP over IPv4 over Ethernet
+ * test_ipv4:
+ *   jneq #0x800, test_ipv6        ; ETH_P_IP ?
+ *   ldb [23]                      ; load proto
+ *   jneq #17, drop_ipv4           ; IPPROTO_UDP ?
+ *   ldh [20]                      ; load frag offset field
+ *   jset #0x1fff, drop_ipv4       ; don't allow fragments
+ *   ldxb 4*([14]&0xf)             ; load IP header len
+ *   ldh [x + 16]                  ; load UDP dst port
+ *   jneq #319, drop_ipv4          ; is port PTP_EV_PORT ?
+ *   ldh [x + 22]                  ; load payload
+ *   and #0xf                      ; mask PTP_CLASS_VMASK
+ *   or #0x10                      ; PTP_CLASS_IPV4
+ *   ret a                         ; return PTP class
+ *   drop_ipv4: ret #0x0           ; PTP_CLASS_NONE
+ *
+ * ; PTP over UDP over IPv6 over Ethernet
+ * test_ipv6:
+ *   jneq #0x86dd, test_8021q      ; ETH_P_IPV6 ?
+ *   ldb [20]                      ; load proto
+ *   jneq #17, drop_ipv6           ; IPPROTO_UDP ?
+ *   ldh [56]                      ; load UDP dst port
+ *   jneq #319, drop_ipv6          ; is port PTP_EV_PORT ?
+ *   ldh [62]                      ; load payload
+ *   and #0xf                      ; mask PTP_CLASS_VMASK
+ *   or #0x20                      ; PTP_CLASS_IPV6
+ *   ret a                         ; return PTP class
+ *   drop_ipv6: ret #0x0           ; PTP_CLASS_NONE
+ *
+ * ; PTP over 802.1Q over Ethernet
+ * test_8021q:
+ *   jneq #0x8100, test_ieee1588   ; ETH_P_8021Q ?
+ *   ldh [16]                      ; load inner type
+ *   jneq #0x88f7, drop_ieee1588   ; ETH_P_1588 ?
+ *   ldb [18]                      ; load payload
+ *   and #0x8                      ; as we don't have ports here, test
+ *   jneq #0x0, drop_ieee1588      ; for PTP_GEN_BIT and drop these
+ *   ldh [18]                      ; reload payload
+ *   and #0xf                      ; mask PTP_CLASS_VMASK
+ *   or #0x40                      ; PTP_CLASS_V2_VLAN
+ *   ret a                         ; return PTP class
+ *
+ * ; PTP over Ethernet
+ * test_ieee1588:
+ *   jneq #0x88f7, drop_ieee1588   ; ETH_P_1588 ?
+ *   ldb [14]                      ; load payload
+ *   and #0x8                      ; as we don't have ports here, test
+ *   jneq #0x0, drop_ieee1588      ; for PTP_GEN_BIT and drop these
+ *   ldh [14]                      ; reload payload
+ *   and #0xf                      ; mask PTP_CLASS_VMASK
+ *   or #0x30                      ; PTP_CLASS_L2
+ *   ret a                         ; return PTP class
+ *   drop_ieee1588: ret #0x0       ; PTP_CLASS_NONE
+ */
+
+#include <linux/skbuff.h>
+#include <linux/filter.h>
+#include <linux/ptp_classify.h>
+
+static struct sk_filter *ptp_insns __read_mostly;
+
+unsigned int ptp_classify_raw(const struct sk_buff *skb)
+{
+        return SK_RUN_FILTER(ptp_insns, skb);
+}
+EXPORT_SYMBOL_GPL(ptp_classify_raw);
+
+void __init ptp_classifier_init(void)
+{
+        static struct sock_filter ptp_filter[] = {
+                { 0x28,  0,  0, 0x0000000c },
+                { 0x15,  0, 12, 0x00000800 },
+                { 0x30,  0,  0, 0x00000017 },
+                { 0x15,  0,  9, 0x00000011 },
+                { 0x28,  0,  0, 0x00000014 },
+                { 0x45,  7,  0, 0x00001fff },
+                { 0xb1,  0,  0, 0x0000000e },
+                { 0x48,  0,  0, 0x00000010 },
+                { 0x15,  0,  4, 0x0000013f },
+                { 0x48,  0,  0, 0x00000016 },
+                { 0x54,  0,  0, 0x0000000f },
+                { 0x44,  0,  0, 0x00000010 },
+                { 0x16,  0,  0, 0x00000000 },
+                { 0x06,  0,  0, 0x00000000 },
+                { 0x15,  0,  9, 0x000086dd },
+                { 0x30,  0,  0, 0x00000014 },
+                { 0x15,  0,  6, 0x00000011 },
+                { 0x28,  0,  0, 0x00000038 },
+                { 0x15,  0,  4, 0x0000013f },
+                { 0x28,  0,  0, 0x0000003e },
+                { 0x54,  0,  0, 0x0000000f },
+                { 0x44,  0,  0, 0x00000020 },
+                { 0x16,  0,  0, 0x00000000 },
+                { 0x06,  0,  0, 0x00000000 },
+                { 0x15,  0,  9, 0x00008100 },
+                { 0x28,  0,  0, 0x00000010 },
+                { 0x15,  0, 15, 0x000088f7 },
+                { 0x30,  0,  0, 0x00000012 },
+                { 0x54,  0,  0, 0x00000008 },
+                { 0x15,  0, 12, 0x00000000 },
+                { 0x28,  0,  0, 0x00000012 },
+                { 0x54,  0,  0, 0x0000000f },
+                { 0x44,  0,  0, 0x00000040 },
+                { 0x16,  0,  0, 0x00000000 },
+                { 0x15,  0,  7, 0x000088f7 },
+                { 0x30,  0,  0, 0x0000000e },
+                { 0x54,  0,  0, 0x00000008 },
+                { 0x15,  0,  4, 0x00000000 },
+                { 0x28,  0,  0, 0x0000000e },
+                { 0x54,  0,  0, 0x0000000f },
+                { 0x44,  0,  0, 0x00000030 },
+                { 0x16,  0,  0, 0x00000000 },
+                { 0x06,  0,  0, 0x00000000 },
+        };
+        struct sock_fprog ptp_prog = {
+                .len = ARRAY_SIZE(ptp_filter), .filter = ptp_filter,
+        };
+
+        BUG_ON(sk_unattached_filter_create(&ptp_insns, &ptp_prog));
+}
diff --git a/net/core/timestamping.c b/net/core/timestamping.c
index 9ff26b3cc021..6521dfd8b7c8 100644
--- a/net/core/timestamping.c
+++ b/net/core/timestamping.c
@@ -23,14 +23,6 @@
 #include <linux/skbuff.h>
 #include <linux/export.h>
 
-static struct sk_filter *ptp_insns __read_mostly;
-
-unsigned int ptp_classify_raw(const struct sk_buff *skb)
-{
-        return SK_RUN_FILTER(ptp_insns, skb);
-}
-EXPORT_SYMBOL_GPL(ptp_classify_raw);
-
 static unsigned int classify(const struct sk_buff *skb)
 {
         if (likely(skb->dev && skb->dev->phydev &&
@@ -140,13 +132,3 @@ bool skb_defer_rx_timestamp(struct sk_buff *skb)
         return false;
 }
 EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp);
-
-void __init skb_timestamping_init(void)
-{
-        static struct sock_filter ptp_filter[] = { PTP_FILTER };
-        struct sock_fprog ptp_prog = {
-                .len = ARRAY_SIZE(ptp_filter), .filter = ptp_filter,
-        };
-
-        BUG_ON(sk_unattached_filter_create(&ptp_insns, &ptp_prog));
-}
diff --git a/net/socket.c b/net/socket.c
index f25eaa30b690..1b1e7e6a960f 100644
--- a/net/socket.c
+++ b/net/socket.c
@@ -72,6 +72,7 @@
 #include <linux/if_bridge.h>
 #include <linux/if_frad.h>
 #include <linux/if_vlan.h>
+#include <linux/ptp_classify.h>
 #include <linux/init.h>
 #include <linux/poll.h>
 #include <linux/cache.h>
@@ -2685,9 +2686,7 @@ static int __init sock_init(void)
                 goto out;
 #endif
 
-#ifdef CONFIG_NETWORK_PHY_TIMESTAMPING
-        skb_timestamping_init();
-#endif
+        ptp_classifier_init();
 
 out:
         return err;