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 
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
xdp_update_stats(struct xdp_md * xdp,bool tx,bool dut)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, 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")
xdp_tester_check_tx(struct xdp_md * xdp)156 int xdp_tester_check_tx(struct xdp_md *xdp)
157 {
158 	xdp_update_stats(xdp, true, false);
159 
160 	return XDP_PASS;
161 }
162 
163 SEC("xdp")
xdp_tester_check_rx(struct xdp_md * xdp)164 int 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 
173 SEC("xdp")
xdp_do_pass(struct xdp_md * xdp)174 int xdp_do_pass(struct xdp_md *xdp)
175 {
176 	xdp_update_stats(xdp, true, true);
177 
178 	return XDP_PASS;
179 }
180 
181 SEC("xdp")
xdp_do_drop(struct xdp_md * xdp)182 int 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 
190 SEC("xdp")
xdp_do_aborted(struct xdp_md * xdp)191 int 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 
199 SEC("xdp")
xdp_do_tx(struct xdp_md * 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, 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 
216 SEC("xdp")
xdp_do_redirect(struct xdp_md * xdp)217 int 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 
225 SEC("tp_btf/xdp_exception")
BPF_PROG(xdp_exception,const struct net_device * dev,const struct bpf_prog * xdp,__u32 act)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")
BPF_PROG(tp_xdp_cpumap_kthread,int map_id,unsigned int processed,unsigned int drops,int sched,struct xdp_cpumap_stats * xdp_stats)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")
xdp_do_redirect_cpumap(struct xdp_md * xdp)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, 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