1// SPDX-License-Identifier: GPL-2.0
2/* Toeplitz test
3 *
4 * 1. Read packets and their rx_hash using PF_PACKET/TPACKET_V3
5 * 2. Compute the rx_hash in software based on the packet contents
6 * 3. Compare the two
7 *
8 * Optionally, either '-C $rx_irq_cpu_list' or '-r $rps_bitmap' may be given.
9 *
10 * If '-C $rx_irq_cpu_list' is given, also
11 *
12 * 4. Identify the cpu on which the packet arrived with PACKET_FANOUT_CPU
13 * 5. Compute the rxqueue that RSS would select based on this rx_hash
14 * 6. Using the $rx_irq_cpu_list map, identify the arriving cpu based on rxq irq
15 * 7. Compare the cpus from 4 and 6
16 *
17 * Else if '-r $rps_bitmap' is given, also
18 *
19 * 4. Identify the cpu on which the packet arrived with PACKET_FANOUT_CPU
20 * 5. Compute the cpu that RPS should select based on rx_hash and $rps_bitmap
21 * 6. Compare the cpus from 4 and 5
22 */
23
24#define _GNU_SOURCE
25
26#include <arpa/inet.h>
27#include <errno.h>
28#include <error.h>
29#include <fcntl.h>
30#include <getopt.h>
31#include <linux/filter.h>
32#include <linux/if_ether.h>
33#include <linux/if_packet.h>
34#include <net/if.h>
35#include <netdb.h>
36#include <netinet/ip.h>
37#include <netinet/ip6.h>
38#include <netinet/tcp.h>
39#include <netinet/udp.h>
40#include <poll.h>
41#include <stdbool.h>
42#include <stddef.h>
43#include <stdint.h>
44#include <stdio.h>
45#include <stdlib.h>
46#include <string.h>
47#include <sys/mman.h>
48#include <sys/socket.h>
49#include <sys/stat.h>
50#include <sys/sysinfo.h>
51#include <sys/time.h>
52#include <sys/types.h>
53#include <unistd.h>
54
55#include "../kselftest.h"
56
57#define TOEPLITZ_KEY_MIN_LEN	40
58#define TOEPLITZ_KEY_MAX_LEN	60
59
60#define TOEPLITZ_STR_LEN(K)	(((K) * 3) - 1)	/* hex encoded: AA:BB:CC:...:ZZ */
61#define TOEPLITZ_STR_MIN_LEN	TOEPLITZ_STR_LEN(TOEPLITZ_KEY_MIN_LEN)
62#define TOEPLITZ_STR_MAX_LEN	TOEPLITZ_STR_LEN(TOEPLITZ_KEY_MAX_LEN)
63
64#define FOUR_TUPLE_MAX_LEN	((sizeof(struct in6_addr) * 2) + (sizeof(uint16_t) * 2))
65
66#define RSS_MAX_CPUS (1 << 16)	/* real constraint is PACKET_FANOUT_MAX */
67
68#define RPS_MAX_CPUS 16UL	/* must be a power of 2 */
69
70/* configuration options (cmdline arguments) */
71static uint16_t cfg_dport =	8000;
72static int cfg_family =		AF_INET6;
73static char *cfg_ifname =	"eth0";
74static int cfg_num_queues;
75static int cfg_num_rps_cpus;
76static bool cfg_sink;
77static int cfg_type =		SOCK_STREAM;
78static int cfg_timeout_msec =	1000;
79static bool cfg_verbose;
80
81/* global vars */
82static int num_cpus;
83static int ring_block_nr;
84static int ring_block_sz;
85
86/* stats */
87static int frames_received;
88static int frames_nohash;
89static int frames_error;
90
91#define log_verbose(args...)	do { if (cfg_verbose) fprintf(stderr, args); } while (0)
92
93/* tpacket ring */
94struct ring_state {
95	int fd;
96	char *mmap;
97	int idx;
98	int cpu;
99};
100
101static unsigned int rx_irq_cpus[RSS_MAX_CPUS];	/* map from rxq to cpu */
102static int rps_silo_to_cpu[RPS_MAX_CPUS];
103static unsigned char toeplitz_key[TOEPLITZ_KEY_MAX_LEN];
104static struct ring_state rings[RSS_MAX_CPUS];
105
106static inline uint32_t toeplitz(const unsigned char *four_tuple,
107				const unsigned char *key)
108{
109	int i, bit, ret = 0;
110	uint32_t key32;
111
112	key32 = ntohl(*((uint32_t *)key));
113	key += 4;
114
115	for (i = 0; i < FOUR_TUPLE_MAX_LEN; i++) {
116		for (bit = 7; bit >= 0; bit--) {
117			if (four_tuple[i] & (1 << bit))
118				ret ^= key32;
119
120			key32 <<= 1;
121			key32 |= !!(key[0] & (1 << bit));
122		}
123		key++;
124	}
125
126	return ret;
127}
128
129/* Compare computed cpu with arrival cpu from packet_fanout_cpu */
130static void verify_rss(uint32_t rx_hash, int cpu)
131{
132	int queue = rx_hash % cfg_num_queues;
133
134	log_verbose(" rxq %d (cpu %d)", queue, rx_irq_cpus[queue]);
135	if (rx_irq_cpus[queue] != cpu) {
136		log_verbose(". error: rss cpu mismatch (%d)", cpu);
137		frames_error++;
138	}
139}
140
141static void verify_rps(uint64_t rx_hash, int cpu)
142{
143	int silo = (rx_hash * cfg_num_rps_cpus) >> 32;
144
145	log_verbose(" silo %d (cpu %d)", silo, rps_silo_to_cpu[silo]);
146	if (rps_silo_to_cpu[silo] != cpu) {
147		log_verbose(". error: rps cpu mismatch (%d)", cpu);
148		frames_error++;
149	}
150}
151
152static void log_rxhash(int cpu, uint32_t rx_hash,
153		       const char *addrs, int addr_len)
154{
155	char saddr[INET6_ADDRSTRLEN], daddr[INET6_ADDRSTRLEN];
156	uint16_t *ports;
157
158	if (!inet_ntop(cfg_family, addrs, saddr, sizeof(saddr)) ||
159	    !inet_ntop(cfg_family, addrs + addr_len, daddr, sizeof(daddr)))
160		error(1, 0, "address parse error");
161
162	ports = (void *)addrs + (addr_len * 2);
163	log_verbose("cpu %d: rx_hash 0x%08x [saddr %s daddr %s sport %02hu dport %02hu]",
164		    cpu, rx_hash, saddr, daddr,
165		    ntohs(ports[0]), ntohs(ports[1]));
166}
167
168/* Compare computed rxhash with rxhash received from tpacket_v3 */
169static void verify_rxhash(const char *pkt, uint32_t rx_hash, int cpu)
170{
171	unsigned char four_tuple[FOUR_TUPLE_MAX_LEN] = {0};
172	uint32_t rx_hash_sw;
173	const char *addrs;
174	int addr_len;
175
176	if (cfg_family == AF_INET) {
177		addr_len = sizeof(struct in_addr);
178		addrs = pkt + offsetof(struct iphdr, saddr);
179	} else {
180		addr_len = sizeof(struct in6_addr);
181		addrs = pkt + offsetof(struct ip6_hdr, ip6_src);
182	}
183
184	memcpy(four_tuple, addrs, (addr_len * 2) + (sizeof(uint16_t) * 2));
185	rx_hash_sw = toeplitz(four_tuple, toeplitz_key);
186
187	if (cfg_verbose)
188		log_rxhash(cpu, rx_hash, addrs, addr_len);
189
190	if (rx_hash != rx_hash_sw) {
191		log_verbose(" != expected 0x%x\n", rx_hash_sw);
192		frames_error++;
193		return;
194	}
195
196	log_verbose(" OK");
197	if (cfg_num_queues)
198		verify_rss(rx_hash, cpu);
199	else if (cfg_num_rps_cpus)
200		verify_rps(rx_hash, cpu);
201	log_verbose("\n");
202}
203
204static char *recv_frame(const struct ring_state *ring, char *frame)
205{
206	struct tpacket3_hdr *hdr = (void *)frame;
207
208	if (hdr->hv1.tp_rxhash)
209		verify_rxhash(frame + hdr->tp_net, hdr->hv1.tp_rxhash,
210			      ring->cpu);
211	else
212		frames_nohash++;
213
214	return frame + hdr->tp_next_offset;
215}
216
217/* A single TPACKET_V3 block can hold multiple frames */
218static bool recv_block(struct ring_state *ring)
219{
220	struct tpacket_block_desc *block;
221	char *frame;
222	int i;
223
224	block = (void *)(ring->mmap + ring->idx * ring_block_sz);
225	if (!(block->hdr.bh1.block_status & TP_STATUS_USER))
226		return false;
227
228	frame = (char *)block;
229	frame += block->hdr.bh1.offset_to_first_pkt;
230
231	for (i = 0; i < block->hdr.bh1.num_pkts; i++) {
232		frame = recv_frame(ring, frame);
233		frames_received++;
234	}
235
236	block->hdr.bh1.block_status = TP_STATUS_KERNEL;
237	ring->idx = (ring->idx + 1) % ring_block_nr;
238
239	return true;
240}
241
242/* simple test: sleep once unconditionally and then process all rings */
243static void process_rings(void)
244{
245	int i;
246
247	usleep(1000 * cfg_timeout_msec);
248
249	for (i = 0; i < num_cpus; i++)
250		do {} while (recv_block(&rings[i]));
251
252	fprintf(stderr, "count: pass=%u nohash=%u fail=%u\n",
253		frames_received - frames_nohash - frames_error,
254		frames_nohash, frames_error);
255}
256
257static char *setup_ring(int fd)
258{
259	struct tpacket_req3 req3 = {0};
260	void *ring;
261
262	req3.tp_retire_blk_tov = cfg_timeout_msec / 8;
263	req3.tp_feature_req_word = TP_FT_REQ_FILL_RXHASH;
264
265	req3.tp_frame_size = 2048;
266	req3.tp_frame_nr = 1 << 10;
267	req3.tp_block_nr = 16;
268
269	req3.tp_block_size = req3.tp_frame_size * req3.tp_frame_nr;
270	req3.tp_block_size /= req3.tp_block_nr;
271
272	if (setsockopt(fd, SOL_PACKET, PACKET_RX_RING, &req3, sizeof(req3)))
273		error(1, errno, "setsockopt PACKET_RX_RING");
274
275	ring_block_sz = req3.tp_block_size;
276	ring_block_nr = req3.tp_block_nr;
277
278	ring = mmap(0, req3.tp_block_size * req3.tp_block_nr,
279		    PROT_READ | PROT_WRITE,
280		    MAP_SHARED | MAP_LOCKED | MAP_POPULATE, fd, 0);
281	if (ring == MAP_FAILED)
282		error(1, 0, "mmap failed");
283
284	return ring;
285}
286
287static void __set_filter(int fd, int off_proto, uint8_t proto, int off_dport)
288{
289	struct sock_filter filter[] = {
290		BPF_STMT(BPF_LD  + BPF_B   + BPF_ABS, SKF_AD_OFF + SKF_AD_PKTTYPE),
291		BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, PACKET_HOST, 0, 4),
292		BPF_STMT(BPF_LD  + BPF_B   + BPF_ABS, off_proto),
293		BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, proto, 0, 2),
294		BPF_STMT(BPF_LD  + BPF_H   + BPF_ABS, off_dport),
295		BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, cfg_dport, 1, 0),
296		BPF_STMT(BPF_RET + BPF_K, 0),
297		BPF_STMT(BPF_RET + BPF_K, 0xFFFF),
298	};
299	struct sock_fprog prog = {};
300
301	prog.filter = filter;
302	prog.len = ARRAY_SIZE(filter);
303	if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)))
304		error(1, errno, "setsockopt filter");
305}
306
307/* filter on transport protocol and destination port */
308static void set_filter(int fd)
309{
310	const int off_dport = offsetof(struct tcphdr, dest);	/* same for udp */
311	uint8_t proto;
312
313	proto = cfg_type == SOCK_STREAM ? IPPROTO_TCP : IPPROTO_UDP;
314	if (cfg_family == AF_INET)
315		__set_filter(fd, offsetof(struct iphdr, protocol), proto,
316			     sizeof(struct iphdr) + off_dport);
317	else
318		__set_filter(fd, offsetof(struct ip6_hdr, ip6_nxt), proto,
319			     sizeof(struct ip6_hdr) + off_dport);
320}
321
322/* drop everything: used temporarily during setup */
323static void set_filter_null(int fd)
324{
325	struct sock_filter filter[] = {
326		BPF_STMT(BPF_RET + BPF_K, 0),
327	};
328	struct sock_fprog prog = {};
329
330	prog.filter = filter;
331	prog.len = ARRAY_SIZE(filter);
332	if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &prog, sizeof(prog)))
333		error(1, errno, "setsockopt filter");
334}
335
336static int create_ring(char **ring)
337{
338	struct fanout_args args = {
339		.id = 1,
340		.type_flags = PACKET_FANOUT_CPU,
341		.max_num_members = RSS_MAX_CPUS
342	};
343	struct sockaddr_ll ll = { 0 };
344	int fd, val;
345
346	fd = socket(PF_PACKET, SOCK_DGRAM, 0);
347	if (fd == -1)
348		error(1, errno, "socket creation failed");
349
350	val = TPACKET_V3;
351	if (setsockopt(fd, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)))
352		error(1, errno, "setsockopt PACKET_VERSION");
353	*ring = setup_ring(fd);
354
355	/* block packets until all rings are added to the fanout group:
356	 * else packets can arrive during setup and get misclassified
357	 */
358	set_filter_null(fd);
359
360	ll.sll_family = AF_PACKET;
361	ll.sll_ifindex = if_nametoindex(cfg_ifname);
362	ll.sll_protocol = cfg_family == AF_INET ? htons(ETH_P_IP) :
363						  htons(ETH_P_IPV6);
364	if (bind(fd, (void *)&ll, sizeof(ll)))
365		error(1, errno, "bind");
366
367	/* must come after bind: verifies all programs in group match */
368	if (setsockopt(fd, SOL_PACKET, PACKET_FANOUT, &args, sizeof(args))) {
369		/* on failure, retry using old API if that is sufficient:
370		 * it has a hard limit of 256 sockets, so only try if
371		 * (a) only testing rxhash, not RSS or (b) <= 256 cpus.
372		 * in this API, the third argument is left implicit.
373		 */
374		if (cfg_num_queues || num_cpus > 256 ||
375		    setsockopt(fd, SOL_PACKET, PACKET_FANOUT,
376			       &args, sizeof(uint32_t)))
377			error(1, errno, "setsockopt PACKET_FANOUT cpu");
378	}
379
380	return fd;
381}
382
383/* setup inet(6) socket to blackhole the test traffic, if arg '-s' */
384static int setup_sink(void)
385{
386	int fd, val;
387
388	fd = socket(cfg_family, cfg_type, 0);
389	if (fd == -1)
390		error(1, errno, "socket %d.%d", cfg_family, cfg_type);
391
392	val = 1 << 20;
393	if (setsockopt(fd, SOL_SOCKET, SO_RCVBUFFORCE, &val, sizeof(val)))
394		error(1, errno, "setsockopt rcvbuf");
395
396	return fd;
397}
398
399static void setup_rings(void)
400{
401	int i;
402
403	for (i = 0; i < num_cpus; i++) {
404		rings[i].cpu = i;
405		rings[i].fd = create_ring(&rings[i].mmap);
406	}
407
408	/* accept packets once all rings in the fanout group are up */
409	for (i = 0; i < num_cpus; i++)
410		set_filter(rings[i].fd);
411}
412
413static void cleanup_rings(void)
414{
415	int i;
416
417	for (i = 0; i < num_cpus; i++) {
418		if (munmap(rings[i].mmap, ring_block_nr * ring_block_sz))
419			error(1, errno, "munmap");
420		if (close(rings[i].fd))
421			error(1, errno, "close");
422	}
423}
424
425static void parse_cpulist(const char *arg)
426{
427	do {
428		rx_irq_cpus[cfg_num_queues++] = strtol(arg, NULL, 10);
429
430		arg = strchr(arg, ',');
431		if (!arg)
432			break;
433		arg++;			// skip ','
434	} while (1);
435}
436
437static void show_cpulist(void)
438{
439	int i;
440
441	for (i = 0; i < cfg_num_queues; i++)
442		fprintf(stderr, "rxq %d: cpu %d\n", i, rx_irq_cpus[i]);
443}
444
445static void show_silos(void)
446{
447	int i;
448
449	for (i = 0; i < cfg_num_rps_cpus; i++)
450		fprintf(stderr, "silo %d: cpu %d\n", i, rps_silo_to_cpu[i]);
451}
452
453static void parse_toeplitz_key(const char *str, int slen, unsigned char *key)
454{
455	int i, ret, off;
456
457	if (slen < TOEPLITZ_STR_MIN_LEN ||
458	    slen > TOEPLITZ_STR_MAX_LEN + 1)
459		error(1, 0, "invalid toeplitz key");
460
461	for (i = 0, off = 0; off < slen; i++, off += 3) {
462		ret = sscanf(str + off, "%hhx", &key[i]);
463		if (ret != 1)
464			error(1, 0, "key parse error at %d off %d len %d",
465			      i, off, slen);
466	}
467}
468
469static void parse_rps_bitmap(const char *arg)
470{
471	unsigned long bitmap;
472	int i;
473
474	bitmap = strtoul(arg, NULL, 0);
475
476	if (bitmap & ~(RPS_MAX_CPUS - 1))
477		error(1, 0, "rps bitmap 0x%lx out of bounds 0..%lu",
478		      bitmap, RPS_MAX_CPUS - 1);
479
480	for (i = 0; i < RPS_MAX_CPUS; i++)
481		if (bitmap & 1UL << i)
482			rps_silo_to_cpu[cfg_num_rps_cpus++] = i;
483}
484
485static void parse_opts(int argc, char **argv)
486{
487	static struct option long_options[] = {
488	    {"dport",	required_argument, 0, 'd'},
489	    {"cpus",	required_argument, 0, 'C'},
490	    {"key",	required_argument, 0, 'k'},
491	    {"iface",	required_argument, 0, 'i'},
492	    {"ipv4",	no_argument, 0, '4'},
493	    {"ipv6",	no_argument, 0, '6'},
494	    {"sink",	no_argument, 0, 's'},
495	    {"tcp",	no_argument, 0, 't'},
496	    {"timeout",	required_argument, 0, 'T'},
497	    {"udp",	no_argument, 0, 'u'},
498	    {"verbose",	no_argument, 0, 'v'},
499	    {"rps",	required_argument, 0, 'r'},
500	    {0, 0, 0, 0}
501	};
502	bool have_toeplitz = false;
503	int index, c;
504
505	while ((c = getopt_long(argc, argv, "46C:d:i:k:r:stT:uv", long_options, &index)) != -1) {
506		switch (c) {
507		case '4':
508			cfg_family = AF_INET;
509			break;
510		case '6':
511			cfg_family = AF_INET6;
512			break;
513		case 'C':
514			parse_cpulist(optarg);
515			break;
516		case 'd':
517			cfg_dport = strtol(optarg, NULL, 0);
518			break;
519		case 'i':
520			cfg_ifname = optarg;
521			break;
522		case 'k':
523			parse_toeplitz_key(optarg, strlen(optarg),
524					   toeplitz_key);
525			have_toeplitz = true;
526			break;
527		case 'r':
528			parse_rps_bitmap(optarg);
529			break;
530		case 's':
531			cfg_sink = true;
532			break;
533		case 't':
534			cfg_type = SOCK_STREAM;
535			break;
536		case 'T':
537			cfg_timeout_msec = strtol(optarg, NULL, 0);
538			break;
539		case 'u':
540			cfg_type = SOCK_DGRAM;
541			break;
542		case 'v':
543			cfg_verbose = true;
544			break;
545
546		default:
547			error(1, 0, "unknown option %c", optopt);
548			break;
549		}
550	}
551
552	if (!have_toeplitz)
553		error(1, 0, "Must supply rss key ('-k')");
554
555	num_cpus = get_nprocs();
556	if (num_cpus > RSS_MAX_CPUS)
557		error(1, 0, "increase RSS_MAX_CPUS");
558
559	if (cfg_num_queues && cfg_num_rps_cpus)
560		error(1, 0,
561		      "Can't supply both RSS cpus ('-C') and RPS map ('-r')");
562	if (cfg_verbose) {
563		show_cpulist();
564		show_silos();
565	}
566}
567
568int main(int argc, char **argv)
569{
570	const int min_tests = 10;
571	int fd_sink = -1;
572
573	parse_opts(argc, argv);
574
575	if (cfg_sink)
576		fd_sink = setup_sink();
577
578	setup_rings();
579	process_rings();
580	cleanup_rings();
581
582	if (cfg_sink && close(fd_sink))
583		error(1, errno, "close sink");
584
585	if (frames_received - frames_nohash < min_tests)
586		error(1, 0, "too few frames for verification");
587
588	return frames_error;
589}
590