237 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * ucon.c4 *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 DEBUG31#define NETLINK_CONNECTOR 1132 33/* Hopefully your userspace connector.h matches this kernel */34#define CN_TEST_IDX CN_NETLINK_USERS + 335#define CN_TEST_VAL 0x45636 37#ifdef DEBUG38#define ulog(f, a...) fprintf(stdout, f, ##a)39#else40#define ulog(f, a...) do {} while (0)41#endif42 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 065 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#endif68 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_VAL94 );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 } else135 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 0158 {159 int on = 0x57; /* Additional group number */160 setsockopt(s, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &on, sizeof(on));161 }162#endif163 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