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
xdp_process_echo_packet(struct xdp_md * xdp,bool dut)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 __u8 *cmd;
74
75 if (eh + 1 > (struct ethhdr *)data_end)
76 return -EINVAL;
77
78 if (eh->h_proto == bpf_htons(ETH_P_IP)) {
79 struct iphdr *ih = (struct iphdr *)(eh + 1);
80 __be32 saddr = dut ? tester_addr.s6_addr32[3]
81 : dut_addr.s6_addr32[3];
82 __be32 daddr = dut ? dut_addr.s6_addr32[3]
83 : tester_addr.s6_addr32[3];
84
85 ih = (struct iphdr *)(eh + 1);
86 if (ih + 1 > (struct iphdr *)data_end)
87 return -EINVAL;
88
89 if (saddr != ih->saddr)
90 return -EINVAL;
91
92 if (daddr != ih->daddr)
93 return -EINVAL;
94
95 if (ih->protocol != IPPROTO_UDP)
96 return -EINVAL;
97
98 uh = (struct udphdr *)(ih + 1);
99 } else if (eh->h_proto == bpf_htons(ETH_P_IPV6)) {
100 struct in6_addr saddr = dut ? tester_addr : dut_addr;
101 struct in6_addr daddr = dut ? dut_addr : tester_addr;
102 struct ipv6hdr *ih6 = (struct ipv6hdr *)(eh + 1);
103
104 if (ih6 + 1 > (struct ipv6hdr *)data_end)
105 return -EINVAL;
106
107 if (!ipv6_addr_equal(saddr, ih6->saddr))
108 return -EINVAL;
109
110 if (!ipv6_addr_equal(daddr, ih6->daddr))
111 return -EINVAL;
112
113 if (ih6->nexthdr != IPPROTO_UDP)
114 return -EINVAL;
115
116 uh = (struct udphdr *)(ih6 + 1);
117 } else {
118 return -EINVAL;
119 }
120
121 if (uh + 1 > (struct udphdr *)data_end)
122 return -EINVAL;
123
124 port = dut ? uh->dest : uh->source;
125 if (port != bpf_htons(DUT_ECHO_PORT))
126 return -EINVAL;
127
128 tlv = (struct tlv_hdr *)(uh + 1);
129 if (tlv + 1 > data_end)
130 return -EINVAL;
131
132 return bpf_htons(tlv->type) == CMD_ECHO ? 0 : -EINVAL;
133 }
134
135 static __always_inline int
xdp_update_stats(struct xdp_md * xdp,bool tx,bool dut)136 xdp_update_stats(struct xdp_md *xdp, bool tx, bool dut)
137 {
138 __u32 *val, key = 0;
139
140 if (xdp_process_echo_packet(xdp, tx))
141 return -EINVAL;
142
143 if (dut)
144 val = bpf_map_lookup_elem(&dut_stats, &key);
145 else
146 val = bpf_map_lookup_elem(&stats, &key);
147
148 if (val)
149 __sync_add_and_fetch(val, 1);
150
151 return 0;
152 }
153
154 /* Tester */
155
156 SEC("xdp")
xdp_tester_check_tx(struct xdp_md * xdp)157 int xdp_tester_check_tx(struct xdp_md *xdp)
158 {
159 xdp_update_stats(xdp, true, false);
160
161 return XDP_PASS;
162 }
163
164 SEC("xdp")
xdp_tester_check_rx(struct xdp_md * xdp)165 int xdp_tester_check_rx(struct xdp_md *xdp)
166 {
167 xdp_update_stats(xdp, false, false);
168
169 return XDP_PASS;
170 }
171
172 /* DUT */
173
174 SEC("xdp")
xdp_do_pass(struct xdp_md * xdp)175 int xdp_do_pass(struct xdp_md *xdp)
176 {
177 xdp_update_stats(xdp, true, true);
178
179 return XDP_PASS;
180 }
181
182 SEC("xdp")
xdp_do_drop(struct xdp_md * xdp)183 int xdp_do_drop(struct xdp_md *xdp)
184 {
185 if (xdp_update_stats(xdp, true, true))
186 return XDP_PASS;
187
188 return XDP_DROP;
189 }
190
191 SEC("xdp")
xdp_do_aborted(struct xdp_md * xdp)192 int xdp_do_aborted(struct xdp_md *xdp)
193 {
194 if (xdp_process_echo_packet(xdp, true))
195 return XDP_PASS;
196
197 return XDP_ABORTED;
198 }
199
200 SEC("xdp")
xdp_do_tx(struct xdp_md * xdp)201 int xdp_do_tx(struct xdp_md *xdp)
202 {
203 void *data = (void *)(long)xdp->data;
204 struct ethhdr *eh = data;
205 __u8 tmp_mac[ETH_ALEN];
206
207 if (xdp_update_stats(xdp, true, true))
208 return XDP_PASS;
209
210 __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN);
211 __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN);
212 __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN);
213
214 return XDP_TX;
215 }
216
217 SEC("xdp")
xdp_do_redirect(struct xdp_md * xdp)218 int xdp_do_redirect(struct xdp_md *xdp)
219 {
220 if (xdp_process_echo_packet(xdp, true))
221 return XDP_PASS;
222
223 return bpf_redirect_map(&cpu_map, 0, 0);
224 }
225
226 SEC("tp_btf/xdp_exception")
BPF_PROG(xdp_exception,const struct net_device * dev,const struct bpf_prog * xdp,__u32 act)227 int BPF_PROG(xdp_exception, const struct net_device *dev,
228 const struct bpf_prog *xdp, __u32 act)
229 {
230 __u32 *val, key = 0;
231
232 val = bpf_map_lookup_elem(&dut_stats, &key);
233 if (val)
234 __sync_add_and_fetch(val, 1);
235
236 return 0;
237 }
238
239 SEC("tp_btf/xdp_cpumap_kthread")
BPF_PROG(tp_xdp_cpumap_kthread,int map_id,unsigned int processed,unsigned int drops,int sched,struct xdp_cpumap_stats * xdp_stats)240 int BPF_PROG(tp_xdp_cpumap_kthread, int map_id, unsigned int processed,
241 unsigned int drops, int sched, struct xdp_cpumap_stats *xdp_stats)
242 {
243 __u32 *val, key = 0;
244
245 val = bpf_map_lookup_elem(&dut_stats, &key);
246 if (val)
247 __sync_add_and_fetch(val, 1);
248
249 return 0;
250 }
251
252 SEC("xdp/cpumap")
xdp_do_redirect_cpumap(struct xdp_md * xdp)253 int xdp_do_redirect_cpumap(struct xdp_md *xdp)
254 {
255 void *data = (void *)(long)xdp->data;
256 struct ethhdr *eh = data;
257 __u8 tmp_mac[ETH_ALEN];
258
259 if (xdp_process_echo_packet(xdp, true))
260 return XDP_PASS;
261
262 __builtin_memcpy(tmp_mac, eh->h_source, ETH_ALEN);
263 __builtin_memcpy(eh->h_source, eh->h_dest, ETH_ALEN);
264 __builtin_memcpy(eh->h_dest, tmp_mac, ETH_ALEN);
265
266 return bpf_redirect_map(&dev_map, 0, 0);
267 }
268
269 char _license[] SEC("license") = "GPL";
270