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 | |
104 | unsigned int ptp_classify_raw(const struct sk_buff *skb) |
105 | { |
106 | return bpf_prog_run(prog: ptp_insns, ctx: skb); |
107 | } |
108 | EXPORT_SYMBOL_GPL(ptp_classify_raw); |
109 | |
110 | struct ptp_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 | |
140 | bool ptp_msg_is_sync(struct sk_buff *skb, unsigned int type) |
141 | { |
142 | struct ptp_header *hdr; |
143 | |
144 | hdr = ptp_parse_header(skb, type); |
145 | if (!hdr) |
146 | return false; |
147 | |
148 | return ptp_get_msgtype(hdr, type) == PTP_MSGTYPE_SYNC; |
149 | } |
150 | EXPORT_SYMBOL_GPL(ptp_msg_is_sync); |
151 | |
152 | void __init ptp_classifier_init(void) |
153 | { |
154 | static struct sock_filter ptp_filter[] __initdata = { |
155 | { 0x28, 0, 0, 0x0000000c }, |
156 | { 0x15, 0, 12, 0x00000800 }, |
157 | { 0x30, 0, 0, 0x00000017 }, |
158 | { 0x15, 0, 9, 0x00000011 }, |
159 | { 0x28, 0, 0, 0x00000014 }, |
160 | { 0x45, 7, 0, 0x00001fff }, |
161 | { 0xb1, 0, 0, 0x0000000e }, |
162 | { 0x48, 0, 0, 0x00000010 }, |
163 | { 0x15, 0, 4, 0x0000013f }, |
164 | { 0x48, 0, 0, 0x00000016 }, |
165 | { 0x54, 0, 0, 0x0000000f }, |
166 | { 0x44, 0, 0, 0x00000010 }, |
167 | { 0x16, 0, 0, 0x00000000 }, |
168 | { 0x06, 0, 0, 0x00000000 }, |
169 | { 0x15, 0, 9, 0x000086dd }, |
170 | { 0x30, 0, 0, 0x00000014 }, |
171 | { 0x15, 0, 6, 0x00000011 }, |
172 | { 0x28, 0, 0, 0x00000038 }, |
173 | { 0x15, 0, 4, 0x0000013f }, |
174 | { 0x28, 0, 0, 0x0000003e }, |
175 | { 0x54, 0, 0, 0x0000000f }, |
176 | { 0x44, 0, 0, 0x00000020 }, |
177 | { 0x16, 0, 0, 0x00000000 }, |
178 | { 0x06, 0, 0, 0x00000000 }, |
179 | { 0x15, 0, 32, 0x00008100 }, |
180 | { 0x28, 0, 0, 0x00000010 }, |
181 | { 0x15, 0, 7, 0x000088f7 }, |
182 | { 0x30, 0, 0, 0x00000012 }, |
183 | { 0x54, 0, 0, 0x00000008 }, |
184 | { 0x15, 0, 35, 0x00000000 }, |
185 | { 0x28, 0, 0, 0x00000012 }, |
186 | { 0x54, 0, 0, 0x0000000f }, |
187 | { 0x44, 0, 0, 0x000000c0 }, |
188 | { 0x16, 0, 0, 0x00000000 }, |
189 | { 0x15, 0, 12, 0x00000800 }, |
190 | { 0x30, 0, 0, 0x0000001b }, |
191 | { 0x15, 0, 9, 0x00000011 }, |
192 | { 0x28, 0, 0, 0x00000018 }, |
193 | { 0x45, 7, 0, 0x00001fff }, |
194 | { 0xb1, 0, 0, 0x00000012 }, |
195 | { 0x48, 0, 0, 0x00000014 }, |
196 | { 0x15, 0, 4, 0x0000013f }, |
197 | { 0x48, 0, 0, 0x0000001a }, |
198 | { 0x54, 0, 0, 0x0000000f }, |
199 | { 0x44, 0, 0, 0x00000090 }, |
200 | { 0x16, 0, 0, 0x00000000 }, |
201 | { 0x06, 0, 0, 0x00000000 }, |
202 | { 0x15, 0, 8, 0x000086dd }, |
203 | { 0x30, 0, 0, 0x00000018 }, |
204 | { 0x15, 0, 6, 0x00000011 }, |
205 | { 0x28, 0, 0, 0x0000003c }, |
206 | { 0x15, 0, 4, 0x0000013f }, |
207 | { 0x28, 0, 0, 0x00000042 }, |
208 | { 0x54, 0, 0, 0x0000000f }, |
209 | { 0x44, 0, 0, 0x000000a0 }, |
210 | { 0x16, 0, 0, 0x00000000 }, |
211 | { 0x06, 0, 0, 0x00000000 }, |
212 | { 0x15, 0, 7, 0x000088f7 }, |
213 | { 0x30, 0, 0, 0x0000000e }, |
214 | { 0x54, 0, 0, 0x00000008 }, |
215 | { 0x15, 0, 4, 0x00000000 }, |
216 | { 0x28, 0, 0, 0x0000000e }, |
217 | { 0x54, 0, 0, 0x0000000f }, |
218 | { 0x44, 0, 0, 0x00000040 }, |
219 | { 0x16, 0, 0, 0x00000000 }, |
220 | { 0x06, 0, 0, 0x00000000 }, |
221 | }; |
222 | struct sock_fprog_kern ptp_prog; |
223 | |
224 | ptp_prog.len = ARRAY_SIZE(ptp_filter); |
225 | ptp_prog.filter = ptp_filter; |
226 | |
227 | BUG_ON(bpf_prog_create(&ptp_insns, &ptp_prog)); |
228 | } |
229 | |