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 cmd_complete(struct sock *sk, u16 cmd, void *rp, size_t rp_len)
  73 {
  74         struct sk_buff *skb;
  75         struct mgmt_hdr *hdr;
  76         struct mgmt_ev_cmd_complete *ev;
  77
  78         BT_DBG("sock %p", sk);
  79
  80         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_ATOMIC);
  81         if (!skb)
  82                 return -ENOMEM;
  83
  84         hdr = (void *) skb_put(skb, sizeof(*hdr));
  85
  86         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
  87         hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
  88
  89         ev = (void *) skb_put(skb, sizeof(*ev) + rp_len);
  90         put_unaligned_le16(cmd, &ev->opcode);
  91         memcpy(ev->data, rp, rp_len);
  92
  93         if (sock_queue_rcv_skb(sk, skb) < 0)
  94                 kfree_skb(skb);
  95
  96         return 0;
  97 }
  98
  99 static int read_version(struct sock *sk)
 100 {
 101         struct mgmt_rp_read_version rp;
 102
 103         BT_DBG("sock %p", sk);
 104
 105         rp.version = MGMT_VERSION;
 106         put_unaligned_le16(MGMT_REVISION, &rp.revision);
 107
 108         return cmd_complete(sk, MGMT_OP_READ_VERSION, &rp, sizeof(rp));
 109 }
 110
 111 static int read_index_list(struct sock *sk)
 112 {
 113         struct mgmt_rp_read_index_list *rp;
 114         struct list_head *p;
 115         size_t rp_len;
 116         u16 count;
 117         int i, err;
 118
 119         BT_DBG("sock %p", sk);
 120
 121         read_lock(&hci_dev_list_lock);
 122
 123         count = 0;
 124         list_for_each(p, &hci_dev_list) {
 125                 count++;
 126         }
 127
 128         rp_len = sizeof(*rp) + (2 * count);
 129         rp = kmalloc(rp_len, GFP_ATOMIC);
 130         if (!rp) {
 131                 read_unlock(&hci_dev_list_lock);
 132                 return -ENOMEM;
 133         }
 134
 135         put_unaligned_le16(count, &rp->num_controllers);
 136
 137         i = 0;
 138         list_for_each(p, &hci_dev_list) {
 139                 struct hci_dev *d = list_entry(p, struct hci_dev, list);
 140
 141                 hci_del_off_timer(d);
 142
 143                 set_bit(HCI_MGMT, &d->flags);
 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         err = cmd_complete(sk, MGMT_OP_READ_INDEX_LIST, rp, rp_len);
 155
 156         kfree(rp);
 157
 158         return err;
 159 }
 160
 161 static int read_controller_info(struct sock *sk, unsigned char *data, u16 len)
 162 {
 163         struct mgmt_rp_read_info rp;
 164         struct mgmt_cp_read_info *cp = (void *) data;
 165         struct hci_dev *hdev;
 166         u16 dev_id;
 167
 168         BT_DBG("sock %p", sk);
 169
 170         if (len != 2)
 171                 return cmd_status(sk, MGMT_OP_READ_INFO, EINVAL);
 172
 173         dev_id = get_unaligned_le16(&cp->index);
 174
 175         BT_DBG("request for hci%u", dev_id);
 176
 177         hdev = hci_dev_get(dev_id);
 178         if (!hdev)
 179                 return cmd_status(sk, MGMT_OP_READ_INFO, ENODEV);
 180
 181         hci_del_off_timer(hdev);
 182
 183         hci_dev_lock_bh(hdev);
 184
 185         set_bit(HCI_MGMT, &hdev->flags);
 186
 187         put_unaligned_le16(hdev->id, &rp.index);
 188         rp.type = hdev->dev_type;
 189
 190         rp.powered = test_bit(HCI_UP, &hdev->flags);
 191         rp.connectable = test_bit(HCI_PSCAN, &hdev->flags);
 192         rp.discoverable = test_bit(HCI_ISCAN, &hdev->flags);
 193         rp.pairable = test_bit(HCI_PSCAN, &hdev->flags);
 194
 195         if (test_bit(HCI_AUTH, &hdev->flags))
 196                 rp.sec_mode = 3;
 197         else if (hdev->ssp_mode > 0)
 198                 rp.sec_mode = 4;
 199         else
 200                 rp.sec_mode = 2;
 201
 202         bacpy(&rp.bdaddr, &hdev->bdaddr);
 203         memcpy(rp.features, hdev->features, 8);
 204         memcpy(rp.dev_class, hdev->dev_class, 3);
 205         put_unaligned_le16(hdev->manufacturer, &rp.manufacturer);
 206         rp.hci_ver = hdev->hci_ver;
 207         put_unaligned_le16(hdev->hci_rev, &rp.hci_rev);
 208
 209         hci_dev_unlock_bh(hdev);
 210         hci_dev_put(hdev);
 211
 212         return cmd_complete(sk, MGMT_OP_READ_INFO, &rp, sizeof(rp));
 213 }
 214
 215 static void mgmt_pending_free(struct pending_cmd *cmd)
 216 {
 217         sock_put(cmd->sk);
 218         kfree(cmd->cmd);
 219         kfree(cmd);
 220 }
 221
 222 static int mgmt_pending_add(struct sock *sk, u16 opcode, int index,
 223                                                         void *data, u16 len)
 224 {
 225         struct pending_cmd *cmd;
 226
 227         cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
 228         if (!cmd)
 229                 return -ENOMEM;
 230
 231         cmd->opcode = opcode;
 232         cmd->index = index;
 233
 234         cmd->cmd = kmalloc(len, GFP_ATOMIC);
 235         if (!cmd->cmd) {
 236                 kfree(cmd);
 237                 return -ENOMEM;
 238         }
 239
 240         memcpy(cmd->cmd, data, len);
 241
 242         cmd->sk = sk;
 243         sock_hold(sk);
 244
 245         list_add(&cmd->list, &cmd_list);
 246
 247         return 0;
 248 }
 249
 250 static void mgmt_pending_foreach(u16 opcode, int index,
 251                                 void (*cb)(struct pending_cmd *cmd, void *data),
 252                                 void *data)
 253 {
 254         struct list_head *p, *n;
 255
 256         list_for_each_safe(p, n, &cmd_list) {
 257                 struct pending_cmd *cmd;
 258
 259                 cmd = list_entry(p, struct pending_cmd, list);
 260
 261                 if (cmd->opcode != opcode)
 262                         continue;
 263
 264                 if (index >= 0 && cmd->index != index)
 265                         continue;
 266
 267                 cb(cmd, data);
 268         }
 269 }
 270
 271 static struct pending_cmd *mgmt_pending_find(u16 opcode, int index)
 272 {
 273         struct list_head *p;
 274
 275         list_for_each(p, &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                 return cmd;
 287         }
 288
 289         return NULL;
 290 }
 291
 292 static void mgmt_pending_remove(u16 opcode, int index)
 293 {
 294         struct pending_cmd *cmd;
 295
 296         cmd = mgmt_pending_find(opcode, index);
 297         if (cmd == NULL)
 298                 return;
 299
 300         list_del(&cmd->list);
 301         mgmt_pending_free(cmd);
 302 }
 303
 304 static int set_powered(struct sock *sk, unsigned char *data, u16 len)
 305 {
 306         struct mgmt_mode *cp;
 307         struct hci_dev *hdev;
 308         u16 dev_id;
 309         int ret, up;
 310
 311         cp = (void *) data;
 312         dev_id = get_unaligned_le16(&cp->index);
 313
 314         BT_DBG("request for hci%u", dev_id);
 315
 316         hdev = hci_dev_get(dev_id);
 317         if (!hdev)
 318                 return cmd_status(sk, MGMT_OP_SET_POWERED, ENODEV);
 319
 320         hci_dev_lock_bh(hdev);
 321
 322         up = test_bit(HCI_UP, &hdev->flags);
 323         if ((cp->val && up) || (!cp->val && !up)) {
 324                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EALREADY);
 325                 goto failed;
 326         }
 327
 328         if (mgmt_pending_find(MGMT_OP_SET_POWERED, dev_id)) {
 329                 ret = cmd_status(sk, MGMT_OP_SET_POWERED, EBUSY);
 330                 goto failed;
 331         }
 332
 333         ret = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, dev_id, data, len);
 334         if (ret < 0)
 335                 goto failed;
 336
 337         if (cp->val)
 338                 queue_work(hdev->workqueue, &hdev->power_on);
 339         else
 340                 queue_work(hdev->workqueue, &hdev->power_off);
 341
 342         ret = 0;
 343
 344 failed:
 345         hci_dev_unlock_bh(hdev);
 346         hci_dev_put(hdev);
 347         return ret;
 348 }
 349
 350 static int set_discoverable(struct sock *sk, unsigned char *data, u16 len)
 351 {
 352         struct mgmt_mode *cp;
 353         struct hci_dev *hdev;
 354         u16 dev_id;
 355         u8 scan;
 356         int err;
 357
 358         cp = (void *) data;
 359         dev_id = get_unaligned_le16(&cp->index);
 360
 361         BT_DBG("request for hci%u", dev_id);
 362
 363         hdev = hci_dev_get(dev_id);
 364         if (!hdev)
 365                 return cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENODEV);
 366
 367         hci_dev_lock_bh(hdev);
 368
 369         if (!test_bit(HCI_UP, &hdev->flags)) {
 370                 err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, ENETDOWN);
 371                 goto failed;
 372         }
 373
 374         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) ||
 375                         mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id)) {
 376                 err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EBUSY);
 377                 goto failed;
 378         }
 379
 380         if (cp->val == test_bit(HCI_ISCAN, &hdev->flags) &&
 381                                         test_bit(HCI_PSCAN, &hdev->flags)) {
 382                 err = cmd_status(sk, MGMT_OP_SET_DISCOVERABLE, EALREADY);
 383                 goto failed;
 384         }
 385
 386         err = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, dev_id, data, len);
 387         if (err < 0)
 388                 goto failed;
 389
 390         scan = SCAN_PAGE;
 391
 392         if (cp->val)
 393                 scan |= SCAN_INQUIRY;
 394
 395         err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 396         if (err < 0)
 397                 mgmt_pending_remove(MGMT_OP_SET_DISCOVERABLE, dev_id);
 398
 399 failed:
 400         hci_dev_unlock_bh(hdev);
 401         hci_dev_put(hdev);
 402
 403         return err;
 404 }
 405
 406 static int set_connectable(struct sock *sk, unsigned char *data, u16 len)
 407 {
 408         struct mgmt_mode *cp;
 409         struct hci_dev *hdev;
 410         u16 dev_id;
 411         u8 scan;
 412         int err;
 413
 414         cp = (void *) data;
 415         dev_id = get_unaligned_le16(&cp->index);
 416
 417         BT_DBG("request for hci%u", dev_id);
 418
 419         hdev = hci_dev_get(dev_id);
 420         if (!hdev)
 421                 return cmd_status(sk, MGMT_OP_SET_CONNECTABLE, ENODEV);
 422
 423         hci_dev_lock_bh(hdev);
 424
 425         if (!test_bit(HCI_UP, &hdev->flags)) {
 426                 err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, ENETDOWN);
 427                 goto failed;
 428         }
 429
 430         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, dev_id) ||
 431                         mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, dev_id)) {
 432                 err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, EBUSY);
 433                 goto failed;
 434         }
 435
 436         if (cp->val == test_bit(HCI_PSCAN, &hdev->flags)) {
 437                 err = cmd_status(sk, MGMT_OP_SET_CONNECTABLE, EALREADY);
 438                 goto failed;
 439         }
 440
 441         err = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, dev_id, data, len);
 442         if (err < 0)
 443                 goto failed;
 444
 445         if (cp->val)
 446                 scan = SCAN_PAGE;
 447         else
 448                 scan = 0;
 449
 450         err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 451         if (err < 0)
 452                 mgmt_pending_remove(MGMT_OP_SET_CONNECTABLE, dev_id);
 453
 454 failed:
 455         hci_dev_unlock_bh(hdev);
 456         hci_dev_put(hdev);
 457
 458         return err;
 459 }
 460
 461 static int mgmt_event(u16 event, void *data, u16 data_len, struct sock *skip_sk)
 462 {
 463         struct sk_buff *skb;
 464         struct mgmt_hdr *hdr;
 465
 466         skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC);
 467         if (!skb)
 468                 return -ENOMEM;
 469
 470         bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;
 471
 472         hdr = (void *) skb_put(skb, sizeof(*hdr));
 473         hdr->opcode = cpu_to_le16(event);
 474         hdr->len = cpu_to_le16(data_len);
 475
 476         memcpy(skb_put(skb, data_len), data, data_len);
 477
 478         hci_send_to_sock(NULL, skb, skip_sk);
 479         kfree_skb(skb);
 480
 481         return 0;
 482 }
 483
 484 static int send_mode_rsp(struct sock *sk, u16 opcode, u16 index, u8 val)
 485 {
 486         struct mgmt_mode rp;
 487
 488         put_unaligned_le16(index, &rp.index);
 489         rp.val = val;
 490
 491         return cmd_complete(sk, opcode, &rp, sizeof(rp));
 492 }
 493
 494 static int set_pairable(struct sock *sk, unsigned char *data, u16 len)
 495 {
 496         struct mgmt_mode *cp, ev;
 497         struct hci_dev *hdev;
 498         u16 dev_id;
 499         int err;
 500
 501         cp = (void *) data;
 502         dev_id = get_unaligned_le16(&cp->index);
 503
 504         BT_DBG("request for hci%u", dev_id);
 505
 506         hdev = hci_dev_get(dev_id);
 507         if (!hdev)
 508                 return cmd_status(sk, MGMT_OP_SET_PAIRABLE, ENODEV);
 509
 510         hci_dev_lock_bh(hdev);
 511
 512         if (cp->val)
 513                 set_bit(HCI_PAIRABLE, &hdev->flags);
 514         else
 515                 clear_bit(HCI_PAIRABLE, &hdev->flags);
 516
 517         err = send_mode_rsp(sk, MGMT_OP_SET_PAIRABLE, dev_id, cp->val);
 518         if (err < 0)
 519                 goto failed;
 520
 521         put_unaligned_le16(dev_id, &ev.index);
 522         ev.val = cp->val;
 523
 524         err = mgmt_event(MGMT_EV_PAIRABLE, &ev, sizeof(ev), sk);
 525
 526 failed:
 527         hci_dev_unlock_bh(hdev);
 528         hci_dev_put(hdev);
 529
 530         return err;
 531 }
 532
 533 static u8 get_service_classes(struct hci_dev *hdev)
 534 {
 535         struct list_head *p;
 536         u8 val = 0;
 537
 538         list_for_each(p, &hdev->uuids) {
 539                 struct bt_uuid *uuid = list_entry(p, struct bt_uuid, list);
 540
 541                 val |= uuid->svc_hint;
 542         }
 543
 544         return val;
 545 }
 546
 547 static int update_class(struct hci_dev *hdev)
 548 {
 549         u8 cod[3];
 550
 551         BT_DBG("%s", hdev->name);
 552
 553         if (test_bit(HCI_SERVICE_CACHE, &hdev->flags))
 554                 return 0;
 555
 556         cod[0] = hdev->minor_class;
 557         cod[1] = hdev->major_class;
 558         cod[2] = get_service_classes(hdev);
 559
 560         if (memcmp(cod, hdev->dev_class, 3) == 0)
 561                 return 0;
 562
 563         return hci_send_cmd(hdev, HCI_OP_WRITE_CLASS_OF_DEV, sizeof(cod), cod);
 564 }
 565
 566 static int add_uuid(struct sock *sk, unsigned char *data, u16 len)
 567 {
 568         struct mgmt_cp_add_uuid *cp;
 569         struct hci_dev *hdev;
 570         struct bt_uuid *uuid;
 571         u16 dev_id;
 572         int err;
 573
 574         cp = (void *) data;
 575         dev_id = get_unaligned_le16(&cp->index);
 576
 577         BT_DBG("request for hci%u", dev_id);
 578
 579         hdev = hci_dev_get(dev_id);
 580         if (!hdev)
 581                 return cmd_status(sk, MGMT_OP_ADD_UUID, ENODEV);
 582
 583         hci_dev_lock_bh(hdev);
 584
 585         uuid = kmalloc(sizeof(*uuid), GFP_ATOMIC);
 586         if (!uuid) {
 587                 err = -ENOMEM;
 588                 goto failed;
 589         }
 590
 591         memcpy(uuid->uuid, cp->uuid, 16);
 592         uuid->svc_hint = cp->svc_hint;
 593
 594         list_add(&uuid->list, &hdev->uuids);
 595
 596         err = update_class(hdev);
 597         if (err < 0)
 598                 goto failed;
 599
 600         err = cmd_complete(sk, MGMT_OP_ADD_UUID, &dev_id, sizeof(dev_id));
 601
 602 failed:
 603         hci_dev_unlock_bh(hdev);
 604         hci_dev_put(hdev);
 605
 606         return err;
 607 }
 608
 609 static int remove_uuid(struct sock *sk, unsigned char *data, u16 len)
 610 {
 611         struct list_head *p, *n;
 612         struct mgmt_cp_add_uuid *cp;
 613         struct hci_dev *hdev;
 614         u8 bt_uuid_any[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
 615         u16 dev_id;
 616         int err, found;
 617
 618         cp = (void *) data;
 619         dev_id = get_unaligned_le16(&cp->index);
 620
 621         BT_DBG("request for hci%u", dev_id);
 622
 623         hdev = hci_dev_get(dev_id);
 624         if (!hdev)
 625                 return cmd_status(sk, MGMT_OP_REMOVE_UUID, ENODEV);
 626
 627         hci_dev_lock_bh(hdev);
 628
 629         if (memcmp(cp->uuid, bt_uuid_any, 16) == 0) {
 630                 err = hci_uuids_clear(hdev);
 631                 goto unlock;
 632         }
 633
 634         found = 0;
 635
 636         list_for_each_safe(p, n, &hdev->uuids) {
 637                 struct bt_uuid *match = list_entry(p, struct bt_uuid, list);
 638
 639                 if (memcmp(match->uuid, cp->uuid, 16) != 0)
 640                         continue;
 641
 642                 list_del(&match->list);
 643                 found++;
 644         }
 645
 646         if (found == 0) {
 647                 err = cmd_status(sk, MGMT_OP_REMOVE_UUID, ENOENT);
 648                 goto unlock;
 649         }
 650
 651         err = update_class(hdev);
 652         if (err < 0)
 653                 goto unlock;
 654
 655         err = cmd_complete(sk, MGMT_OP_REMOVE_UUID, &dev_id, sizeof(dev_id));
 656
 657 unlock:
 658         hci_dev_unlock_bh(hdev);
 659         hci_dev_put(hdev);
 660
 661         return err;
 662 }
 663
 664 static int set_dev_class(struct sock *sk, unsigned char *data, u16 len)
 665 {
 666         struct hci_dev *hdev;
 667         struct mgmt_cp_set_dev_class *cp;
 668         u16 dev_id;
 669         int err;
 670
 671         cp = (void *) data;
 672         dev_id = get_unaligned_le16(&cp->index);
 673
 674         BT_DBG("request for hci%u", dev_id);
 675
 676         hdev = hci_dev_get(dev_id);
 677         if (!hdev)
 678                 return cmd_status(sk, MGMT_OP_SET_DEV_CLASS, ENODEV);
 679
 680         hci_dev_lock_bh(hdev);
 681
 682         hdev->major_class = cp->major;
 683         hdev->minor_class = cp->minor;
 684
 685         err = update_class(hdev);
 686
 687         if (err == 0)
 688                 err = cmd_complete(sk, MGMT_OP_SET_DEV_CLASS, &dev_id,
 689                                                         sizeof(dev_id));
 690
 691         hci_dev_unlock_bh(hdev);
 692         hci_dev_put(hdev);
 693
 694         return err;
 695 }
 696
 697 static int set_service_cache(struct sock *sk, unsigned char *data, u16 len)
 698 {
 699         struct hci_dev *hdev;
 700         struct mgmt_cp_set_service_cache *cp;
 701         u16 dev_id;
 702         int err;
 703
 704         cp = (void *) data;
 705         dev_id = get_unaligned_le16(&cp->index);
 706
 707         hdev = hci_dev_get(dev_id);
 708         if (!hdev)
 709                 return cmd_status(sk, MGMT_OP_SET_SERVICE_CACHE, ENODEV);
 710
 711         hci_dev_lock_bh(hdev);
 712
 713         BT_DBG("hci%u enable %d", dev_id, cp->enable);
 714
 715         if (cp->enable) {
 716                 set_bit(HCI_SERVICE_CACHE, &hdev->flags);
 717                 err = 0;
 718         } else {
 719                 clear_bit(HCI_SERVICE_CACHE, &hdev->flags);
 720                 err = update_class(hdev);
 721         }
 722
 723         if (err == 0)
 724                 err = cmd_complete(sk, MGMT_OP_SET_SERVICE_CACHE, &dev_id,
 725                                                         sizeof(dev_id));
 726
 727         hci_dev_unlock_bh(hdev);
 728         hci_dev_put(hdev);
 729
 730         return err;
 731 }
 732
 733 static int load_keys(struct sock *sk, unsigned char *data, u16 len)
 734 {
 735         struct hci_dev *hdev;
 736         struct mgmt_cp_load_keys *cp;
 737         u16 dev_id, key_count, expected_len;
 738         int i;
 739
 740         cp = (void *) data;
 741         dev_id = get_unaligned_le16(&cp->index);
 742         key_count = get_unaligned_le16(&cp->key_count);
 743
 744         expected_len = sizeof(*cp) + key_count * sizeof(struct mgmt_key_info);
 745         if (expected_len != len) {
 746                 BT_ERR("load_keys: expected %u bytes, got %u bytes",
 747                                                         len, expected_len);
 748                 return -EINVAL;
 749         }
 750
 751         hdev = hci_dev_get(dev_id);
 752         if (!hdev)
 753                 return cmd_status(sk, MGMT_OP_LOAD_KEYS, ENODEV);
 754
 755         BT_DBG("hci%u debug_keys %u key_count %u", dev_id, cp->debug_keys,
 756                                                                 key_count);
 757
 758         hci_dev_lock_bh(hdev);
 759
 760         hci_link_keys_clear(hdev);
 761
 762         set_bit(HCI_LINK_KEYS, &hdev->flags);
 763
 764         if (cp->debug_keys)
 765                 set_bit(HCI_DEBUG_KEYS, &hdev->flags);
 766         else
 767                 clear_bit(HCI_DEBUG_KEYS, &hdev->flags);
 768
 769         for (i = 0; i < key_count; i++) {
 770                 struct mgmt_key_info *key = &cp->keys[i];
 771
 772                 hci_add_link_key(hdev, 0, &key->bdaddr, key->val, key->type,
 773                                                                 key->pin_len);
 774         }
 775
 776         hci_dev_unlock_bh(hdev);
 777         hci_dev_put(hdev);
 778
 779         return 0;
 780 }
 781
 782 static int remove_key(struct sock *sk, unsigned char *data, u16 len)
 783 {
 784         struct hci_dev *hdev;
 785         struct mgmt_cp_remove_key *cp;
 786         struct hci_conn *conn;
 787         u16 dev_id;
 788         int err;
 789
 790         cp = (void *) data;
 791         dev_id = get_unaligned_le16(&cp->index);
 792
 793         hdev = hci_dev_get(dev_id);
 794         if (!hdev)
 795                 return cmd_status(sk, MGMT_OP_REMOVE_KEY, ENODEV);
 796
 797         hci_dev_lock_bh(hdev);
 798
 799         err = hci_remove_link_key(hdev, &cp->bdaddr);
 800         if (err < 0) {
 801                 err = cmd_status(sk, MGMT_OP_REMOVE_KEY, -err);
 802                 goto unlock;
 803         }
 804
 805         err = 0;
 806
 807         if (!test_bit(HCI_UP, &hdev->flags) || !cp->disconnect)
 808                 goto unlock;
 809
 810         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
 811         if (conn) {
 812                 struct hci_cp_disconnect dc;
 813
 814                 put_unaligned_le16(conn->handle, &dc.handle);
 815                 dc.reason = 0x13; /* Remote User Terminated Connection */
 816                 err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, 0, NULL);
 817         }
 818
 819 unlock:
 820         hci_dev_unlock_bh(hdev);
 821         hci_dev_put(hdev);
 822
 823         return err;
 824 }
 825
 826 static int disconnect(struct sock *sk, unsigned char *data, u16 len)
 827 {
 828         struct hci_dev *hdev;
 829         struct mgmt_cp_disconnect *cp;
 830         struct hci_cp_disconnect dc;
 831         struct hci_conn *conn;
 832         u16 dev_id;
 833         int err;
 834
 835         BT_DBG("");
 836
 837         cp = (void *) data;
 838         dev_id = get_unaligned_le16(&cp->index);
 839
 840         hdev = hci_dev_get(dev_id);
 841         if (!hdev)
 842                 return cmd_status(sk, MGMT_OP_DISCONNECT, ENODEV);
 843
 844         hci_dev_lock_bh(hdev);
 845
 846         if (!test_bit(HCI_UP, &hdev->flags)) {
 847                 err = cmd_status(sk, MGMT_OP_DISCONNECT, ENETDOWN);
 848                 goto failed;
 849         }
 850
 851         if (mgmt_pending_find(MGMT_OP_DISCONNECT, dev_id)) {
 852                 err = cmd_status(sk, MGMT_OP_DISCONNECT, EBUSY);
 853                 goto failed;
 854         }
 855
 856         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
 857         if (!conn) {
 858                 err = cmd_status(sk, MGMT_OP_DISCONNECT, ENOTCONN);
 859                 goto failed;
 860         }
 861
 862         err = mgmt_pending_add(sk, MGMT_OP_DISCONNECT, dev_id, data, len);
 863         if (err < 0)
 864                 goto failed;
 865
 866         put_unaligned_le16(conn->handle, &dc.handle);
 867         dc.reason = 0x13; /* Remote User Terminated Connection */
 868
 869         err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc);
 870         if (err < 0)
 871                 mgmt_pending_remove(MGMT_OP_DISCONNECT, dev_id);
 872
 873 failed:
 874         hci_dev_unlock_bh(hdev);
 875         hci_dev_put(hdev);
 876
 877         return err;
 878 }
 879
 880 static int get_connections(struct sock *sk, unsigned char *data, u16 len)
 881 {
 882         struct mgmt_cp_get_connections *cp;
 883         struct mgmt_rp_get_connections *rp;
 884         struct hci_dev *hdev;
 885         struct list_head *p;
 886         size_t rp_len;
 887         u16 dev_id, count;
 888         int i, err;
 889
 890         BT_DBG("");
 891
 892         cp = (void *) data;
 893         dev_id = get_unaligned_le16(&cp->index);
 894
 895         hdev = hci_dev_get(dev_id);
 896         if (!hdev)
 897                 return cmd_status(sk, MGMT_OP_GET_CONNECTIONS, ENODEV);
 898
 899         hci_dev_lock_bh(hdev);
 900
 901         count = 0;
 902         list_for_each(p, &hdev->conn_hash.list) {
 903                 count++;
 904         }
 905
 906         rp_len = sizeof(*rp) + (count * sizeof(bdaddr_t));
 907         rp = kmalloc(rp_len, GFP_ATOMIC);
 908         if (!rp) {
 909                 err = -ENOMEM;
 910                 goto unlock;
 911         }
 912
 913         put_unaligned_le16(dev_id, &rp->index);
 914         put_unaligned_le16(count, &rp->conn_count);
 915
 916         read_lock(&hci_dev_list_lock);
 917
 918         i = 0;
 919         list_for_each(p, &hdev->conn_hash.list) {
 920                 struct hci_conn *c = list_entry(p, struct hci_conn, list);
 921
 922                 bacpy(&rp->conn[i++], &c->dst);
 923         }
 924
 925         read_unlock(&hci_dev_list_lock);
 926
 927         err = cmd_complete(sk, MGMT_OP_GET_CONNECTIONS, rp, rp_len);
 928
 929 unlock:
 930         kfree(rp);
 931         hci_dev_unlock_bh(hdev);
 932         hci_dev_put(hdev);
 933         return err;
 934 }
 935
 936 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
 937 {
 938         unsigned char *buf;
 939         struct mgmt_hdr *hdr;
 940         u16 opcode, len;
 941         int err;
 942
 943         BT_DBG("got %zu bytes", msglen);
 944
 945         if (msglen < sizeof(*hdr))
 946                 return -EINVAL;
 947
 948         buf = kmalloc(msglen, GFP_ATOMIC);
 949         if (!buf)
 950                 return -ENOMEM;
 951
 952         if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
 953                 err = -EFAULT;
 954                 goto done;
 955         }
 956
 957         hdr = (struct mgmt_hdr *) buf;
 958         opcode = get_unaligned_le16(&hdr->opcode);
 959         len = get_unaligned_le16(&hdr->len);
 960
 961         if (len != msglen - sizeof(*hdr)) {
 962                 err = -EINVAL;
 963                 goto done;
 964         }
 965
 966         switch (opcode) {
 967         case MGMT_OP_READ_VERSION:
 968                 err = read_version(sk);
 969                 break;
 970         case MGMT_OP_READ_INDEX_LIST:
 971                 err = read_index_list(sk);
 972                 break;
 973         case MGMT_OP_READ_INFO:
 974                 err = read_controller_info(sk, buf + sizeof(*hdr), len);
 975                 break;
 976         case MGMT_OP_SET_POWERED:
 977                 err = set_powered(sk, buf + sizeof(*hdr), len);
 978                 break;
 979         case MGMT_OP_SET_DISCOVERABLE:
 980                 err = set_discoverable(sk, buf + sizeof(*hdr), len);
 981                 break;
 982         case MGMT_OP_SET_CONNECTABLE:
 983                 err = set_connectable(sk, buf + sizeof(*hdr), len);
 984                 break;
 985         case MGMT_OP_SET_PAIRABLE:
 986                 err = set_pairable(sk, buf + sizeof(*hdr), len);
 987                 break;
 988         case MGMT_OP_ADD_UUID:
 989                 err = add_uuid(sk, buf + sizeof(*hdr), len);
 990                 break;
 991         case MGMT_OP_REMOVE_UUID:
 992                 err = remove_uuid(sk, buf + sizeof(*hdr), len);
 993                 break;
 994         case MGMT_OP_SET_DEV_CLASS:
 995                 err = set_dev_class(sk, buf + sizeof(*hdr), len);
 996                 break;
 997         case MGMT_OP_SET_SERVICE_CACHE:
 998                 err = set_service_cache(sk, buf + sizeof(*hdr), len);
 999                 break;
1000         case MGMT_OP_LOAD_KEYS:
1001                 err = load_keys(sk, buf + sizeof(*hdr), len);
1002                 break;
1003         case MGMT_OP_REMOVE_KEY:
1004                 err = remove_key(sk, buf + sizeof(*hdr), len);
1005                 break;
1006         case MGMT_OP_DISCONNECT:
1007                 err = disconnect(sk, buf + sizeof(*hdr), len);
1008                 break;
1009         case MGMT_OP_GET_CONNECTIONS:
1010                 err = get_connections(sk, buf + sizeof(*hdr), len);
1011                 break;
1012         default:
1013                 BT_DBG("Unknown op %u", opcode);
1014                 err = cmd_status(sk, opcode, 0x01);
1015                 break;
1016         }
1017
1018         if (err < 0)
1019                 goto done;
1020
1021         err = msglen;
1022
1023 done:
1024         kfree(buf);
1025         return err;
1026 }
1027
1028 int mgmt_index_added(u16 index)
1029 {
1030         struct mgmt_ev_index_added ev;
1031
1032         put_unaligned_le16(index, &ev.index);
1033
1034         return mgmt_event(MGMT_EV_INDEX_ADDED, &ev, sizeof(ev), NULL);
1035 }
1036
1037 int mgmt_index_removed(u16 index)
1038 {
1039         struct mgmt_ev_index_added ev;
1040
1041         put_unaligned_le16(index, &ev.index);
1042
1043         return mgmt_event(MGMT_EV_INDEX_REMOVED, &ev, sizeof(ev), NULL);
1044 }
1045
1046 struct cmd_lookup {
1047         u8 val;
1048         struct sock *sk;
1049 };
1050
1051 static void mode_rsp(struct pending_cmd *cmd, void *data)
1052 {
1053         struct mgmt_mode *cp = cmd->cmd;
1054         struct cmd_lookup *match = data;
1055
1056         if (cp->val != match->val)
1057                 return;
1058
1059         send_mode_rsp(cmd->sk, cmd->opcode, cmd->index, cp->val);
1060
1061         list_del(&cmd->list);
1062
1063         if (match->sk == NULL) {
1064                 match->sk = cmd->sk;
1065                 sock_hold(match->sk);
1066         }
1067
1068         mgmt_pending_free(cmd);
1069 }
1070
1071 int mgmt_powered(u16 index, u8 powered)
1072 {
1073         struct mgmt_mode ev;
1074         struct cmd_lookup match = { powered, NULL };
1075         int ret;
1076
1077         mgmt_pending_foreach(MGMT_OP_SET_POWERED, index, mode_rsp, &match);
1078
1079         put_unaligned_le16(index, &ev.index);
1080         ev.val = powered;
1081
1082         ret = mgmt_event(MGMT_EV_POWERED, &ev, sizeof(ev), match.sk);
1083
1084         if (match.sk)
1085                 sock_put(match.sk);
1086
1087         return ret;
1088 }
1089
1090 int mgmt_discoverable(u16 index, u8 discoverable)
1091 {
1092         struct mgmt_mode ev;
1093         struct cmd_lookup match = { discoverable, NULL };
1094         int ret;
1095
1096         mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, index,
1097                                                         mode_rsp, &match);
1098
1099         put_unaligned_le16(index, &ev.index);
1100         ev.val = discoverable;
1101
1102         ret = mgmt_event(MGMT_EV_DISCOVERABLE, &ev, sizeof(ev), match.sk);
1103
1104         if (match.sk)
1105                 sock_put(match.sk);
1106
1107         return ret;
1108 }
1109
1110 int mgmt_connectable(u16 index, u8 connectable)
1111 {
1112         struct mgmt_mode ev;
1113         struct cmd_lookup match = { connectable, NULL };
1114         int ret;
1115
1116         mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, index, mode_rsp, &match);
1117
1118         put_unaligned_le16(index, &ev.index);
1119         ev.val = connectable;
1120
1121         ret = mgmt_event(MGMT_EV_CONNECTABLE, &ev, sizeof(ev), match.sk);
1122
1123         if (match.sk)
1124                 sock_put(match.sk);
1125
1126         return ret;
1127 }
1128
1129 int mgmt_new_key(u16 index, struct link_key *key, u8 old_key_type)
1130 {
1131         struct mgmt_ev_new_key ev;
1132
1133         memset(&ev, 0, sizeof(ev));
1134
1135         put_unaligned_le16(index, &ev.index);
1136
1137         bacpy(&ev.key.bdaddr, &key->bdaddr);
1138         ev.key.type = key->type;
1139         memcpy(ev.key.val, key->val, 16);
1140         ev.key.pin_len = key->pin_len;
1141         ev.old_key_type = old_key_type;
1142
1143         return mgmt_event(MGMT_EV_NEW_KEY, &ev, sizeof(ev), NULL);
1144 }
1145
1146 int mgmt_connected(u16 index, bdaddr_t *bdaddr)
1147 {
1148         struct mgmt_ev_connected ev;
1149
1150         put_unaligned_le16(index, &ev.index);
1151         bacpy(&ev.bdaddr, bdaddr);
1152
1153         return mgmt_event(MGMT_EV_CONNECTED, &ev, sizeof(ev), NULL);
1154 }
1155
1156 static void disconnect_rsp(struct pending_cmd *cmd, void *data)
1157 {
1158         struct mgmt_cp_disconnect *cp = cmd->cmd;
1159         struct sock **sk = data;
1160         struct mgmt_rp_disconnect rp;
1161
1162         put_unaligned_le16(cmd->index, &rp.index);
1163         bacpy(&rp.bdaddr, &cp->bdaddr);
1164
1165         cmd_complete(cmd->sk, MGMT_OP_DISCONNECT, &rp, sizeof(rp));
1166
1167         *sk = cmd->sk;
1168         sock_hold(*sk);
1169
1170         list_del(&cmd->list);
1171         mgmt_pending_free(cmd);
1172 }
1173
1174 int mgmt_disconnected(u16 index, bdaddr_t *bdaddr)
1175 {
1176         struct mgmt_ev_disconnected ev;
1177         struct sock *sk = NULL;
1178         int err;
1179
1180         mgmt_pending_foreach(MGMT_OP_DISCONNECT, index, disconnect_rsp, &sk);
1181
1182         put_unaligned_le16(index, &ev.index);
1183         bacpy(&ev.bdaddr, bdaddr);
1184
1185         err = mgmt_event(MGMT_EV_DISCONNECTED, &ev, sizeof(ev), sk);
1186
1187         if (sk)
1188                 sock_put(sk);
1189
1190         return err;
1191 }
1192
1193 int mgmt_disconnect_failed(u16 index)
1194 {
1195         struct pending_cmd *cmd;
1196         int err;
1197
1198         cmd = mgmt_pending_find(MGMT_OP_DISCONNECT, index);
1199         if (!cmd)
1200                 return -ENOENT;
1201
1202         err = cmd_status(cmd->sk, MGMT_OP_DISCONNECT, EIO);
1203
1204         list_del(&cmd->list);
1205         mgmt_pending_free(cmd);
1206
1207         return err;
1208 }
1209
1210 int mgmt_connect_failed(u16 index, bdaddr_t *bdaddr, u8 status)
1211 {
1212         struct mgmt_ev_connect_failed ev;
1213
1214         put_unaligned_le16(index, &ev.index);
1215         bacpy(&ev.bdaddr, bdaddr);
1216         ev.status = status;
1217
1218         return mgmt_event(MGMT_EV_CONNECT_FAILED, &ev, sizeof(ev), NULL);
1219 }