1 // SPDX-License-Identifier: GPL-2.0
2 #include <linux/bpf.h>
3 #include <bpf/bpf_helpers.h>
4 #include <linux/if_ether.h>
5 #include <linux/in.h>
6 #include <linux/ip.h>
7 #include <linux/udp.h>
8 #include <linux/pkt_cls.h>
9
10 long change_tail_ret = 1;
11
parse_ip_header(struct __sk_buff * skb,int * ip_proto)12 static __always_inline struct iphdr *parse_ip_header(struct __sk_buff *skb, int *ip_proto)
13 {
14 void *data_end = (void *)(long)skb->data_end;
15 void *data = (void *)(long)skb->data;
16 struct ethhdr *eth = data;
17 struct iphdr *iph;
18
19 /* Verify Ethernet header */
20 if ((void *)(data + sizeof(*eth)) > data_end)
21 return NULL;
22
23 /* Skip Ethernet header to get to IP header */
24 iph = (void *)(data + sizeof(struct ethhdr));
25
26 /* Verify IP header */
27 if ((void *)(data + sizeof(struct ethhdr) + sizeof(*iph)) > data_end)
28 return NULL;
29
30 /* Basic IP header validation */
31 if (iph->version != 4) /* Only support IPv4 */
32 return NULL;
33
34 if (iph->ihl < 5) /* Minimum IP header length */
35 return NULL;
36
37 *ip_proto = iph->protocol;
38 return iph;
39 }
40
parse_udp_header(struct __sk_buff * skb,struct iphdr * iph)41 static __always_inline struct udphdr *parse_udp_header(struct __sk_buff *skb, struct iphdr *iph)
42 {
43 void *data_end = (void *)(long)skb->data_end;
44 void *hdr = (void *)iph;
45 struct udphdr *udp;
46
47 /* Calculate UDP header position */
48 udp = hdr + (iph->ihl * 4);
49 hdr = (void *)udp;
50
51 /* Verify UDP header bounds */
52 if ((void *)(hdr + sizeof(*udp)) > data_end)
53 return NULL;
54
55 return udp;
56 }
57
58 SEC("tc/ingress")
change_tail(struct __sk_buff * skb)59 int change_tail(struct __sk_buff *skb)
60 {
61 int len = skb->len;
62 struct udphdr *udp;
63 struct iphdr *iph;
64 void *data_end;
65 char *payload;
66 int ip_proto;
67
68 bpf_skb_pull_data(skb, len);
69
70 data_end = (void *)(long)skb->data_end;
71 iph = parse_ip_header(skb, &ip_proto);
72 if (!iph)
73 return TCX_PASS;
74
75 if (ip_proto != IPPROTO_UDP)
76 return TCX_PASS;
77
78 udp = parse_udp_header(skb, iph);
79 if (!udp)
80 return TCX_PASS;
81
82 payload = (char *)udp + (sizeof(struct udphdr));
83 if (payload + 1 > (char *)data_end)
84 return TCX_PASS;
85
86 if (payload[0] == 'T') { /* Trim the packet */
87 change_tail_ret = bpf_skb_change_tail(skb, len - 1, 0);
88 if (!change_tail_ret)
89 bpf_skb_change_tail(skb, len, 0);
90 return TCX_PASS;
91 } else if (payload[0] == 'G') { /* Grow the packet */
92 change_tail_ret = bpf_skb_change_tail(skb, len + 1, 0);
93 if (!change_tail_ret)
94 bpf_skb_change_tail(skb, len, 0);
95 return TCX_PASS;
96 } else if (payload[0] == 'E') { /* Error */
97 change_tail_ret = bpf_skb_change_tail(skb, 65535, 0);
98 return TCX_PASS;
99 } else if (payload[0] == 'Z') { /* Zero */
100 change_tail_ret = bpf_skb_change_tail(skb, 0, 0);
101 return TCX_PASS;
102 }
103 return TCX_DROP;
104 }
105
106 char _license[] SEC("license") = "GPL";
107