znl.c 3.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160
  1. /* Netlink helpers for zbuf
  2. * Copyright (c) 2014-2015 Timo Teräs
  3. *
  4. * This file is free software: you may copy, redistribute and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation, either version 2 of the License, or
  7. * (at your option) any later version.
  8. */
  9. #include <fcntl.h>
  10. #include <errno.h>
  11. #include <string.h>
  12. #include <unistd.h>
  13. #include <sys/types.h>
  14. #include <sys/socket.h>
  15. #include <linux/netlink.h>
  16. #include <linux/rtnetlink.h>
  17. #include "znl.h"
  18. #define ZNL_ALIGN(len) (((len)+3) & ~3)
  19. void *znl_push(struct zbuf *zb, size_t n)
  20. {
  21. return zbuf_pushn(zb, ZNL_ALIGN(n));
  22. }
  23. void *znl_pull(struct zbuf *zb, size_t n)
  24. {
  25. return zbuf_pulln(zb, ZNL_ALIGN(n));
  26. }
  27. struct nlmsghdr *znl_nlmsg_push(struct zbuf *zb, uint16_t type, uint16_t flags)
  28. {
  29. struct nlmsghdr *n;
  30. n = znl_push(zb, sizeof(*n));
  31. if (!n) return NULL;
  32. *n = (struct nlmsghdr) {
  33. .nlmsg_type = type,
  34. .nlmsg_flags = flags,
  35. };
  36. return n;
  37. }
  38. void znl_nlmsg_complete(struct zbuf *zb, struct nlmsghdr *n)
  39. {
  40. n->nlmsg_len = zb->tail - (uint8_t*)n;
  41. }
  42. struct nlmsghdr *znl_nlmsg_pull(struct zbuf *zb, struct zbuf *payload)
  43. {
  44. struct nlmsghdr *n;
  45. size_t plen;
  46. n = znl_pull(zb, sizeof(*n));
  47. if (!n) return NULL;
  48. plen = n->nlmsg_len - sizeof(*n);
  49. zbuf_init(payload, znl_pull(zb, plen), plen, plen);
  50. zbuf_may_pulln(zb, ZNL_ALIGN(plen) - plen);
  51. return n;
  52. }
  53. struct rtattr *znl_rta_push(struct zbuf *zb, uint16_t type, const void *val, size_t len)
  54. {
  55. struct rtattr *rta;
  56. uint8_t *dst;
  57. rta = znl_push(zb, ZNL_ALIGN(sizeof(*rta)) + ZNL_ALIGN(len));
  58. if (!rta) return NULL;
  59. *rta = (struct rtattr) {
  60. .rta_type = type,
  61. .rta_len = ZNL_ALIGN(sizeof(*rta)) + len,
  62. };
  63. dst = (uint8_t *)(rta+1);
  64. memcpy(dst, val, len);
  65. memset(dst+len, 0, ZNL_ALIGN(len) - len);
  66. return rta;
  67. }
  68. struct rtattr *znl_rta_push_u32(struct zbuf *zb, uint16_t type, uint32_t val)
  69. {
  70. return znl_rta_push(zb, type, &val, sizeof(val));
  71. }
  72. struct rtattr *znl_rta_nested_push(struct zbuf *zb, uint16_t type)
  73. {
  74. struct rtattr *rta;
  75. rta = znl_push(zb, sizeof(*rta));
  76. if (!rta) return NULL;
  77. *rta = (struct rtattr) {
  78. .rta_type = type,
  79. };
  80. return rta;
  81. }
  82. void znl_rta_nested_complete(struct zbuf *zb, struct rtattr *rta)
  83. {
  84. size_t len = zb->tail - (uint8_t*) rta;
  85. size_t align = ZNL_ALIGN(len) - len;
  86. if (align) {
  87. void *dst = zbuf_pushn(zb, align);
  88. if (dst) memset(dst, 0, align);
  89. }
  90. rta->rta_len = len;
  91. }
  92. struct rtattr *znl_rta_pull(struct zbuf *zb, struct zbuf *payload)
  93. {
  94. struct rtattr *rta;
  95. size_t plen;
  96. rta = znl_pull(zb, sizeof(*rta));
  97. if (!rta) return NULL;
  98. if (rta->rta_len > sizeof(*rta)) {
  99. plen = rta->rta_len - sizeof(*rta);
  100. zbuf_init(payload, znl_pull(zb, plen), plen, plen);
  101. } else {
  102. zbuf_init(payload, NULL, 0, 0);
  103. }
  104. return rta;
  105. }
  106. int znl_open(int protocol, int groups)
  107. {
  108. struct sockaddr_nl addr;
  109. int fd, buf = 128 * 1024;
  110. fd = socket(AF_NETLINK, SOCK_RAW, protocol);
  111. if (fd < 0)
  112. return -1;
  113. fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK);
  114. fcntl(fd, F_SETFD, FD_CLOEXEC);
  115. if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &buf, sizeof(buf)) < 0)
  116. goto error;
  117. memset(&addr, 0, sizeof(addr));
  118. addr.nl_family = AF_NETLINK;
  119. addr.nl_groups = groups;
  120. if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0)
  121. goto error;
  122. return fd;
  123. error:
  124. close(fd);
  125. return -1;
  126. }