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