Commit | Line | Data |
---|---|---|
5b497af4 | 1 | // SPDX-License-Identifier: GPL-2.0-only |
408eccce | 2 | /* PTP classifier |
408eccce DB |
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 | |
ae5c6c6d | 47 | * jneq #0x88f7, test_8021q_ipv4 ; ETH_P_1588 ? |
408eccce DB |
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 | |
5f94c943 | 53 | * or #0xc0 ; PTP_CLASS_VLAN|PTP_CLASS_L2 |
ae5c6c6d SS |
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 | |
5f94c943 | 68 | * or #0x90 ; PTP_CLASS_VLAN|PTP_CLASS_IPV4 |
ae5c6c6d SS |
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 | |
5f94c943 | 81 | * or #0xa0 ; PTP_CLASS_VLAN|PTP_CLASS_IPV6 |
408eccce | 82 | * ret a ; return PTP class |
ae5c6c6d | 83 | * drop_8021q_ipv6: ret #0x0 ; PTP_CLASS_NONE |
408eccce DB |
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 | |
5f94c943 | 93 | * or #0x40 ; PTP_CLASS_L2 |
408eccce DB |
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 | ||
7ae457c1 | 102 | static struct bpf_prog *ptp_insns __read_mostly; |
408eccce DB |
103 | |
104 | unsigned int ptp_classify_raw(const struct sk_buff *skb) | |
105 | { | |
fb7dd8bc | 106 | return bpf_prog_run(ptp_insns, skb); |
408eccce DB |
107 | } |
108 | EXPORT_SYMBOL_GPL(ptp_classify_raw); | |
109 | ||
bdfbb63c KK |
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 | ||
f72de02e KK |
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 | ||
408eccce DB |
152 | void __init ptp_classifier_init(void) |
153 | { | |
0f49ff07 | 154 | static struct sock_filter ptp_filter[] __initdata = { |
408eccce DB |
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 }, | |
ae5c6c6d | 179 | { 0x15, 0, 32, 0x00008100 }, |
408eccce | 180 | { 0x28, 0, 0, 0x00000010 }, |
ae5c6c6d | 181 | { 0x15, 0, 7, 0x000088f7 }, |
408eccce DB |
182 | { 0x30, 0, 0, 0x00000012 }, |
183 | { 0x54, 0, 0, 0x00000008 }, | |
ae5c6c6d | 184 | { 0x15, 0, 35, 0x00000000 }, |
408eccce DB |
185 | { 0x28, 0, 0, 0x00000012 }, |
186 | { 0x54, 0, 0, 0x0000000f }, | |
5f94c943 | 187 | { 0x44, 0, 0, 0x000000c0 }, |
ae5c6c6d SS |
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 }, | |
5f94c943 | 199 | { 0x44, 0, 0, 0x00000090 }, |
ae5c6c6d SS |
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 }, | |
5f94c943 | 209 | { 0x44, 0, 0, 0x000000a0 }, |
408eccce | 210 | { 0x16, 0, 0, 0x00000000 }, |
ae5c6c6d | 211 | { 0x06, 0, 0, 0x00000000 }, |
408eccce DB |
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 }, | |
5f94c943 | 218 | { 0x44, 0, 0, 0x00000040 }, |
408eccce DB |
219 | { 0x16, 0, 0, 0x00000000 }, |
220 | { 0x06, 0, 0, 0x00000000 }, | |
221 | }; | |
ad910c7c AB |
222 | struct sock_fprog_kern ptp_prog; |
223 | ||
224 | ptp_prog.len = ARRAY_SIZE(ptp_filter); | |
225 | ptp_prog.filter = ptp_filter; | |
408eccce | 226 | |
7ae457c1 | 227 | BUG_ON(bpf_prog_create(&ptp_insns, &ptp_prog)); |
408eccce | 228 | } |