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