1 /* 2 * Driver interaction with Linux nl80211/cfg80211 3 * Copyright (c) 2002-2015, Jouni Malinen <j (at) w1.fi> 4 * Copyright (c) 2003-2004, Instant802 Networks, Inc. 5 * Copyright (c) 2005-2006, Devicescape Software, Inc. 6 * Copyright (c) 2007, Johannes Berg <johannes (at) sipsolutions.net> 7 * Copyright (c) 2009-2010, Atheros Communications 8 * Copyright (c) 2017, The Linux Foundation. All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions are 12 * met: 13 * * Redistributions of source code must retain the above copyright 14 * * Redistributions in binary form must reproduce the above 15 * copyright notice, this list of conditions and the following 16 * disclaimer in the documentation and/or other materials provided 17 * with the distribution. 18 * * Neither the name of The Linux Foundation nor the names of its 19 * contributors may be used to endorse or promote products derived 20 * from this software without specific prior written permission. 21 * 22 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 23 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 24 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 25 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 26 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 27 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 28 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 29 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 30 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 31 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 32 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE 33 * 34 */ 35 36 #include <errno.h> 37 #include <netlink/genl/family.h> 38 #include <netlink/genl/ctrl.h> 39 #include <linux/pkt_sched.h> 40 #include <unistd.h> 41 #include <log/log.h> 42 #include "cld80211_lib.h" 43 44 #undef LOG_TAG 45 #define LOG_TAG "CLD80211" 46 #define SOCK_BUF_SIZE (256*1024) 47 48 struct family_data { 49 const char *group; 50 int id; 51 }; 52 53 54 static struct nl_sock * create_nl_socket(int protocol) 55 { 56 struct nl_sock *sock; 57 58 sock = nl_socket_alloc(); 59 if (sock == NULL) { 60 ALOGE("%s: Failed to create NL socket, err: %d", 61 getprogname(), errno); 62 return NULL; 63 } 64 65 if (nl_connect(sock, protocol)) { 66 ALOGE("%s: Could not connect sock, err: %d", 67 getprogname(), errno); 68 nl_socket_free(sock); 69 return NULL; 70 } 71 72 return sock; 73 } 74 75 76 static int init_exit_sockets(struct cld80211_ctx *ctx) 77 { 78 ctx->exit_sockets[0] = -1; 79 ctx->exit_sockets[1] = -1; 80 if (socketpair(AF_UNIX, SOCK_STREAM, 0, &ctx->exit_sockets[0]) == -1) { 81 ALOGE("%s: Failed to create exit socket pair", getprogname()); 82 return -1; 83 } 84 ALOGI("%s: initialized exit socket pair", getprogname()); 85 86 return 0; 87 } 88 89 90 static void cleanup_exit_sockets(struct cld80211_ctx *ctx) 91 { 92 if (ctx->exit_sockets[0] >= 0) { 93 close(ctx->exit_sockets[0]); 94 ctx->exit_sockets[0] = -1; 95 } 96 97 if (ctx->exit_sockets[1] >= 0) { 98 close(ctx->exit_sockets[1]); 99 ctx->exit_sockets[1] = -1; 100 } 101 } 102 103 104 void exit_cld80211_recv(struct cld80211_ctx *ctx) 105 { 106 if (!ctx) { 107 ALOGE("%s: ctx is NULL: %s", getprogname(), __func__); 108 return; 109 } 110 TEMP_FAILURE_RETRY(write(ctx->exit_sockets[0], "E", 1)); 111 ALOGI("%s: Sent msg on exit sock to unblock poll()", getprogname()); 112 } 113 114 115 /* Event handlers */ 116 static int response_handler(struct nl_msg *msg, void *arg) 117 { 118 UNUSED(msg); 119 UNUSED(arg); 120 ALOGI("%s: Received nlmsg response: no callback registered;drop it", 121 getprogname()); 122 123 return NL_SKIP; 124 } 125 126 127 static int ack_handler(struct nl_msg *msg, void *arg) 128 { 129 int *err = (int *)arg; 130 *err = 0; 131 UNUSED(msg); 132 return NL_STOP; 133 } 134 135 136 static int finish_handler(struct nl_msg *msg, void *arg) 137 { 138 int *ret = (int *)arg; 139 *ret = 0; 140 UNUSED(msg); 141 return NL_SKIP; 142 } 143 144 145 static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, 146 void *arg) 147 { 148 int *ret = (int *)arg; 149 *ret = err->error; 150 151 UNUSED(nla); 152 ALOGE("%s: error_handler received : %d", getprogname(), err->error); 153 return NL_SKIP; 154 } 155 156 157 static int no_seq_check(struct nl_msg *msg, void *arg) 158 { 159 UNUSED(msg); 160 UNUSED(arg); 161 return NL_OK; 162 } 163 164 165 int cld80211_recv_msg(struct nl_sock *sock, struct nl_cb *cb) 166 { 167 if (!sock || !cb) { 168 ALOGE("%s: %s is NULL", getprogname(), sock?"cb":"sock"); 169 return -EINVAL; 170 } 171 172 int res = nl_recvmsgs(sock, cb); 173 if(res) 174 ALOGE("%s: Error :%d while reading nl msg , err: %d", 175 getprogname(), res, errno); 176 return res; 177 } 178 179 180 static void cld80211_handle_event(int events, struct nl_sock *sock, 181 struct nl_cb *cb) 182 { 183 if (events & POLLERR) { 184 ALOGE("%s: Error reading from socket", getprogname()); 185 cld80211_recv_msg(sock, cb); 186 } else if (events & POLLHUP) { 187 ALOGE("%s: Remote side hung up", getprogname()); 188 } else if (events & POLLIN) { 189 cld80211_recv_msg(sock, cb); 190 } else { 191 ALOGE("%s: Unknown event - %0x", getprogname(), events); 192 } 193 } 194 195 196 static int family_handler(struct nl_msg *msg, void *arg) 197 { 198 struct family_data *res = arg; 199 struct nlattr *tb[CTRL_ATTR_MAX + 1]; 200 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); 201 struct nlattr *mcgrp; 202 int i; 203 204 nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0), 205 genlmsg_attrlen(gnlh, 0), NULL); 206 if (!tb[CTRL_ATTR_MCAST_GROUPS]) 207 return NL_SKIP; 208 209 nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], i) { 210 struct nlattr *tb2[CTRL_ATTR_MCAST_GRP_MAX + 1]; 211 nla_parse(tb2, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp), 212 nla_len(mcgrp), NULL); 213 214 if (!tb2[CTRL_ATTR_MCAST_GRP_NAME] || 215 !tb2[CTRL_ATTR_MCAST_GRP_ID] || 216 strncmp(nla_data(tb2[CTRL_ATTR_MCAST_GRP_NAME]), 217 res->group, 218 nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0) 219 continue; 220 res->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]); 221 break; 222 }; 223 224 return NL_SKIP; 225 } 226 227 228 static int get_multicast_id(struct cld80211_ctx *ctx, const char *group) 229 { 230 struct family_data res = { group, -ENOENT }; 231 struct nl_msg *nlmsg = nlmsg_alloc(); 232 233 if (!nlmsg) { 234 return -1; 235 } 236 237 genlmsg_put(nlmsg, 0, 0, ctx->nlctrl_familyid, 0, 0, 238 CTRL_CMD_GETFAMILY, 0); 239 nla_put_string(nlmsg, CTRL_ATTR_FAMILY_NAME, "cld80211"); 240 241 cld80211_send_recv_msg(ctx, nlmsg, family_handler, &res); 242 ALOGI("%s: nlctrl family id: %d group: %s mcast_id: %d", getprogname(), 243 ctx->nlctrl_familyid, group, res.id); 244 nlmsg_free(nlmsg); 245 return res.id; 246 } 247 248 249 int cld80211_add_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup) 250 { 251 if (!ctx || !mcgroup) { 252 ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__); 253 return 0; 254 } 255 int id = get_multicast_id(ctx, mcgroup); 256 if (id < 0) { 257 ALOGE("%s: Could not find group %s, errno: %d id: %d", 258 getprogname(), mcgroup, errno, id); 259 return id; 260 } 261 262 int ret = nl_socket_add_membership(ctx->sock, id); 263 if (ret < 0) { 264 ALOGE("%s: Could not add membership to group %s, errno: %d", 265 getprogname(), mcgroup, errno); 266 } 267 268 return ret; 269 } 270 271 272 int cld80211_remove_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup) 273 { 274 if (!ctx || !mcgroup) { 275 ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__); 276 return 0; 277 } 278 int id = get_multicast_id(ctx, mcgroup); 279 if (id < 0) { 280 ALOGE("%s: Could not find group %s, errno: %d id: %d", 281 getprogname(), mcgroup, errno, id); 282 return id; 283 } 284 285 int ret = nl_socket_drop_membership(ctx->sock, id); 286 if (ret < 0) { 287 ALOGE("%s: Could not drop membership from group %s, errno: %d," 288 " ret: %d", getprogname(), mcgroup, errno, ret); 289 return ret; 290 } 291 292 return 0; 293 } 294 295 296 struct nl_msg *cld80211_msg_alloc(struct cld80211_ctx *ctx, int cmd, 297 struct nlattr **nla_data, int pid) 298 { 299 struct nl_msg *nlmsg; 300 301 if (!ctx || !nla_data) { 302 ALOGE("%s: ctx is null: %s", getprogname(), __func__); 303 return NULL; 304 } 305 306 nlmsg = nlmsg_alloc(); 307 if (nlmsg == NULL) { 308 ALOGE("%s: Out of memory", getprogname()); 309 return NULL; 310 } 311 312 genlmsg_put(nlmsg, pid, /* seq = */ 0, ctx->netlink_familyid, 313 0, 0, cmd, /* version = */ 0); 314 315 *nla_data = nla_nest_start(nlmsg, CLD80211_ATTR_VENDOR_DATA); 316 if (!nla_data) 317 goto cleanup; 318 319 return nlmsg; 320 321 cleanup: 322 if (nlmsg) 323 nlmsg_free(nlmsg); 324 return NULL; 325 } 326 327 328 int cld80211_send_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg) 329 { 330 int err; 331 332 if (!ctx || !ctx->sock || !nlmsg) { 333 ALOGE("%s: Invalid data from client", getprogname()); 334 return -EINVAL; 335 } 336 337 err = nl_send_auto_complete(ctx->sock, nlmsg); /* send message */ 338 if (err < 0) { 339 ALOGE("%s: failed to send msg: %d", getprogname(), err); 340 return err; 341 } 342 343 return 0; 344 } 345 346 347 int cld80211_send_recv_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg, 348 int (*valid_handler)(struct nl_msg *, void *), 349 void *valid_data) 350 { 351 int err; 352 353 if (!ctx || !ctx->sock || !nlmsg) { 354 ALOGE("%s: Invalid data from client", getprogname()); 355 return -EINVAL; 356 } 357 358 struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT); 359 if (!cb) 360 return -ENOMEM; 361 362 err = nl_send_auto_complete(ctx->sock, nlmsg); /* send message */ 363 if (err < 0) 364 goto out; 365 366 err = 1; 367 368 nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); 369 nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); 370 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); 371 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); 372 373 if (valid_handler) 374 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, 375 valid_handler, valid_data); 376 else 377 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, 378 response_handler, valid_data); 379 380 while (err > 0) { /* wait for reply */ 381 int res = nl_recvmsgs(ctx->sock, cb); 382 if (res) { 383 ALOGE("%s: cld80211: nl_recvmsgs failed: %d", 384 getprogname(), res); 385 } 386 } 387 out: 388 nl_cb_put(cb); 389 return err; 390 } 391 392 393 int cld80211_recv(struct cld80211_ctx *ctx, int timeout, bool recv_multi_msg, 394 int (*valid_handler)(struct nl_msg *, void *), 395 void *cbctx) 396 { 397 struct pollfd pfd[2]; 398 struct nl_cb *cb; 399 int err; 400 401 if (!ctx || !ctx->sock || !valid_handler) { 402 ALOGE("%s: Invalid data from client", getprogname()); 403 return -EINVAL; 404 } 405 406 cb = nl_cb_alloc(NL_CB_DEFAULT); 407 if (!cb) 408 return -ENOMEM; 409 410 memset(&pfd[0], 0, 2*sizeof(struct pollfd)); 411 412 err = 1; 413 414 nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); 415 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); 416 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); 417 nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); 418 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler, cbctx); 419 420 pfd[0].fd = nl_socket_get_fd(ctx->sock); 421 pfd[0].events = POLLIN; 422 423 pfd[1].fd = ctx->exit_sockets[1]; 424 pfd[1].events = POLLIN; 425 426 do { 427 pfd[0].revents = 0; 428 pfd[1].revents = 0; 429 int result = poll(pfd, 2, timeout); 430 if (result < 0) { 431 ALOGE("%s: Error polling socket", getprogname()); 432 } else if (pfd[0].revents & (POLLIN | POLLHUP | POLLERR)) { 433 cld80211_handle_event(pfd[0].revents, ctx->sock, cb); 434 if (!recv_multi_msg) 435 break; 436 } else { 437 ALOGI("%s: Exiting poll", getprogname()); 438 break; 439 } 440 } while (1); 441 442 nl_cb_put(cb); 443 return 0; 444 } 445 446 447 struct cld80211_ctx * cld80211_init() 448 { 449 struct cld80211_ctx *ctx; 450 451 ctx = (struct cld80211_ctx *)malloc(sizeof(struct cld80211_ctx)); 452 if (ctx == NULL) { 453 ALOGE("%s: Failed to alloc cld80211_ctx", getprogname()); 454 return NULL; 455 } 456 memset(ctx, 0, sizeof(struct cld80211_ctx)); 457 458 ctx->sock = create_nl_socket(NETLINK_GENERIC); 459 if (ctx->sock == NULL) { 460 ALOGE("%s: Failed to create socket port", getprogname()); 461 goto cleanup; 462 } 463 464 /* Set the socket buffer size */ 465 if (nl_socket_set_buffer_size(ctx->sock, SOCK_BUF_SIZE , 0) < 0) { 466 ALOGE("%s: Could not set nl_socket RX buffer size for sock: %s", 467 getprogname(), strerror(errno)); 468 /* continue anyway with the default (smaller) buffer */ 469 } 470 471 ctx->netlink_familyid = genl_ctrl_resolve(ctx->sock, "cld80211"); 472 if (ctx->netlink_familyid < 0) { 473 ALOGE("%s: Could not resolve cld80211 familty id", 474 getprogname()); 475 goto cleanup; 476 } 477 478 ctx->nlctrl_familyid = genl_ctrl_resolve(ctx->sock, "nlctrl"); 479 if (ctx->nlctrl_familyid < 0) { 480 ALOGE("%s: net link family nlctrl is not present: %d err:%d", 481 getprogname(), ctx->nlctrl_familyid, errno); 482 goto cleanup; 483 } 484 485 486 if (init_exit_sockets(ctx) != 0) { 487 ALOGE("%s: Failed to initialize exit sockets", getprogname()); 488 goto cleanup; 489 } 490 491 return ctx; 492 cleanup: 493 if (ctx->sock) { 494 nl_socket_free(ctx->sock); 495 } 496 free (ctx); 497 return NULL; 498 } 499 500 501 void cld80211_deinit(struct cld80211_ctx *ctx) 502 { 503 if (!ctx || !ctx->sock) { 504 ALOGE("%s: ctx/sock is NULL", getprogname()); 505 return; 506 } 507 nl_socket_free(ctx->sock); 508 cleanup_exit_sockets(ctx); 509 free (ctx); 510 } 511
