1 // SPDX-License-Identifier: GPL-2.0
2
3 #include <stdbool.h>
4 #include <stddef.h>
5
6 #include <linux/bpf.h>
7 #include <linux/icmp.h>
8 #include <linux/if_ether.h>
9 #include <linux/in.h>
10 #include <linux/ip.h>
11 #include <linux/tcp.h>
12
13 #include <bpf/bpf_helpers.h>
14 #include <bpf/bpf_endian.h>
15
16 #define ICMP_SAMPLE_LEN (sizeof(struct iphdr) + 8)
17 #define ICMP_HDRS_LEN (sizeof(struct iphdr) + sizeof(struct icmphdr))
18
19 __be16 server_port = 0;
20 __u16 pmtu = 0;
21
22 long change_tail_ret = 1;
23 long adjust_room_ret = 0;
24 bool icmp_sent = false;
25 bool icmp_err = false;
26
csum_fold(__wsum csum)27 static __always_inline __sum16 csum_fold(__wsum csum)
28 {
29 csum = (csum & 0xffff) + (csum >> 16);
30 csum = (csum & 0xffff) + (csum >> 16);
31
32 return (__sum16)~csum;
33 }
34
35 SEC("tc/egress")
change_tail_icmp(struct __sk_buff * skb)36 int change_tail_icmp(struct __sk_buff *skb)
37 {
38 __u8 smac[ETH_ALEN], dmac[ETH_ALEN];
39 void *data, *data_end;
40 struct icmphdr *icmp;
41 struct ethhdr *eth;
42 struct tcphdr *tcp;
43 __be32 saddr, daddr;
44 struct iphdr *ip;
45 __wsum csum;
46
47 if (icmp_sent || icmp_err)
48 return TCX_PASS;
49
50 data = (void *)(long)skb->data;
51 data_end = (void *)(long)skb->data_end;
52
53 eth = data;
54 if ((void *)(eth + 1) > data_end)
55 return TCX_PASS;
56 if (eth->h_proto != bpf_htons(ETH_P_IP))
57 return TCX_PASS;
58
59 ip = (void *)(eth + 1);
60 if ((void *)(ip + 1) > data_end)
61 return TCX_PASS;
62 if (ip->ihl != 5 || ip->protocol != IPPROTO_TCP)
63 return TCX_PASS;
64
65 tcp = (void *)(ip + 1);
66 if ((void *)(tcp + 1) > data_end)
67 return TCX_PASS;
68 if (tcp->dest != server_port)
69 return TCX_PASS;
70 if (bpf_ntohs(ip->tot_len) <= sizeof(*ip) + tcp->doff * 4)
71 return TCX_PASS;
72
73 __builtin_memcpy(smac, eth->h_source, ETH_ALEN);
74 __builtin_memcpy(dmac, eth->h_dest, ETH_ALEN);
75 saddr = ip->saddr;
76 daddr = ip->daddr;
77
78 change_tail_ret = bpf_skb_change_tail(skb, ETH_HLEN + ICMP_SAMPLE_LEN, 0);
79 if (change_tail_ret) {
80 icmp_err = true;
81 return TCX_PASS;
82 }
83
84 adjust_room_ret = bpf_skb_adjust_room(skb, ICMP_HDRS_LEN,
85 BPF_ADJ_ROOM_MAC,
86 BPF_F_ADJ_ROOM_NO_CSUM_RESET);
87 if (adjust_room_ret) {
88 icmp_err = true;
89 return TCX_DROP;
90 }
91
92 data = (void *)(long)skb->data;
93 data_end = (void *)(long)skb->data_end;
94
95 eth = data;
96 ip = (void *)(eth + 1);
97 icmp = (void *)(ip + 1);
98 if ((void *)icmp + sizeof(*icmp) + ICMP_SAMPLE_LEN > data_end) {
99 icmp_err = true;
100 return TCX_DROP;
101 }
102
103 __builtin_memcpy(eth->h_dest, smac, ETH_ALEN);
104 __builtin_memcpy(eth->h_source, dmac, ETH_ALEN);
105
106 __builtin_memset(icmp, 0, sizeof(*icmp));
107 icmp->type = ICMP_DEST_UNREACH;
108 icmp->code = ICMP_FRAG_NEEDED;
109 icmp->un.frag.mtu = bpf_htons(pmtu);
110
111 __builtin_memset(ip, 0, sizeof(*ip));
112 ip->version = 4;
113 ip->ihl = 5;
114 ip->ttl = 64;
115 ip->protocol = IPPROTO_ICMP;
116 ip->tot_len = bpf_htons(ICMP_HDRS_LEN + ICMP_SAMPLE_LEN);
117 ip->saddr = daddr;
118 ip->daddr = saddr;
119
120 csum = bpf_csum_diff(NULL, 0, (__be32 *)icmp,
121 sizeof(*icmp) + ICMP_SAMPLE_LEN, 0);
122 icmp->checksum = csum_fold(csum);
123 csum = bpf_csum_diff(NULL, 0, (__be32 *)ip, sizeof(*ip), 0);
124 ip->check = csum_fold(csum);
125 icmp_sent = true;
126 return bpf_redirect(skb->ifindex, BPF_F_INGRESS);
127 }
128
129 char _license[] SEC("license") = "GPL";
130