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