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
24struct net_device;
25struct bpf_prog;
26
27struct xdp_cpumap_stats {
28	unsigned int redirect;
29	unsigned int pass;
30	unsigned int drop;
31};
32
33struct {
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
40struct {
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
47struct {
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
54struct {
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
61const volatile struct in6_addr tester_addr;
62const volatile struct in6_addr dut_addr;
63
64static __always_inline int
65xdp_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
134static __always_inline int
135xdp_update_stats(struct xdp_md *xdp, bool tx, bool dut)
136{
137	__u32 *val, key = 0;
138
139	if (xdp_process_echo_packet(xdp, 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
155SEC("xdp")
156int xdp_tester_check_tx(struct xdp_md *xdp)
157{
158	xdp_update_stats(xdp, true, false);
159
160	return XDP_PASS;
161}
162
163SEC("xdp")
164int xdp_tester_check_rx(struct xdp_md *xdp)
165{
166	xdp_update_stats(xdp, false, false);
167
168	return XDP_PASS;
169}
170
171/* DUT */
172
173SEC("xdp")
174int xdp_do_pass(struct xdp_md *xdp)
175{
176	xdp_update_stats(xdp, true, true);
177
178	return XDP_PASS;
179}
180
181SEC("xdp")
182int xdp_do_drop(struct xdp_md *xdp)
183{
184	if (xdp_update_stats(xdp, true, true))
185		return XDP_PASS;
186
187	return XDP_DROP;
188}
189
190SEC("xdp")
191int xdp_do_aborted(struct xdp_md *xdp)
192{
193	if (xdp_process_echo_packet(xdp, true))
194		return XDP_PASS;
195
196	return XDP_ABORTED;
197}
198
199SEC("xdp")
200int 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, true, 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
216SEC("xdp")
217int xdp_do_redirect(struct xdp_md *xdp)
218{
219	if (xdp_process_echo_packet(xdp, true))
220		return XDP_PASS;
221
222	return bpf_redirect_map(&cpu_map, 0, 0);
223}
224
225SEC("tp_btf/xdp_exception")
226int 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
238SEC("tp_btf/xdp_cpumap_kthread")
239int 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
251SEC("xdp/cpumap")
252int 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, 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
268char _license[] SEC("license") = "GPL";
269