1// SPDX-License-Identifier: GPL-2.0
2#include <linux/bpf.h>
3#include <linux/if_link.h>
4#include <assert.h>
5#include <errno.h>
6#include <signal.h>
7#include <stdio.h>
8#include <stdlib.h>
9#include <string.h>
10#include <net/if.h>
11#include <unistd.h>
12#include <libgen.h>
13#include <sys/ioctl.h>
14#include <sys/types.h>
15#include <sys/socket.h>
16#include <netinet/in.h>
17
18#include "bpf_util.h"
19#include <bpf/bpf.h>
20#include <bpf/libbpf.h>
21
22#define MAX_IFACE_NUM 32
23#define MAX_INDEX_NUM 1024
24
25static __u32 xdp_flags = XDP_FLAGS_UPDATE_IF_NOEXIST;
26static int ifaces[MAX_IFACE_NUM] = {};
27
28static void int_exit(int sig)
29{
30	__u32 prog_id = 0;
31	int i;
32
33	for (i = 0; ifaces[i] > 0; i++) {
34		if (bpf_xdp_query_id(ifaces[i], xdp_flags, &prog_id)) {
35			printf("bpf_xdp_query_id failed\n");
36			exit(1);
37		}
38		if (prog_id)
39			bpf_xdp_detach(ifaces[i], xdp_flags, NULL);
40	}
41
42	exit(0);
43}
44
45static int get_mac_addr(unsigned int ifindex, void *mac_addr)
46{
47	char ifname[IF_NAMESIZE];
48	struct ifreq ifr;
49	int fd, ret = -1;
50
51	fd = socket(AF_INET, SOCK_DGRAM, 0);
52	if (fd < 0)
53		return ret;
54
55	if (!if_indextoname(ifindex, ifname))
56		goto err_out;
57
58	strcpy(ifr.ifr_name, ifname);
59
60	if (ioctl(fd, SIOCGIFHWADDR, &ifr) != 0)
61		goto err_out;
62
63	memcpy(mac_addr, ifr.ifr_hwaddr.sa_data, 6 * sizeof(char));
64	ret = 0;
65
66err_out:
67	close(fd);
68	return ret;
69}
70
71static void usage(const char *prog)
72{
73	fprintf(stderr,
74		"usage: %s [OPTS] <IFNAME|IFINDEX> <IFNAME|IFINDEX> ...\n"
75		"OPTS:\n"
76		"    -S    use skb-mode\n"
77		"    -N    enforce native mode\n"
78		"    -F    force loading prog\n"
79		"    -X    load xdp program on egress\n",
80		prog);
81}
82
83int main(int argc, char **argv)
84{
85	int prog_fd, group_all, mac_map;
86	struct bpf_program *ingress_prog, *egress_prog;
87	int i, err, ret, opt, egress_prog_fd = 0;
88	struct bpf_devmap_val devmap_val;
89	bool attach_egress_prog = false;
90	unsigned char mac_addr[6];
91	char ifname[IF_NAMESIZE];
92	struct bpf_object *obj;
93	unsigned int ifindex;
94	char filename[256];
95
96	while ((opt = getopt(argc, argv, "SNFX")) != -1) {
97		switch (opt) {
98		case 'S':
99			xdp_flags |= XDP_FLAGS_SKB_MODE;
100			break;
101		case 'N':
102			/* default, set below */
103			break;
104		case 'F':
105			xdp_flags &= ~XDP_FLAGS_UPDATE_IF_NOEXIST;
106			break;
107		case 'X':
108			attach_egress_prog = true;
109			break;
110		default:
111			usage(basename(argv[0]));
112			return 1;
113		}
114	}
115
116	if (!(xdp_flags & XDP_FLAGS_SKB_MODE)) {
117		xdp_flags |= XDP_FLAGS_DRV_MODE;
118	} else if (attach_egress_prog) {
119		printf("Load xdp program on egress with SKB mode not supported yet\n");
120		goto err_out;
121	}
122
123	if (optind == argc) {
124		printf("usage: %s <IFNAME|IFINDEX> <IFNAME|IFINDEX> ...\n", argv[0]);
125		goto err_out;
126	}
127
128	printf("Get interfaces:");
129	for (i = 0; i < MAX_IFACE_NUM && argv[optind + i]; i++) {
130		ifaces[i] = if_nametoindex(argv[optind + i]);
131		if (!ifaces[i])
132			ifaces[i] = strtoul(argv[optind + i], NULL, 0);
133		if (!if_indextoname(ifaces[i], ifname)) {
134			perror("Invalid interface name or i");
135			goto err_out;
136		}
137		if (ifaces[i] > MAX_INDEX_NUM) {
138			printf(" interface index too large\n");
139			goto err_out;
140		}
141		printf(" %d", ifaces[i]);
142	}
143	printf("\n");
144
145	snprintf(filename, sizeof(filename), "%s_kern.bpf.o", argv[0]);
146	obj = bpf_object__open_file(filename, NULL);
147	err = libbpf_get_error(obj);
148	if (err)
149		goto err_out;
150	err = bpf_object__load(obj);
151	if (err)
152		goto err_out;
153	prog_fd = bpf_program__fd(bpf_object__next_program(obj, NULL));
154
155	if (attach_egress_prog)
156		group_all = bpf_object__find_map_fd_by_name(obj, "map_egress");
157	else
158		group_all = bpf_object__find_map_fd_by_name(obj, "map_all");
159	mac_map = bpf_object__find_map_fd_by_name(obj, "mac_map");
160
161	if (group_all < 0 || mac_map < 0) {
162		printf("bpf_object__find_map_fd_by_name failed\n");
163		goto err_out;
164	}
165
166	if (attach_egress_prog) {
167		/* Find ingress/egress prog for 2nd xdp prog */
168		ingress_prog = bpf_object__find_program_by_name(obj, "xdp_redirect_map_all_prog");
169		egress_prog = bpf_object__find_program_by_name(obj, "xdp_devmap_prog");
170		if (!ingress_prog || !egress_prog) {
171			printf("finding ingress/egress_prog in obj file failed\n");
172			goto err_out;
173		}
174		prog_fd = bpf_program__fd(ingress_prog);
175		egress_prog_fd = bpf_program__fd(egress_prog);
176		if (prog_fd < 0 || egress_prog_fd < 0) {
177			printf("find egress_prog fd failed\n");
178			goto err_out;
179		}
180	}
181
182	signal(SIGINT, int_exit);
183	signal(SIGTERM, int_exit);
184
185	/* Init forward multicast groups and exclude group */
186	for (i = 0; ifaces[i] > 0; i++) {
187		ifindex = ifaces[i];
188
189		if (attach_egress_prog) {
190			ret = get_mac_addr(ifindex, mac_addr);
191			if (ret < 0) {
192				printf("get interface %d mac failed\n", ifindex);
193				goto err_out;
194			}
195			ret = bpf_map_update_elem(mac_map, &ifindex, mac_addr, 0);
196			if (ret) {
197				perror("bpf_update_elem mac_map failed\n");
198				goto err_out;
199			}
200		}
201
202		/* Add all the interfaces to group all */
203		devmap_val.ifindex = ifindex;
204		devmap_val.bpf_prog.fd = egress_prog_fd;
205		ret = bpf_map_update_elem(group_all, &ifindex, &devmap_val, 0);
206		if (ret) {
207			perror("bpf_map_update_elem");
208			goto err_out;
209		}
210
211		/* bind prog_fd to each interface */
212		ret = bpf_xdp_attach(ifindex, prog_fd, xdp_flags, NULL);
213		if (ret) {
214			printf("Set xdp fd failed on %d\n", ifindex);
215			goto err_out;
216		}
217	}
218
219	/* sleep some time for testing */
220	sleep(999);
221
222	return 0;
223
224err_out:
225	return 1;
226}
227