ucon.c 4.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206
  1. /*
  2. * ucon.c
  3. *
  4. * Copyright (c) 2004+ Evgeniy Polyakov <johnpol@2ka.mipt.ru>
  5. *
  6. *
  7. * This program is free software; you can redistribute it and/or modify
  8. * it under the terms of the GNU General Public License as published by
  9. * the Free Software Foundation; either version 2 of the License, or
  10. * (at your option) any later version.
  11. *
  12. * This program is distributed in the hope that it will be useful,
  13. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  15. * GNU General Public License for more details.
  16. *
  17. * You should have received a copy of the GNU General Public License
  18. * along with this program; if not, write to the Free Software
  19. * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  20. */
  21. #include <asm/types.h>
  22. #include <sys/types.h>
  23. #include <sys/socket.h>
  24. #include <sys/poll.h>
  25. #include <linux/netlink.h>
  26. #include <linux/rtnetlink.h>
  27. #include <arpa/inet.h>
  28. #include <stdio.h>
  29. #include <stdlib.h>
  30. #include <unistd.h>
  31. #include <string.h>
  32. #include <errno.h>
  33. #include <time.h>
  34. #include <linux/connector.h>
  35. #define DEBUG
  36. #define NETLINK_CONNECTOR 11
  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. static int need_exit;
  43. static __u32 seq;
  44. static int netlink_send(int s, struct cn_msg *msg)
  45. {
  46. struct nlmsghdr *nlh;
  47. unsigned int size;
  48. int err;
  49. char buf[128];
  50. struct cn_msg *m;
  51. size = NLMSG_SPACE(sizeof(struct cn_msg) + msg->len);
  52. nlh = (struct nlmsghdr *)buf;
  53. nlh->nlmsg_seq = seq++;
  54. nlh->nlmsg_pid = getpid();
  55. nlh->nlmsg_type = NLMSG_DONE;
  56. nlh->nlmsg_len = NLMSG_LENGTH(size - sizeof(*nlh));
  57. nlh->nlmsg_flags = 0;
  58. m = NLMSG_DATA(nlh);
  59. #if 0
  60. ulog("%s: [%08x.%08x] len=%u, seq=%u, ack=%u.\n",
  61. __func__, msg->id.idx, msg->id.val, msg->len, msg->seq, msg->ack);
  62. #endif
  63. memcpy(m, msg, sizeof(*m) + msg->len);
  64. err = send(s, nlh, size, 0);
  65. if (err == -1)
  66. ulog("Failed to send: %s [%d].\n",
  67. strerror(errno), errno);
  68. return err;
  69. }
  70. int main(int argc, char *argv[])
  71. {
  72. int s;
  73. char buf[1024];
  74. int len;
  75. struct nlmsghdr *reply;
  76. struct sockaddr_nl l_local;
  77. struct cn_msg *data;
  78. FILE *out;
  79. time_t tm;
  80. struct pollfd pfd;
  81. if (argc < 2)
  82. out = stdout;
  83. else {
  84. out = fopen(argv[1], "a+");
  85. if (!out) {
  86. ulog("Unable to open %s for writing: %s\n",
  87. argv[1], strerror(errno));
  88. out = stdout;
  89. }
  90. }
  91. memset(buf, 0, sizeof(buf));
  92. s = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_CONNECTOR);
  93. if (s == -1) {
  94. perror("socket");
  95. return -1;
  96. }
  97. l_local.nl_family = AF_NETLINK;
  98. l_local.nl_groups = 0x123; /* bitmask of requested groups */
  99. l_local.nl_pid = 0;
  100. if (bind(s, (struct sockaddr *)&l_local, sizeof(struct sockaddr_nl)) == -1) {
  101. perror("bind");
  102. close(s);
  103. return -1;
  104. }
  105. #if 0
  106. {
  107. int on = 0x57; /* Additional group number */
  108. setsockopt(s, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP, &on, sizeof(on));
  109. }
  110. #endif
  111. if (0) {
  112. int i, j;
  113. memset(buf, 0, sizeof(buf));
  114. data = (struct cn_msg *)buf;
  115. data->id.idx = 0x123;
  116. data->id.val = 0x456;
  117. data->seq = seq++;
  118. data->ack = 0;
  119. data->len = 0;
  120. for (j=0; j<10; ++j) {
  121. for (i=0; i<1000; ++i) {
  122. len = netlink_send(s, data);
  123. }
  124. ulog("%d messages have been sent to %08x.%08x.\n", i, data->id.idx, data->id.val);
  125. }
  126. return 0;
  127. }
  128. pfd.fd = s;
  129. while (!need_exit) {
  130. pfd.events = POLLIN;
  131. pfd.revents = 0;
  132. switch (poll(&pfd, 1, -1)) {
  133. case 0:
  134. need_exit = 1;
  135. break;
  136. case -1:
  137. if (errno != EINTR) {
  138. need_exit = 1;
  139. break;
  140. }
  141. continue;
  142. }
  143. if (need_exit)
  144. break;
  145. memset(buf, 0, sizeof(buf));
  146. len = recv(s, buf, sizeof(buf), 0);
  147. if (len == -1) {
  148. perror("recv buf");
  149. close(s);
  150. return -1;
  151. }
  152. reply = (struct nlmsghdr *)buf;
  153. switch (reply->nlmsg_type) {
  154. case NLMSG_ERROR:
  155. fprintf(out, "Error message received.\n");
  156. fflush(out);
  157. break;
  158. case NLMSG_DONE:
  159. data = (struct cn_msg *)NLMSG_DATA(reply);
  160. time(&tm);
  161. fprintf(out, "%.24s : [%x.%x] [%08u.%08u].\n",
  162. ctime(&tm), data->id.idx, data->id.val, data->seq, data->ack);
  163. fflush(out);
  164. break;
  165. default:
  166. break;
  167. }
  168. }
  169. close(s);
  170. return 0;
  171. }