ucon.c 4.4 KB

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