unl.c

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