1/*
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright 2023 Baptiste Daroussin <bapt@FreeBSD.org>
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted providing that the following conditions~
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 *    notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 *    notice, this list of conditions and the following disclaimer in the
13 *    documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
16 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
19 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
23 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
24 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 * POSSIBILITY OF SUCH DAMAGE.
26 */
27
28#include <sys/param.h>
29#include <sys/module.h>
30#include <sys/socket.h>
31
32#include <stdint.h>
33#include <stdlib.h>
34#include <unistd.h>
35#include <err.h>
36#include <stdio.h>
37#include <poll.h>
38
39#include <netlink/netlink.h>
40#include <netlink/netlink_generic.h>
41#include <netlink/netlink_snl.h>
42#include <netlink/netlink_snl_generic.h>
43
44static int monitor_mcast(int argc, char **argv);
45static int list_families(int argc, char **argv);
46static void parser_nlctrl_notify(struct snl_state *ss, struct nlmsghdr *hdr);
47static void parser_fallback(struct snl_state *ss, struct nlmsghdr *hdr);
48
49static struct commands {
50	const char *name;
51	const char *usage;
52	int (*cmd)(int argc, char **argv);
53} cmds[] = {
54	{ "monitor", "monitor <family> <multicast group>", monitor_mcast },
55	{ "list", "list", list_families },
56};
57
58static struct mcast_parsers {
59	const char *family;
60	void (*parser)(struct snl_state *ss, struct nlmsghdr *hdr);
61} mcast_parsers [] = {
62	{ "nlctrl", parser_nlctrl_notify },
63};
64
65struct genl_ctrl_op {
66	uint32_t id;
67	uint32_t flags;
68};
69
70struct genl_ctrl_ops {
71	uint32_t num_ops;
72	struct genl_ctrl_op **ops;
73};
74
75#define _OUT(_field)	offsetof(struct genl_ctrl_op, _field)
76static struct snl_attr_parser _nla_p_getops[] = {
77	{ .type = CTRL_ATTR_OP_ID, .off = _OUT(id), .cb = snl_attr_get_uint32},
78	{ .type = CTRL_ATTR_OP_FLAGS, .off = _OUT(flags), .cb = snl_attr_get_uint32 },
79};
80#undef _OUT
81SNL_DECLARE_ATTR_PARSER_EXT(genl_ctrl_op_parser,
82		sizeof(struct genl_ctrl_op),
83		_nla_p_getops, NULL);
84
85struct genl_family {
86	uint16_t id;
87	char *name;
88	uint32_t version;
89	uint32_t hdrsize;
90	uint32_t max_attr;
91	struct snl_genl_ctrl_mcast_groups mcast_groups;
92	struct genl_ctrl_ops ops;
93};
94
95#define	_OUT(_field)	offsetof(struct genl_family, _field)
96static struct snl_attr_parser _nla_p_getfamily[] = {
97	{ .type = CTRL_ATTR_FAMILY_ID , .off = _OUT(id), .cb = snl_attr_get_uint16 },
98	{ .type = CTRL_ATTR_FAMILY_NAME, .off = _OUT(name), .cb = snl_attr_get_string },
99	{ .type = CTRL_ATTR_VERSION, .off = _OUT(version), .cb = snl_attr_get_uint32 },
100	{ .type = CTRL_ATTR_VERSION, .off = _OUT(hdrsize), .cb = snl_attr_get_uint32 },
101	{ .type = CTRL_ATTR_MAXATTR, .off = _OUT(max_attr), .cb = snl_attr_get_uint32 },
102	{
103		.type = CTRL_ATTR_OPS,
104		.off = _OUT(ops),
105		.cb = snl_attr_get_parray,
106		.arg = &genl_ctrl_op_parser,
107	},
108	{
109		.type = CTRL_ATTR_MCAST_GROUPS,
110		.off = _OUT(mcast_groups),
111		.cb = snl_attr_get_parray,
112		.arg = &_genl_ctrl_mc_parser,
113	},
114};
115#undef _OUT
116SNL_DECLARE_GENL_PARSER(genl_family_parser, _nla_p_getfamily);
117
118static struct op_capability {
119	uint32_t flag;
120	const char *str;
121} op_caps[] = {
122	{ GENL_ADMIN_PERM, "requires admin permission" },
123	{ GENL_CMD_CAP_DO, "can modify" },
124	{ GENL_CMD_CAP_DUMP, "can get/dump" },
125	{ GENL_CMD_CAP_HASPOL, "has policy" },
126};
127
128static void
129dump_operations(struct genl_ctrl_ops *ops)
130{
131	if (ops->num_ops == 0)
132		return;
133	printf("\tsupported operations: \n");
134	for (uint32_t i = 0; i < ops->num_ops; i++) {
135		printf("\t  - ID: %#02x, Capabilities: %#02x (",
136		    ops->ops[i]->id,
137		    ops->ops[i]->flags);
138		for (size_t j = 0; j < nitems(op_caps); j++)
139			if ((ops->ops[i]->flags & op_caps[j].flag) == op_caps[j].flag)
140				printf("%s; ", op_caps[j].str);
141		printf("\b\b)\n");
142	}
143}
144
145static void
146dump_mcast_groups( struct snl_genl_ctrl_mcast_groups *mcast_groups)
147{
148	if (mcast_groups->num_groups == 0)
149		return;
150	printf("\tmulticast groups: \n");
151	for (uint32_t i = 0; i < mcast_groups->num_groups; i++)
152		printf("\t  - ID: %#02x, Name: %s\n",
153		    mcast_groups->groups[i]->mcast_grp_id,
154		    mcast_groups->groups[i]->mcast_grp_name);
155}
156
157static void
158usage(void)
159{
160	fprintf(stderr, "Usage: %s\n", getprogname());
161	for (size_t i = 0; i < nitems(cmds); i++)
162		fprintf(stderr, "       %s %s\n", getprogname(), cmds[i].usage);
163}
164
165static void
166dump_family(struct genl_family *family)
167{
168	printf("Name: %s\n\tID: %#02hx, Version: %#02x, "
169	    "header size: %d, max attributes: %d\n",
170	    family->name, family->id, family->version,
171	    family->hdrsize, family->max_attr);
172	dump_operations(&family->ops);
173	dump_mcast_groups(&family->mcast_groups);
174}
175
176void
177parser_nlctrl_notify(struct snl_state *ss, struct nlmsghdr *hdr)
178{
179	struct genl_family family = {};
180
181	if (snl_parse_nlmsg(ss, hdr, &genl_family_parser,
182				&family))
183		dump_family(&family);
184}
185
186void
187parser_fallback(struct snl_state *ss __unused, struct nlmsghdr *hdr __unused)
188{
189	printf("New unknown message\n");
190}
191
192int
193monitor_mcast(int argc __unused, char **argv)
194{
195	struct snl_state ss;
196	struct nlmsghdr *hdr;
197	struct _getfamily_attrs attrs;
198	struct pollfd pfd;
199	bool found = false;
200	void (*parser)(struct snl_state *ss, struct nlmsghdr *hdr);
201
202	parser = parser_fallback;
203
204	if (!snl_init(&ss, NETLINK_GENERIC))
205		err(EXIT_FAILURE, "snl_init()");
206
207	if (argc != 2) {
208		usage();
209		return (EXIT_FAILURE);
210	}
211	if (!snl_get_genl_family_info(&ss, argv[0], &attrs))
212		errx(EXIT_FAILURE, "Unknown family '%s'", argv[0]);
213	for (uint32_t i = 0; i < attrs.mcast_groups.num_groups; i++) {
214		if (strcmp(attrs.mcast_groups.groups[i]->mcast_grp_name,
215		    argv[1]) == 0) {
216			found = true;
217			if (setsockopt(ss.fd, SOL_NETLINK,
218			    NETLINK_ADD_MEMBERSHIP,
219			    (void *)&attrs.mcast_groups.groups[i]->mcast_grp_id,
220			    sizeof(attrs.mcast_groups.groups[i]->mcast_grp_id))
221			    == -1)
222				err(EXIT_FAILURE, "Cannot subscribe to command "
223				    "notify");
224			break;
225		}
226	}
227	if (!found)
228		errx(EXIT_FAILURE, "No such multicat group '%s'"
229		    " in family '%s'", argv[1], argv[0]);
230	for (size_t i= 0; i < nitems(mcast_parsers); i++) {
231		if (strcmp(mcast_parsers[i].family, argv[0]) == 0) {
232			parser = mcast_parsers[i].parser;
233			break;
234		}
235	}
236	memset(&pfd, 0, sizeof(pfd));
237	pfd.fd = ss.fd;
238	pfd.events = POLLIN | POLLERR;
239	while (true) {
240		pfd.revents = 0;
241		if (poll(&pfd, 1, -1) == -1) {
242			if (errno == EINTR)
243				continue;
244			err(EXIT_FAILURE, "poll()");
245		}
246		hdr = snl_read_message(&ss);
247		if (hdr != NULL && hdr->nlmsg_type != NLMSG_ERROR)
248			parser(&ss, hdr);
249
250	}
251
252	return (EXIT_SUCCESS);
253}
254
255int
256list_families(int argc, char **argv __unused)
257{
258	struct snl_state ss;
259	struct snl_writer nw;
260	struct nlmsghdr *hdr;
261	struct snl_errmsg_data e = {};
262	uint32_t seq_id;
263
264	if (argc != 0) {
265		usage();
266		return (EXIT_FAILURE);
267	}
268	if (!snl_init(&ss, NETLINK_GENERIC))
269		err(EXIT_FAILURE, "snl_init()");
270
271	snl_init_writer(&ss, &nw);
272	hdr = snl_create_genl_msg_request(&nw, GENL_ID_CTRL,
273	    CTRL_CMD_GETFAMILY);
274	if ((hdr = snl_finalize_msg(&nw)) == NULL)
275		err(EXIT_FAILURE, "snl_finalize_msg");
276	seq_id = hdr->nlmsg_seq;
277	if (!snl_send_message(&ss, hdr))
278		err(EXIT_FAILURE, "snl_send_message");
279
280	while ((hdr = snl_read_reply_multi(&ss, seq_id, &e)) != NULL) {
281		if (e.error != 0) {
282			err(EXIT_FAILURE, "Error reading generic netlink");
283		}
284		struct genl_family family = {};
285		if (snl_parse_nlmsg(&ss, hdr, &genl_family_parser, &family))
286			dump_family(&family);
287	}
288
289	return (EXIT_SUCCESS);
290}
291
292int
293main(int argc, char **argv)
294{
295	if (modfind("netlink") == -1)
296		err(EXIT_FAILURE, "require netlink module to be loaded");
297
298	if (argc == 1)
299		return (list_families(0, NULL));
300
301	for (size_t i = 0; i < nitems(cmds); i++) {
302		if (strcmp(argv[1], cmds[i].name) == 0)
303			return (cmds[i].cmd(argc - 2, argv + 2));
304	}
305	usage();
306
307	return (EXIT_FAILURE);
308}
309