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, bool sync_driver) 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 if (sync_driver == true) { 242 cld80211_send_recv_msg(ctx, nlmsg, family_handler, &res); 243 ALOGI("%s: nlctrl family id: %d group: %s mcast_id: %d", getprogname(), 244 ctx->nlctrl_familyid, group, res.id); 245 } 246 nlmsg_free(nlmsg); 247 return res.id; 248 } 249 250 251 int cld80211_add_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup) 252 { 253 if (!ctx || !mcgroup) { 254 ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__); 255 return 0; 256 } 257 int id = get_multicast_id(ctx, mcgroup, true); 258 if (id < 0) { 259 ALOGE("%s: Could not find group %s, errno: %d id: %d", 260 getprogname(), mcgroup, errno, id); 261 return id; 262 } 263 264 int ret = nl_socket_add_membership(ctx->sock, id); 265 if (ret < 0) { 266 ALOGE("%s: Could not add membership to group %s, errno: %d", 267 getprogname(), mcgroup, errno); 268 } 269 270 return ret; 271 } 272 273 274 int cld80211_remove_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup) 275 { 276 if (!ctx || !mcgroup) { 277 ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__); 278 return 0; 279 } 280 int id = get_multicast_id(ctx, mcgroup, false); 281 if (id < 0) { 282 ALOGE("%s: Could not find group %s, errno: %d id: %d", 283 getprogname(), mcgroup, errno, id); 284 return id; 285 } 286 287 int ret = nl_socket_drop_membership(ctx->sock, id); 288 if (ret < 0) { 289 ALOGE("%s: Could not drop membership from group %s, errno: %d," 290 " ret: %d", getprogname(), mcgroup, errno, ret); 291 return ret; 292 } 293 294 return 0; 295 } 296 297 298 struct nl_msg *cld80211_msg_alloc(struct cld80211_ctx *ctx, int cmd, 299 struct nlattr **nla_data, int pid) 300 { 301 struct nl_msg *nlmsg; 302 303 if (!ctx || !nla_data) { 304 ALOGE("%s: ctx is null: %s", getprogname(), __func__); 305 return NULL; 306 } 307 308 nlmsg = nlmsg_alloc(); 309 if (nlmsg == NULL) { 310 ALOGE("%s: Out of memory", getprogname()); 311 return NULL; 312 } 313 314 genlmsg_put(nlmsg, pid, /* seq = */ 0, ctx->netlink_familyid, 315 0, 0, cmd, /* version = */ 0); 316 317 *nla_data = nla_nest_start(nlmsg, CLD80211_ATTR_VENDOR_DATA); 318 if (!nla_data) 319 goto cleanup; 320 321 return nlmsg; 322 323 cleanup: 324 if (nlmsg) 325 nlmsg_free(nlmsg); 326 return NULL; 327 } 328 329 330 int cld80211_send_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg) 331 { 332 int err; 333 334 if (!ctx || !ctx->sock || !nlmsg) { 335 ALOGE("%s: Invalid data from client", getprogname()); 336 return -EINVAL; 337 } 338 339 err = nl_send_auto_complete(ctx->sock, nlmsg); /* send message */ 340 if (err < 0) { 341 ALOGE("%s: failed to send msg: %d", getprogname(), err); 342 return err; 343 } 344 345 return 0; 346 } 347 348 349 int cld80211_send_recv_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg, 350 int (*valid_handler)(struct nl_msg *, void *), 351 void *valid_data) 352 { 353 int err; 354 355 if (!ctx || !ctx->sock || !nlmsg) { 356 ALOGE("%s: Invalid data from client", getprogname()); 357 return -EINVAL; 358 } 359 360 struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT); 361 if (!cb) 362 return -ENOMEM; 363 364 err = nl_send_auto_complete(ctx->sock, nlmsg); /* send message */ 365 if (err < 0) 366 goto out; 367 368 err = 1; 369 370 nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); 371 nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); 372 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); 373 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); 374 375 if (valid_handler) 376 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, 377 valid_handler, valid_data); 378 else 379 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, 380 response_handler, valid_data); 381 382 while (err > 0) { /* wait for reply */ 383 int res = nl_recvmsgs(ctx->sock, cb); 384 if (res) { 385 ALOGE("%s: cld80211: nl_recvmsgs failed: %d", 386 getprogname(), res); 387 } 388 } 389 out: 390 nl_cb_put(cb); 391 return err; 392 } 393 394 395 int cld80211_recv(struct cld80211_ctx *ctx, int timeout, bool recv_multi_msg, 396 int (*valid_handler)(struct nl_msg *, void *), 397 void *cbctx) 398 { 399 struct pollfd pfd[2]; 400 struct nl_cb *cb; 401 int err; 402 403 if (!ctx || !ctx->sock || !valid_handler) { 404 ALOGE("%s: Invalid data from client", getprogname()); 405 return -EINVAL; 406 } 407 408 cb = nl_cb_alloc(NL_CB_DEFAULT); 409 if (!cb) 410 return -ENOMEM; 411 412 memset(&pfd[0], 0, 2*sizeof(struct pollfd)); 413 414 err = 1; 415 416 nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); 417 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); 418 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); 419 nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); 420 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler, cbctx); 421 422 pfd[0].fd = nl_socket_get_fd(ctx->sock); 423 pfd[0].events = POLLIN; 424 425 pfd[1].fd = ctx->exit_sockets[1]; 426 pfd[1].events = POLLIN; 427 428 do { 429 pfd[0].revents = 0; 430 pfd[1].revents = 0; 431 int result = poll(pfd, 2, timeout); 432 if (result < 0) { 433 ALOGE("%s: Error polling socket", getprogname()); 434 } else if (pfd[0].revents & (POLLIN | POLLHUP | POLLERR)) { 435 cld80211_handle_event(pfd[0].revents, ctx->sock, cb); 436 if (!recv_multi_msg) 437 break; 438 } else { 439 ALOGI("%s: Exiting poll", getprogname()); 440 break; 441 } 442 } while (1); 443 444 nl_cb_put(cb); 445 return 0; 446 } 447 448 449 struct cld80211_ctx * cld80211_init() 450 { 451 struct cld80211_ctx *ctx; 452 453 ctx = (struct cld80211_ctx *)malloc(sizeof(struct cld80211_ctx)); 454 if (ctx == NULL) { 455 ALOGE("%s: Failed to alloc cld80211_ctx", getprogname()); 456 return NULL; 457 } 458 memset(ctx, 0, sizeof(struct cld80211_ctx)); 459 460 ctx->sock = create_nl_socket(NETLINK_GENERIC); 461 if (ctx->sock == NULL) { 462 ALOGE("%s: Failed to create socket port", getprogname()); 463 goto cleanup; 464 } 465 466 /* Set the socket buffer size */ 467 if (nl_socket_set_buffer_size(ctx->sock, SOCK_BUF_SIZE , 0) < 0) { 468 ALOGE("%s: Could not set nl_socket RX buffer size for sock: %s", 469 getprogname(), strerror(errno)); 470 /* continue anyway with the default (smaller) buffer */ 471 } 472 473 ctx->netlink_familyid = genl_ctrl_resolve(ctx->sock, "cld80211"); 474 if (ctx->netlink_familyid < 0) { 475 ALOGE("%s: Could not resolve cld80211 familty id", 476 getprogname()); 477 goto cleanup; 478 } 479 480 ctx->nlctrl_familyid = genl_ctrl_resolve(ctx->sock, "nlctrl"); 481 if (ctx->nlctrl_familyid < 0) { 482 ALOGE("%s: net link family nlctrl is not present: %d err:%d", 483 getprogname(), ctx->nlctrl_familyid, errno); 484 goto cleanup; 485 } 486 487 488 if (init_exit_sockets(ctx) != 0) { 489 ALOGE("%s: Failed to initialize exit sockets", getprogname()); 490 goto cleanup; 491 } 492 493 return ctx; 494 cleanup: 495 if (ctx->sock) { 496 nl_socket_free(ctx->sock); 497 } 498 free (ctx); 499 return NULL; 500 } 501 502 503 void cld80211_deinit(struct cld80211_ctx *ctx) 504 { 505 if (!ctx || !ctx->sock) { 506 ALOGE("%s: ctx/sock is NULL", getprogname()); 507 return; 508 } 509 nl_socket_free(ctx->sock); 510 cleanup_exit_sockets(ctx); 511 free (ctx); 512 } 513
