1 // SPDX-License-Identifier: GPL-2.0
2 
3 /* Reference program for verifying XDP metadata on real HW. Functional test
4  * only, doesn't test the performance.
5  *
6  * RX:
7  * - UDP 9091 packets are diverted into AF_XDP
8  * - Metadata verified:
9  *   - rx_timestamp
10  *   - rx_hash
11  *
12  * TX:
13  * - TBD
14  */
15 
16 #include <test_progs.h>
17 #include <network_helpers.h>
18 #include "xdp_hw_metadata.skel.h"
19 #include "xsk.h"
20 
21 #include <error.h>
22 #include <linux/errqueue.h>
23 #include <linux/if_link.h>
24 #include <linux/net_tstamp.h>
25 #include <linux/udp.h>
26 #include <linux/sockios.h>
27 #include <sys/mman.h>
28 #include <net/if.h>
29 #include <poll.h>
30 
31 #include "xdp_metadata.h"
32 
33 #define UMEM_NUM 16
34 #define UMEM_FRAME_SIZE XSK_UMEM__DEFAULT_FRAME_SIZE
35 #define UMEM_SIZE (UMEM_FRAME_SIZE * UMEM_NUM)
36 #define XDP_FLAGS (XDP_FLAGS_DRV_MODE | XDP_FLAGS_REPLACE)
37 
38 struct xsk {
39 	void *umem_area;
40 	struct xsk_umem *umem;
41 	struct xsk_ring_prod fill;
42 	struct xsk_ring_cons comp;
43 	struct xsk_ring_prod tx;
44 	struct xsk_ring_cons rx;
45 	struct xsk_socket *socket;
46 };
47 
48 struct xdp_hw_metadata *bpf_obj;
49 struct xsk *rx_xsk;
50 const char *ifname;
51 int ifindex;
52 int rxq;
53 
54 void test__fail(void) { /* for network_helpers.c */ }
55 
56 static int open_xsk(int ifindex, struct xsk *xsk, __u32 queue_id)
57 {
58 	int mmap_flags = MAP_PRIVATE | MAP_ANONYMOUS | MAP_NORESERVE;
59 	const struct xsk_socket_config socket_config = {
60 		.rx_size = XSK_RING_PROD__DEFAULT_NUM_DESCS,
61 		.tx_size = XSK_RING_PROD__DEFAULT_NUM_DESCS,
62 		.bind_flags = XDP_COPY,
63 	};
64 	const struct xsk_umem_config umem_config = {
65 		.fill_size = XSK_RING_PROD__DEFAULT_NUM_DESCS,
66 		.comp_size = XSK_RING_CONS__DEFAULT_NUM_DESCS,
67 		.frame_size = XSK_UMEM__DEFAULT_FRAME_SIZE,
68 		.flags = XDP_UMEM_UNALIGNED_CHUNK_FLAG,
69 	};
70 	__u32 idx;
71 	u64 addr;
72 	int ret;
73 	int i;
74 
75 	xsk->umem_area = mmap(NULL, UMEM_SIZE, PROT_READ | PROT_WRITE, mmap_flags, -1, 0);
76 	if (xsk->umem_area == MAP_FAILED)
77 		return -ENOMEM;
78 
79 	ret = xsk_umem__create(&xsk->umem,
80 			       xsk->umem_area, UMEM_SIZE,
81 			       &xsk->fill,
82 			       &xsk->comp,
83 			       &umem_config);
84 	if (ret)
85 		return ret;
86 
87 	ret = xsk_socket__create(&xsk->socket, ifindex, queue_id,
88 				 xsk->umem,
89 				 &xsk->rx,
90 				 &xsk->tx,
91 				 &socket_config);
92 	if (ret)
93 		return ret;
94 
95 	/* First half of umem is for TX. This way address matches 1-to-1
96 	 * to the completion queue index.
97 	 */
98 
99 	for (i = 0; i < UMEM_NUM / 2; i++) {
100 		addr = i * UMEM_FRAME_SIZE;
101 		printf("%p: tx_desc[%d] -> %lx\n", xsk, i, addr);
102 	}
103 
104 	/* Second half of umem is for RX. */
105 
106 	ret = xsk_ring_prod__reserve(&xsk->fill, UMEM_NUM / 2, &idx);
107 	for (i = 0; i < UMEM_NUM / 2; i++) {
108 		addr = (UMEM_NUM / 2 + i) * UMEM_FRAME_SIZE;
109 		printf("%p: rx_desc[%d] -> %lx\n", xsk, i, addr);
110 		*xsk_ring_prod__fill_addr(&xsk->fill, i) = addr;
111 	}
112 	xsk_ring_prod__submit(&xsk->fill, ret);
113 
114 	return 0;
115 }
116 
117 static void close_xsk(struct xsk *xsk)
118 {
119 	if (xsk->umem)
120 		xsk_umem__delete(xsk->umem);
121 	if (xsk->socket)
122 		xsk_socket__delete(xsk->socket);
123 	munmap(xsk->umem_area, UMEM_SIZE);
124 }
125 
126 static void refill_rx(struct xsk *xsk, __u64 addr)
127 {
128 	__u32 idx;
129 
130 	if (xsk_ring_prod__reserve(&xsk->fill, 1, &idx) == 1) {
131 		printf("%p: complete idx=%u addr=%llx\n", xsk, idx, addr);
132 		*xsk_ring_prod__fill_addr(&xsk->fill, idx) = addr;
133 		xsk_ring_prod__submit(&xsk->fill, 1);
134 	}
135 }
136 
137 static void verify_xdp_metadata(void *data)
138 {
139 	struct xdp_meta *meta;
140 
141 	meta = data - sizeof(*meta);
142 
143 	printf("rx_timestamp: %llu\n", meta->rx_timestamp);
144 	printf("rx_hash: %u\n", meta->rx_hash);
145 }
146 
147 static void verify_skb_metadata(int fd)
148 {
149 	char cmsg_buf[1024];
150 	char packet_buf[128];
151 
152 	struct scm_timestamping *ts;
153 	struct iovec packet_iov;
154 	struct cmsghdr *cmsg;
155 	struct msghdr hdr;
156 
157 	memset(&hdr, 0, sizeof(hdr));
158 	hdr.msg_iov = &packet_iov;
159 	hdr.msg_iovlen = 1;
160 	packet_iov.iov_base = packet_buf;
161 	packet_iov.iov_len = sizeof(packet_buf);
162 
163 	hdr.msg_control = cmsg_buf;
164 	hdr.msg_controllen = sizeof(cmsg_buf);
165 
166 	if (recvmsg(fd, &hdr, 0) < 0)
167 		error(1, errno, "recvmsg");
168 
169 	for (cmsg = CMSG_FIRSTHDR(&hdr); cmsg != NULL;
170 	     cmsg = CMSG_NXTHDR(&hdr, cmsg)) {
171 
172 		if (cmsg->cmsg_level != SOL_SOCKET)
173 			continue;
174 
175 		switch (cmsg->cmsg_type) {
176 		case SCM_TIMESTAMPING:
177 			ts = (struct scm_timestamping *)CMSG_DATA(cmsg);
178 			if (ts->ts[2].tv_sec || ts->ts[2].tv_nsec) {
179 				printf("found skb hwtstamp = %lu.%lu\n",
180 				       ts->ts[2].tv_sec, ts->ts[2].tv_nsec);
181 				return;
182 			}
183 			break;
184 		default:
185 			break;
186 		}
187 	}
188 
189 	printf("skb hwtstamp is not found!\n");
190 }
191 
192 static int verify_metadata(struct xsk *rx_xsk, int rxq, int server_fd)
193 {
194 	const struct xdp_desc *rx_desc;
195 	struct pollfd fds[rxq + 1];
196 	__u64 comp_addr;
197 	__u64 addr;
198 	__u32 idx;
199 	int ret;
200 	int i;
201 
202 	for (i = 0; i < rxq; i++) {
203 		fds[i].fd = xsk_socket__fd(rx_xsk[i].socket);
204 		fds[i].events = POLLIN;
205 		fds[i].revents = 0;
206 	}
207 
208 	fds[rxq].fd = server_fd;
209 	fds[rxq].events = POLLIN;
210 	fds[rxq].revents = 0;
211 
212 	while (true) {
213 		errno = 0;
214 		ret = poll(fds, rxq + 1, 1000);
215 		printf("poll: %d (%d)\n", ret, errno);
216 		if (ret < 0)
217 			break;
218 		if (ret == 0)
219 			continue;
220 
221 		if (fds[rxq].revents)
222 			verify_skb_metadata(server_fd);
223 
224 		for (i = 0; i < rxq; i++) {
225 			if (fds[i].revents == 0)
226 				continue;
227 
228 			struct xsk *xsk = &rx_xsk[i];
229 
230 			ret = xsk_ring_cons__peek(&xsk->rx, 1, &idx);
231 			printf("xsk_ring_cons__peek: %d\n", ret);
232 			if (ret != 1)
233 				continue;
234 
235 			rx_desc = xsk_ring_cons__rx_desc(&xsk->rx, idx);
236 			comp_addr = xsk_umem__extract_addr(rx_desc->addr);
237 			addr = xsk_umem__add_offset_to_addr(rx_desc->addr);
238 			printf("%p: rx_desc[%u]->addr=%llx addr=%llx comp_addr=%llx\n",
239 			       xsk, idx, rx_desc->addr, addr, comp_addr);
240 			verify_xdp_metadata(xsk_umem__get_data(xsk->umem_area, addr));
241 			xsk_ring_cons__release(&xsk->rx, 1);
242 			refill_rx(xsk, comp_addr);
243 		}
244 	}
245 
246 	return 0;
247 }
248 
249 struct ethtool_channels {
250 	__u32	cmd;
251 	__u32	max_rx;
252 	__u32	max_tx;
253 	__u32	max_other;
254 	__u32	max_combined;
255 	__u32	rx_count;
256 	__u32	tx_count;
257 	__u32	other_count;
258 	__u32	combined_count;
259 };
260 
261 #define ETHTOOL_GCHANNELS	0x0000003c /* Get no of channels */
262 
263 static int rxq_num(const char *ifname)
264 {
265 	struct ethtool_channels ch = {
266 		.cmd = ETHTOOL_GCHANNELS,
267 	};
268 
269 	struct ifreq ifr = {
270 		.ifr_data = (void *)&ch,
271 	};
272 	strncpy(ifr.ifr_name, ifname, IF_NAMESIZE - 1);
273 	int fd, ret;
274 
275 	fd = socket(AF_UNIX, SOCK_DGRAM, 0);
276 	if (fd < 0)
277 		error(1, errno, "socket");
278 
279 	ret = ioctl(fd, SIOCETHTOOL, &ifr);
280 	if (ret < 0)
281 		error(1, errno, "ioctl(SIOCETHTOOL)");
282 
283 	close(fd);
284 
285 	return ch.rx_count + ch.combined_count;
286 }
287 
288 static void hwtstamp_ioctl(int op, const char *ifname, struct hwtstamp_config *cfg)
289 {
290 	struct ifreq ifr = {
291 		.ifr_data = (void *)cfg,
292 	};
293 	strncpy(ifr.ifr_name, ifname, IF_NAMESIZE - 1);
294 	int fd, ret;
295 
296 	fd = socket(AF_UNIX, SOCK_DGRAM, 0);
297 	if (fd < 0)
298 		error(1, errno, "socket");
299 
300 	ret = ioctl(fd, op, &ifr);
301 	if (ret < 0)
302 		error(1, errno, "ioctl(%d)", op);
303 
304 	close(fd);
305 }
306 
307 static struct hwtstamp_config saved_hwtstamp_cfg;
308 static const char *saved_hwtstamp_ifname;
309 
310 static void hwtstamp_restore(void)
311 {
312 	hwtstamp_ioctl(SIOCSHWTSTAMP, saved_hwtstamp_ifname, &saved_hwtstamp_cfg);
313 }
314 
315 static void hwtstamp_enable(const char *ifname)
316 {
317 	struct hwtstamp_config cfg = {
318 		.rx_filter = HWTSTAMP_FILTER_ALL,
319 	};
320 
321 	hwtstamp_ioctl(SIOCGHWTSTAMP, ifname, &saved_hwtstamp_cfg);
322 	saved_hwtstamp_ifname = strdup(ifname);
323 	atexit(hwtstamp_restore);
324 
325 	hwtstamp_ioctl(SIOCSHWTSTAMP, ifname, &cfg);
326 }
327 
328 static void cleanup(void)
329 {
330 	LIBBPF_OPTS(bpf_xdp_attach_opts, opts);
331 	int ret;
332 	int i;
333 
334 	if (bpf_obj) {
335 		opts.old_prog_fd = bpf_program__fd(bpf_obj->progs.rx);
336 		if (opts.old_prog_fd >= 0) {
337 			printf("detaching bpf program....\n");
338 			ret = bpf_xdp_detach(ifindex, XDP_FLAGS, &opts);
339 			if (ret)
340 				printf("failed to detach XDP program: %d\n", ret);
341 		}
342 	}
343 
344 	for (i = 0; i < rxq; i++)
345 		close_xsk(&rx_xsk[i]);
346 
347 	if (bpf_obj)
348 		xdp_hw_metadata__destroy(bpf_obj);
349 }
350 
351 static void handle_signal(int sig)
352 {
353 	/* interrupting poll() is all we need */
354 }
355 
356 static void timestamping_enable(int fd, int val)
357 {
358 	int ret;
359 
360 	ret = setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &val, sizeof(val));
361 	if (ret < 0)
362 		error(1, errno, "setsockopt(SO_TIMESTAMPING)");
363 }
364 
365 int main(int argc, char *argv[])
366 {
367 	int server_fd = -1;
368 	int ret;
369 	int i;
370 
371 	struct bpf_program *prog;
372 
373 	if (argc != 2) {
374 		fprintf(stderr, "pass device name\n");
375 		return -1;
376 	}
377 
378 	ifname = argv[1];
379 	ifindex = if_nametoindex(ifname);
380 	rxq = rxq_num(ifname);
381 
382 	printf("rxq: %d\n", rxq);
383 
384 	hwtstamp_enable(ifname);
385 
386 	rx_xsk = malloc(sizeof(struct xsk) * rxq);
387 	if (!rx_xsk)
388 		error(1, ENOMEM, "malloc");
389 
390 	for (i = 0; i < rxq; i++) {
391 		printf("open_xsk(%s, %p, %d)\n", ifname, &rx_xsk[i], i);
392 		ret = open_xsk(ifindex, &rx_xsk[i], i);
393 		if (ret)
394 			error(1, -ret, "open_xsk");
395 
396 		printf("xsk_socket__fd() -> %d\n", xsk_socket__fd(rx_xsk[i].socket));
397 	}
398 
399 	printf("open bpf program...\n");
400 	bpf_obj = xdp_hw_metadata__open();
401 	if (libbpf_get_error(bpf_obj))
402 		error(1, libbpf_get_error(bpf_obj), "xdp_hw_metadata__open");
403 
404 	prog = bpf_object__find_program_by_name(bpf_obj->obj, "rx");
405 	bpf_program__set_ifindex(prog, ifindex);
406 	bpf_program__set_flags(prog, BPF_F_XDP_DEV_BOUND_ONLY);
407 
408 	printf("load bpf program...\n");
409 	ret = xdp_hw_metadata__load(bpf_obj);
410 	if (ret)
411 		error(1, -ret, "xdp_hw_metadata__load");
412 
413 	printf("prepare skb endpoint...\n");
414 	server_fd = start_server(AF_INET6, SOCK_DGRAM, NULL, 9092, 1000);
415 	if (server_fd < 0)
416 		error(1, errno, "start_server");
417 	timestamping_enable(server_fd,
418 			    SOF_TIMESTAMPING_SOFTWARE |
419 			    SOF_TIMESTAMPING_RAW_HARDWARE);
420 
421 	printf("prepare xsk map...\n");
422 	for (i = 0; i < rxq; i++) {
423 		int sock_fd = xsk_socket__fd(rx_xsk[i].socket);
424 		__u32 queue_id = i;
425 
426 		printf("map[%d] = %d\n", queue_id, sock_fd);
427 		ret = bpf_map_update_elem(bpf_map__fd(bpf_obj->maps.xsk), &queue_id, &sock_fd, 0);
428 		if (ret)
429 			error(1, -ret, "bpf_map_update_elem");
430 	}
431 
432 	printf("attach bpf program...\n");
433 	ret = bpf_xdp_attach(ifindex,
434 			     bpf_program__fd(bpf_obj->progs.rx),
435 			     XDP_FLAGS, NULL);
436 	if (ret)
437 		error(1, -ret, "bpf_xdp_attach");
438 
439 	signal(SIGINT, handle_signal);
440 	ret = verify_metadata(rx_xsk, rxq, server_fd);
441 	close(server_fd);
442 	cleanup();
443 	if (ret)
444 		error(1, -ret, "verify_metadata");
445 }
446