This source file includes following definitions.
- set_ipv4_csum
- encap_ipv4
- encap_ipv6
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- SEC
- decap_internal
- decap_ipv4
- decap_ipv6
- SEC
1
2
3
4
5 #include <stdbool.h>
6 #include <string.h>
7
8 #include <linux/stddef.h>
9 #include <linux/bpf.h>
10 #include <linux/if_ether.h>
11 #include <linux/in.h>
12 #include <linux/ip.h>
13 #include <linux/ipv6.h>
14 #include <linux/mpls.h>
15 #include <linux/tcp.h>
16 #include <linux/udp.h>
17 #include <linux/pkt_cls.h>
18 #include <linux/types.h>
19
20 #include "bpf_endian.h"
21 #include "bpf_helpers.h"
22
23 static const int cfg_port = 8000;
24
25 static const int cfg_udp_src = 20000;
26
27 #define UDP_PORT 5555
28 #define MPLS_OVER_UDP_PORT 6635
29 #define ETH_OVER_UDP_PORT 7777
30
31
32 static const __u32 mpls_label = __bpf_constant_htonl(1000 << 12 |
33 MPLS_LS_S_MASK | 0xff);
34
35 struct gre_hdr {
36 __be16 flags;
37 __be16 protocol;
38 } __attribute__((packed));
39
40 union l4hdr {
41 struct udphdr udp;
42 struct gre_hdr gre;
43 };
44
45 struct v4hdr {
46 struct iphdr ip;
47 union l4hdr l4hdr;
48 __u8 pad[16];
49 } __attribute__((packed));
50
51 struct v6hdr {
52 struct ipv6hdr ip;
53 union l4hdr l4hdr;
54 __u8 pad[16];
55 } __attribute__((packed));
56
57 static __always_inline void set_ipv4_csum(struct iphdr *iph)
58 {
59 __u16 *iph16 = (__u16 *)iph;
60 __u32 csum;
61 int i;
62
63 iph->check = 0;
64
65 #pragma clang loop unroll(full)
66 for (i = 0, csum = 0; i < sizeof(*iph) >> 1; i++)
67 csum += *iph16++;
68
69 iph->check = ~((csum & 0xffff) + (csum >> 16));
70 }
71
72 static __always_inline int encap_ipv4(struct __sk_buff *skb, __u8 encap_proto,
73 __u16 l2_proto)
74 {
75 __u16 udp_dst = UDP_PORT;
76 struct iphdr iph_inner;
77 struct v4hdr h_outer;
78 struct tcphdr tcph;
79 int olen, l2_len;
80 int tcp_off;
81 __u64 flags;
82
83
84
85
86
87
88
89
90
91 if (encap_proto == IPPROTO_IPV6) {
92 const __u32 saddr = (192 << 24) | (168 << 16) | (1 << 8) | 1;
93 const __u32 daddr = (192 << 24) | (168 << 16) | (1 << 8) | 2;
94 struct ipv6hdr iph6_inner;
95
96
97 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph6_inner,
98 sizeof(iph6_inner)) < 0)
99 return TC_ACT_OK;
100
101
102 memset(&iph_inner, 0, sizeof(iph_inner));
103 iph_inner.version = 4;
104 iph_inner.ihl = 5;
105 iph_inner.tot_len = bpf_htons(sizeof(iph6_inner) +
106 bpf_ntohs(iph6_inner.payload_len));
107 iph_inner.ttl = iph6_inner.hop_limit - 1;
108 iph_inner.protocol = iph6_inner.nexthdr;
109 iph_inner.saddr = __bpf_constant_htonl(saddr);
110 iph_inner.daddr = __bpf_constant_htonl(daddr);
111
112 tcp_off = sizeof(iph6_inner);
113 } else {
114 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_inner,
115 sizeof(iph_inner)) < 0)
116 return TC_ACT_OK;
117
118 tcp_off = sizeof(iph_inner);
119 }
120
121
122 if (iph_inner.ihl != 5 || iph_inner.protocol != IPPROTO_TCP)
123 return TC_ACT_OK;
124
125 if (bpf_skb_load_bytes(skb, ETH_HLEN + tcp_off,
126 &tcph, sizeof(tcph)) < 0)
127 return TC_ACT_OK;
128
129 if (tcph.dest != __bpf_constant_htons(cfg_port))
130 return TC_ACT_OK;
131
132 olen = sizeof(h_outer.ip);
133 l2_len = 0;
134
135 flags = BPF_F_ADJ_ROOM_FIXED_GSO | BPF_F_ADJ_ROOM_ENCAP_L3_IPV4;
136
137 switch (l2_proto) {
138 case ETH_P_MPLS_UC:
139 l2_len = sizeof(mpls_label);
140 udp_dst = MPLS_OVER_UDP_PORT;
141 break;
142 case ETH_P_TEB:
143 l2_len = ETH_HLEN;
144 udp_dst = ETH_OVER_UDP_PORT;
145 break;
146 }
147 flags |= BPF_F_ADJ_ROOM_ENCAP_L2(l2_len);
148
149 switch (encap_proto) {
150 case IPPROTO_GRE:
151 flags |= BPF_F_ADJ_ROOM_ENCAP_L4_GRE;
152 olen += sizeof(h_outer.l4hdr.gre);
153 h_outer.l4hdr.gre.protocol = bpf_htons(l2_proto);
154 h_outer.l4hdr.gre.flags = 0;
155 break;
156 case IPPROTO_UDP:
157 flags |= BPF_F_ADJ_ROOM_ENCAP_L4_UDP;
158 olen += sizeof(h_outer.l4hdr.udp);
159 h_outer.l4hdr.udp.source = __bpf_constant_htons(cfg_udp_src);
160 h_outer.l4hdr.udp.dest = bpf_htons(udp_dst);
161 h_outer.l4hdr.udp.check = 0;
162 h_outer.l4hdr.udp.len = bpf_htons(bpf_ntohs(iph_inner.tot_len) +
163 sizeof(h_outer.l4hdr.udp) +
164 l2_len);
165 break;
166 case IPPROTO_IPIP:
167 case IPPROTO_IPV6:
168 break;
169 default:
170 return TC_ACT_OK;
171 }
172
173
174 switch (l2_proto) {
175 case ETH_P_MPLS_UC:
176 *((__u32 *)((__u8 *)&h_outer + olen)) = mpls_label;
177 break;
178 case ETH_P_TEB:
179 if (bpf_skb_load_bytes(skb, 0, (__u8 *)&h_outer + olen,
180 ETH_HLEN))
181 return TC_ACT_SHOT;
182 break;
183 }
184 olen += l2_len;
185
186
187 if (bpf_skb_adjust_room(skb, olen, BPF_ADJ_ROOM_MAC, flags))
188 return TC_ACT_SHOT;
189
190
191 h_outer.ip = iph_inner;
192 h_outer.ip.tot_len = bpf_htons(olen +
193 bpf_ntohs(h_outer.ip.tot_len));
194 h_outer.ip.protocol = encap_proto;
195
196 set_ipv4_csum((void *)&h_outer.ip);
197
198
199 if (bpf_skb_store_bytes(skb, ETH_HLEN, &h_outer, olen,
200 BPF_F_INVALIDATE_HASH) < 0)
201 return TC_ACT_SHOT;
202
203
204 if (encap_proto == IPPROTO_IPV6) {
205 struct ethhdr eth;
206
207 if (bpf_skb_load_bytes(skb, 0, ð, sizeof(eth)) < 0)
208 return TC_ACT_SHOT;
209 eth.h_proto = bpf_htons(ETH_P_IP);
210 if (bpf_skb_store_bytes(skb, 0, ð, sizeof(eth), 0) < 0)
211 return TC_ACT_SHOT;
212 }
213
214 return TC_ACT_OK;
215 }
216
217 static __always_inline int encap_ipv6(struct __sk_buff *skb, __u8 encap_proto,
218 __u16 l2_proto)
219 {
220 __u16 udp_dst = UDP_PORT;
221 struct ipv6hdr iph_inner;
222 struct v6hdr h_outer;
223 struct tcphdr tcph;
224 int olen, l2_len;
225 __u16 tot_len;
226 __u64 flags;
227
228 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_inner,
229 sizeof(iph_inner)) < 0)
230 return TC_ACT_OK;
231
232
233 if (bpf_skb_load_bytes(skb, ETH_HLEN + sizeof(iph_inner),
234 &tcph, sizeof(tcph)) < 0)
235 return TC_ACT_OK;
236
237 if (tcph.dest != __bpf_constant_htons(cfg_port))
238 return TC_ACT_OK;
239
240 olen = sizeof(h_outer.ip);
241 l2_len = 0;
242
243 flags = BPF_F_ADJ_ROOM_FIXED_GSO | BPF_F_ADJ_ROOM_ENCAP_L3_IPV6;
244
245 switch (l2_proto) {
246 case ETH_P_MPLS_UC:
247 l2_len = sizeof(mpls_label);
248 udp_dst = MPLS_OVER_UDP_PORT;
249 break;
250 case ETH_P_TEB:
251 l2_len = ETH_HLEN;
252 udp_dst = ETH_OVER_UDP_PORT;
253 break;
254 }
255 flags |= BPF_F_ADJ_ROOM_ENCAP_L2(l2_len);
256
257 switch (encap_proto) {
258 case IPPROTO_GRE:
259 flags |= BPF_F_ADJ_ROOM_ENCAP_L4_GRE;
260 olen += sizeof(h_outer.l4hdr.gre);
261 h_outer.l4hdr.gre.protocol = bpf_htons(l2_proto);
262 h_outer.l4hdr.gre.flags = 0;
263 break;
264 case IPPROTO_UDP:
265 flags |= BPF_F_ADJ_ROOM_ENCAP_L4_UDP;
266 olen += sizeof(h_outer.l4hdr.udp);
267 h_outer.l4hdr.udp.source = __bpf_constant_htons(cfg_udp_src);
268 h_outer.l4hdr.udp.dest = bpf_htons(udp_dst);
269 tot_len = bpf_ntohs(iph_inner.payload_len) + sizeof(iph_inner) +
270 sizeof(h_outer.l4hdr.udp);
271 h_outer.l4hdr.udp.check = 0;
272 h_outer.l4hdr.udp.len = bpf_htons(tot_len);
273 break;
274 case IPPROTO_IPV6:
275 break;
276 default:
277 return TC_ACT_OK;
278 }
279
280
281 switch (l2_proto) {
282 case ETH_P_MPLS_UC:
283 *((__u32 *)((__u8 *)&h_outer + olen)) = mpls_label;
284 break;
285 case ETH_P_TEB:
286 if (bpf_skb_load_bytes(skb, 0, (__u8 *)&h_outer + olen,
287 ETH_HLEN))
288 return TC_ACT_SHOT;
289 break;
290 }
291 olen += l2_len;
292
293
294 if (bpf_skb_adjust_room(skb, olen, BPF_ADJ_ROOM_MAC, flags))
295 return TC_ACT_SHOT;
296
297
298 h_outer.ip = iph_inner;
299 h_outer.ip.payload_len = bpf_htons(olen +
300 bpf_ntohs(h_outer.ip.payload_len));
301
302 h_outer.ip.nexthdr = encap_proto;
303
304
305 if (bpf_skb_store_bytes(skb, ETH_HLEN, &h_outer, olen,
306 BPF_F_INVALIDATE_HASH) < 0)
307 return TC_ACT_SHOT;
308
309 return TC_ACT_OK;
310 }
311
312 SEC("encap_ipip_none")
313 int __encap_ipip_none(struct __sk_buff *skb)
314 {
315 if (skb->protocol == __bpf_constant_htons(ETH_P_IP))
316 return encap_ipv4(skb, IPPROTO_IPIP, ETH_P_IP);
317 else
318 return TC_ACT_OK;
319 }
320
321 SEC("encap_gre_none")
322 int __encap_gre_none(struct __sk_buff *skb)
323 {
324 if (skb->protocol == __bpf_constant_htons(ETH_P_IP))
325 return encap_ipv4(skb, IPPROTO_GRE, ETH_P_IP);
326 else
327 return TC_ACT_OK;
328 }
329
330 SEC("encap_gre_mpls")
331 int __encap_gre_mpls(struct __sk_buff *skb)
332 {
333 if (skb->protocol == __bpf_constant_htons(ETH_P_IP))
334 return encap_ipv4(skb, IPPROTO_GRE, ETH_P_MPLS_UC);
335 else
336 return TC_ACT_OK;
337 }
338
339 SEC("encap_gre_eth")
340 int __encap_gre_eth(struct __sk_buff *skb)
341 {
342 if (skb->protocol == __bpf_constant_htons(ETH_P_IP))
343 return encap_ipv4(skb, IPPROTO_GRE, ETH_P_TEB);
344 else
345 return TC_ACT_OK;
346 }
347
348 SEC("encap_udp_none")
349 int __encap_udp_none(struct __sk_buff *skb)
350 {
351 if (skb->protocol == __bpf_constant_htons(ETH_P_IP))
352 return encap_ipv4(skb, IPPROTO_UDP, ETH_P_IP);
353 else
354 return TC_ACT_OK;
355 }
356
357 SEC("encap_udp_mpls")
358 int __encap_udp_mpls(struct __sk_buff *skb)
359 {
360 if (skb->protocol == __bpf_constant_htons(ETH_P_IP))
361 return encap_ipv4(skb, IPPROTO_UDP, ETH_P_MPLS_UC);
362 else
363 return TC_ACT_OK;
364 }
365
366 SEC("encap_udp_eth")
367 int __encap_udp_eth(struct __sk_buff *skb)
368 {
369 if (skb->protocol == __bpf_constant_htons(ETH_P_IP))
370 return encap_ipv4(skb, IPPROTO_UDP, ETH_P_TEB);
371 else
372 return TC_ACT_OK;
373 }
374
375 SEC("encap_sit_none")
376 int __encap_sit_none(struct __sk_buff *skb)
377 {
378 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
379 return encap_ipv4(skb, IPPROTO_IPV6, ETH_P_IP);
380 else
381 return TC_ACT_OK;
382 }
383
384 SEC("encap_ip6tnl_none")
385 int __encap_ip6tnl_none(struct __sk_buff *skb)
386 {
387 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
388 return encap_ipv6(skb, IPPROTO_IPV6, ETH_P_IPV6);
389 else
390 return TC_ACT_OK;
391 }
392
393 SEC("encap_ip6gre_none")
394 int __encap_ip6gre_none(struct __sk_buff *skb)
395 {
396 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
397 return encap_ipv6(skb, IPPROTO_GRE, ETH_P_IPV6);
398 else
399 return TC_ACT_OK;
400 }
401
402 SEC("encap_ip6gre_mpls")
403 int __encap_ip6gre_mpls(struct __sk_buff *skb)
404 {
405 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
406 return encap_ipv6(skb, IPPROTO_GRE, ETH_P_MPLS_UC);
407 else
408 return TC_ACT_OK;
409 }
410
411 SEC("encap_ip6gre_eth")
412 int __encap_ip6gre_eth(struct __sk_buff *skb)
413 {
414 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
415 return encap_ipv6(skb, IPPROTO_GRE, ETH_P_TEB);
416 else
417 return TC_ACT_OK;
418 }
419
420 SEC("encap_ip6udp_none")
421 int __encap_ip6udp_none(struct __sk_buff *skb)
422 {
423 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
424 return encap_ipv6(skb, IPPROTO_UDP, ETH_P_IPV6);
425 else
426 return TC_ACT_OK;
427 }
428
429 SEC("encap_ip6udp_mpls")
430 int __encap_ip6udp_mpls(struct __sk_buff *skb)
431 {
432 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
433 return encap_ipv6(skb, IPPROTO_UDP, ETH_P_MPLS_UC);
434 else
435 return TC_ACT_OK;
436 }
437
438 SEC("encap_ip6udp_eth")
439 int __encap_ip6udp_eth(struct __sk_buff *skb)
440 {
441 if (skb->protocol == __bpf_constant_htons(ETH_P_IPV6))
442 return encap_ipv6(skb, IPPROTO_UDP, ETH_P_TEB);
443 else
444 return TC_ACT_OK;
445 }
446
447 static int decap_internal(struct __sk_buff *skb, int off, int len, char proto)
448 {
449 char buf[sizeof(struct v6hdr)];
450 struct gre_hdr greh;
451 struct udphdr udph;
452 int olen = len;
453
454 switch (proto) {
455 case IPPROTO_IPIP:
456 case IPPROTO_IPV6:
457 break;
458 case IPPROTO_GRE:
459 olen += sizeof(struct gre_hdr);
460 if (bpf_skb_load_bytes(skb, off + len, &greh, sizeof(greh)) < 0)
461 return TC_ACT_OK;
462 switch (bpf_ntohs(greh.protocol)) {
463 case ETH_P_MPLS_UC:
464 olen += sizeof(mpls_label);
465 break;
466 case ETH_P_TEB:
467 olen += ETH_HLEN;
468 break;
469 }
470 break;
471 case IPPROTO_UDP:
472 olen += sizeof(struct udphdr);
473 if (bpf_skb_load_bytes(skb, off + len, &udph, sizeof(udph)) < 0)
474 return TC_ACT_OK;
475 switch (bpf_ntohs(udph.dest)) {
476 case MPLS_OVER_UDP_PORT:
477 olen += sizeof(mpls_label);
478 break;
479 case ETH_OVER_UDP_PORT:
480 olen += ETH_HLEN;
481 break;
482 }
483 break;
484 default:
485 return TC_ACT_OK;
486 }
487
488 if (bpf_skb_adjust_room(skb, -olen, BPF_ADJ_ROOM_MAC,
489 BPF_F_ADJ_ROOM_FIXED_GSO))
490 return TC_ACT_SHOT;
491
492 return TC_ACT_OK;
493 }
494
495 static int decap_ipv4(struct __sk_buff *skb)
496 {
497 struct iphdr iph_outer;
498
499 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_outer,
500 sizeof(iph_outer)) < 0)
501 return TC_ACT_OK;
502
503 if (iph_outer.ihl != 5)
504 return TC_ACT_OK;
505
506 return decap_internal(skb, ETH_HLEN, sizeof(iph_outer),
507 iph_outer.protocol);
508 }
509
510 static int decap_ipv6(struct __sk_buff *skb)
511 {
512 struct ipv6hdr iph_outer;
513
514 if (bpf_skb_load_bytes(skb, ETH_HLEN, &iph_outer,
515 sizeof(iph_outer)) < 0)
516 return TC_ACT_OK;
517
518 return decap_internal(skb, ETH_HLEN, sizeof(iph_outer),
519 iph_outer.nexthdr);
520 }
521
522 SEC("decap")
523 int decap_f(struct __sk_buff *skb)
524 {
525 switch (skb->protocol) {
526 case __bpf_constant_htons(ETH_P_IP):
527 return decap_ipv4(skb);
528 case __bpf_constant_htons(ETH_P_IPV6):
529 return decap_ipv6(skb);
530 default:
531
532 return TC_ACT_OK;
533 }
534 }
535
536 char __license[] SEC("license") = "GPL";