net/bluetooth/mgmt.c

   1 /*
   2    BlueZ - Bluetooth protocol stack for Linux
   3    Copyright (C) 2010  Nokia Corporation
   4
   5    This program is free software; you can redistribute it and/or modify
   6    it under the terms of the GNU General Public License version 2 as
   7    published by the Free Software Foundation;
   8
   9    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  10    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  11    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
  12    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
  13    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
  14    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  15    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  16    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  17
  18    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
  19    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
  20    SOFTWARE IS DISCLAIMED.
  21 */
  22
  23 /* Bluetooth HCI Management interface */
  24
  25 #include <asm/uaccess.h>
  26 #include <asm/unaligned.h>
  27
  28 #include <net/bluetooth/bluetooth.h>
  29 #include <net/bluetooth/hci_core.h>
  30 #include <net/bluetooth/mgmt.h>
  31
  32 #define MGMT_VERSION    0
  33 #define MGMT_REVISION   1
  34
  35 struct pending_cmd {
  36         struct list_head list;
  37         __u16 opcode;
  38         int index;
  39         void *cmd;
  40         struct sock *sk;
  41 };
  42
  43 LIST_HEAD(cmd_list);
  44
  45 static int cmd_status(struct sock *sk, u16 cmd, u8 status)
  46 {
  47         struct sk_buff *skb;
  48         struct mgmt_hdr *hdr;
  49         struct mgmt_ev_cmd_status *ev;
  50
  51         BT_DBG("sock %p", sk);
  52
  53         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_ATOMIC);
  54         if (!skb)
  55                 return -ENOMEM;
  56
  57         hdr = (void *) skb_put(skb, sizeof(*hdr));
  58
  59         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
  60         hdr->len = cpu_to_le16(sizeof(*ev));
  61
  62         ev = (void *) skb_put(skb, sizeof(*ev));
  63         ev->status = status;
  64         put_unaligned_le16(cmd, &ev->opcode);
  65
  66         if (sock_queue_rcv_skb(sk, skb) < 0)
  67                 kfree_skb(skb);
  68
  69         return 0;
  70 }
  71
  72 static int read_version(struct sock *sk)
  73 {
  74         struct sk_buff *skb;
  75         struct mgmt_hdr *hdr;
  76         struct mgmt_ev_cmd_complete *ev;
  77         struct mgmt_rp_read_version *rp;
  78
  79         BT_DBG("sock %p", sk);
  80
  81         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
  82         if (!skb)
  83                 return -ENOMEM;
  84
  85         hdr = (void *) skb_put(skb, sizeof(*hdr));
  86         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
  87         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
  88
  89         ev = (void *) skb_put(skb, sizeof(*ev));
  90         put_unaligned_le16(MGMT_OP_READ_VERSION, &ev->opcode);
  91
  92         rp = (void *) skb_put(skb, sizeof(*rp));
  93         rp->version = MGMT_VERSION;
  94         put_unaligned_le16(MGMT_REVISION, &rp->revision);
  95
  96         if (sock_queue_rcv_skb(sk, skb) < 0)
  97                 kfree_skb(skb);
  98
  99         return 0;
 100 }
 101
 102 static int read_index_list(struct sock *sk)
 103 {
 104         struct sk_buff *skb;
 105         struct mgmt_hdr *hdr;
 106         struct mgmt_ev_cmd_complete *ev;
 107         struct mgmt_rp_read_index_list *rp;
 108         struct list_head *p;
 109         size_t body_len;
 110         u16 count;
 111         int i;
 112
 113         BT_DBG("sock %p", sk);
 114
 115         read_lock(&hci_dev_list_lock);
 116
 117         count = 0;
 118         list_for_each(p, &hci_dev_list) {
 119                 count++;
 120         }
 121
 122         body_len = sizeof(*ev) + sizeof(*rp) + (2 * count);
 123         skb = alloc_skb(sizeof(*hdr) + body_len, GFP_ATOMIC);
 124         if (!skb) {
 125                 read_unlock(&hci_dev_list_lock);
 126                 return -ENOMEM;
 127         }
 128
 129         hdr = (void *) skb_put(skb, sizeof(*hdr));
 130         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 131         hdr->len = cpu_to_le16(body_len);
 132
 133         ev = (void *) skb_put(skb, sizeof(*ev));
 134         put_unaligned_le16(MGMT_OP_READ_INDEX_LIST, &ev->opcode);
 135
 136         rp = (void *) skb_put(skb, sizeof(*rp) + (2 * count));
 137         put_unaligned_le16(count, &rp->num_controllers);
 138
 139         i = 0;
 140         list_for_each(p, &hci_dev_list) {
 141                 struct hci_dev *d = list_entry(p, struct hci_dev, list);
 142
 143                 hci_del_off_timer(d);
 144
 145                 if (test_bit(HCI_SETUP, &d->flags))
 146                         continue;
 147
 148                 put_unaligned_le16(d->id, &rp->index[i++]);
 149                 BT_DBG("Added hci%u", d->id);
 150         }
 151
 152         read_unlock(&hci_dev_list_lock);
 153
 154         if (sock_queue_rcv_skb(sk, skb) < 0)
 155                 kfree_skb(skb);
 156
 157         return 0;
 158 }
 159
 160 static int read_controller_info(struct sock *sk, unsigned char *data, u16 len)
 161 {
 162         struct sk_buff *skb;
 163         struct mgmt_hdr *hdr;
 164         struct mgmt_ev_cmd_complete *ev;
 165         struct mgmt_rp_read_info *rp;
 166         struct mgmt_cp_read_info *cp;
 167         struct hci_dev *hdev;
 168         u16 dev_id;
 169
 170         BT_DBG("sock %p", sk);
 171
 172         if (len != 2)
 173                 return cmd_status(sk, MGMT_OP_READ_INFO, EINVAL);
 174
 175         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
 176         if (!skb)
 177                 return -ENOMEM;
 178
 179         hdr = (void *) skb_put(skb, sizeof(*hdr));
 180         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 181         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
 182
 183         ev = (void *) skb_put(skb, sizeof(*ev));
 184         put_unaligned_le16(MGMT_OP_READ_INFO, &ev->opcode);
 185
 186         rp = (void *) skb_put(skb, sizeof(*rp));
 187
 188         cp = (void *) data;
 189         dev_id = get_unaligned_le16(&cp->index);
 190
 191         BT_DBG("request for hci%u", dev_id);
 192
 193         hdev = hci_dev_get(dev_id);
 194         if (!hdev) {
 195                 kfree_skb(skb);
 196                 return cmd_status(sk, MGMT_OP_READ_INFO, ENODEV);
 197         }
 198
 199         hci_del_off_timer(hdev);
 200
 201         hci_dev_lock_bh(hdev);
 202
 203         put_unaligned_le16(hdev->id, &rp->index);
 204         rp->type = hdev->dev_type;
 205
 206         rp->powered = test_bit(HCI_UP, &hdev->flags);
 207         rp->connectable = test_bit(HCI_PSCAN, &hdev->flags);
 208         rp->discoverable = test_bit(HCI_ISCAN, &hdev->flags);
 209         rp->pairable = test_bit(HCI_PSCAN, &hdev->flags);
 210
 211         if (test_bit(HCI_AUTH, &hdev->flags))
 212                 rp->sec_mode = 3;
 213         else if (hdev->ssp_mode > 0)
 214                 rp->sec_mode = 4;
 215         else
 216                 rp->sec_mode = 2;
 217
 218         bacpy(&rp->bdaddr, &hdev->bdaddr);
 219         memcpy(rp->features, hdev->features, 8);
 220         memcpy(rp->dev_class, hdev->dev_class, 3);
 221         put_unaligned_le16(hdev->manufacturer, &rp->manufacturer);
 222         rp->hci_ver = hdev->hci_ver;
 223         put_unaligned_le16(hdev->hci_rev, &rp->hci_rev);
 224
 225         hci_dev_unlock_bh(hdev);
 226         hci_dev_put(hdev);
 227
 228         if (sock_queue_rcv_skb(sk, skb) < 0)
 229                 kfree_skb(skb);
 230
 231         return 0;
 232 }
 233
 234 static void mgmt_pending_free(struct pending_cmd *cmd)
 235 {
 236         sock_put(cmd->sk);
 237         kfree(cmd->cmd);
 238         kfree(cmd);
 239 }
 240
 241 static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
 242                                                         void *data, u16 len)
 243 {
 244         struct pending_cmd *cmd;
 245
 246         cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
 247         if (!cmd)
 248                 return -ENOMEM;
 249
 250         cmd->opcode = opcode;
 251         cmd->index = index;
 252
 253         cmd->cmd = kmalloc(len, GFP_ATOMIC);
 254         if (!cmd->cmd) {
 255                 kfree(cmd);
 256                 return -ENOMEM;
 257         }
 258
 259         memcpy(cmd->cmd, data, len);
 260
 261         cmd->sk = sk;
 262         sock_hold(sk);
 263
 264         list_add(&cmd->list, &cmd_list);
 265
 266         return 0;
 267 }
 268
 269 static void mgmt_pending_foreach(u16 opcode, int index,
 270                                 void (*cb)(struct pending_cmd *cmd, void *data),
 271                                 void *data)
 272 {
 273         struct list_head *p, *n;
 274
 275         list_for_each_safe(p, n, &cmd_list) {
 276                 struct pending_cmd *cmd;
 277
 278                 cmd = list_entry(p, struct pending_cmd, list);
 279
 280                 if (cmd->opcode != opcode)
 281                         continue;
 282
 283                 if (index >= 0 && cmd->index != index)
 284                         continue;
 285
 286                 cb(cmd, data);
 287         }
 288 }
 289
 290 static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
 291 {
 292         struct list_head *p;
 293
 294         list_for_each(p, &cmd_list) {
 295                 struct pending_cmd *cmd;
 296
 297                 cmd = list_entry(p, struct pending_cmd, list);
 298
 299                 if (cmd->opcode != opcode)
 300                         continue;
 301
 302                 if (index >= 0 && cmd->index != index)
 303                         continue;
 304
 305                 return cmd;
 306         }
 307
 308         return NULL;
 309 }
 310
 311 static void mgmt_pending_remove(u16 opcode, int index)
 312 {
 313         struct pending_cmd *cmd;
 314
 315         cmd = mgmt_pending_find(opcode, index);
 316         if (cmd == NULL)
 317                 return;
 318
 319         list_del(&cmd->list);
 320         mgmt_pending_free(cmd);
 321 }
 322
 323 static int set_powered(struct sock *sk, unsigned char *data, u16 len)
 324 {
 325         struct mgmt_cp_set_powered *cp;
 326         struct hci_dev *hdev;
 327         u16 dev_id;
 328         int ret, up;
 329
 330         cp = (void *) data;
 331         dev_id = get_unaligned_le16(&cp->index);
 332
 333         BT_DBG("request for hci%u", dev_id);
 334
 335         hdev = hci_dev_get(dev_id);
 336         if (!hdev)
 337                 return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);
 338
 339         hci_dev_lock_bh(hdev);
 340
 341         up = test_bit(HCI_UP, &hdev->flags);
 342         if ((cp->powered && up) || (!cp->powered && !up)) {
 343                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
 344                 goto failed;
 345         }
 346
 347         if (mgmt_pending_find(MGMT_OP_SET_POWERED, dev_id)) {
 348                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EBUSY);
 349                 goto failed;
 350         }
 351
 352         ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
 353         if (ret < 0)
 354                 goto failed;
 355
 356         if (cp->powered)
 357                 queue_work(hdev->workqueue, &hdev->power_on);
 358         else
 359                 queue_work(hdev->workqueue, &hdev->power_off);
 360
 361         ret = 0;
 362
 363 failed:
 364         hci_dev_unlock_bh(hdev);
 365         hci_dev_put(hdev);
 366         return ret;
 367 }
 368
 369 static int set_discoverable(struct sock *sk, unsigned char *data, u16 len)
 370 {
 371         struct mgmt_cp_set_discoverable *cp;
 372         struct hci_dev *hdev;
 373         u16 dev_id;
 374         u8 scan;
 375         int err;
 376
 377         cp = (void *) data;
 378         dev_id = get_unaligned_le16(&cp->index);
 379
 380         BT_DBG("request for hci%u", dev_id);
 381
 382         hdev = hci_dev_get(dev_id);
 383         if (!hdev)
 384                 return cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENODEV);
 385
 386         hci_dev_lock_bh(hdev);
 387
 388         if (!test_bit(HCI_UP, &hdev->flags)) {
 389                 err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENETDOWN);
 390                 goto failed;
 391         }
 392
 393         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) ||
 394                         mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id)) {
 395                 err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EBUSY);
 396                 goto failed;
 397         }
 398
 399         if (cp->discoverable == test_bit(HCI_ISCAN, &hdev->flags) &&
 400                                         test_bit(HCI_PSCAN, &hdev->flags)) {
 401                 err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EALREADY);
 402                 goto failed;
 403         }
 404
 405         err = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, dev_id, data, len);
 406         if (err < 0)
 407                 goto failed;
 408
 409         scan = SCAN_PAGE;
 410
 411         if (cp->discoverable)
 412                 scan |= SCAN_INQUIRY;
 413
 414         err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 415         if (err < 0)
 416                 mgmt_pending_remove(MGMT_OP_SET_DISCOVERABLE, dev_id);
 417
 418 failed:
 419         hci_dev_unlock_bh(hdev);
 420         hci_dev_put(hdev);
 421
 422         return err;
 423 }
 424
 425 static int set_connectable(struct sock *sk, unsigned char *data, u16 len)
 426 {
 427         struct mgmt_cp_set_connectable *cp;
 428         struct hci_dev *hdev;
 429         u16 dev_id;
 430         u8 scan;
 431         int err;
 432
 433         cp = (void *) data;
 434         dev_id = get_unaligned_le16(&cp->index);
 435
 436         BT_DBG("request for hci%u", dev_id);
 437
 438         hdev = hci_dev_get(dev_id);
 439         if (!hdev)
 440                 return cmd_status(sk, MGMT_OP_SET_CONNECTABLE, ENODEV);
 441
 442         hci_dev_lock_bh(hdev);
 443
 444         if (!test_bit(HCI_UP, &hdev->flags)) {
 445                 err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, ENETDOWN);
 446                 goto failed;
 447         }
 448
 449         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) ||
 450                         mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id)) {
 451                 err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, EBUSY);
 452                 goto failed;
 453         }
 454
 455         if (cp->connectable == test_bit(HCI_PSCAN, &hdev->flags)) {
 456                 err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, EALREADY);
 457                 goto failed;
 458         }
 459
 460         err = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, dev_id, data, len);
 461         if (err < 0)
 462                 goto failed;
 463
 464         if (cp->connectable)
 465                 scan = SCAN_PAGE;
 466         else
 467                 scan = 0;
 468
 469         err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 470         if (err < 0)
 471                 mgmt_pending_remove(MGMT_OP_SET_CONNECTABLE, dev_id);
 472
 473 failed:
 474         hci_dev_unlock_bh(hdev);
 475         hci_dev_put(hdev);
 476
 477         return err;
 478 }
 479
 480 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 481 {
 482         unsigned char *buf;
 483         struct mgmt_hdr *hdr;
 484         u16 opcode, len;
 485         int err;
 486
 487         BT_DBG("got %zu bytes", msglen);
 488
 489         if (msglen < sizeof(*hdr))
 490                 return -EINVAL;
 491
 492         buf = kmalloc(msglen, GFP_ATOMIC);
 493         if (!buf)
 494                 return -ENOMEM;
 495
 496         if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
 497                 err = -EFAULT;
 498                 goto done;
 499         }
 500
 501         hdr = (struct mgmt_hdr *) buf;
 502         opcode = get_unaligned_le16(&hdr->opcode);
 503         len = get_unaligned_le16(&hdr->len);
 504
 505         if (len != msglen - sizeof(*hdr)) {
 506                 err = -EINVAL;
 507                 goto done;
 508         }
 509
 510         switch (opcode) {
 511         case MGMT_OP_READ_VERSION:
 512                 err = read_version(sk);
 513                 break;
 514         case MGMT_OP_READ_INDEX_LIST:
 515                 err = read_index_list(sk);
 516                 break;
 517         case MGMT_OP_READ_INFO:
 518                 err = read_controller_info(sk, buf + sizeof(*hdr), len);
 519                 break;
 520         case MGMT_OP_SET_POWERED:
 521                 err = set_powered(sk, buf + sizeof(*hdr), len);
 522                 break;
 523         case MGMT_OP_SET_DISCOVERABLE:
 524                 err = set_discoverable(sk, buf + sizeof(*hdr), len);
 525                 break;
 526         case MGMT_OP_SET_CONNECTABLE:
 527                 err = set_connectable(sk, buf + sizeof(*hdr), len);
 528                 break;
 529         default:
 530                 BT_DBG("Unknown op %u", opcode);
 531                 err = cmd_status(sk, opcode, 0x01);
 532                 break;
 533         }
 534
 535         if (err < 0)
 536                 goto done;
 537
 538         err = msglen;
 539
 540 done:
 541         kfree(buf);
 542         return err;
 543 }
 544
 545 static int mgmt_event(u16 event, void *data, u16 data_len, struct sock *skip_sk)
 546 {
 547         struct sk_buff *skb;
 548         struct mgmt_hdr *hdr;
 549
 550         skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC);
 551         if (!skb)
 552                 return -ENOMEM;
 553
 554         bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;
 555
 556         hdr = (void *) skb_put(skb, sizeof(*hdr));
 557         hdr->opcode = cpu_to_le16(event);
 558         hdr->len = cpu_to_le16(data_len);
 559
 560         memcpy(skb_put(skb, data_len), data, data_len);
 561
 562         hci_send_to_sock(NULL, skb, skip_sk);
 563         kfree_skb(skb);
 564
 565         return 0;
 566 }
 567
 568 int mgmt_index_added(u16 index)
 569 {
 570         struct mgmt_ev_index_added ev;
 571
 572         put_unaligned_le16(index, &ev.index);
 573
 574         return mgmt_event(MGMT_EV_INDEX_ADDED, &ev, sizeof(ev), NULL);
 575 }
 576
 577 int mgmt_index_removed(u16 index)
 578 {
 579         struct mgmt_ev_index_added ev;
 580
 581         put_unaligned_le16(index, &ev.index);
 582
 583         return mgmt_event(MGMT_EV_INDEX_REMOVED, &ev, sizeof(ev), NULL);
 584 }
 585
 586 struct cmd_lookup {
 587         u8 value;
 588         struct sock *sk;
 589 };
 590
 591 static void power_rsp(struct pending_cmd *cmd, void *data)
 592 {
 593         struct mgmt_hdr *hdr;
 594         struct mgmt_ev_cmd_complete *ev;
 595         struct mgmt_rp_set_powered *rp;
 596         struct mgmt_cp_set_powered *cp = cmd->cmd;
 597         struct sk_buff *skb;
 598         struct cmd_lookup *match = data;
 599
 600         if (cp->powered != match->value)
 601                 return;
 602
 603         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
 604         if (!skb)
 605                 return;
 606
 607         hdr = (void *) skb_put(skb, sizeof(*hdr));
 608         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 609         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
 610
 611         ev = (void *) skb_put(skb, sizeof(*ev));
 612         put_unaligned_le16(cmd->opcode, &ev->opcode);
 613
 614         rp = (void *) skb_put(skb, sizeof(*rp));
 615         put_unaligned_le16(cmd->index, &rp->index);
 616         rp->powered = cp->powered;
 617
 618         if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
 619                 kfree_skb(skb);
 620
 621         list_del(&cmd->list);
 622
 623         if (match->sk == NULL) {
 624                 match->sk = cmd->sk;
 625                 sock_hold(match->sk);
 626         }
 627
 628         mgmt_pending_free(cmd);
 629 }
 630
 631 int mgmt_powered(u16 index, u8 powered)
 632 {
 633         struct mgmt_ev_powered ev;
 634         struct cmd_lookup match = { powered, NULL };
 635         int ret;
 636
 637         put_unaligned_le16(index, &ev.index);
 638         ev.powered = powered;
 639
 640         mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, power_rsp, &match);
 641
 642         ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);
 643
 644         if (match.sk)
 645                 sock_put(match.sk);
 646
 647         return ret;
 648 }
 649
 650 static void discoverable_rsp(struct pending_cmd *cmd, void *data)
 651 {
 652         struct mgmt_cp_set_discoverable *cp = cmd->cmd;
 653         struct cmd_lookup *match = data;
 654         struct sk_buff *skb;
 655         struct mgmt_hdr *hdr;
 656         struct mgmt_ev_cmd_complete *ev;
 657         struct mgmt_rp_set_discoverable *rp;
 658
 659         if (cp->discoverable != match->value)
 660                 return;
 661
 662         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
 663         if (!skb)
 664                 return;
 665
 666         hdr = (void *) skb_put(skb, sizeof(*hdr));
 667         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 668         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
 669
 670         ev = (void *) skb_put(skb, sizeof(*ev));
 671         put_unaligned_le16(MGMT_OP_SET_DISCOVERABLE, &ev->opcode);
 672
 673         rp = (void *) skb_put(skb, sizeof(*rp));
 674         put_unaligned_le16(cmd->index, &rp->index);
 675         rp->discoverable = cp->discoverable;
 676
 677         if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
 678                 kfree_skb(skb);
 679
 680         list_del(&cmd->list);
 681
 682         if (match->sk == NULL) {
 683                 match->sk = cmd->sk;
 684                 sock_hold(match->sk);
 685         }
 686
 687         mgmt_pending_free(cmd);
 688 }
 689
 690 int mgmt_discoverable(u16 index, u8 discoverable)
 691 {
 692         struct mgmt_ev_discoverable ev;
 693         struct cmd_lookup match = { discoverable, NULL };
 694         int ret;
 695
 696         put_unaligned_le16(index, &ev.index);
 697         ev.discoverable = discoverable;
 698
 699         mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index,
 700                                                 discoverable_rsp, &match);
 701
 702         ret = mgmt_event(MGMT_EV_DISCOVERABLE, &ev, sizeof(ev), match.sk);
 703
 704         if (match.sk)
 705                 sock_put(match.sk);
 706
 707         return ret;
 708 }
 709
 710 static void connectable_rsp(struct pending_cmd *cmd, void *data)
 711 {
 712         struct mgmt_cp_set_connectable *cp = cmd->cmd;
 713         struct cmd_lookup *match = data;
 714         struct sk_buff *skb;
 715         struct mgmt_hdr *hdr;
 716         struct mgmt_ev_cmd_complete *ev;
 717         struct mgmt_rp_set_connectable *rp;
 718
 719         if (cp->connectable != match->value)
 720                 return;
 721
 722         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
 723         if (!skb)
 724                 return;
 725
 726         hdr = (void *) skb_put(skb, sizeof(*hdr));
 727         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 728         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
 729
 730         ev = (void *) skb_put(skb, sizeof(*ev));
 731         put_unaligned_le16(MGMT_OP_SET_CONNECTABLE, &ev->opcode);
 732
 733         rp = (void *) skb_put(skb, sizeof(*rp));
 734         put_unaligned_le16(cmd->index, &rp->index);
 735         rp->connectable = cp->connectable;
 736
 737         if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
 738                 kfree_skb(skb);
 739
 740         list_del(&cmd->list);
 741
 742         if (match->sk == NULL) {
 743                 match->sk = cmd->sk;
 744                 sock_hold(match->sk);
 745         }
 746
 747         mgmt_pending_free(cmd);
 748 }
 749
 750 int mgmt_connectable(u16 index, u8 connectable)
 751 {
 752         struct mgmt_ev_connectable ev;
 753         struct cmd_lookup match = { connectable, NULL };
 754         int ret;
 755
 756         put_unaligned_le16(index, &ev.index);
 757         ev.connectable = connectable;
 758
 759         mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, index,
 760                                                 connectable_rsp, &match);
 761
 762         ret = mgmt_event(MGMT_EV_CONNECTABLE, &ev, sizeof(ev), match.sk);
 763
 764         if (match.sk)
 765                 sock_put(match.sk);
 766
 767         return ret;
 768 }