1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98#include <linux/skbuff.h>
99#include <linux/filter.h>
100#include <linux/ptp_classify.h>
101
102static struct bpf_prog *ptp_insns __read_mostly;
103
104unsigned int ptp_classify_raw(const struct sk_buff *skb)
105{
106 return bpf_prog_run(ptp_insns, skb);
107}
108EXPORT_SYMBOL_GPL(ptp_classify_raw);
109
110struct 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
133 if (ptr + sizeof(struct ptp_header) > skb->data + skb->len)
134 return NULL;
135
136 return (struct ptp_header *)ptr;
137}
138EXPORT_SYMBOL_GPL(ptp_parse_header);
139
140void __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