package/libnl-tiny/src/unl.c

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