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 14static 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 23int 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 47error: 48 unl_free(unl); 49error_out: 50 return -1; 51} 52 53void 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 67static int 68ack_handler(struct nl_msg *msg, void *arg) 69{ 70 int *err = arg; 71 *err = 0; 72 return NL_STOP; 73} 74 75static int 76finish_handler(struct nl_msg *msg, void *arg) 77{ 78 int *err = arg; 79 *err = 0; 80 return NL_SKIP; 81} 82 83static int 84error_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 91struct 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 106out: 107 return msg; 108} 109 110int unl_genl_request(struct unl *unl, struct nl_msg *msg, unl_cb handler, void *arg) 111{ 112 struct nl_cb *cb; 113 int err; 114 115 cb = nl_cb_alloc(NL_CB_CUSTOM); 116 err = nl_send_auto_complete(unl->sock, msg); 117 if (err < 0) 118 goto out; 119 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 127 while (err > 0) 128 nl_recvmsgs(unl->sock, cb); 129 130out: 131 nlmsg_free(msg); 132 nl_cb_put(cb); 133 return err; 134} 135 136static int request_single_cb(struct nl_msg *msg, void *arg) 137{ 138 struct nl_msg **dest = arg; 139 140 if (!*dest) { 141 nlmsg_get(msg); 142 *dest = msg; 143 } 144 return NL_SKIP; 145} 146 147int unl_genl_request_single(struct unl *unl, struct nl_msg *msg, struct nl_msg **dest) 148{ 149 *dest = NULL; 150 return unl_genl_request(unl, msg, request_single_cb, dest); 151} 152 153static int no_seq_check(struct nl_msg *msg, void *arg) 154{ 155 return NL_OK; 156} 157 158void unl_genl_loop(struct unl *unl, unl_cb handler, void *arg) 159{ 160 struct nl_cb *cb; 161 162 cb = nl_cb_alloc(NL_CB_CUSTOM); 163 unl->loop_done = false; 164 nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); 165 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handler, arg); 166 167 while (!unl->loop_done) 168 nl_recvmsgs(unl->sock, cb); 169 170 nl_cb_put(cb); 171} 172 173int unl_genl_multicast_id(struct unl *unl, const char *name) 174{ 175 struct nlattr *tb[CTRL_ATTR_MCAST_GRP_MAX + 1]; 176 struct nlattr *groups, *group; 177 struct nl_msg *msg; 178 int ctrlid; 179 int ret = -1; 180 int rem; 181 182 msg = nlmsg_alloc(); 183 if (!msg) 184 return -1; 185 186 ctrlid = genl_ctrl_resolve(unl->sock, "nlctrl"); 187 genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0); 188 NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, unl->family_name); 189 unl_genl_request_single(unl, msg, &msg); 190 if (!msg) 191 return -1; 192 193 groups = unl_find_attr(unl, msg, CTRL_ATTR_MCAST_GROUPS); 194 if (!groups) 195 goto nla_put_failure; 196 197 nla_for_each_nested(group, groups, rem) { 198 const char *gn; 199 200 nla_parse(tb, CTRL_ATTR_MCAST_GRP_MAX, nla_data(group), 201 nla_len(group), NULL); 202 203 if (!tb[CTRL_ATTR_MCAST_GRP_NAME] || 204 !tb[CTRL_ATTR_MCAST_GRP_ID]) 205 continue; 206 207 gn = nla_data(tb[CTRL_ATTR_MCAST_GRP_NAME]); 208 if (strcmp(gn, name) != 0) 209 continue; 210 211 ret = nla_get_u32(tb[CTRL_ATTR_MCAST_GRP_ID]); 212 break; 213 } 214 215nla_put_failure: 216 nlmsg_free(msg); 217 return ret; 218} 219 220int unl_genl_subscribe(struct unl *unl, const char *name) 221{ 222 int mcid; 223 224 mcid = unl_genl_multicast_id(unl, name); 225 if (mcid < 0) 226 return mcid; 227 228 return nl_socket_add_membership(unl->sock, mcid); 229} 230 231int unl_genl_unsubscribe(struct unl *unl, const char *name) 232{ 233 int mcid; 234 235 mcid = unl_genl_multicast_id(unl, name); 236 if (mcid < 0) 237 return mcid; 238 239 return nl_socket_drop_membership(unl->sock, mcid); 240} 241 242int unl_nl80211_phy_lookup(const char *name) 243{ 244 char buf[32]; 245 int fd, pos; 246 247 snprintf(buf, sizeof(buf), "/sys/class/ieee80211/%s/index", name); 248 249 fd = open(buf, O_RDONLY); 250 if (fd < 0) 251 return -1; 252 pos = read(fd, buf, sizeof(buf) - 1); 253 if (pos < 0) { 254 close(fd); 255 return -1; 256 } 257 buf[pos] = '\0'; 258 close(fd); 259 return atoi(buf); 260} 261 262int unl_nl80211_wdev_to_phy(struct unl *unl, int wdev) 263{ 264 struct nl_msg *msg; 265 struct nlattr *attr; 266 int ret = -1; 267 268 msg = unl_genl_msg(unl, NL80211_CMD_GET_INTERFACE, false); 269 if (!msg) 270 return -1; 271 272 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, wdev); 273 if (unl_genl_request_single(unl, msg, &msg) < 0) 274 return -1; 275 276 attr = unl_find_attr(unl, msg, NL80211_ATTR_WIPHY); 277 if (!attr) 278 goto out; 279 280 ret = nla_get_u32(attr); 281out: 282nla_put_failure: 283 nlmsg_free(msg); 284 return ret; 285} 286 287 288