1 // SPDX-License-Identifier: GPL-2.0-only
2 /* PTP classifier
3  */
4 
5 /* The below program is the bpf_asm (tools/net/) representation of
6  * the opcode array in the ptp_filter structure.
7  *
8  * For convenience, this can easily be altered and reviewed with
9  * bpf_asm and bpf_dbg, e.g. `./bpf_asm -c prog` where prog is a
10  * simple file containing the below program:
11  *
12  * ldh [12]                        ; load ethertype
13  *
14  * ; PTP over UDP over IPv4 over Ethernet
15  * test_ipv4:
16  *   jneq #0x800, test_ipv6        ; ETH_P_IP ?
17  *   ldb [23]                      ; load proto
18  *   jneq #17, drop_ipv4           ; IPPROTO_UDP ?
19  *   ldh [20]                      ; load frag offset field
20  *   jset #0x1fff, drop_ipv4       ; don't allow fragments
21  *   ldxb 4*([14]&0xf)             ; load IP header len
22  *   ldh [x + 16]                  ; load UDP dst port
23  *   jneq #319, drop_ipv4          ; is port PTP_EV_PORT ?
24  *   ldh [x + 22]                  ; load payload
25  *   and #0xf                      ; mask PTP_CLASS_VMASK
26  *   or #0x10                      ; PTP_CLASS_IPV4
27  *   ret a                         ; return PTP class
28  *   drop_ipv4: ret #0x0           ; PTP_CLASS_NONE
29  *
30  * ; PTP over UDP over IPv6 over Ethernet
31  * test_ipv6:
32  *   jneq #0x86dd, test_8021q      ; ETH_P_IPV6 ?
33  *   ldb [20]                      ; load proto
34  *   jneq #17, drop_ipv6           ; IPPROTO_UDP ?
35  *   ldh [56]                      ; load UDP dst port
36  *   jneq #319, drop_ipv6          ; is port PTP_EV_PORT ?
37  *   ldh [62]                      ; load payload
38  *   and #0xf                      ; mask PTP_CLASS_VMASK
39  *   or #0x20                      ; PTP_CLASS_IPV6
40  *   ret a                         ; return PTP class
41  *   drop_ipv6: ret #0x0           ; PTP_CLASS_NONE
42  *
43  * ; PTP over 802.1Q over Ethernet
44  * test_8021q:
45  *   jneq #0x8100, test_ieee1588   ; ETH_P_8021Q ?
46  *   ldh [16]                      ; load inner type
47  *   jneq #0x88f7, test_8021q_ipv4 ; ETH_P_1588 ?
48  *   ldb [18]                      ; load payload
49  *   and #0x8                      ; as we don't have ports here, test
50  *   jneq #0x0, drop_ieee1588      ; for PTP_GEN_BIT and drop these
51  *   ldh [18]                      ; reload payload
52  *   and #0xf                      ; mask PTP_CLASS_VMASK
53  *   or #0xc0                      ; PTP_CLASS_VLAN|PTP_CLASS_L2
54  *   ret a                         ; return PTP class
55  *
56  * ; PTP over UDP over IPv4 over 802.1Q over Ethernet
57  * test_8021q_ipv4:
58  *   jneq #0x800, test_8021q_ipv6  ; ETH_P_IP ?
59  *   ldb [27]                      ; load proto
60  *   jneq #17, drop_8021q_ipv4     ; IPPROTO_UDP ?
61  *   ldh [24]                      ; load frag offset field
62  *   jset #0x1fff, drop_8021q_ipv4; don't allow fragments
63  *   ldxb 4*([18]&0xf)             ; load IP header len
64  *   ldh [x + 20]                  ; load UDP dst port
65  *   jneq #319, drop_8021q_ipv4    ; is port PTP_EV_PORT ?
66  *   ldh [x + 26]                  ; load payload
67  *   and #0xf                      ; mask PTP_CLASS_VMASK
68  *   or #0x90                      ; PTP_CLASS_VLAN|PTP_CLASS_IPV4
69  *   ret a                         ; return PTP class
70  *   drop_8021q_ipv4: ret #0x0     ; PTP_CLASS_NONE
71  *
72  * ; PTP over UDP over IPv6 over 802.1Q over Ethernet
73  * test_8021q_ipv6:
74  *   jneq #0x86dd, drop_8021q_ipv6 ; ETH_P_IPV6 ?
75  *   ldb [24]                      ; load proto
76  *   jneq #17, drop_8021q_ipv6           ; IPPROTO_UDP ?
77  *   ldh [60]                      ; load UDP dst port
78  *   jneq #319, drop_8021q_ipv6          ; is port PTP_EV_PORT ?
79  *   ldh [66]                      ; load payload
80  *   and #0xf                      ; mask PTP_CLASS_VMASK
81  *   or #0xa0                      ; PTP_CLASS_VLAN|PTP_CLASS_IPV6
82  *   ret a                         ; return PTP class
83  *   drop_8021q_ipv6: ret #0x0     ; PTP_CLASS_NONE
84  *
85  * ; PTP over Ethernet
86  * test_ieee1588:
87  *   jneq #0x88f7, drop_ieee1588   ; ETH_P_1588 ?
88  *   ldb [14]                      ; load payload
89  *   and #0x8                      ; as we don't have ports here, test
90  *   jneq #0x0, drop_ieee1588      ; for PTP_GEN_BIT and drop these
91  *   ldh [14]                      ; reload payload
92  *   and #0xf                      ; mask PTP_CLASS_VMASK
93  *   or #0x40                      ; PTP_CLASS_L2
94  *   ret a                         ; return PTP class
95  *   drop_ieee1588: ret #0x0       ; PTP_CLASS_NONE
96  */
97 
98 #include <linux/skbuff.h>
99 #include <linux/filter.h>
100 #include <linux/ptp_classify.h>
101 
102 static struct bpf_prog *ptp_insns __read_mostly;
103 
ptp_classify_raw(const struct sk_buff * skb)104 unsigned int ptp_classify_raw(const struct sk_buff *skb)
105 {
106 	return BPF_PROG_RUN(ptp_insns, skb);
107 }
108 EXPORT_SYMBOL_GPL(ptp_classify_raw);
109 
ptp_parse_header(struct sk_buff * skb,unsigned int type)110 struct ptp_header *ptp_parse_header(struct sk_buff *skb, unsigned int type)
111 {
112 	u8 *ptr = skb_mac_header(skb);
113 
114 	if (type & PTP_CLASS_VLAN)
115 		ptr += VLAN_HLEN;
116 
117 	switch (type & PTP_CLASS_PMASK) {
118 	case PTP_CLASS_IPV4:
119 		ptr += IPV4_HLEN(ptr) + UDP_HLEN;
120 		break;
121 	case PTP_CLASS_IPV6:
122 		ptr += IP6_HLEN + UDP_HLEN;
123 		break;
124 	case PTP_CLASS_L2:
125 		break;
126 	default:
127 		return NULL;
128 	}
129 
130 	ptr += ETH_HLEN;
131 
132 	/* Ensure that the entire header is present in this packet. */
133 	if (ptr + sizeof(struct ptp_header) > skb->data + skb->len)
134 		return NULL;
135 
136 	return (struct ptp_header *)ptr;
137 }
138 EXPORT_SYMBOL_GPL(ptp_parse_header);
139 
ptp_classifier_init(void)140 void __init ptp_classifier_init(void)
141 {
142 	static struct sock_filter ptp_filter[] __initdata = {
143 		{ 0x28,  0,  0, 0x0000000c },
144 		{ 0x15,  0, 12, 0x00000800 },
145 		{ 0x30,  0,  0, 0x00000017 },
146 		{ 0x15,  0,  9, 0x00000011 },
147 		{ 0x28,  0,  0, 0x00000014 },
148 		{ 0x45,  7,  0, 0x00001fff },
149 		{ 0xb1,  0,  0, 0x0000000e },
150 		{ 0x48,  0,  0, 0x00000010 },
151 		{ 0x15,  0,  4, 0x0000013f },
152 		{ 0x48,  0,  0, 0x00000016 },
153 		{ 0x54,  0,  0, 0x0000000f },
154 		{ 0x44,  0,  0, 0x00000010 },
155 		{ 0x16,  0,  0, 0x00000000 },
156 		{ 0x06,  0,  0, 0x00000000 },
157 		{ 0x15,  0,  9, 0x000086dd },
158 		{ 0x30,  0,  0, 0x00000014 },
159 		{ 0x15,  0,  6, 0x00000011 },
160 		{ 0x28,  0,  0, 0x00000038 },
161 		{ 0x15,  0,  4, 0x0000013f },
162 		{ 0x28,  0,  0, 0x0000003e },
163 		{ 0x54,  0,  0, 0x0000000f },
164 		{ 0x44,  0,  0, 0x00000020 },
165 		{ 0x16,  0,  0, 0x00000000 },
166 		{ 0x06,  0,  0, 0x00000000 },
167 		{ 0x15,  0, 32, 0x00008100 },
168 		{ 0x28,  0,  0, 0x00000010 },
169 		{ 0x15,  0,  7, 0x000088f7 },
170 		{ 0x30,  0,  0, 0x00000012 },
171 		{ 0x54,  0,  0, 0x00000008 },
172 		{ 0x15,  0, 35, 0x00000000 },
173 		{ 0x28,  0,  0, 0x00000012 },
174 		{ 0x54,  0,  0, 0x0000000f },
175 		{ 0x44,  0,  0, 0x000000c0 },
176 		{ 0x16,  0,  0, 0x00000000 },
177 		{ 0x15,  0, 12, 0x00000800 },
178 		{ 0x30,  0,  0, 0x0000001b },
179 		{ 0x15,  0,  9, 0x00000011 },
180 		{ 0x28,  0,  0, 0x00000018 },
181 		{ 0x45,  7,  0, 0x00001fff },
182 		{ 0xb1,  0,  0, 0x00000012 },
183 		{ 0x48,  0,  0, 0x00000014 },
184 		{ 0x15,  0,  4, 0x0000013f },
185 		{ 0x48,  0,  0, 0x0000001a },
186 		{ 0x54,  0,  0, 0x0000000f },
187 		{ 0x44,  0,  0, 0x00000090 },
188 		{ 0x16,  0,  0, 0x00000000 },
189 		{ 0x06,  0,  0, 0x00000000 },
190 		{ 0x15,  0,  8, 0x000086dd },
191 		{ 0x30,  0,  0, 0x00000018 },
192 		{ 0x15,  0,  6, 0x00000011 },
193 		{ 0x28,  0,  0, 0x0000003c },
194 		{ 0x15,  0,  4, 0x0000013f },
195 		{ 0x28,  0,  0, 0x00000042 },
196 		{ 0x54,  0,  0, 0x0000000f },
197 		{ 0x44,  0,  0, 0x000000a0 },
198 		{ 0x16,  0,  0, 0x00000000 },
199 		{ 0x06,  0,  0, 0x00000000 },
200 		{ 0x15,  0,  7, 0x000088f7 },
201 		{ 0x30,  0,  0, 0x0000000e },
202 		{ 0x54,  0,  0, 0x00000008 },
203 		{ 0x15,  0,  4, 0x00000000 },
204 		{ 0x28,  0,  0, 0x0000000e },
205 		{ 0x54,  0,  0, 0x0000000f },
206 		{ 0x44,  0,  0, 0x00000040 },
207 		{ 0x16,  0,  0, 0x00000000 },
208 		{ 0x06,  0,  0, 0x00000000 },
209 	};
210 	struct sock_fprog_kern ptp_prog;
211 
212 	ptp_prog.len = ARRAY_SIZE(ptp_filter);
213 	ptp_prog.filter = ptp_filter;
214 
215 	BUG_ON(bpf_prog_create(&ptp_insns, &ptp_prog));
216 }
217