samples/connector/ucon.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  *  ucon.c
0004  *
0005  * Copyright (c) 2004+ Evgeniy Polyakov <zbr@ioremap.net>
0006  */
0007
0008 #include <asm/types.h>
0009
0010 #include <sys/types.h>
0011 #include <sys/socket.h>
0012 #include <sys/poll.h>
0013
0014 #include <linux/netlink.h>
0015 #include <linux/rtnetlink.h>
0016
0017 #include <arpa/inet.h>
0018
0019 #include <stdbool.h>
0020 #include <stdio.h>
0021 #include <stdlib.h>
0022 #include <unistd.h>
0023 #include <string.h>
0024 #include <errno.h>
0025 #include <time.h>
0026 #include <getopt.h>
0027
0028 #include <linux/connector.h>
0029
0030 #define DEBUG
0031 #define NETLINK_CONNECTOR   11
0032
0033 /* Hopefully your userspace connector.h matches this kernel */
0034 #define CN_TEST_IDX     CN_NETLINK_USERS + 3
0035 #define CN_TEST_VAL     0x456
0036
0037 #ifdef DEBUG
0038 #define ulog(f, a...) fprintf(stdout, f, ##a)
0039 #else
0040 #define ulog(f, a...) do {} while (0)
0041 #endif
0042
0043 static int need_exit;
0044 static __u32 seq;
0045
0046 static int netlink_send(int s, struct cn_msg *msg)
0047 {
0048     struct nlmsghdr *nlh;
0049     unsigned int size;
0050     int err;
0051     char buf[128];
0052     struct cn_msg *m;
0053
0054     size = NLMSG_SPACE(sizeof(struct cn_msg) + msg->len);
0055
0056     nlh = (struct nlmsghdr *)buf;
0057     nlh->nlmsg_seq = seq++;
0058     nlh->nlmsg_pid = getpid();
0059     nlh->nlmsg_type = NLMSG_DONE;
0060     nlh->nlmsg_len = size;
0061     nlh->nlmsg_flags = 0;
0062
0063     m = NLMSG_DATA(nlh);
0064 #if 0
0065     ulog("%s: [%08x.%08x] len=%u, seq=%u, ack=%u.\n",
0066            __func__, msg->id.idx, msg->id.val, msg->len, msg->seq, msg->ack);
0067 #endif
0068     memcpy(m, msg, sizeof(*m) + msg->len);
0069
0070     err = send(s, nlh, size, 0);
0071     if (err == -1)
0072         ulog("Failed to send: %s [%d].\n",
0073             strerror(errno), errno);
0074
0075     return err;
0076 }
0077
0078 static void usage(void)
0079 {
0080     printf(
0081         "Usage: ucon [options] [output file]\n"
0082         "\n"
0083         "\t-h\tthis help screen\n"
0084         "\t-s\tsend buffers to the test module\n"
0085         "\n"
0086         "The default behavior of ucon is to subscribe to the test module\n"
0087         "and wait for state messages.  Any ones received are dumped to the\n"
0088         "specified output file (or stdout).  The test module is assumed to\n"
0089         "have an id of {%u.%u}\n"
0090         "\n"
0091         "If you get no output, then verify the cn_test module id matches\n"
0092         "the expected id above.\n"
0093         , CN_TEST_IDX, CN_TEST_VAL
0094     );
0095 }
0096
0097 int main(int argc, char *argv[])
0098 {
0099     int s;
0100     char buf[1024];
0101     int len;
0102     struct nlmsghdr *reply;
0103     struct sockaddr_nl l_local;
0104     struct cn_msg *data;
0105     FILE *out;
0106     time_t tm;
0107     struct pollfd pfd;
0108     bool send_msgs = false;
0109
0110     while ((s = getopt(argc, argv, "hs")) != -1) {
0111         switch (s) {
0112         case 's':
0113             send_msgs = true;
0114             break;
0115
0116         case 'h':
0117             usage();
0118             return 0;
0119
0120         default:
0121             /* getopt() outputs an error for us */
0122             usage();
0123             return 1;
0124         }
0125     }
0126
0127     if (argc != optind) {
0128         out = fopen(argv[optind], "a+");
0129         if (!out) {
0130             ulog("Unable to open %s for writing: %s\n",
0131                 argv[1], strerror(errno));
0132             out = stdout;
0133         }
0134     } else
0135         out = stdout;
0136
0137     memset(buf, 0, sizeof(buf));
0138
0139     s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_CONNECTOR);
0140     if (s == -1) {
0141         perror("socket");
0142         return -1;
0143     }
0144
0145     l_local.nl_family = AF_NETLINK;
0146     l_local.nl_groups = -1; /* bitmask of requested groups */
0147     l_local.nl_pid = 0;
0148
0149     ulog("subscribing to %u.%u\n", CN_TEST_IDX, CN_TEST_VAL);
0150
0151     if (bind(s, (struct sockaddr *)&l_local, sizeof(struct sockaddr_nl)) == -1) {
0152         perror("bind");
0153         close(s);
0154         return -1;
0155     }
0156
0157 #if 0
0158     {
0159         int on = 0x57; /* Additional group number */
0160         setsockopt(s, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &on, sizeof(on));
0161     }
0162 #endif
0163     if (send_msgs) {
0164         int i, j;
0165
0166         memset(buf, 0, sizeof(buf));
0167
0168         data = (struct cn_msg *)buf;
0169
0170         data->id.idx = CN_TEST_IDX;
0171         data->id.val = CN_TEST_VAL;
0172         data->seq = seq++;
0173         data->ack = 0;
0174         data->len = 0;
0175
0176         for (j=0; j<10; ++j) {
0177             for (i=0; i<1000; ++i) {
0178                 len = netlink_send(s, data);
0179             }
0180
0181             ulog("%d messages have been sent to %08x.%08x.\n", i, data->id.idx, data->id.val);
0182         }
0183
0184         return 0;
0185     }
0186
0187
0188     pfd.fd = s;
0189
0190     while (!need_exit) {
0191         pfd.events = POLLIN;
0192         pfd.revents = 0;
0193         switch (poll(&pfd, 1, -1)) {
0194             case 0:
0195                 need_exit = 1;
0196                 break;
0197             case -1:
0198                 if (errno != EINTR) {
0199                     need_exit = 1;
0200                     break;
0201                 }
0202                 continue;
0203         }
0204         if (need_exit)
0205             break;
0206
0207         memset(buf, 0, sizeof(buf));
0208         len = recv(s, buf, sizeof(buf), 0);
0209         if (len == -1) {
0210             perror("recv buf");
0211             close(s);
0212             return -1;
0213         }
0214         reply = (struct nlmsghdr *)buf;
0215
0216         switch (reply->nlmsg_type) {
0217         case NLMSG_ERROR:
0218             fprintf(out, "Error message received.\n");
0219             fflush(out);
0220             break;
0221         case NLMSG_DONE:
0222             data = (struct cn_msg *)NLMSG_DATA(reply);
0223
0224             time(&tm);
0225             fprintf(out, "%.24s : [%x.%x] [%08u.%08u].\n",
0226                 ctime(&tm), data->id.idx, data->id.val, data->seq, data->ack);
0227             fflush(out);
0228             break;
0229         default:
0230             break;
0231         }
0232     }
0233
0234     close(s);
0235     return 0;
0236 }