1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (c) 2022 Meta */ |
3 | #include <stddef.h> |
4 | #include <string.h> |
5 | #include <stdbool.h> |
6 | #include <linux/bpf.h> |
7 | #include <linux/if_ether.h> |
8 | #include <linux/if_packet.h> |
9 | #include <linux/ip.h> |
10 | #include <linux/ipv6.h> |
11 | #include <linux/in.h> |
12 | #include <linux/udp.h> |
13 | #include <linux/tcp.h> |
14 | #include <linux/pkt_cls.h> |
15 | #include <sys/socket.h> |
16 | #include <bpf/bpf_helpers.h> |
17 | #include <bpf/bpf_endian.h> |
18 | #include "test_iptunnel_common.h" |
19 | #include "bpf_kfuncs.h" |
20 | |
21 | #define tcphdr_sz sizeof(struct tcphdr) |
22 | #define udphdr_sz sizeof(struct udphdr) |
23 | #define ethhdr_sz sizeof(struct ethhdr) |
24 | #define iphdr_sz sizeof(struct iphdr) |
25 | #define ipv6hdr_sz sizeof(struct ipv6hdr) |
26 | |
27 | struct { |
28 | __uint(type, BPF_MAP_TYPE_PERCPU_ARRAY); |
29 | __uint(max_entries, 256); |
30 | __type(key, __u32); |
31 | __type(value, __u64); |
32 | } rxcnt SEC(".maps" ); |
33 | |
34 | struct { |
35 | __uint(type, BPF_MAP_TYPE_HASH); |
36 | __uint(max_entries, MAX_IPTNL_ENTRIES); |
37 | __type(key, struct vip); |
38 | __type(value, struct iptnl_info); |
39 | } vip2tnl SEC(".maps" ); |
40 | |
41 | static __always_inline void count_tx(__u32 protocol) |
42 | { |
43 | __u64 *rxcnt_count; |
44 | |
45 | rxcnt_count = bpf_map_lookup_elem(&rxcnt, &protocol); |
46 | if (rxcnt_count) |
47 | *rxcnt_count += 1; |
48 | } |
49 | |
50 | static __always_inline int get_dport(void *trans_data, __u8 protocol) |
51 | { |
52 | struct tcphdr *th; |
53 | struct udphdr *uh; |
54 | |
55 | switch (protocol) { |
56 | case IPPROTO_TCP: |
57 | th = (struct tcphdr *)trans_data; |
58 | return th->dest; |
59 | case IPPROTO_UDP: |
60 | uh = (struct udphdr *)trans_data; |
61 | return uh->dest; |
62 | default: |
63 | return 0; |
64 | } |
65 | } |
66 | |
67 | static __always_inline void set_ethhdr(struct ethhdr *new_eth, |
68 | const struct ethhdr *old_eth, |
69 | const struct iptnl_info *tnl, |
70 | __be16 h_proto) |
71 | { |
72 | memcpy(new_eth->h_source, old_eth->h_dest, sizeof(new_eth->h_source)); |
73 | memcpy(new_eth->h_dest, tnl->dmac, sizeof(new_eth->h_dest)); |
74 | new_eth->h_proto = h_proto; |
75 | } |
76 | |
77 | static __always_inline int handle_ipv4(struct xdp_md *xdp, struct bpf_dynptr *xdp_ptr) |
78 | { |
79 | __u8 eth_buffer[ethhdr_sz + iphdr_sz + ethhdr_sz]; |
80 | __u8 iph_buffer_tcp[iphdr_sz + tcphdr_sz]; |
81 | __u8 iph_buffer_udp[iphdr_sz + udphdr_sz]; |
82 | struct bpf_dynptr new_xdp_ptr; |
83 | struct iptnl_info *tnl; |
84 | struct ethhdr *new_eth; |
85 | struct ethhdr *old_eth; |
86 | struct iphdr *iph; |
87 | __u16 *next_iph; |
88 | __u16 payload_len; |
89 | struct vip vip = {}; |
90 | int dport; |
91 | __u32 csum = 0; |
92 | int i; |
93 | |
94 | __builtin_memset(eth_buffer, 0, sizeof(eth_buffer)); |
95 | __builtin_memset(iph_buffer_tcp, 0, sizeof(iph_buffer_tcp)); |
96 | __builtin_memset(iph_buffer_udp, 0, sizeof(iph_buffer_udp)); |
97 | |
98 | if (ethhdr_sz + iphdr_sz + tcphdr_sz > xdp->data_end - xdp->data) |
99 | iph = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, iph_buffer_udp, sizeof(iph_buffer_udp)); |
100 | else |
101 | iph = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, iph_buffer_tcp, sizeof(iph_buffer_tcp)); |
102 | |
103 | if (!iph) |
104 | return XDP_DROP; |
105 | |
106 | dport = get_dport(trans_data: iph + 1, protocol: iph->protocol); |
107 | if (dport == -1) |
108 | return XDP_DROP; |
109 | |
110 | vip.protocol = iph->protocol; |
111 | vip.family = AF_INET; |
112 | vip.daddr.v4 = iph->daddr; |
113 | vip.dport = dport; |
114 | payload_len = bpf_ntohs(iph->tot_len); |
115 | |
116 | tnl = bpf_map_lookup_elem(&vip2tnl, &vip); |
117 | /* It only does v4-in-v4 */ |
118 | if (!tnl || tnl->family != AF_INET) |
119 | return XDP_PASS; |
120 | |
121 | if (bpf_xdp_adjust_head(xdp, 0 - (int)iphdr_sz)) |
122 | return XDP_DROP; |
123 | |
124 | bpf_dynptr_from_xdp(xdp, 0, &new_xdp_ptr); |
125 | new_eth = bpf_dynptr_slice_rdwr(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer)); |
126 | if (!new_eth) |
127 | return XDP_DROP; |
128 | |
129 | iph = (struct iphdr *)(new_eth + 1); |
130 | old_eth = (struct ethhdr *)(iph + 1); |
131 | |
132 | set_ethhdr(new_eth, old_eth, tnl, h_proto: bpf_htons(ETH_P_IP)); |
133 | |
134 | if (new_eth == eth_buffer) |
135 | bpf_dynptr_write(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer), 0); |
136 | |
137 | iph->version = 4; |
138 | iph->ihl = iphdr_sz >> 2; |
139 | iph->frag_off = 0; |
140 | iph->protocol = IPPROTO_IPIP; |
141 | iph->check = 0; |
142 | iph->tos = 0; |
143 | iph->tot_len = bpf_htons(payload_len + iphdr_sz); |
144 | iph->daddr = tnl->daddr.v4; |
145 | iph->saddr = tnl->saddr.v4; |
146 | iph->ttl = 8; |
147 | |
148 | next_iph = (__u16 *)iph; |
149 | for (i = 0; i < iphdr_sz >> 1; i++) |
150 | csum += *next_iph++; |
151 | |
152 | iph->check = ~((csum & 0xffff) + (csum >> 16)); |
153 | |
154 | count_tx(protocol: vip.protocol); |
155 | |
156 | return XDP_TX; |
157 | } |
158 | |
159 | static __always_inline int handle_ipv6(struct xdp_md *xdp, struct bpf_dynptr *xdp_ptr) |
160 | { |
161 | __u8 eth_buffer[ethhdr_sz + ipv6hdr_sz + ethhdr_sz]; |
162 | __u8 ip6h_buffer_tcp[ipv6hdr_sz + tcphdr_sz]; |
163 | __u8 ip6h_buffer_udp[ipv6hdr_sz + udphdr_sz]; |
164 | struct bpf_dynptr new_xdp_ptr; |
165 | struct iptnl_info *tnl; |
166 | struct ethhdr *new_eth; |
167 | struct ethhdr *old_eth; |
168 | struct ipv6hdr *ip6h; |
169 | __u16 payload_len; |
170 | struct vip vip = {}; |
171 | int dport; |
172 | |
173 | __builtin_memset(eth_buffer, 0, sizeof(eth_buffer)); |
174 | __builtin_memset(ip6h_buffer_tcp, 0, sizeof(ip6h_buffer_tcp)); |
175 | __builtin_memset(ip6h_buffer_udp, 0, sizeof(ip6h_buffer_udp)); |
176 | |
177 | if (ethhdr_sz + iphdr_sz + tcphdr_sz > xdp->data_end - xdp->data) |
178 | ip6h = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, ip6h_buffer_udp, sizeof(ip6h_buffer_udp)); |
179 | else |
180 | ip6h = bpf_dynptr_slice(xdp_ptr, ethhdr_sz, ip6h_buffer_tcp, sizeof(ip6h_buffer_tcp)); |
181 | |
182 | if (!ip6h) |
183 | return XDP_DROP; |
184 | |
185 | dport = get_dport(trans_data: ip6h + 1, protocol: ip6h->nexthdr); |
186 | if (dport == -1) |
187 | return XDP_DROP; |
188 | |
189 | vip.protocol = ip6h->nexthdr; |
190 | vip.family = AF_INET6; |
191 | memcpy(vip.daddr.v6, ip6h->daddr.s6_addr32, sizeof(vip.daddr)); |
192 | vip.dport = dport; |
193 | payload_len = ip6h->payload_len; |
194 | |
195 | tnl = bpf_map_lookup_elem(&vip2tnl, &vip); |
196 | /* It only does v6-in-v6 */ |
197 | if (!tnl || tnl->family != AF_INET6) |
198 | return XDP_PASS; |
199 | |
200 | if (bpf_xdp_adjust_head(xdp, 0 - (int)ipv6hdr_sz)) |
201 | return XDP_DROP; |
202 | |
203 | bpf_dynptr_from_xdp(xdp, 0, &new_xdp_ptr); |
204 | new_eth = bpf_dynptr_slice_rdwr(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer)); |
205 | if (!new_eth) |
206 | return XDP_DROP; |
207 | |
208 | ip6h = (struct ipv6hdr *)(new_eth + 1); |
209 | old_eth = (struct ethhdr *)(ip6h + 1); |
210 | |
211 | set_ethhdr(new_eth, old_eth, tnl, h_proto: bpf_htons(ETH_P_IPV6)); |
212 | |
213 | if (new_eth == eth_buffer) |
214 | bpf_dynptr_write(&new_xdp_ptr, 0, eth_buffer, sizeof(eth_buffer), 0); |
215 | |
216 | ip6h->version = 6; |
217 | ip6h->priority = 0; |
218 | memset(ip6h->flow_lbl, 0, sizeof(ip6h->flow_lbl)); |
219 | ip6h->payload_len = bpf_htons(bpf_ntohs(payload_len) + ipv6hdr_sz); |
220 | ip6h->nexthdr = IPPROTO_IPV6; |
221 | ip6h->hop_limit = 8; |
222 | memcpy(ip6h->saddr.s6_addr32, tnl->saddr.v6, sizeof(tnl->saddr.v6)); |
223 | memcpy(ip6h->daddr.s6_addr32, tnl->daddr.v6, sizeof(tnl->daddr.v6)); |
224 | |
225 | count_tx(protocol: vip.protocol); |
226 | |
227 | return XDP_TX; |
228 | } |
229 | |
230 | SEC("xdp" ) |
231 | int _xdp_tx_iptunnel(struct xdp_md *xdp) |
232 | { |
233 | __u8 buffer[ethhdr_sz]; |
234 | struct bpf_dynptr ptr; |
235 | struct ethhdr *eth; |
236 | __u16 h_proto; |
237 | |
238 | __builtin_memset(buffer, 0, sizeof(buffer)); |
239 | |
240 | bpf_dynptr_from_xdp(xdp, 0, &ptr); |
241 | eth = bpf_dynptr_slice(&ptr, 0, buffer, sizeof(buffer)); |
242 | if (!eth) |
243 | return XDP_DROP; |
244 | |
245 | h_proto = eth->h_proto; |
246 | |
247 | if (h_proto == bpf_htons(ETH_P_IP)) |
248 | return handle_ipv4(xdp, xdp_ptr: &ptr); |
249 | else if (h_proto == bpf_htons(ETH_P_IPV6)) |
250 | |
251 | return handle_ipv6(xdp, xdp_ptr: &ptr); |
252 | else |
253 | return XDP_DROP; |
254 | } |
255 | |
256 | char _license[] SEC("license" ) = "GPL" ; |
257 | |