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