2
0

unl.c 5.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322
  1. #define _GNU_SOURCE
  2. #include <netlink/netlink.h>
  3. #include <netlink/genl/genl.h>
  4. #include <netlink/genl/ctrl.h>
  5. #include <netlink/genl/family.h>
  6. #include <sys/types.h>
  7. #include <net/if.h>
  8. #include <unistd.h>
  9. #include <fcntl.h>
  10. #include <linux/nl80211.h>
  11. #include "unl.h"
  12. static int unl_init(struct unl *unl)
  13. {
  14. memset(unl, 0, sizeof(*unl));
  15. unl->sock = nl_socket_alloc();
  16. if (!unl->sock)
  17. return -1;
  18. return 0;
  19. }
  20. int unl_genl_init(struct unl *unl, const char *family)
  21. {
  22. if (unl_init(unl))
  23. goto error_out;
  24. unl->hdrlen = NLMSG_ALIGN(sizeof(struct genlmsghdr));
  25. unl->family_name = strdup(family);
  26. if (!unl->family_name)
  27. goto error;
  28. if (genl_connect(unl->sock))
  29. goto error;
  30. if (genl_ctrl_alloc_cache(unl->sock, &unl->cache))
  31. goto error;
  32. unl->family = genl_ctrl_search_by_name(unl->cache, family);
  33. if (!unl->family)
  34. goto error;
  35. return 0;
  36. error:
  37. unl_free(unl);
  38. error_out:
  39. return -1;
  40. }
  41. int unl_rtnl_init(struct unl *unl)
  42. {
  43. if (unl_init(unl))
  44. goto error_out;
  45. unl->hdrlen = 0;
  46. if (nl_connect(unl->sock, NETLINK_ROUTE))
  47. goto error;
  48. return 0;
  49. error:
  50. unl_free(unl);
  51. error_out:
  52. return -1;
  53. }
  54. void unl_free(struct unl *unl)
  55. {
  56. if (unl->family_name)
  57. free(unl->family_name);
  58. if (unl->sock)
  59. nl_socket_free(unl->sock);
  60. if (unl->cache)
  61. nl_cache_free(unl->cache);
  62. memset(unl, 0, sizeof(*unl));
  63. }
  64. static int
  65. ack_handler(struct nl_msg *msg, void *arg)
  66. {
  67. int *err = arg;
  68. *err = 0;
  69. return NL_STOP;
  70. }
  71. static int
  72. finish_handler(struct nl_msg *msg, void *arg)
  73. {
  74. int *err = arg;
  75. *err = 0;
  76. return NL_SKIP;
  77. }
  78. static int
  79. error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
  80. {
  81. int *ret = arg;
  82. *ret = err->error;
  83. return NL_SKIP;
  84. }
  85. struct nl_msg *unl_genl_msg(struct unl *unl, int cmd, bool dump)
  86. {
  87. struct nl_msg *msg;
  88. int flags = 0;
  89. msg = nlmsg_alloc();
  90. if (!msg)
  91. goto out;
  92. if (dump)
  93. flags |= NLM_F_DUMP;
  94. genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ,
  95. genl_family_get_id(unl->family), 0, flags, cmd, 0);
  96. out:
  97. return msg;
  98. }
  99. struct nl_msg *unl_rtnl_msg(struct unl *unl, int cmd, bool dump)
  100. {
  101. struct nl_msg *msg;
  102. int flags = 0;
  103. msg = nlmsg_alloc();
  104. if (!msg)
  105. goto out;
  106. if (dump)
  107. flags |= NLM_F_DUMP;
  108. nlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, cmd, 0, flags);
  109. out:
  110. return msg;
  111. }
  112. int unl_request(struct unl *unl, struct nl_msg *msg, unl_cb handler, void *arg)
  113. {
  114. struct nl_cb *cb;
  115. int err;
  116. cb = nl_cb_alloc(NL_CB_CUSTOM);
  117. err = nl_send_auto_complete(unl->sock, msg);
  118. if (err < 0)
  119. goto out;
  120. err = 1;
  121. nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
  122. nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
  123. nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
  124. if (handler)
  125. nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handler, arg);
  126. while (err > 0)
  127. nl_recvmsgs(unl->sock, cb);
  128. out:
  129. nlmsg_free(msg);
  130. nl_cb_put(cb);
  131. return err;
  132. }
  133. static int request_single_cb(struct nl_msg *msg, void *arg)
  134. {
  135. struct nl_msg **dest = arg;
  136. if (!*dest) {
  137. nlmsg_get(msg);
  138. *dest = msg;
  139. }
  140. return NL_SKIP;
  141. }
  142. int unl_request_single(struct unl *unl, struct nl_msg *msg, struct nl_msg **dest)
  143. {
  144. *dest = NULL;
  145. return unl_request(unl, msg, request_single_cb, dest);
  146. }
  147. static int no_seq_check(struct nl_msg *msg, void *arg)
  148. {
  149. return NL_OK;
  150. }
  151. void unl_loop(struct unl *unl, unl_cb handler, void *arg)
  152. {
  153. struct nl_cb *cb;
  154. cb = nl_cb_alloc(NL_CB_CUSTOM);
  155. unl->loop_done = false;
  156. nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
  157. nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handler, arg);
  158. while (!unl->loop_done)
  159. nl_recvmsgs(unl->sock, cb);
  160. nl_cb_put(cb);
  161. }
  162. int unl_genl_multicast_id(struct unl *unl, const char *name)
  163. {
  164. struct nlattr *tb[CTRL_ATTR_MCAST_GRP_MAX + 1];
  165. struct nlattr *groups, *group;
  166. struct nl_msg *msg;
  167. int ctrlid;
  168. int ret = -1;
  169. int rem;
  170. msg = nlmsg_alloc();
  171. if (!msg)
  172. return -1;
  173. ctrlid = genl_ctrl_resolve(unl->sock, "nlctrl");
  174. genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
  175. NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, unl->family_name);
  176. unl_request_single(unl, msg, &msg);
  177. if (!msg)
  178. return -1;
  179. groups = unl_find_attr(unl, msg, CTRL_ATTR_MCAST_GROUPS);
  180. if (!groups)
  181. goto nla_put_failure;
  182. nla_for_each_nested(group, groups, rem) {
  183. const char *gn;
  184. nla_parse(tb, CTRL_ATTR_MCAST_GRP_MAX, nla_data(group),
  185. nla_len(group), NULL);
  186. if (!tb[CTRL_ATTR_MCAST_GRP_NAME] ||
  187. !tb[CTRL_ATTR_MCAST_GRP_ID])
  188. continue;
  189. gn = nla_data(tb[CTRL_ATTR_MCAST_GRP_NAME]);
  190. if (strcmp(gn, name) != 0)
  191. continue;
  192. ret = nla_get_u32(tb[CTRL_ATTR_MCAST_GRP_ID]);
  193. break;
  194. }
  195. nla_put_failure:
  196. nlmsg_free(msg);
  197. return ret;
  198. }
  199. int unl_genl_subscribe(struct unl *unl, const char *name)
  200. {
  201. int mcid;
  202. mcid = unl_genl_multicast_id(unl, name);
  203. if (mcid < 0)
  204. return mcid;
  205. return nl_socket_add_membership(unl->sock, mcid);
  206. }
  207. int unl_genl_unsubscribe(struct unl *unl, const char *name)
  208. {
  209. int mcid;
  210. mcid = unl_genl_multicast_id(unl, name);
  211. if (mcid < 0)
  212. return mcid;
  213. return nl_socket_drop_membership(unl->sock, mcid);
  214. }
  215. int unl_nl80211_phy_lookup(const char *name)
  216. {
  217. char buf[32];
  218. int fd, pos;
  219. snprintf(buf, sizeof(buf), "/sys/class/ieee80211/%s/index", name);
  220. fd = open(buf, O_RDONLY);
  221. if (fd < 0)
  222. return -1;
  223. pos = read(fd, buf, sizeof(buf) - 1);
  224. if (pos < 0) {
  225. close(fd);
  226. return -1;
  227. }
  228. buf[pos] = '\0';
  229. close(fd);
  230. return atoi(buf);
  231. }
  232. int unl_nl80211_wdev_to_phy(struct unl *unl, int wdev)
  233. {
  234. struct nl_msg *msg;
  235. struct nlattr *attr;
  236. int ret = -1;
  237. msg = unl_genl_msg(unl, NL80211_CMD_GET_INTERFACE, false);
  238. if (!msg)
  239. return -1;
  240. NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, wdev);
  241. if (unl_request_single(unl, msg, &msg) < 0)
  242. return -1;
  243. attr = unl_find_attr(unl, msg, NL80211_ATTR_WIPHY);
  244. if (!attr)
  245. goto out;
  246. ret = nla_get_u32(attr);
  247. out:
  248. nla_put_failure:
  249. nlmsg_free(msg);
  250. return ret;
  251. }