1// SPDX-License-Identifier: GPL-2.0-or-later
2/*
3 * 	ucon.c
4 *
5 * Copyright (c) 2004+ Evgeniy Polyakov <zbr@ioremap.net>
6 */
7
8#include <asm/types.h>
9
10#include <sys/types.h>
11#include <sys/socket.h>
12#include <sys/poll.h>
13
14#include <linux/netlink.h>
15#include <linux/rtnetlink.h>
16
17#include <arpa/inet.h>
18
19#include <stdbool.h>
20#include <stdio.h>
21#include <stdlib.h>
22#include <unistd.h>
23#include <string.h>
24#include <errno.h>
25#include <time.h>
26#include <getopt.h>
27
28#include <linux/connector.h>
29
30#define DEBUG
31#define NETLINK_CONNECTOR 	11
32
33/* Hopefully your userspace connector.h matches this kernel */
34#define CN_TEST_IDX		CN_NETLINK_USERS + 3
35#define CN_TEST_VAL		0x456
36
37#ifdef DEBUG
38#define ulog(f, a...) fprintf(stdout, f, ##a)
39#else
40#define ulog(f, a...) do {} while (0)
41#endif
42
43static int need_exit;
44static __u32 seq;
45
46static int netlink_send(int s, struct cn_msg *msg)
47{
48	struct nlmsghdr *nlh;
49	unsigned int size;
50	int err;
51	char buf[128];
52	struct cn_msg *m;
53
54	size = NLMSG_SPACE(sizeof(struct cn_msg) + msg->len);
55
56	nlh = (struct nlmsghdr *)buf;
57	nlh->nlmsg_seq = seq++;
58	nlh->nlmsg_pid = getpid();
59	nlh->nlmsg_type = NLMSG_DONE;
60	nlh->nlmsg_len = size;
61	nlh->nlmsg_flags = 0;
62
63	m = NLMSG_DATA(nlh);
64#if 0
65	ulog("%s: [%08x.%08x] len=%u, seq=%u, ack=%u.\n",
66	       __func__, msg->id.idx, msg->id.val, msg->len, msg->seq, msg->ack);
67#endif
68	memcpy(m, msg, sizeof(*m) + msg->len);
69
70	err = send(s, nlh, size, 0);
71	if (err == -1)
72		ulog("Failed to send: %s [%d].\n",
73			strerror(errno), errno);
74
75	return err;
76}
77
78static void usage(void)
79{
80	printf(
81		"Usage: ucon [options] [output file]\n"
82		"\n"
83		"\t-h\tthis help screen\n"
84		"\t-s\tsend buffers to the test module\n"
85		"\n"
86		"The default behavior of ucon is to subscribe to the test module\n"
87		"and wait for state messages.  Any ones received are dumped to the\n"
88		"specified output file (or stdout).  The test module is assumed to\n"
89		"have an id of {%u.%u}\n"
90		"\n"
91		"If you get no output, then verify the cn_test module id matches\n"
92		"the expected id above.\n"
93		, CN_TEST_IDX, CN_TEST_VAL
94	);
95}
96
97int main(int argc, char *argv[])
98{
99	int s;
100	char buf[1024];
101	int len;
102	struct nlmsghdr *reply;
103	struct sockaddr_nl l_local;
104	struct cn_msg *data;
105	FILE *out;
106	time_t tm;
107	struct pollfd pfd;
108	bool send_msgs = false;
109
110	while ((s = getopt(argc, argv, "hs")) != -1) {
111		switch (s) {
112		case 's':
113			send_msgs = true;
114			break;
115
116		case 'h':
117			usage();
118			return 0;
119
120		default:
121			/* getopt() outputs an error for us */
122			usage();
123			return 1;
124		}
125	}
126
127	if (argc != optind) {
128		out = fopen(argv[optind], "a+");
129		if (!out) {
130			ulog("Unable to open %s for writing: %s\n",
131				argv[1], strerror(errno));
132			out = stdout;
133		}
134	} else
135		out = stdout;
136
137	memset(buf, 0, sizeof(buf));
138
139	s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_CONNECTOR);
140	if (s == -1) {
141		perror("socket");
142		return -1;
143	}
144
145	l_local.nl_family = AF_NETLINK;
146	l_local.nl_groups = -1; /* bitmask of requested groups */
147	l_local.nl_pid = 0;
148
149	ulog("subscribing to %u.%u\n", CN_TEST_IDX, CN_TEST_VAL);
150
151	if (bind(s, (struct sockaddr *)&l_local, sizeof(struct sockaddr_nl)) == -1) {
152		perror("bind");
153		close(s);
154		return -1;
155	}
156
157#if 0
158	{
159		int on = 0x57; /* Additional group number */
160		setsockopt(s, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &on, sizeof(on));
161	}
162#endif
163	if (send_msgs) {
164		int i, j;
165
166		memset(buf, 0, sizeof(buf));
167
168		data = (struct cn_msg *)buf;
169
170		data->id.idx = CN_TEST_IDX;
171		data->id.val = CN_TEST_VAL;
172		data->seq = seq++;
173		data->ack = 0;
174		data->len = 0;
175
176		for (j=0; j<10; ++j) {
177			for (i=0; i<1000; ++i) {
178				len = netlink_send(s, data);
179			}
180
181			ulog("%d messages have been sent to %08x.%08x.\n", i, data->id.idx, data->id.val);
182		}
183
184		return 0;
185	}
186
187
188	pfd.fd = s;
189
190	while (!need_exit) {
191		pfd.events = POLLIN;
192		pfd.revents = 0;
193		switch (poll(&pfd, 1, -1)) {
194			case 0:
195				need_exit = 1;
196				break;
197			case -1:
198				if (errno != EINTR) {
199					need_exit = 1;
200					break;
201				}
202				continue;
203		}
204		if (need_exit)
205			break;
206
207		memset(buf, 0, sizeof(buf));
208		len = recv(s, buf, sizeof(buf), 0);
209		if (len == -1) {
210			perror("recv buf");
211			close(s);
212			return -1;
213		}
214		reply = (struct nlmsghdr *)buf;
215
216		switch (reply->nlmsg_type) {
217		case NLMSG_ERROR:
218			fprintf(out, "Error message received.\n");
219			fflush(out);
220			break;
221		case NLMSG_DONE:
222			data = (struct cn_msg *)NLMSG_DATA(reply);
223
224			time(&tm);
225			fprintf(out, "%.24s : [%x.%x] [%08u.%08u].\n",
226				ctime(&tm), data->id.idx, data->id.val, data->seq, data->ack);
227			fflush(out);
228			break;
229		default:
230			break;
231		}
232	}
233
234	close(s);
235	return 0;
236}
237