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