1 | // SPDX-License-Identifier: GPL-2.0 |
2 | |
3 | #include <stdbool.h> |
4 | #include <linux/bpf.h> |
5 | #include <linux/netdev.h> |
6 | #include <bpf/bpf_helpers.h> |
7 | #include <bpf/bpf_endian.h> |
8 | #include <bpf/bpf_tracing.h> |
9 | #include <linux/if_ether.h> |
10 | #include <linux/ip.h> |
11 | #include <linux/ipv6.h> |
12 | #include <linux/in.h> |
13 | #include <linux/in6.h> |
14 | #include <linux/udp.h> |
15 | #include <asm-generic/errno-base.h> |
16 | |
17 | #include "xdp_features.h" |
18 | |
19 | #define ipv6_addr_equal(a, b) ((a).s6_addr32[0] == (b).s6_addr32[0] && \ |
20 | (a).s6_addr32[1] == (b).s6_addr32[1] && \ |
21 | (a).s6_addr32[2] == (b).s6_addr32[2] && \ |
22 | (a).s6_addr32[3] == (b).s6_addr32[3]) |
23 | |
24 | struct net_device; |
25 | struct bpf_prog; |
26 | |
27 | struct xdp_cpumap_stats { |
28 | unsigned int redirect; |
29 | unsigned int pass; |
30 | unsigned int drop; |
31 | }; |
32 | |
33 | struct { |
34 | __uint(type, BPF_MAP_TYPE_ARRAY); |
35 | __type(key, __u32); |
36 | __type(value, __u32); |
37 | __uint(max_entries, 1); |
38 | } stats SEC(".maps" ); |
39 | |
40 | struct { |
41 | __uint(type, BPF_MAP_TYPE_ARRAY); |
42 | __type(key, __u32); |
43 | __type(value, __u32); |
44 | __uint(max_entries, 1); |
45 | } dut_stats SEC(".maps" ); |
46 | |
47 | struct { |
48 | __uint(type, BPF_MAP_TYPE_CPUMAP); |
49 | __uint(key_size, sizeof(__u32)); |
50 | __uint(value_size, sizeof(struct bpf_cpumap_val)); |
51 | __uint(max_entries, 1); |
52 | } cpu_map SEC(".maps" ); |
53 | |
54 | struct { |
55 | __uint(type, BPF_MAP_TYPE_DEVMAP); |
56 | __uint(key_size, sizeof(__u32)); |
57 | __uint(value_size, sizeof(struct bpf_devmap_val)); |
58 | __uint(max_entries, 1); |
59 | } dev_map SEC(".maps" ); |
60 | |
61 | const volatile struct in6_addr tester_addr; |
62 | const volatile struct in6_addr dut_addr; |
63 | |
64 | static __always_inline int |
65 | xdp_process_echo_packet(struct xdp_md *xdp, bool dut) |
66 | { |
67 | void *data_end = (void *)(long)xdp->data_end; |
68 | void *data = (void *)(long)xdp->data; |
69 | struct ethhdr *eh = data; |
70 | struct tlv_hdr *tlv; |
71 | struct udphdr *uh; |
72 | __be16 port; |
73 | |
74 | if (eh + 1 > (struct ethhdr *)data_end) |
75 | return -EINVAL; |
76 | |
77 | if (eh->h_proto == bpf_htons(ETH_P_IP)) { |
78 | struct iphdr *ih = (struct iphdr *)(eh + 1); |
79 | __be32 saddr = dut ? tester_addr.s6_addr32[3] |
80 | : dut_addr.s6_addr32[3]; |
81 | __be32 daddr = dut ? dut_addr.s6_addr32[3] |
82 | : tester_addr.s6_addr32[3]; |
83 | |
84 | ih = (struct iphdr *)(eh + 1); |
85 | if (ih + 1 > (struct iphdr *)data_end) |
86 | return -EINVAL; |
87 | |
88 | if (saddr != ih->saddr) |
89 | return -EINVAL; |
90 | |
91 | if (daddr != ih->daddr) |
92 | return -EINVAL; |
93 | |
94 | if (ih->protocol != IPPROTO_UDP) |
95 | return -EINVAL; |
96 | |
97 | uh = (struct udphdr *)(ih + 1); |
98 | } else if (eh->h_proto == bpf_htons(ETH_P_IPV6)) { |
99 | struct in6_addr saddr = dut ? tester_addr : dut_addr; |
100 | struct in6_addr daddr = dut ? dut_addr : tester_addr; |
101 | struct ipv6hdr *ih6 = (struct ipv6hdr *)(eh + 1); |
102 | |
103 | if (ih6 + 1 > (struct ipv6hdr *)data_end) |
104 | return -EINVAL; |
105 | |
106 | if (!ipv6_addr_equal(saddr, ih6->saddr)) |
107 | return -EINVAL; |
108 | |
109 | if (!ipv6_addr_equal(daddr, ih6->daddr)) |
110 | return -EINVAL; |
111 | |
112 | if (ih6->nexthdr != IPPROTO_UDP) |
113 | return -EINVAL; |
114 | |
115 | uh = (struct udphdr *)(ih6 + 1); |
116 | } else { |
117 | return -EINVAL; |
118 | } |
119 | |
120 | if (uh + 1 > (struct udphdr *)data_end) |
121 | return -EINVAL; |
122 | |
123 | port = dut ? uh->dest : uh->source; |
124 | if (port != bpf_htons(DUT_ECHO_PORT)) |
125 | return -EINVAL; |
126 | |
127 | tlv = (struct tlv_hdr *)(uh + 1); |
128 | if (tlv + 1 > data_end) |
129 | return -EINVAL; |
130 | |
131 | return bpf_htons(tlv->type) == CMD_ECHO ? 0 : -EINVAL; |
132 | } |
133 | |
134 | static __always_inline int |
135 | xdp_update_stats(struct xdp_md *xdp, bool tx, bool dut) |
136 | { |
137 | __u32 *val, key = 0; |
138 | |
139 | if (xdp_process_echo_packet(xdp, dut: tx)) |
140 | return -EINVAL; |
141 | |
142 | if (dut) |
143 | val = bpf_map_lookup_elem(&dut_stats, &key); |
144 | else |
145 | val = bpf_map_lookup_elem(&stats, &key); |
146 | |
147 | if (val) |
148 | __sync_add_and_fetch(val, 1); |
149 | |
150 | return 0; |
151 | } |
152 | |
153 | /* Tester */ |
154 | |
155 | SEC("xdp" ) |
156 | int xdp_tester_check_tx(struct xdp_md *xdp) |
157 | { |
158 | xdp_update_stats(xdp, tx: true, dut: false); |
159 | |
160 | return XDP_PASS; |
161 | } |
162 | |
163 | SEC("xdp" ) |
164 | int xdp_tester_check_rx(struct xdp_md *xdp) |
165 | { |
166 | xdp_update_stats(xdp, tx: false, dut: false); |
167 | |
168 | return XDP_PASS; |
169 | } |
170 | |
171 | /* DUT */ |
172 | |
173 | SEC("xdp" ) |
174 | int xdp_do_pass(struct xdp_md *xdp) |
175 | { |
176 | xdp_update_stats(xdp, tx: true, dut: true); |
177 | |
178 | return XDP_PASS; |
179 | } |
180 | |
181 | SEC("xdp" ) |
182 | int xdp_do_drop(struct xdp_md *xdp) |
183 | { |
184 | if (xdp_update_stats(xdp, tx: true, dut: true)) |
185 | return XDP_PASS; |
186 | |
187 | return XDP_DROP; |
188 | } |
189 | |
190 | SEC("xdp" ) |
191 | int xdp_do_aborted(struct xdp_md *xdp) |
192 | { |
193 | if (xdp_process_echo_packet(xdp, dut: true)) |
194 | return XDP_PASS; |
195 | |
196 | return XDP_ABORTED; |
197 | } |
198 | |
199 | SEC("xdp" ) |
200 | int xdp_do_tx(struct xdp_md *xdp) |
201 | { |
202 | void *data = (void *)(long)xdp->data; |
203 | struct ethhdr *eh = data; |
204 | __u8 tmp_mac[ETH_ALEN]; |
205 | |
206 | if (xdp_update_stats(xdp, tx: true, dut: true)) |
207 | return XDP_PASS; |
208 | |
209 | __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN); |
210 | __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN); |
211 | __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN); |
212 | |
213 | return XDP_TX; |
214 | } |
215 | |
216 | SEC("xdp" ) |
217 | int xdp_do_redirect(struct xdp_md *xdp) |
218 | { |
219 | if (xdp_process_echo_packet(xdp, dut: true)) |
220 | return XDP_PASS; |
221 | |
222 | return bpf_redirect_map(&cpu_map, 0, 0); |
223 | } |
224 | |
225 | SEC("tp_btf/xdp_exception" ) |
226 | int BPF_PROG(xdp_exception, const struct net_device *dev, |
227 | const struct bpf_prog *xdp, __u32 act) |
228 | { |
229 | __u32 *val, key = 0; |
230 | |
231 | val = bpf_map_lookup_elem(&dut_stats, &key); |
232 | if (val) |
233 | __sync_add_and_fetch(val, 1); |
234 | |
235 | return 0; |
236 | } |
237 | |
238 | SEC("tp_btf/xdp_cpumap_kthread" ) |
239 | int BPF_PROG(tp_xdp_cpumap_kthread, int map_id, unsigned int processed, |
240 | unsigned int drops, int sched, struct xdp_cpumap_stats *xdp_stats) |
241 | { |
242 | __u32 *val, key = 0; |
243 | |
244 | val = bpf_map_lookup_elem(&dut_stats, &key); |
245 | if (val) |
246 | __sync_add_and_fetch(val, 1); |
247 | |
248 | return 0; |
249 | } |
250 | |
251 | SEC("xdp/cpumap" ) |
252 | int xdp_do_redirect_cpumap(struct xdp_md *xdp) |
253 | { |
254 | void *data = (void *)(long)xdp->data; |
255 | struct ethhdr *eh = data; |
256 | __u8 tmp_mac[ETH_ALEN]; |
257 | |
258 | if (xdp_process_echo_packet(xdp, dut: true)) |
259 | return XDP_PASS; |
260 | |
261 | __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN); |
262 | __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN); |
263 | __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN); |
264 | |
265 | return bpf_redirect_map(&dev_map, 0, 0); |
266 | } |
267 | |
268 | char _license[] SEC("license" ) = "GPL" ; |
269 | |