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->discoverable = test_bit(HCI_ISCAN, &hdev->flags);
 208         rp->pairable = test_bit(HCI_PSCAN, &hdev->flags);
 209
 210         if (test_bit(HCI_AUTH, &hdev->flags))
 211                 rp->sec_mode = 3;
 212         else if (hdev->ssp_mode > 0)
 213                 rp->sec_mode = 4;
 214         else
 215                 rp->sec_mode = 2;
 216
 217         bacpy(&rp->bdaddr, &hdev->bdaddr);
 218         memcpy(rp->features, hdev->features, 8);
 219         memcpy(rp->dev_class, hdev->dev_class, 3);
 220         put_unaligned_le16(hdev->manufacturer, &rp->manufacturer);
 221         rp->hci_ver = hdev->hci_ver;
 222         put_unaligned_le16(hdev->hci_rev, &rp->hci_rev);
 223
 224         hci_dev_unlock_bh(hdev);
 225         hci_dev_put(hdev);
 226
 227         if (sock_queue_rcv_skb(sk, skb) < 0)
 228                 kfree_skb(skb);
 229
 230         return 0;
 231 }
 232
 233 static void mgmt_pending_free(struct pending_cmd *cmd)
 234 {
 235         sock_put(cmd->sk);
 236         kfree(cmd->cmd);
 237         kfree(cmd);
 238 }
 239
 240 static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
 241                                                         void *data, u16 len)
 242 {
 243         struct pending_cmd *cmd;
 244
 245         cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
 246         if (!cmd)
 247                 return -ENOMEM;
 248
 249         cmd->opcode = opcode;
 250         cmd->index = index;
 251
 252         cmd->cmd = kmalloc(len, GFP_ATOMIC);
 253         if (!cmd->cmd) {
 254                 kfree(cmd);
 255                 return -ENOMEM;
 256         }
 257
 258         memcpy(cmd->cmd, data, len);
 259
 260         cmd->sk = sk;
 261         sock_hold(sk);
 262
 263         list_add(&cmd->list, &cmd_list);
 264
 265         return 0;
 266 }
 267
 268 static void mgmt_pending_foreach(u16 opcode, int index,
 269                                 void (*cb)(struct pending_cmd *cmd, void *data),
 270                                 void *data)
 271 {
 272         struct list_head *p, *n;
 273
 274         list_for_each_safe(p, n, &cmd_list) {
 275                 struct pending_cmd *cmd;
 276
 277                 cmd = list_entry(p, struct pending_cmd, list);
 278
 279                 if (cmd->opcode != opcode)
 280                         continue;
 281
 282                 if (index >= 0 && cmd->index != index)
 283                         continue;
 284
 285                 cb(cmd, data);
 286         }
 287 }
 288
 289 static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
 290 {
 291         struct list_head *p;
 292
 293         list_for_each(p, &cmd_list) {
 294                 struct pending_cmd *cmd;
 295
 296                 cmd = list_entry(p, struct pending_cmd, list);
 297
 298                 if (cmd->opcode != opcode)
 299                         continue;
 300
 301                 if (index >= 0 && cmd->index != index)
 302                         continue;
 303
 304                 return cmd;
 305         }
 306
 307         return NULL;
 308 }
 309
 310 static void mgmt_pending_remove(u16 opcode, int index)
 311 {
 312         struct pending_cmd *cmd;
 313
 314         cmd = mgmt_pending_find(opcode, index);
 315         if (cmd == NULL)
 316                 return;
 317
 318         list_del(&cmd->list);
 319         mgmt_pending_free(cmd);
 320 }
 321
 322 static int set_powered(struct sock *sk, unsigned char *data, u16 len)
 323 {
 324         struct mgmt_cp_set_powered *cp;
 325         struct hci_dev *hdev;
 326         u16 dev_id;
 327         int ret, up;
 328
 329         cp = (void *) data;
 330         dev_id = get_unaligned_le16(&cp->index);
 331
 332         BT_DBG("request for hci%u", dev_id);
 333
 334         hdev = hci_dev_get(dev_id);
 335         if (!hdev)
 336                 return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);
 337
 338         hci_dev_lock_bh(hdev);
 339
 340         up = test_bit(HCI_UP, &hdev->flags);
 341         if ((cp->powered && up) || (!cp->powered && !up)) {
 342                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
 343                 goto failed;
 344         }
 345
 346         if (mgmt_pending_find(MGMT_OP_SET_POWERED, dev_id)) {
 347                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EBUSY);
 348                 goto failed;
 349         }
 350
 351         ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
 352         if (ret < 0)
 353                 goto failed;
 354
 355         if (cp->powered)
 356                 queue_work(hdev->workqueue, &hdev->power_on);
 357         else
 358                 queue_work(hdev->workqueue, &hdev->power_off);
 359
 360         ret = 0;
 361
 362 failed:
 363         hci_dev_unlock_bh(hdev);
 364         hci_dev_put(hdev);
 365         return ret;
 366 }
 367
 368 static int set_discoverable(struct sock *sk, unsigned char *data, u16 len)
 369 {
 370         struct mgmt_cp_set_discoverable *cp;
 371         struct hci_dev *hdev;
 372         u16 dev_id;
 373         u8 scan;
 374         int err;
 375
 376         cp = (void *) data;
 377         dev_id = get_unaligned_le16(&cp->index);
 378
 379         BT_DBG("request for hci%u", dev_id);
 380
 381         hdev = hci_dev_get(dev_id);
 382         if (!hdev)
 383                 return cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENODEV);
 384
 385         hci_dev_lock_bh(hdev);
 386
 387         if (!test_bit(HCI_UP, &hdev->flags)) {
 388                 err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENETDOWN);
 389                 goto failed;
 390         }
 391
 392         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) ||
 393                         mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id) ||
 394                         hci_sent_cmd_data(hdev, HCI_OP_WRITE_SCAN_ENABLE)) {
 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 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 426 {
 427         unsigned char *buf;
 428         struct mgmt_hdr *hdr;
 429         u16 opcode, len;
 430         int err;
 431
 432         BT_DBG("got %zu bytes", msglen);
 433
 434         if (msglen < sizeof(*hdr))
 435                 return -EINVAL;
 436
 437         buf = kmalloc(msglen, GFP_ATOMIC);
 438         if (!buf)
 439                 return -ENOMEM;
 440
 441         if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
 442                 err = -EFAULT;
 443                 goto done;
 444         }
 445
 446         hdr = (struct mgmt_hdr *) buf;
 447         opcode = get_unaligned_le16(&hdr->opcode);
 448         len = get_unaligned_le16(&hdr->len);
 449
 450         if (len != msglen - sizeof(*hdr)) {
 451                 err = -EINVAL;
 452                 goto done;
 453         }
 454
 455         switch (opcode) {
 456         case MGMT_OP_READ_VERSION:
 457                 err = read_version(sk);
 458                 break;
 459         case MGMT_OP_READ_INDEX_LIST:
 460                 err = read_index_list(sk);
 461                 break;
 462         case MGMT_OP_READ_INFO:
 463                 err = read_controller_info(sk, buf + sizeof(*hdr), len);
 464                 break;
 465         case MGMT_OP_SET_POWERED:
 466                 err = set_powered(sk, buf + sizeof(*hdr), len);
 467                 break;
 468         case MGMT_OP_SET_DISCOVERABLE:
 469                 err = set_discoverable(sk, buf + sizeof(*hdr), len);
 470                 break;
 471         default:
 472                 BT_DBG("Unknown op %u", opcode);
 473                 err = cmd_status(sk, opcode, 0x01);
 474                 break;
 475         }
 476
 477         if (err < 0)
 478                 goto done;
 479
 480         err = msglen;
 481
 482 done:
 483         kfree(buf);
 484         return err;
 485 }
 486
 487 static int mgmt_event(u16 event, void *data, u16 data_len, struct sock *skip_sk)
 488 {
 489         struct sk_buff *skb;
 490         struct mgmt_hdr *hdr;
 491
 492         skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC);
 493         if (!skb)
 494                 return -ENOMEM;
 495
 496         bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;
 497
 498         hdr = (void *) skb_put(skb, sizeof(*hdr));
 499         hdr->opcode = cpu_to_le16(event);
 500         hdr->len = cpu_to_le16(data_len);
 501
 502         memcpy(skb_put(skb, data_len), data, data_len);
 503
 504         hci_send_to_sock(NULL, skb, skip_sk);
 505         kfree_skb(skb);
 506
 507         return 0;
 508 }
 509
 510 int mgmt_index_added(u16 index)
 511 {
 512         struct mgmt_ev_index_added ev;
 513
 514         put_unaligned_le16(index, &ev.index);
 515
 516         return mgmt_event(MGMT_EV_INDEX_ADDED, &ev, sizeof(ev), NULL);
 517 }
 518
 519 int mgmt_index_removed(u16 index)
 520 {
 521         struct mgmt_ev_index_added ev;
 522
 523         put_unaligned_le16(index, &ev.index);
 524
 525         return mgmt_event(MGMT_EV_INDEX_REMOVED, &ev, sizeof(ev), NULL);
 526 }
 527
 528 struct cmd_lookup {
 529         u8 value;
 530         struct sock *sk;
 531 };
 532
 533 static void power_rsp(struct pending_cmd *cmd, void *data)
 534 {
 535         struct mgmt_hdr *hdr;
 536         struct mgmt_ev_cmd_complete *ev;
 537         struct mgmt_rp_set_powered *rp;
 538         struct mgmt_cp_set_powered *cp = cmd->cmd;
 539         struct sk_buff *skb;
 540         struct cmd_lookup *match = data;
 541
 542         if (cp->powered != match->value)
 543                 return;
 544
 545         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
 546         if (!skb)
 547                 return;
 548
 549         hdr = (void *) skb_put(skb, sizeof(*hdr));
 550         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 551         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
 552
 553         ev = (void *) skb_put(skb, sizeof(*ev));
 554         put_unaligned_le16(cmd->opcode, &ev->opcode);
 555
 556         rp = (void *) skb_put(skb, sizeof(*rp));
 557         put_unaligned_le16(cmd->index, &rp->index);
 558         rp->powered = cp->powered;
 559
 560         if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
 561                 kfree_skb(skb);
 562
 563         list_del(&cmd->list);
 564
 565         if (match->sk == NULL) {
 566                 match->sk = cmd->sk;
 567                 sock_hold(match->sk);
 568         }
 569
 570         mgmt_pending_free(cmd);
 571 }
 572
 573 int mgmt_powered(u16 index, u8 powered)
 574 {
 575         struct mgmt_ev_powered ev;
 576         struct cmd_lookup match = { powered, NULL };
 577         int ret;
 578
 579         put_unaligned_le16(index, &ev.index);
 580         ev.powered = powered;
 581
 582         mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, power_rsp, &match);
 583
 584         ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);
 585
 586         if (match.sk)
 587                 sock_put(match.sk);
 588
 589         return ret;
 590 }
 591
 592 static void discoverable_rsp(struct pending_cmd *cmd, void *data)
 593 {
 594         struct mgmt_cp_set_discoverable *cp = cmd->cmd;
 595         struct cmd_lookup *match = data;
 596         struct sk_buff *skb;
 597         struct mgmt_hdr *hdr;
 598         struct mgmt_ev_cmd_complete *ev;
 599         struct mgmt_rp_set_discoverable *rp;
 600
 601         if (cp->discoverable != match->value)
 602                 return;
 603
 604         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + sizeof(*rp), GFP_ATOMIC);
 605         if (!skb)
 606                 return;
 607
 608         hdr = (void *) skb_put(skb, sizeof(*hdr));
 609         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
 610         hdr->len = cpu_to_le16(sizeof(*ev) + sizeof(*rp));
 611
 612         ev = (void *) skb_put(skb, sizeof(*ev));
 613         put_unaligned_le16(MGMT_OP_SET_DISCOVERABLE, &ev->opcode);
 614
 615         rp = (void *) skb_put(skb, sizeof(*rp));
 616         put_unaligned_le16(cmd->index, &rp->index);
 617         rp->discoverable = cp->discoverable;
 618
 619         if (sock_queue_rcv_skb(cmd->sk, skb) < 0)
 620                 kfree_skb(skb);
 621
 622         list_del(&cmd->list);
 623
 624         if (match->sk == NULL) {
 625                 match->sk = cmd->sk;
 626                 sock_hold(match->sk);
 627         }
 628
 629         mgmt_pending_free(cmd);
 630 }
 631
 632 int mgmt_discoverable(u16 index, u8 discoverable)
 633 {
 634         struct mgmt_ev_discoverable ev;
 635         struct cmd_lookup match = { discoverable, NULL };
 636         int ret;
 637
 638         put_unaligned_le16(index, &ev.index);
 639         ev.discoverable = discoverable;
 640
 641         mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index,
 642                                                 discoverable_rsp, &match);
 643
 644         ret = mgmt_event(MGMT_EV_DISCOVERABLE, &ev, sizeof(ev), match.sk);
 645
 646         if (match.sk)
 647                 sock_put(match.sk);
 648
 649         return ret;
 650 }