Pileus Git - ~andy/linux/blob - 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 <linux/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 #define INQUIRY_LEN_BREDR 0x08 /* TGAP(100) */
  36
  37 struct pending_cmd {
  38         struct list_head list;
  39         u16 opcode;
  40         int index;
  41         void *param;
  42         struct sock *sk;
  43         void *user_data;
  44 };
  45
  46 static int cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
  47 {
  48         struct sk_buff *skb;
  49         struct mgmt_hdr *hdr;
  50         struct mgmt_ev_cmd_status *ev;
  51         int err;
  52
  53         BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
  54
  55         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_ATOMIC);
  56         if (!skb)
  57                 return -ENOMEM;
  58
  59         hdr = (void *) skb_put(skb, sizeof(*hdr));
  60
  61         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
  62         hdr->index = cpu_to_le16(index);
  63         hdr->len = cpu_to_le16(sizeof(*ev));
  64
  65         ev = (void *) skb_put(skb, sizeof(*ev));
  66         ev->status = status;
  67         put_unaligned_le16(cmd, &ev->opcode);
  68
  69         err = sock_queue_rcv_skb(sk, skb);
  70         if (err < 0)
  71                 kfree_skb(skb);
  72
  73         return err;
  74 }
  75
  76 static int cmd_complete(struct sock *sk, u16 index, u16 cmd, void *rp,
  77                                                                 size_t rp_len)
  78 {
  79         struct sk_buff *skb;
  80         struct mgmt_hdr *hdr;
  81         struct mgmt_ev_cmd_complete *ev;
  82         int err;
  83
  84         BT_DBG("sock %p", sk);
  85
  86         skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_ATOMIC);
  87         if (!skb)
  88                 return -ENOMEM;
  89
  90         hdr = (void *) skb_put(skb, sizeof(*hdr));
  91
  92         hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
  93         hdr->index = cpu_to_le16(index);
  94         hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
  95
  96         ev = (void *) skb_put(skb, sizeof(*ev) + rp_len);
  97         put_unaligned_le16(cmd, &ev->opcode);
  98
  99         if (rp)
 100                 memcpy(ev->data, rp, rp_len);
 101
 102         err = sock_queue_rcv_skb(sk, skb);
 103         if (err < 0)
 104                 kfree_skb(skb);
 105
 106         return err;;
 107 }
 108
 109 static int read_version(struct sock *sk)
 110 {
 111         struct mgmt_rp_read_version rp;
 112
 113         BT_DBG("sock %p", sk);
 114
 115         rp.version = MGMT_VERSION;
 116         put_unaligned_le16(MGMT_REVISION, &rp.revision);
 117
 118         return cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_VERSION, &rp,
 119                                                                 sizeof(rp));
 120 }
 121
 122 static int read_index_list(struct sock *sk)
 123 {
 124         struct mgmt_rp_read_index_list *rp;
 125         struct list_head *p;
 126         struct hci_dev *d;
 127         size_t rp_len;
 128         u16 count;
 129         int i, err;
 130
 131         BT_DBG("sock %p", sk);
 132
 133         read_lock(&hci_dev_list_lock);
 134
 135         count = 0;
 136         list_for_each(p, &hci_dev_list) {
 137                 count++;
 138         }
 139
 140         rp_len = sizeof(*rp) + (2 * count);
 141         rp = kmalloc(rp_len, GFP_ATOMIC);
 142         if (!rp) {
 143                 read_unlock(&hci_dev_list_lock);
 144                 return -ENOMEM;
 145         }
 146
 147         put_unaligned_le16(count, &rp->num_controllers);
 148
 149         i = 0;
 150         list_for_each_entry(d, &hci_dev_list, list) {
 151                 if (test_and_clear_bit(HCI_AUTO_OFF, &d->flags))
 152                         cancel_delayed_work(&d->power_off);
 153
 154                 if (test_bit(HCI_SETUP, &d->flags))
 155                         continue;
 156
 157                 put_unaligned_le16(d->id, &rp->index[i++]);
 158                 BT_DBG("Added hci%u", d->id);
 159         }
 160
 161         read_unlock(&hci_dev_list_lock);
 162
 163         err = cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_INDEX_LIST, rp,
 164                                                                         rp_len);
 165
 166         kfree(rp);
 167
 168         return err;
 169 }
 170
 171 static int read_controller_info(struct sock *sk, u16 index)
 172 {
 173         struct mgmt_rp_read_info rp;
 174         struct hci_dev *hdev;
 175
 176         BT_DBG("sock %p hci%u", sk, index);
 177
 178         hdev = hci_dev_get(index);
 179         if (!hdev)
 180                 return cmd_status(sk, index, MGMT_OP_READ_INFO, ENODEV);
 181
 182         if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->flags))
 183                 cancel_delayed_work_sync(&hdev->power_off);
 184
 185         hci_dev_lock_bh(hdev);
 186
 187         set_bit(HCI_MGMT, &hdev->flags);
 188
 189         memset(&rp, 0, sizeof(rp));
 190
 191         rp.type = hdev->dev_type;
 192
 193         rp.powered = test_bit(HCI_UP, &hdev->flags);
 194         rp.connectable = test_bit(HCI_PSCAN, &hdev->flags);
 195         rp.discoverable = test_bit(HCI_ISCAN, &hdev->flags);
 196         rp.pairable = test_bit(HCI_PSCAN, &hdev->flags);
 197
 198         if (test_bit(HCI_AUTH, &hdev->flags))
 199                 rp.sec_mode = 3;
 200         else if (hdev->ssp_mode > 0)
 201                 rp.sec_mode = 4;
 202         else
 203                 rp.sec_mode = 2;
 204
 205         bacpy(&rp.bdaddr, &hdev->bdaddr);
 206         memcpy(rp.features, hdev->features, 8);
 207         memcpy(rp.dev_class, hdev->dev_class, 3);
 208         put_unaligned_le16(hdev->manufacturer, &rp.manufacturer);
 209         rp.hci_ver = hdev->hci_ver;
 210         put_unaligned_le16(hdev->hci_rev, &rp.hci_rev);
 211
 212         memcpy(rp.name, hdev->dev_name, sizeof(hdev->dev_name));
 213
 214         hci_dev_unlock_bh(hdev);
 215         hci_dev_put(hdev);
 216
 217         return cmd_complete(sk, index, MGMT_OP_READ_INFO, &rp, sizeof(rp));
 218 }
 219
 220 static void mgmt_pending_free(struct pending_cmd *cmd)
 221 {
 222         sock_put(cmd->sk);
 223         kfree(cmd->param);
 224         kfree(cmd);
 225 }
 226
 227 static struct pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
 228                                                         struct hci_dev *hdev,
 229                                                         void *data, u16 len)
 230 {
 231         struct pending_cmd *cmd;
 232
 233         cmd = kmalloc(sizeof(*cmd), GFP_ATOMIC);
 234         if (!cmd)
 235                 return NULL;
 236
 237         cmd->opcode = opcode;
 238         cmd->index = hdev->id;
 239
 240         cmd->param = kmalloc(len, GFP_ATOMIC);
 241         if (!cmd->param) {
 242                 kfree(cmd);
 243                 return NULL;
 244         }
 245
 246         if (data)
 247                 memcpy(cmd->param, data, len);
 248
 249         cmd->sk = sk;
 250         sock_hold(sk);
 251
 252         list_add(&cmd->list, &hdev->mgmt_pending);
 253
 254         return cmd;
 255 }
 256
 257 static void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
 258                                 void (*cb)(struct pending_cmd *cmd, void *data),
 259                                 void *data)
 260 {
 261         struct list_head *p, *n;
 262
 263         list_for_each_safe(p, n, &hdev->mgmt_pending) {
 264                 struct pending_cmd *cmd;
 265
 266                 cmd = list_entry(p, struct pending_cmd, list);
 267
 268                 if (opcode > 0 && cmd->opcode != opcode)
 269                         continue;
 270
 271                 cb(cmd, data);
 272         }
 273 }
 274
 275 static struct pending_cmd *mgmt_pending_find(u16 opcode, struct hci_dev *hdev)
 276 {
 277         struct pending_cmd *cmd;
 278
 279         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
 280                 if (cmd->opcode == opcode)
 281                         return cmd;
 282         }
 283
 284         return NULL;
 285 }
 286
 287 static void mgmt_pending_remove(struct pending_cmd *cmd)
 288 {
 289         list_del(&cmd->list);
 290         mgmt_pending_free(cmd);
 291 }
 292
 293 static int set_powered(struct sock *sk, u16 index, unsigned char *data, u16 len)
 294 {
 295         struct mgmt_mode *cp;
 296         struct hci_dev *hdev;
 297         struct pending_cmd *cmd;
 298         int err, up;
 299
 300         cp = (void *) data;
 301
 302         BT_DBG("request for hci%u", index);
 303
 304         if (len != sizeof(*cp))
 305                 return cmd_status(sk, index, MGMT_OP_SET_POWERED, EINVAL);
 306
 307         hdev = hci_dev_get(index);
 308         if (!hdev)
 309                 return cmd_status(sk, index, MGMT_OP_SET_POWERED, ENODEV);
 310
 311         hci_dev_lock_bh(hdev);
 312
 313         up = test_bit(HCI_UP, &hdev->flags);
 314         if ((cp->val && up) || (!cp->val && !up)) {
 315                 err = cmd_status(sk, index, MGMT_OP_SET_POWERED, EALREADY);
 316                 goto failed;
 317         }
 318
 319         if (mgmt_pending_find(MGMT_OP_SET_POWERED, hdev)) {
 320                 err = cmd_status(sk, index, MGMT_OP_SET_POWERED, EBUSY);
 321                 goto failed;
 322         }
 323
 324         cmd = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, hdev, data, len);
 325         if (!cmd) {
 326                 err = -ENOMEM;
 327                 goto failed;
 328         }
 329
 330         if (cp->val)
 331                 queue_work(hdev->workqueue, &hdev->power_on);
 332         else
 333                 queue_work(hdev->workqueue, &hdev->power_off.work);
 334
 335         err = 0;
 336
 337 failed:
 338         hci_dev_unlock_bh(hdev);
 339         hci_dev_put(hdev);
 340         return err;
 341 }
 342
 343 static int set_discoverable(struct sock *sk, u16 index, unsigned char *data,
 344                                                                         u16 len)
 345 {
 346         struct mgmt_cp_set_discoverable *cp;
 347         struct hci_dev *hdev;
 348         struct pending_cmd *cmd;
 349         u8 scan;
 350         int err;
 351
 352         cp = (void *) data;
 353
 354         BT_DBG("request for hci%u", index);
 355
 356         if (len != sizeof(*cp))
 357                 return cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, EINVAL);
 358
 359         hdev = hci_dev_get(index);
 360         if (!hdev)
 361                 return cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, ENODEV);
 362
 363         hci_dev_lock_bh(hdev);
 364
 365         if (!test_bit(HCI_UP, &hdev->flags)) {
 366                 err = cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, ENETDOWN);
 367                 goto failed;
 368         }
 369
 370         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) ||
 371                         mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) {
 372                 err = cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, EBUSY);
 373                 goto failed;
 374         }
 375
 376         if (cp->val == test_bit(HCI_ISCAN, &hdev->flags) &&
 377                                         test_bit(HCI_PSCAN, &hdev->flags)) {
 378                 err = cmd_status(sk, index, MGMT_OP_SET_DISCOVERABLE, EALREADY);
 379                 goto failed;
 380         }
 381
 382         cmd = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, hdev, data, len);
 383         if (!cmd) {
 384                 err = -ENOMEM;
 385                 goto failed;
 386         }
 387
 388         scan = SCAN_PAGE;
 389
 390         if (cp->val)
 391                 scan |= SCAN_INQUIRY;
 392         else
 393                 cancel_delayed_work(&hdev->discov_off);
 394
 395         err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 396         if (err < 0)
 397                 mgmt_pending_remove(cmd);
 398
 399         if (cp->val)
 400                 hdev->discov_timeout = get_unaligned_le16(&cp->timeout);
 401
 402 failed:
 403         hci_dev_unlock_bh(hdev);
 404         hci_dev_put(hdev);
 405
 406         return err;
 407 }
 408
 409 static int set_connectable(struct sock *sk, u16 index, unsigned char *data,
 410                                                                         u16 len)
 411 {
 412         struct mgmt_mode *cp;
 413         struct hci_dev *hdev;
 414         struct pending_cmd *cmd;
 415         u8 scan;
 416         int err;
 417
 418         cp = (void *) data;
 419
 420         BT_DBG("request for hci%u", index);
 421
 422         if (len != sizeof(*cp))
 423                 return cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, EINVAL);
 424
 425         hdev = hci_dev_get(index);
 426         if (!hdev)
 427                 return cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, ENODEV);
 428
 429         hci_dev_lock_bh(hdev);
 430
 431         if (!test_bit(HCI_UP, &hdev->flags)) {
 432                 err = cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, ENETDOWN);
 433                 goto failed;
 434         }
 435
 436         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) ||
 437                         mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) {
 438                 err = cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, EBUSY);
 439                 goto failed;
 440         }
 441
 442         if (cp->val == test_bit(HCI_PSCAN, &hdev->flags)) {
 443                 err = cmd_status(sk, index, MGMT_OP_SET_CONNECTABLE, EALREADY);
 444                 goto failed;
 445         }
 446
 447         cmd = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, hdev, data, len);
 448         if (!cmd) {
 449                 err = -ENOMEM;
 450                 goto failed;
 451         }
 452
 453         if (cp->val)
 454                 scan = SCAN_PAGE;
 455         else
 456                 scan = 0;
 457
 458         err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
 459         if (err < 0)
 460                 mgmt_pending_remove(cmd);
 461
 462 failed:
 463         hci_dev_unlock_bh(hdev);
 464         hci_dev_put(hdev);
 465
 466         return err;
 467 }
 468
 469 static int mgmt_event(u16 event, struct hci_dev *hdev, void *data,
 470                                         u16 data_len, struct sock *skip_sk)
 471 {
 472         struct sk_buff *skb;
 473         struct mgmt_hdr *hdr;
 474
 475         skb = alloc_skb(sizeof(*hdr) + data_len, GFP_ATOMIC);
 476         if (!skb)
 477                 return -ENOMEM;
 478
 479         bt_cb(skb)->channel = HCI_CHANNEL_CONTROL;
 480
 481         hdr = (void *) skb_put(skb, sizeof(*hdr));
 482         hdr->opcode = cpu_to_le16(event);
 483         if (hdev)
 484                 hdr->index = cpu_to_le16(hdev->id);
 485         else
 486                 hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
 487         hdr->len = cpu_to_le16(data_len);
 488
 489         if (data)
 490                 memcpy(skb_put(skb, data_len), data, data_len);
 491
 492         hci_send_to_sock(NULL, skb, skip_sk);
 493         kfree_skb(skb);
 494
 495         return 0;
 496 }
 497
 498 static int send_mode_rsp(struct sock *sk, u16 opcode, u16 index, u8 val)
 499 {
 500         struct mgmt_mode rp;
 501
 502         rp.val = val;
 503
 504         return cmd_complete(sk, index, opcode, &rp, sizeof(rp));
 505 }
 506
 507 static int set_pairable(struct sock *sk, u16 index, unsigned char *data,
 508                                                                         u16 len)
 509 {
 510         struct mgmt_mode *cp, ev;
 511         struct hci_dev *hdev;
 512         int err;
 513
 514         cp = (void *) data;
 515
 516         BT_DBG("request for hci%u", index);
 517
 518         if (len != sizeof(*cp))
 519                 return cmd_status(sk, index, MGMT_OP_SET_PAIRABLE, EINVAL);
 520
 521         hdev = hci_dev_get(index);
 522         if (!hdev)
 523                 return cmd_status(sk, index, MGMT_OP_SET_PAIRABLE, ENODEV);
 524
 525         hci_dev_lock_bh(hdev);
 526
 527         if (cp->val)
 528                 set_bit(HCI_PAIRABLE, &hdev->flags);
 529         else
 530                 clear_bit(HCI_PAIRABLE, &hdev->flags);
 531
 532         err = send_mode_rsp(sk, MGMT_OP_SET_PAIRABLE, index, cp->val);
 533         if (err < 0)
 534                 goto failed;
 535
 536         ev.val = cp->val;
 537
 538         err = mgmt_event(MGMT_EV_PAIRABLE, hdev, &ev, sizeof(ev), sk);
 539
 540 failed:
 541         hci_dev_unlock_bh(hdev);
 542         hci_dev_put(hdev);
 543
 544         return err;
 545 }
 546
 547 #define EIR_FLAGS               0x01 /* flags */
 548 #define EIR_UUID16_SOME         0x02 /* 16-bit UUID, more available */
 549 #define EIR_UUID16_ALL          0x03 /* 16-bit UUID, all listed */
 550 #define EIR_UUID32_SOME         0x04 /* 32-bit UUID, more available */
 551 #define EIR_UUID32_ALL          0x05 /* 32-bit UUID, all listed */
 552 #define EIR_UUID128_SOME        0x06 /* 128-bit UUID, more available */
 553 #define EIR_UUID128_ALL         0x07 /* 128-bit UUID, all listed */
 554 #define EIR_NAME_SHORT          0x08 /* shortened local name */
 555 #define EIR_NAME_COMPLETE       0x09 /* complete local name */
 556 #define EIR_TX_POWER            0x0A /* transmit power level */
 557 #define EIR_DEVICE_ID           0x10 /* device ID */
 558
 559 #define PNP_INFO_SVCLASS_ID             0x1200
 560
 561 static u8 bluetooth_base_uuid[] = {
 562                         0xFB, 0x34, 0x9B, 0x5F, 0x80, 0x00, 0x00, 0x80,
 563                         0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 564 };
 565
 566 static u16 get_uuid16(u8 *uuid128)
 567 {
 568         u32 val;
 569         int i;
 570
 571         for (i = 0; i < 12; i++) {
 572                 if (bluetooth_base_uuid[i] != uuid128[i])
 573                         return 0;
 574         }
 575
 576         memcpy(&val, &uuid128[12], 4);
 577
 578         val = le32_to_cpu(val);
 579         if (val > 0xffff)
 580                 return 0;
 581
 582         return (u16) val;
 583 }
 584
 585 static void create_eir(struct hci_dev *hdev, u8 *data)
 586 {
 587         u8 *ptr = data;
 588         u16 eir_len = 0;
 589         u16 uuid16_list[HCI_MAX_EIR_LENGTH / sizeof(u16)];
 590         int i, truncated = 0;
 591         struct bt_uuid *uuid;
 592         size_t name_len;
 593
 594         name_len = strlen(hdev->dev_name);
 595
 596         if (name_len > 0) {
 597                 /* EIR Data type */
 598                 if (name_len > 48) {
 599                         name_len = 48;
 600                         ptr[1] = EIR_NAME_SHORT;
 601                 } else
 602                         ptr[1] = EIR_NAME_COMPLETE;
 603
 604                 /* EIR Data length */
 605                 ptr[0] = name_len + 1;
 606
 607                 memcpy(ptr + 2, hdev->dev_name, name_len);
 608
 609                 eir_len += (name_len + 2);
 610                 ptr += (name_len + 2);
 611         }
 612
 613         memset(uuid16_list, 0, sizeof(uuid16_list));
 614
 615         /* Group all UUID16 types */
 616         list_for_each_entry(uuid, &hdev->uuids, list) {
 617                 u16 uuid16;
 618
 619                 uuid16 = get_uuid16(uuid->uuid);
 620                 if (uuid16 == 0)
 621                         return;
 622
 623                 if (uuid16 < 0x1100)
 624                         continue;
 625
 626                 if (uuid16 == PNP_INFO_SVCLASS_ID)
 627                         continue;
 628
 629                 /* Stop if not enough space to put next UUID */
 630                 if (eir_len + 2 + sizeof(u16) > HCI_MAX_EIR_LENGTH) {
 631                         truncated = 1;
 632                         break;
 633                 }
 634
 635                 /* Check for duplicates */
 636                 for (i = 0; uuid16_list[i] != 0; i++)
 637                         if (uuid16_list[i] == uuid16)
 638                                 break;
 639
 640                 if (uuid16_list[i] == 0) {
 641                         uuid16_list[i] = uuid16;
 642                         eir_len += sizeof(u16);
 643                 }
 644         }
 645
 646         if (uuid16_list[0] != 0) {
 647                 u8 *length = ptr;
 648
 649                 /* EIR Data type */
 650                 ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL;
 651
 652                 ptr += 2;
 653                 eir_len += 2;
 654
 655                 for (i = 0; uuid16_list[i] != 0; i++) {
 656                         *ptr++ = (uuid16_list[i] & 0x00ff);
 657                         *ptr++ = (uuid16_list[i] & 0xff00) >> 8;
 658                 }
 659
 660                 /* EIR Data length */
 661                 *length = (i * sizeof(u16)) + 1;
 662         }
 663 }
 664
 665 static int update_eir(struct hci_dev *hdev)
 666 {
 667         struct hci_cp_write_eir cp;
 668
 669         if (!(hdev->features[6] & LMP_EXT_INQ))
 670                 return 0;
 671
 672         if (hdev->ssp_mode == 0)
 673                 return 0;
 674
 675         if (test_bit(HCI_SERVICE_CACHE, &hdev->flags))
 676                 return 0;
 677
 678         memset(&cp, 0, sizeof(cp));
 679
 680         create_eir(hdev, cp.data);
 681
 682         if (memcmp(cp.data, hdev->eir, sizeof(cp.data)) == 0)
 683                 return 0;
 684
 685         memcpy(hdev->eir, cp.data, sizeof(cp.data));
 686
 687         return hci_send_cmd(hdev, HCI_OP_WRITE_EIR, sizeof(cp), &cp);
 688 }
 689
 690 static u8 get_service_classes(struct hci_dev *hdev)
 691 {
 692         struct bt_uuid *uuid;
 693         u8 val = 0;
 694
 695         list_for_each_entry(uuid, &hdev->uuids, list)
 696                 val |= uuid->svc_hint;
 697
 698         return val;
 699 }
 700
 701 static int update_class(struct hci_dev *hdev)
 702 {
 703         u8 cod[3];
 704
 705         BT_DBG("%s", hdev->name);
 706
 707         if (test_bit(HCI_SERVICE_CACHE, &hdev->flags))
 708                 return 0;
 709
 710         cod[0] = hdev->minor_class;
 711         cod[1] = hdev->major_class;
 712         cod[2] = get_service_classes(hdev);
 713
 714         if (memcmp(cod, hdev->dev_class, 3) == 0)
 715                 return 0;
 716
 717         return hci_send_cmd(hdev, HCI_OP_WRITE_CLASS_OF_DEV, sizeof(cod), cod);
 718 }
 719
 720 static int add_uuid(struct sock *sk, u16 index, unsigned char *data, u16 len)
 721 {
 722         struct mgmt_cp_add_uuid *cp;
 723         struct hci_dev *hdev;
 724         struct bt_uuid *uuid;
 725         int err;
 726
 727         cp = (void *) data;
 728
 729         BT_DBG("request for hci%u", index);
 730
 731         if (len != sizeof(*cp))
 732                 return cmd_status(sk, index, MGMT_OP_ADD_UUID, EINVAL);
 733
 734         hdev = hci_dev_get(index);
 735         if (!hdev)
 736                 return cmd_status(sk, index, MGMT_OP_ADD_UUID, ENODEV);
 737
 738         hci_dev_lock_bh(hdev);
 739
 740         uuid = kmalloc(sizeof(*uuid), GFP_ATOMIC);
 741         if (!uuid) {
 742                 err = -ENOMEM;
 743                 goto failed;
 744         }
 745
 746         memcpy(uuid->uuid, cp->uuid, 16);
 747         uuid->svc_hint = cp->svc_hint;
 748
 749         list_add(&uuid->list, &hdev->uuids);
 750
 751         err = update_class(hdev);
 752         if (err < 0)
 753                 goto failed;
 754
 755         err = update_eir(hdev);
 756         if (err < 0)
 757                 goto failed;
 758
 759         err = cmd_complete(sk, index, MGMT_OP_ADD_UUID, NULL, 0);
 760
 761 failed:
 762         hci_dev_unlock_bh(hdev);
 763         hci_dev_put(hdev);
 764
 765         return err;
 766 }
 767
 768 static int remove_uuid(struct sock *sk, u16 index, unsigned char *data, u16 len)
 769 {
 770         struct list_head *p, *n;
 771         struct mgmt_cp_remove_uuid *cp;
 772         struct hci_dev *hdev;
 773         u8 bt_uuid_any[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
 774         int err, found;
 775
 776         cp = (void *) data;
 777
 778         BT_DBG("request for hci%u", index);
 779
 780         if (len != sizeof(*cp))
 781                 return cmd_status(sk, index, MGMT_OP_REMOVE_UUID, EINVAL);
 782
 783         hdev = hci_dev_get(index);
 784         if (!hdev)
 785                 return cmd_status(sk, index, MGMT_OP_REMOVE_UUID, ENODEV);
 786
 787         hci_dev_lock_bh(hdev);
 788
 789         if (memcmp(cp->uuid, bt_uuid_any, 16) == 0) {
 790                 err = hci_uuids_clear(hdev);
 791                 goto unlock;
 792         }
 793
 794         found = 0;
 795
 796         list_for_each_safe(p, n, &hdev->uuids) {
 797                 struct bt_uuid *match = list_entry(p, struct bt_uuid, list);
 798
 799                 if (memcmp(match->uuid, cp->uuid, 16) != 0)
 800                         continue;
 801
 802                 list_del(&match->list);
 803                 found++;
 804         }
 805
 806         if (found == 0) {
 807                 err = cmd_status(sk, index, MGMT_OP_REMOVE_UUID, ENOENT);
 808                 goto unlock;
 809         }
 810
 811         err = update_class(hdev);
 812         if (err < 0)
 813                 goto unlock;
 814
 815         err = update_eir(hdev);
 816         if (err < 0)
 817                 goto unlock;
 818
 819         err = cmd_complete(sk, index, MGMT_OP_REMOVE_UUID, NULL, 0);
 820
 821 unlock:
 822         hci_dev_unlock_bh(hdev);
 823         hci_dev_put(hdev);
 824
 825         return err;
 826 }
 827
 828 static int set_dev_class(struct sock *sk, u16 index, unsigned char *data,
 829                                                                         u16 len)
 830 {
 831         struct hci_dev *hdev;
 832         struct mgmt_cp_set_dev_class *cp;
 833         int err;
 834
 835         cp = (void *) data;
 836
 837         BT_DBG("request for hci%u", index);
 838
 839         if (len != sizeof(*cp))
 840                 return cmd_status(sk, index, MGMT_OP_SET_DEV_CLASS, EINVAL);
 841
 842         hdev = hci_dev_get(index);
 843         if (!hdev)
 844                 return cmd_status(sk, index, MGMT_OP_SET_DEV_CLASS, ENODEV);
 845
 846         hci_dev_lock_bh(hdev);
 847
 848         hdev->major_class = cp->major;
 849         hdev->minor_class = cp->minor;
 850
 851         err = update_class(hdev);
 852
 853         if (err == 0)
 854                 err = cmd_complete(sk, index, MGMT_OP_SET_DEV_CLASS, NULL, 0);
 855
 856         hci_dev_unlock_bh(hdev);
 857         hci_dev_put(hdev);
 858
 859         return err;
 860 }
 861
 862 static int set_service_cache(struct sock *sk, u16 index,  unsigned char *data,
 863                                                                         u16 len)
 864 {
 865         struct hci_dev *hdev;
 866         struct mgmt_cp_set_service_cache *cp;
 867         int err;
 868
 869         cp = (void *) data;
 870
 871         if (len != sizeof(*cp))
 872                 return cmd_status(sk, index, MGMT_OP_SET_SERVICE_CACHE, EINVAL);
 873
 874         hdev = hci_dev_get(index);
 875         if (!hdev)
 876                 return cmd_status(sk, index, MGMT_OP_SET_SERVICE_CACHE, ENODEV);
 877
 878         hci_dev_lock_bh(hdev);
 879
 880         BT_DBG("hci%u enable %d", index, cp->enable);
 881
 882         if (cp->enable) {
 883                 set_bit(HCI_SERVICE_CACHE, &hdev->flags);
 884                 err = 0;
 885         } else {
 886                 clear_bit(HCI_SERVICE_CACHE, &hdev->flags);
 887                 err = update_class(hdev);
 888                 if (err == 0)
 889                         err = update_eir(hdev);
 890         }
 891
 892         if (err == 0)
 893                 err = cmd_complete(sk, index, MGMT_OP_SET_SERVICE_CACHE, NULL,
 894                                                                         0);
 895         else
 896                 cmd_status(sk, index, MGMT_OP_SET_SERVICE_CACHE, -err);
 897
 898
 899         hci_dev_unlock_bh(hdev);
 900         hci_dev_put(hdev);
 901
 902         return err;
 903 }
 904
 905 static int load_link_keys(struct sock *sk, u16 index, unsigned char *data,
 906                                                                 u16 len)
 907 {
 908         struct hci_dev *hdev;
 909         struct mgmt_cp_load_link_keys *cp;
 910         u16 key_count, expected_len;
 911         int i;
 912
 913         cp = (void *) data;
 914
 915         if (len < sizeof(*cp))
 916                 return cmd_status(sk, index, MGMT_OP_LOAD_LINK_KEYS, EINVAL);
 917
 918         key_count = get_unaligned_le16(&cp->key_count);
 919
 920         expected_len = sizeof(*cp) + key_count *
 921                                         sizeof(struct mgmt_link_key_info);
 922         if (expected_len != len) {
 923                 BT_ERR("load_link_keys: expected %u bytes, got %u bytes",
 924                                                         len, expected_len);
 925                 return cmd_status(sk, index, MGMT_OP_LOAD_LINK_KEYS, EINVAL);
 926         }
 927
 928         hdev = hci_dev_get(index);
 929         if (!hdev)
 930                 return cmd_status(sk, index, MGMT_OP_LOAD_LINK_KEYS, ENODEV);
 931
 932         BT_DBG("hci%u debug_keys %u key_count %u", index, cp->debug_keys,
 933                                                                 key_count);
 934
 935         hci_dev_lock_bh(hdev);
 936
 937         hci_link_keys_clear(hdev);
 938
 939         set_bit(HCI_LINK_KEYS, &hdev->flags);
 940
 941         if (cp->debug_keys)
 942                 set_bit(HCI_DEBUG_KEYS, &hdev->flags);
 943         else
 944                 clear_bit(HCI_DEBUG_KEYS, &hdev->flags);
 945
 946         for (i = 0; i < key_count; i++) {
 947                 struct mgmt_link_key_info *key = &cp->keys[i];
 948
 949                 hci_add_link_key(hdev, NULL, 0, &key->bdaddr, key->val, key->type,
 950                                                                 key->pin_len);
 951         }
 952
 953         hci_dev_unlock_bh(hdev);
 954         hci_dev_put(hdev);
 955
 956         return 0;
 957 }
 958
 959 static int remove_keys(struct sock *sk, u16 index, unsigned char *data,
 960                                                                 u16 len)
 961 {
 962         struct hci_dev *hdev;
 963         struct mgmt_cp_remove_keys *cp;
 964         struct mgmt_rp_remove_keys rp;
 965         struct hci_cp_disconnect dc;
 966         struct pending_cmd *cmd;
 967         struct hci_conn *conn;
 968         int err;
 969
 970         cp = (void *) data;
 971
 972         if (len != sizeof(*cp))
 973                 return cmd_status(sk, index, MGMT_OP_REMOVE_KEYS, EINVAL);
 974
 975         hdev = hci_dev_get(index);
 976         if (!hdev)
 977                 return cmd_status(sk, index, MGMT_OP_REMOVE_KEYS, ENODEV);
 978
 979         hci_dev_lock_bh(hdev);
 980
 981         memset(&rp, 0, sizeof(rp));
 982         bacpy(&rp.bdaddr, &cp->bdaddr);
 983
 984         err = hci_remove_link_key(hdev, &cp->bdaddr);
 985         if (err < 0)
 986                 goto unlock;
 987
 988         if (!test_bit(HCI_UP, &hdev->flags) || !cp->disconnect) {
 989                 err = cmd_complete(sk, index, MGMT_OP_REMOVE_KEYS, &rp,
 990                                                                 sizeof(rp));
 991                 goto unlock;
 992         }
 993
 994         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
 995         if (!conn) {
 996                 err = cmd_complete(sk, index, MGMT_OP_REMOVE_KEYS, &rp,
 997                                                                 sizeof(rp));
 998                 goto unlock;
 999         }
1000
1001         cmd = mgmt_pending_add(sk, MGMT_OP_REMOVE_KEYS, hdev, cp, sizeof(*cp));
1002         if (!cmd) {
1003                 err = -ENOMEM;
1004                 goto unlock;
1005         }
1006
1007         put_unaligned_le16(conn->handle, &dc.handle);
1008         dc.reason = 0x13; /* Remote User Terminated Connection */
1009         err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc);
1010         if (err < 0)
1011                 mgmt_pending_remove(cmd);
1012
1013 unlock:
1014         if (err < 0) {
1015                 rp.status = -err;
1016                 err = cmd_complete(sk, index, MGMT_OP_REMOVE_KEYS, &rp,
1017                                                                 sizeof(rp));
1018         }
1019         hci_dev_unlock_bh(hdev);
1020         hci_dev_put(hdev);
1021
1022         return err;
1023 }
1024
1025 static int disconnect(struct sock *sk, u16 index, unsigned char *data, u16 len)
1026 {
1027         struct hci_dev *hdev;
1028         struct mgmt_cp_disconnect *cp;
1029         struct hci_cp_disconnect dc;
1030         struct pending_cmd *cmd;
1031         struct hci_conn *conn;
1032         int err;
1033
1034         BT_DBG("");
1035
1036         cp = (void *) data;
1037
1038         if (len != sizeof(*cp))
1039                 return cmd_status(sk, index, MGMT_OP_DISCONNECT, EINVAL);
1040
1041         hdev = hci_dev_get(index);
1042         if (!hdev)
1043                 return cmd_status(sk, index, MGMT_OP_DISCONNECT, ENODEV);
1044
1045         hci_dev_lock_bh(hdev);
1046
1047         if (!test_bit(HCI_UP, &hdev->flags)) {
1048                 err = cmd_status(sk, index, MGMT_OP_DISCONNECT, ENETDOWN);
1049                 goto failed;
1050         }
1051
1052         if (mgmt_pending_find(MGMT_OP_DISCONNECT, hdev)) {
1053                 err = cmd_status(sk, index, MGMT_OP_DISCONNECT, EBUSY);
1054                 goto failed;
1055         }
1056
1057         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
1058         if (!conn)
1059                 conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->bdaddr);
1060
1061         if (!conn) {
1062                 err = cmd_status(sk, index, MGMT_OP_DISCONNECT, ENOTCONN);
1063                 goto failed;
1064         }
1065
1066         cmd = mgmt_pending_add(sk, MGMT_OP_DISCONNECT, hdev, data, len);
1067         if (!cmd) {
1068                 err = -ENOMEM;
1069                 goto failed;
1070         }
1071
1072         put_unaligned_le16(conn->handle, &dc.handle);
1073         dc.reason = 0x13; /* Remote User Terminated Connection */
1074
1075         err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc);
1076         if (err < 0)
1077                 mgmt_pending_remove(cmd);
1078
1079 failed:
1080         hci_dev_unlock_bh(hdev);
1081         hci_dev_put(hdev);
1082
1083         return err;
1084 }
1085
1086 static u8 link_to_mgmt(u8 link_type, u8 addr_type)
1087 {
1088         switch (link_type) {
1089         case LE_LINK:
1090                 switch (addr_type) {
1091                 case ADDR_LE_DEV_PUBLIC:
1092                         return MGMT_ADDR_LE_PUBLIC;
1093                 case ADDR_LE_DEV_RANDOM:
1094                         return MGMT_ADDR_LE_RANDOM;
1095                 default:
1096                         return MGMT_ADDR_INVALID;
1097                 }
1098         case ACL_LINK:
1099                 return MGMT_ADDR_BREDR;
1100         default:
1101                 return MGMT_ADDR_INVALID;
1102         }
1103 }
1104
1105 static int get_connections(struct sock *sk, u16 index)
1106 {
1107         struct mgmt_rp_get_connections *rp;
1108         struct hci_dev *hdev;
1109         struct hci_conn *c;
1110         struct list_head *p;
1111         size_t rp_len;
1112         u16 count;
1113         int i, err;
1114
1115         BT_DBG("");
1116
1117         hdev = hci_dev_get(index);
1118         if (!hdev)
1119                 return cmd_status(sk, index, MGMT_OP_GET_CONNECTIONS, ENODEV);
1120
1121         hci_dev_lock_bh(hdev);
1122
1123         count = 0;
1124         list_for_each(p, &hdev->conn_hash.list) {
1125                 count++;
1126         }
1127
1128         rp_len = sizeof(*rp) + (count * sizeof(struct mgmt_addr_info));
1129         rp = kmalloc(rp_len, GFP_ATOMIC);
1130         if (!rp) {
1131                 err = -ENOMEM;
1132                 goto unlock;
1133         }
1134
1135         put_unaligned_le16(count, &rp->conn_count);
1136
1137         i = 0;
1138         list_for_each_entry(c, &hdev->conn_hash.list, list) {
1139                 bacpy(&rp->addr[i].bdaddr, &c->dst);
1140                 rp->addr[i].type = link_to_mgmt(c->type, c->dst_type);
1141                 if (rp->addr[i].type == MGMT_ADDR_INVALID)
1142                         continue;
1143                 i++;
1144         }
1145
1146         /* Recalculate length in case of filtered SCO connections, etc */
1147         rp_len = sizeof(*rp) + (i * sizeof(struct mgmt_addr_info));
1148
1149         err = cmd_complete(sk, index, MGMT_OP_GET_CONNECTIONS, rp, rp_len);
1150
1151 unlock:
1152         kfree(rp);
1153         hci_dev_unlock_bh(hdev);
1154         hci_dev_put(hdev);
1155         return err;
1156 }
1157
1158 static int send_pin_code_neg_reply(struct sock *sk, u16 index,
1159                 struct hci_dev *hdev, struct mgmt_cp_pin_code_neg_reply *cp)
1160 {
1161         struct pending_cmd *cmd;
1162         int err;
1163
1164         cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_NEG_REPLY, hdev, cp,
1165                                                                 sizeof(*cp));
1166         if (!cmd)
1167                 return -ENOMEM;
1168
1169         err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY, sizeof(cp->bdaddr),
1170                                                                 &cp->bdaddr);
1171         if (err < 0)
1172                 mgmt_pending_remove(cmd);
1173
1174         return err;
1175 }
1176
1177 static int pin_code_reply(struct sock *sk, u16 index, unsigned char *data,
1178                                                                         u16 len)
1179 {
1180         struct hci_dev *hdev;
1181         struct hci_conn *conn;
1182         struct mgmt_cp_pin_code_reply *cp;
1183         struct mgmt_cp_pin_code_neg_reply ncp;
1184         struct hci_cp_pin_code_reply reply;
1185         struct pending_cmd *cmd;
1186         int err;
1187
1188         BT_DBG("");
1189
1190         cp = (void *) data;
1191
1192         if (len != sizeof(*cp))
1193                 return cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY, EINVAL);
1194
1195         hdev = hci_dev_get(index);
1196         if (!hdev)
1197                 return cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY, ENODEV);
1198
1199         hci_dev_lock_bh(hdev);
1200
1201         if (!test_bit(HCI_UP, &hdev->flags)) {
1202                 err = cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY, ENETDOWN);
1203                 goto failed;
1204         }
1205
1206         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
1207         if (!conn) {
1208                 err = cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY, ENOTCONN);
1209                 goto failed;
1210         }
1211
1212         if (conn->pending_sec_level == BT_SECURITY_HIGH && cp->pin_len != 16) {
1213                 bacpy(&ncp.bdaddr, &cp->bdaddr);
1214
1215                 BT_ERR("PIN code is not 16 bytes long");
1216
1217                 err = send_pin_code_neg_reply(sk, index, hdev, &ncp);
1218                 if (err >= 0)
1219                         err = cmd_status(sk, index, MGMT_OP_PIN_CODE_REPLY,
1220                                                                 EINVAL);
1221
1222                 goto failed;
1223         }
1224
1225         cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_REPLY, hdev, data, len);
1226         if (!cmd) {
1227                 err = -ENOMEM;
1228                 goto failed;
1229         }
1230
1231         bacpy(&reply.bdaddr, &cp->bdaddr);
1232         reply.pin_len = cp->pin_len;
1233         memcpy(reply.pin_code, cp->pin_code, sizeof(reply.pin_code));
1234
1235         err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_REPLY, sizeof(reply), &reply);
1236         if (err < 0)
1237                 mgmt_pending_remove(cmd);
1238
1239 failed:
1240         hci_dev_unlock_bh(hdev);
1241         hci_dev_put(hdev);
1242
1243         return err;
1244 }
1245
1246 static int pin_code_neg_reply(struct sock *sk, u16 index, unsigned char *data,
1247                                                                         u16 len)
1248 {
1249         struct hci_dev *hdev;
1250         struct mgmt_cp_pin_code_neg_reply *cp;
1251         int err;
1252
1253         BT_DBG("");
1254
1255         cp = (void *) data;
1256
1257         if (len != sizeof(*cp))
1258                 return cmd_status(sk, index, MGMT_OP_PIN_CODE_NEG_REPLY,
1259                                                                         EINVAL);
1260
1261         hdev = hci_dev_get(index);
1262         if (!hdev)
1263                 return cmd_status(sk, index, MGMT_OP_PIN_CODE_NEG_REPLY,
1264                                                                         ENODEV);
1265
1266         hci_dev_lock_bh(hdev);
1267
1268         if (!test_bit(HCI_UP, &hdev->flags)) {
1269                 err = cmd_status(sk, index, MGMT_OP_PIN_CODE_NEG_REPLY,
1270                                                                 ENETDOWN);
1271                 goto failed;
1272         }
1273
1274         err = send_pin_code_neg_reply(sk, index, hdev, cp);
1275
1276 failed:
1277         hci_dev_unlock_bh(hdev);
1278         hci_dev_put(hdev);
1279
1280         return err;
1281 }
1282
1283 static int set_io_capability(struct sock *sk, u16 index, unsigned char *data,
1284                                                                         u16 len)
1285 {
1286         struct hci_dev *hdev;
1287         struct mgmt_cp_set_io_capability *cp;
1288
1289         BT_DBG("");
1290
1291         cp = (void *) data;
1292
1293         if (len != sizeof(*cp))
1294                 return cmd_status(sk, index, MGMT_OP_SET_IO_CAPABILITY, EINVAL);
1295
1296         hdev = hci_dev_get(index);
1297         if (!hdev)
1298                 return cmd_status(sk, index, MGMT_OP_SET_IO_CAPABILITY, ENODEV);
1299
1300         hci_dev_lock_bh(hdev);
1301
1302         hdev->io_capability = cp->io_capability;
1303
1304         BT_DBG("%s IO capability set to 0x%02x", hdev->name,
1305                                                         hdev->io_capability);
1306
1307         hci_dev_unlock_bh(hdev);
1308         hci_dev_put(hdev);
1309
1310         return cmd_complete(sk, index, MGMT_OP_SET_IO_CAPABILITY, NULL, 0);
1311 }
1312
1313 static inline struct pending_cmd *find_pairing(struct hci_conn *conn)
1314 {
1315         struct hci_dev *hdev = conn->hdev;
1316         struct pending_cmd *cmd;
1317
1318         list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
1319                 if (cmd->opcode != MGMT_OP_PAIR_DEVICE)
1320                         continue;
1321
1322                 if (cmd->user_data != conn)
1323                         continue;
1324
1325                 return cmd;
1326         }
1327
1328         return NULL;
1329 }
1330
1331 static void pairing_complete(struct pending_cmd *cmd, u8 status)
1332 {
1333         struct mgmt_rp_pair_device rp;
1334         struct hci_conn *conn = cmd->user_data;
1335
1336         bacpy(&rp.addr.bdaddr, &conn->dst);
1337         rp.addr.type = link_to_mgmt(conn->type, conn->dst_type);
1338         rp.status = status;
1339
1340         cmd_complete(cmd->sk, cmd->index, MGMT_OP_PAIR_DEVICE, &rp, sizeof(rp));
1341
1342         /* So we don't get further callbacks for this connection */
1343         conn->connect_cfm_cb = NULL;
1344         conn->security_cfm_cb = NULL;
1345         conn->disconn_cfm_cb = NULL;
1346
1347         hci_conn_put(conn);
1348
1349         mgmt_pending_remove(cmd);
1350 }
1351
1352 static void pairing_complete_cb(struct hci_conn *conn, u8 status)
1353 {
1354         struct pending_cmd *cmd;
1355
1356         BT_DBG("status %u", status);
1357
1358         cmd = find_pairing(conn);
1359         if (!cmd)
1360                 BT_DBG("Unable to find a pending command");
1361         else
1362                 pairing_complete(cmd, status);
1363 }
1364
1365 static int pair_device(struct sock *sk, u16 index, unsigned char *data, u16 len)
1366 {
1367         struct hci_dev *hdev;
1368         struct mgmt_cp_pair_device *cp;
1369         struct pending_cmd *cmd;
1370         u8 sec_level, auth_type;
1371         struct hci_conn *conn;
1372         int err;
1373
1374         BT_DBG("");
1375
1376         cp = (void *) data;
1377
1378         if (len != sizeof(*cp))
1379                 return cmd_status(sk, index, MGMT_OP_PAIR_DEVICE, EINVAL);
1380
1381         hdev = hci_dev_get(index);
1382         if (!hdev)
1383                 return cmd_status(sk, index, MGMT_OP_PAIR_DEVICE, ENODEV);
1384
1385         hci_dev_lock_bh(hdev);
1386
1387         sec_level = BT_SECURITY_MEDIUM;
1388         if (cp->io_cap == 0x03)
1389                 auth_type = HCI_AT_DEDICATED_BONDING;
1390         else
1391                 auth_type = HCI_AT_DEDICATED_BONDING_MITM;
1392
1393         if (cp->addr.type == MGMT_ADDR_BREDR)
1394                 conn = hci_connect(hdev, ACL_LINK, &cp->addr.bdaddr, sec_level,
1395                                                                 auth_type);
1396         else
1397                 conn = hci_connect(hdev, LE_LINK, &cp->addr.bdaddr, sec_level,
1398                                                                 auth_type);
1399
1400         if (IS_ERR(conn)) {
1401                 err = PTR_ERR(conn);
1402                 goto unlock;
1403         }
1404
1405         if (conn->connect_cfm_cb) {
1406                 hci_conn_put(conn);
1407                 err = cmd_status(sk, index, MGMT_OP_PAIR_DEVICE, EBUSY);
1408                 goto unlock;
1409         }
1410
1411         cmd = mgmt_pending_add(sk, MGMT_OP_PAIR_DEVICE, hdev, data, len);
1412         if (!cmd) {
1413                 err = -ENOMEM;
1414                 hci_conn_put(conn);
1415                 goto unlock;
1416         }
1417
1418         /* For LE, just connecting isn't a proof that the pairing finished */
1419         if (cp->addr.type == MGMT_ADDR_BREDR)
1420                 conn->connect_cfm_cb = pairing_complete_cb;
1421
1422         conn->security_cfm_cb = pairing_complete_cb;
1423         conn->disconn_cfm_cb = pairing_complete_cb;
1424         conn->io_capability = cp->io_cap;
1425         cmd->user_data = conn;
1426
1427         if (conn->state == BT_CONNECTED &&
1428                                 hci_conn_security(conn, sec_level, auth_type))
1429                 pairing_complete(cmd, 0);
1430
1431         err = 0;
1432
1433 unlock:
1434         hci_dev_unlock_bh(hdev);
1435         hci_dev_put(hdev);
1436
1437         return err;
1438 }
1439
1440 static int user_confirm_reply(struct sock *sk, u16 index, unsigned char *data,
1441                                                         u16 len, int success)
1442 {
1443         struct mgmt_cp_user_confirm_reply *cp = (void *) data;
1444         u16 mgmt_op, hci_op;
1445         struct pending_cmd *cmd;
1446         struct hci_dev *hdev;
1447         int err;
1448
1449         BT_DBG("");
1450
1451         if (success) {
1452                 mgmt_op = MGMT_OP_USER_CONFIRM_REPLY;
1453                 hci_op = HCI_OP_USER_CONFIRM_REPLY;
1454         } else {
1455                 mgmt_op = MGMT_OP_USER_CONFIRM_NEG_REPLY;
1456                 hci_op = HCI_OP_USER_CONFIRM_NEG_REPLY;
1457         }
1458
1459         if (len != sizeof(*cp))
1460                 return cmd_status(sk, index, mgmt_op, EINVAL);
1461
1462         hdev = hci_dev_get(index);
1463         if (!hdev)
1464                 return cmd_status(sk, index, mgmt_op, ENODEV);
1465
1466         hci_dev_lock_bh(hdev);
1467
1468         if (!test_bit(HCI_UP, &hdev->flags)) {
1469                 err = cmd_status(sk, index, mgmt_op, ENETDOWN);
1470                 goto failed;
1471         }
1472
1473         cmd = mgmt_pending_add(sk, mgmt_op, hdev, data, len);
1474         if (!cmd) {
1475                 err = -ENOMEM;
1476                 goto failed;
1477         }
1478
1479         err = hci_send_cmd(hdev, hci_op, sizeof(cp->bdaddr), &cp->bdaddr);
1480         if (err < 0)
1481                 mgmt_pending_remove(cmd);
1482
1483 failed:
1484         hci_dev_unlock_bh(hdev);
1485         hci_dev_put(hdev);
1486
1487         return err;
1488 }
1489
1490 static int set_local_name(struct sock *sk, u16 index, unsigned char *data,
1491                                                                 u16 len)
1492 {
1493         struct mgmt_cp_set_local_name *mgmt_cp = (void *) data;
1494         struct hci_cp_write_local_name hci_cp;
1495         struct hci_dev *hdev;
1496         struct pending_cmd *cmd;
1497         int err;
1498
1499         BT_DBG("");
1500
1501         if (len != sizeof(*mgmt_cp))
1502                 return cmd_status(sk, index, MGMT_OP_SET_LOCAL_NAME, EINVAL);
1503
1504         hdev = hci_dev_get(index);
1505         if (!hdev)
1506                 return cmd_status(sk, index, MGMT_OP_SET_LOCAL_NAME, ENODEV);
1507
1508         hci_dev_lock_bh(hdev);
1509
1510         cmd = mgmt_pending_add(sk, MGMT_OP_SET_LOCAL_NAME, hdev, data, len);
1511         if (!cmd) {
1512                 err = -ENOMEM;
1513                 goto failed;
1514         }
1515
1516         memcpy(hci_cp.name, mgmt_cp->name, sizeof(hci_cp.name));
1517         err = hci_send_cmd(hdev, HCI_OP_WRITE_LOCAL_NAME, sizeof(hci_cp),
1518                                                                 &hci_cp);
1519         if (err < 0)
1520                 mgmt_pending_remove(cmd);
1521
1522 failed:
1523         hci_dev_unlock_bh(hdev);
1524         hci_dev_put(hdev);
1525
1526         return err;
1527 }
1528
1529 static int read_local_oob_data(struct sock *sk, u16 index)
1530 {
1531         struct hci_dev *hdev;
1532         struct pending_cmd *cmd;
1533         int err;
1534
1535         BT_DBG("hci%u", index);
1536
1537         hdev = hci_dev_get(index);
1538         if (!hdev)
1539                 return cmd_status(sk, index, MGMT_OP_READ_LOCAL_OOB_DATA,
1540                                                                         ENODEV);
1541
1542         hci_dev_lock_bh(hdev);
1543
1544         if (!test_bit(HCI_UP, &hdev->flags)) {
1545                 err = cmd_status(sk, index, MGMT_OP_READ_LOCAL_OOB_DATA,
1546                                                                 ENETDOWN);
1547                 goto unlock;
1548         }
1549
1550         if (!(hdev->features[6] & LMP_SIMPLE_PAIR)) {
1551                 err = cmd_status(sk, index, MGMT_OP_READ_LOCAL_OOB_DATA,
1552                                                                 EOPNOTSUPP);
1553                 goto unlock;
1554         }
1555
1556         if (mgmt_pending_find(MGMT_OP_READ_LOCAL_OOB_DATA, hdev)) {
1557                 err = cmd_status(sk, index, MGMT_OP_READ_LOCAL_OOB_DATA, EBUSY);
1558                 goto unlock;
1559         }
1560
1561         cmd = mgmt_pending_add(sk, MGMT_OP_READ_LOCAL_OOB_DATA, hdev, NULL, 0);
1562         if (!cmd) {
1563                 err = -ENOMEM;
1564                 goto unlock;
1565         }
1566
1567         err = hci_send_cmd(hdev, HCI_OP_READ_LOCAL_OOB_DATA, 0, NULL);
1568         if (err < 0)
1569                 mgmt_pending_remove(cmd);
1570
1571 unlock:
1572         hci_dev_unlock_bh(hdev);
1573         hci_dev_put(hdev);
1574
1575         return err;
1576 }
1577
1578 static int add_remote_oob_data(struct sock *sk, u16 index, unsigned char *data,
1579                                                                         u16 len)
1580 {
1581         struct hci_dev *hdev;
1582         struct mgmt_cp_add_remote_oob_data *cp = (void *) data;
1583         int err;
1584
1585         BT_DBG("hci%u ", index);
1586
1587         if (len != sizeof(*cp))
1588                 return cmd_status(sk, index, MGMT_OP_ADD_REMOTE_OOB_DATA,
1589                                                                         EINVAL);
1590
1591         hdev = hci_dev_get(index);
1592         if (!hdev)
1593                 return cmd_status(sk, index, MGMT_OP_ADD_REMOTE_OOB_DATA,
1594                                                                         ENODEV);
1595
1596         hci_dev_lock_bh(hdev);
1597
1598         err = hci_add_remote_oob_data(hdev, &cp->bdaddr, cp->hash,
1599                                                                 cp->randomizer);
1600         if (err < 0)
1601                 err = cmd_status(sk, index, MGMT_OP_ADD_REMOTE_OOB_DATA, -err);
1602         else
1603                 err = cmd_complete(sk, index, MGMT_OP_ADD_REMOTE_OOB_DATA, NULL,
1604                                                                         0);
1605
1606         hci_dev_unlock_bh(hdev);
1607         hci_dev_put(hdev);
1608
1609         return err;
1610 }
1611
1612 static int remove_remote_oob_data(struct sock *sk, u16 index,
1613                                                 unsigned char *data, u16 len)
1614 {
1615         struct hci_dev *hdev;
1616         struct mgmt_cp_remove_remote_oob_data *cp = (void *) data;
1617         int err;
1618
1619         BT_DBG("hci%u ", index);
1620
1621         if (len != sizeof(*cp))
1622                 return cmd_status(sk, index, MGMT_OP_REMOVE_REMOTE_OOB_DATA,
1623                                                                         EINVAL);
1624
1625         hdev = hci_dev_get(index);
1626         if (!hdev)
1627                 return cmd_status(sk, index, MGMT_OP_REMOVE_REMOTE_OOB_DATA,
1628                                                                         ENODEV);
1629
1630         hci_dev_lock_bh(hdev);
1631
1632         err = hci_remove_remote_oob_data(hdev, &cp->bdaddr);
1633         if (err < 0)
1634                 err = cmd_status(sk, index, MGMT_OP_REMOVE_REMOTE_OOB_DATA,
1635                                                                         -err);
1636         else
1637                 err = cmd_complete(sk, index, MGMT_OP_REMOVE_REMOTE_OOB_DATA,
1638                                                                 NULL, 0);
1639
1640         hci_dev_unlock_bh(hdev);
1641         hci_dev_put(hdev);
1642
1643         return err;
1644 }
1645
1646 static int start_discovery(struct sock *sk, u16 index)
1647 {
1648         struct pending_cmd *cmd;
1649         struct hci_dev *hdev;
1650         int err;
1651
1652         BT_DBG("hci%u", index);
1653
1654         hdev = hci_dev_get(index);
1655         if (!hdev)
1656                 return cmd_status(sk, index, MGMT_OP_START_DISCOVERY, ENODEV);
1657
1658         hci_dev_lock_bh(hdev);
1659
1660         if (!test_bit(HCI_UP, &hdev->flags)) {
1661                 err = cmd_status(sk, index, MGMT_OP_START_DISCOVERY, ENETDOWN);
1662                 goto failed;
1663         }
1664
1665         cmd = mgmt_pending_add(sk, MGMT_OP_START_DISCOVERY, hdev, NULL, 0);
1666         if (!cmd) {
1667                 err = -ENOMEM;
1668                 goto failed;
1669         }
1670
1671         err = hci_do_inquiry(hdev, INQUIRY_LEN_BREDR);
1672         if (err < 0)
1673                 mgmt_pending_remove(cmd);
1674
1675 failed:
1676         hci_dev_unlock_bh(hdev);
1677         hci_dev_put(hdev);
1678
1679         return err;
1680 }
1681
1682 static int stop_discovery(struct sock *sk, u16 index)
1683 {
1684         struct hci_dev *hdev;
1685         struct pending_cmd *cmd;
1686         int err;
1687
1688         BT_DBG("hci%u", index);
1689
1690         hdev = hci_dev_get(index);
1691         if (!hdev)
1692                 return cmd_status(sk, index, MGMT_OP_STOP_DISCOVERY, ENODEV);
1693
1694         hci_dev_lock_bh(hdev);
1695
1696         cmd = mgmt_pending_add(sk, MGMT_OP_STOP_DISCOVERY, hdev, NULL, 0);
1697         if (!cmd) {
1698                 err = -ENOMEM;
1699                 goto failed;
1700         }
1701
1702         err = hci_cancel_inquiry(hdev);
1703         if (err < 0)
1704                 mgmt_pending_remove(cmd);
1705
1706 failed:
1707         hci_dev_unlock_bh(hdev);
1708         hci_dev_put(hdev);
1709
1710         return err;
1711 }
1712
1713 static int block_device(struct sock *sk, u16 index, unsigned char *data,
1714                                                                 u16 len)
1715 {
1716         struct hci_dev *hdev;
1717         struct mgmt_cp_block_device *cp = (void *) data;
1718         int err;
1719
1720         BT_DBG("hci%u", index);
1721
1722         if (len != sizeof(*cp))
1723                 return cmd_status(sk, index, MGMT_OP_BLOCK_DEVICE,
1724                                                         EINVAL);
1725
1726         hdev = hci_dev_get(index);
1727         if (!hdev)
1728                 return cmd_status(sk, index, MGMT_OP_BLOCK_DEVICE,
1729                                                         ENODEV);
1730
1731         hci_dev_lock_bh(hdev);
1732
1733         err = hci_blacklist_add(hdev, &cp->bdaddr);
1734         if (err < 0)
1735                 err = cmd_status(sk, index, MGMT_OP_BLOCK_DEVICE, -err);
1736         else
1737                 err = cmd_complete(sk, index, MGMT_OP_BLOCK_DEVICE,
1738                                                         NULL, 0);
1739
1740         hci_dev_unlock_bh(hdev);
1741         hci_dev_put(hdev);
1742
1743         return err;
1744 }
1745
1746 static int unblock_device(struct sock *sk, u16 index, unsigned char *data,
1747                                                                 u16 len)
1748 {
1749         struct hci_dev *hdev;
1750         struct mgmt_cp_unblock_device *cp = (void *) data;
1751         int err;
1752
1753         BT_DBG("hci%u", index);
1754
1755         if (len != sizeof(*cp))
1756                 return cmd_status(sk, index, MGMT_OP_UNBLOCK_DEVICE,
1757                                                                 EINVAL);
1758
1759         hdev = hci_dev_get(index);
1760         if (!hdev)
1761                 return cmd_status(sk, index, MGMT_OP_UNBLOCK_DEVICE,
1762                                                                 ENODEV);
1763
1764         hci_dev_lock_bh(hdev);
1765
1766         err = hci_blacklist_del(hdev, &cp->bdaddr);
1767
1768         if (err < 0)
1769                 err = cmd_status(sk, index, MGMT_OP_UNBLOCK_DEVICE, -err);
1770         else
1771                 err = cmd_complete(sk, index, MGMT_OP_UNBLOCK_DEVICE,
1772                                                                 NULL, 0);
1773
1774         hci_dev_unlock_bh(hdev);
1775         hci_dev_put(hdev);
1776
1777         return err;
1778 }
1779
1780 static int set_fast_connectable(struct sock *sk, u16 index,
1781                                         unsigned char *data, u16 len)
1782 {
1783         struct hci_dev *hdev;
1784         struct mgmt_cp_set_fast_connectable *cp = (void *) data;
1785         struct hci_cp_write_page_scan_activity acp;
1786         u8 type;
1787         int err;
1788
1789         BT_DBG("hci%u", index);
1790
1791         if (len != sizeof(*cp))
1792                 return cmd_status(sk, index, MGMT_OP_SET_FAST_CONNECTABLE,
1793                                                                 EINVAL);
1794
1795         hdev = hci_dev_get(index);
1796         if (!hdev)
1797                 return cmd_status(sk, index, MGMT_OP_SET_FAST_CONNECTABLE,
1798                                                                 ENODEV);
1799
1800         hci_dev_lock(hdev);
1801
1802         if (cp->enable) {
1803                 type = PAGE_SCAN_TYPE_INTERLACED;
1804                 acp.interval = 0x0024;  /* 22.5 msec page scan interval */
1805         } else {
1806                 type = PAGE_SCAN_TYPE_STANDARD; /* default */
1807                 acp.interval = 0x0800;  /* default 1.28 sec page scan */
1808         }
1809
1810         acp.window = 0x0012;    /* default 11.25 msec page scan window */
1811
1812         err = hci_send_cmd(hdev, HCI_OP_WRITE_PAGE_SCAN_ACTIVITY,
1813                                                 sizeof(acp), &acp);
1814         if (err < 0) {
1815                 err = cmd_status(sk, index, MGMT_OP_SET_FAST_CONNECTABLE,
1816                                                                 -err);
1817                 goto done;
1818         }
1819
1820         err = hci_send_cmd(hdev, HCI_OP_WRITE_PAGE_SCAN_TYPE, 1, &type);
1821         if (err < 0) {
1822                 err = cmd_status(sk, index, MGMT_OP_SET_FAST_CONNECTABLE,
1823                                                                 -err);
1824                 goto done;
1825         }
1826
1827         err = cmd_complete(sk, index, MGMT_OP_SET_FAST_CONNECTABLE,
1828                                                         NULL, 0);
1829 done:
1830         hci_dev_unlock(hdev);
1831         hci_dev_put(hdev);
1832
1833         return err;
1834 }
1835
1836 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
1837 {
1838         unsigned char *buf;
1839         struct mgmt_hdr *hdr;
1840         u16 opcode, index, len;
1841         int err;
1842
1843         BT_DBG("got %zu bytes", msglen);
1844
1845         if (msglen < sizeof(*hdr))
1846                 return -EINVAL;
1847
1848         buf = kmalloc(msglen, GFP_KERNEL);
1849         if (!buf)
1850                 return -ENOMEM;
1851
1852         if (memcpy_fromiovec(buf, msg->msg_iov, msglen)) {
1853                 err = -EFAULT;
1854                 goto done;
1855         }
1856
1857         hdr = (struct mgmt_hdr *) buf;
1858         opcode = get_unaligned_le16(&hdr->opcode);
1859         index = get_unaligned_le16(&hdr->index);
1860         len = get_unaligned_le16(&hdr->len);
1861
1862         if (len != msglen - sizeof(*hdr)) {
1863                 err = -EINVAL;
1864                 goto done;
1865         }
1866
1867         switch (opcode) {
1868         case MGMT_OP_READ_VERSION:
1869                 err = read_version(sk);
1870                 break;
1871         case MGMT_OP_READ_INDEX_LIST:
1872                 err = read_index_list(sk);
1873                 break;
1874         case MGMT_OP_READ_INFO:
1875                 err = read_controller_info(sk, index);
1876                 break;
1877         case MGMT_OP_SET_POWERED:
1878                 err = set_powered(sk, index, buf + sizeof(*hdr), len);
1879                 break;
1880         case MGMT_OP_SET_DISCOVERABLE:
1881                 err = set_discoverable(sk, index, buf + sizeof(*hdr), len);
1882                 break;
1883         case MGMT_OP_SET_CONNECTABLE:
1884                 err = set_connectable(sk, index, buf + sizeof(*hdr), len);
1885                 break;
1886         case MGMT_OP_SET_PAIRABLE:
1887                 err = set_pairable(sk, index, buf + sizeof(*hdr), len);
1888                 break;
1889         case MGMT_OP_ADD_UUID:
1890                 err = add_uuid(sk, index, buf + sizeof(*hdr), len);
1891                 break;
1892         case MGMT_OP_REMOVE_UUID:
1893                 err = remove_uuid(sk, index, buf + sizeof(*hdr), len);
1894                 break;
1895         case MGMT_OP_SET_DEV_CLASS:
1896                 err = set_dev_class(sk, index, buf + sizeof(*hdr), len);
1897                 break;
1898         case MGMT_OP_SET_SERVICE_CACHE:
1899                 err = set_service_cache(sk, index, buf + sizeof(*hdr), len);
1900                 break;
1901         case MGMT_OP_LOAD_LINK_KEYS:
1902                 err = load_link_keys(sk, index, buf + sizeof(*hdr), len);
1903                 break;
1904         case MGMT_OP_REMOVE_KEYS:
1905                 err = remove_keys(sk, index, buf + sizeof(*hdr), len);
1906                 break;
1907         case MGMT_OP_DISCONNECT:
1908                 err = disconnect(sk, index, buf + sizeof(*hdr), len);
1909                 break;
1910         case MGMT_OP_GET_CONNECTIONS:
1911                 err = get_connections(sk, index);
1912                 break;
1913         case MGMT_OP_PIN_CODE_REPLY:
1914                 err = pin_code_reply(sk, index, buf + sizeof(*hdr), len);
1915                 break;
1916         case MGMT_OP_PIN_CODE_NEG_REPLY:
1917                 err = pin_code_neg_reply(sk, index, buf + sizeof(*hdr), len);
1918                 break;
1919         case MGMT_OP_SET_IO_CAPABILITY:
1920                 err = set_io_capability(sk, index, buf + sizeof(*hdr), len);
1921                 break;
1922         case MGMT_OP_PAIR_DEVICE:
1923                 err = pair_device(sk, index, buf + sizeof(*hdr), len);
1924                 break;
1925         case MGMT_OP_USER_CONFIRM_REPLY:
1926                 err = user_confirm_reply(sk, index, buf + sizeof(*hdr), len, 1);
1927                 break;
1928         case MGMT_OP_USER_CONFIRM_NEG_REPLY:
1929                 err = user_confirm_reply(sk, index, buf + sizeof(*hdr), len, 0);
1930                 break;
1931         case MGMT_OP_SET_LOCAL_NAME:
1932                 err = set_local_name(sk, index, buf + sizeof(*hdr), len);
1933                 break;
1934         case MGMT_OP_READ_LOCAL_OOB_DATA:
1935                 err = read_local_oob_data(sk, index);
1936                 break;
1937         case MGMT_OP_ADD_REMOTE_OOB_DATA:
1938                 err = add_remote_oob_data(sk, index, buf + sizeof(*hdr), len);
1939                 break;
1940         case MGMT_OP_REMOVE_REMOTE_OOB_DATA:
1941                 err = remove_remote_oob_data(sk, index, buf + sizeof(*hdr),
1942                                                                         len);
1943                 break;
1944         case MGMT_OP_START_DISCOVERY:
1945                 err = start_discovery(sk, index);
1946                 break;
1947         case MGMT_OP_STOP_DISCOVERY:
1948                 err = stop_discovery(sk, index);
1949                 break;
1950         case MGMT_OP_BLOCK_DEVICE:
1951                 err = block_device(sk, index, buf + sizeof(*hdr), len);
1952                 break;
1953         case MGMT_OP_UNBLOCK_DEVICE:
1954                 err = unblock_device(sk, index, buf + sizeof(*hdr), len);
1955                 break;
1956         case MGMT_OP_SET_FAST_CONNECTABLE:
1957                 err = set_fast_connectable(sk, index, buf + sizeof(*hdr),
1958                                                                 len);
1959                 break;
1960         default:
1961                 BT_DBG("Unknown op %u", opcode);
1962                 err = cmd_status(sk, index, opcode, 0x01);
1963                 break;
1964         }
1965
1966         if (err < 0)
1967                 goto done;
1968
1969         err = msglen;
1970
1971 done:
1972         kfree(buf);
1973         return err;
1974 }
1975
1976 static void cmd_status_rsp(struct pending_cmd *cmd, void *data)
1977 {
1978         u8 *status = data;
1979
1980         cmd_status(cmd->sk, cmd->index, cmd->opcode, *status);
1981         mgmt_pending_remove(cmd);
1982 }
1983
1984 int mgmt_index_added(struct hci_dev *hdev)
1985 {
1986         return mgmt_event(MGMT_EV_INDEX_ADDED, hdev, NULL, 0, NULL);
1987 }
1988
1989 int mgmt_index_removed(struct hci_dev *hdev)
1990 {
1991         u8 status = ENODEV;
1992
1993         mgmt_pending_foreach(0, hdev, cmd_status_rsp, &status);
1994
1995         return mgmt_event(MGMT_EV_INDEX_REMOVED, hdev, NULL, 0, NULL);
1996 }
1997
1998 struct cmd_lookup {
1999         u8 val;
2000         struct sock *sk;
2001 };
2002
2003 static void mode_rsp(struct pending_cmd *cmd, void *data)
2004 {
2005         struct mgmt_mode *cp = cmd->param;
2006         struct cmd_lookup *match = data;
2007
2008         if (cp->val != match->val)
2009                 return;
2010
2011         send_mode_rsp(cmd->sk, cmd->opcode, cmd->index, cp->val);
2012
2013         list_del(&cmd->list);
2014
2015         if (match->sk == NULL) {
2016                 match->sk = cmd->sk;
2017                 sock_hold(match->sk);
2018         }
2019
2020         mgmt_pending_free(cmd);
2021 }
2022
2023 int mgmt_powered(struct hci_dev *hdev, u8 powered)
2024 {
2025         struct mgmt_mode ev;
2026         struct cmd_lookup match = { powered, NULL };
2027         int ret;
2028
2029         mgmt_pending_foreach(MGMT_OP_SET_POWERED, hdev, mode_rsp, &match);
2030
2031         if (!powered) {
2032                 u8 status = ENETDOWN;
2033                 mgmt_pending_foreach(0, hdev, cmd_status_rsp, &status);
2034         }
2035
2036         ev.val = powered;
2037
2038         ret = mgmt_event(MGMT_EV_POWERED, hdev, &ev, sizeof(ev), match.sk);
2039
2040         if (match.sk)
2041                 sock_put(match.sk);
2042
2043         return ret;
2044 }
2045
2046 int mgmt_discoverable(struct hci_dev *hdev, u8 discoverable)
2047 {
2048         struct mgmt_mode ev;
2049         struct cmd_lookup match = { discoverable, NULL };
2050         int ret;
2051
2052         mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, hdev, mode_rsp, &match);
2053
2054         ev.val = discoverable;
2055
2056         ret = mgmt_event(MGMT_EV_DISCOVERABLE, hdev, &ev, sizeof(ev),
2057                                                                 match.sk);
2058
2059         if (match.sk)
2060                 sock_put(match.sk);
2061
2062         return ret;
2063 }
2064
2065 int mgmt_connectable(struct hci_dev *hdev, u8 connectable)
2066 {
2067         struct mgmt_mode ev;
2068         struct cmd_lookup match = { connectable, NULL };
2069         int ret;
2070
2071         mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, hdev, mode_rsp, &match);
2072
2073         ev.val = connectable;
2074
2075         ret = mgmt_event(MGMT_EV_CONNECTABLE, hdev, &ev, sizeof(ev), match.sk);
2076
2077         if (match.sk)
2078                 sock_put(match.sk);
2079
2080         return ret;
2081 }
2082
2083 int mgmt_write_scan_failed(struct hci_dev *hdev, u8 scan, u8 status)
2084 {
2085         if (scan & SCAN_PAGE)
2086                 mgmt_pending_foreach(MGMT_OP_SET_CONNECTABLE, hdev,
2087                                                 cmd_status_rsp, &status);
2088
2089         if (scan & SCAN_INQUIRY)
2090                 mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, hdev,
2091                                                 cmd_status_rsp, &status);
2092
2093         return 0;
2094 }
2095
2096 int mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key,
2097                                                                 u8 persistent)
2098 {
2099         struct mgmt_ev_new_link_key ev;
2100
2101         memset(&ev, 0, sizeof(ev));
2102
2103         ev.store_hint = persistent;
2104         bacpy(&ev.key.bdaddr, &key->bdaddr);
2105         ev.key.type = key->type;
2106         memcpy(ev.key.val, key->val, 16);
2107         ev.key.pin_len = key->pin_len;
2108
2109         return mgmt_event(MGMT_EV_NEW_LINK_KEY, hdev, &ev, sizeof(ev), NULL);
2110 }
2111
2112 int mgmt_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
2113                                                                 u8 addr_type)
2114 {
2115         struct mgmt_addr_info ev;
2116
2117         bacpy(&ev.bdaddr, bdaddr);
2118         ev.type = link_to_mgmt(link_type, addr_type);
2119
2120         return mgmt_event(MGMT_EV_CONNECTED, hdev, &ev, sizeof(ev), NULL);
2121 }
2122
2123 static void disconnect_rsp(struct pending_cmd *cmd, void *data)
2124 {
2125         struct mgmt_cp_disconnect *cp = cmd->param;
2126         struct sock **sk = data;
2127         struct mgmt_rp_disconnect rp;
2128
2129         bacpy(&rp.bdaddr, &cp->bdaddr);
2130         rp.status = 0;
2131
2132         cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT, &rp, sizeof(rp));
2133
2134         *sk = cmd->sk;
2135         sock_hold(*sk);
2136
2137         mgmt_pending_remove(cmd);
2138 }
2139
2140 static void remove_keys_rsp(struct pending_cmd *cmd, void *data)
2141 {
2142         u8 *status = data;
2143         struct mgmt_cp_remove_keys *cp = cmd->param;
2144         struct mgmt_rp_remove_keys rp;
2145
2146         memset(&rp, 0, sizeof(rp));
2147         bacpy(&rp.bdaddr, &cp->bdaddr);
2148         if (status != NULL)
2149                 rp.status = *status;
2150
2151         cmd_complete(cmd->sk, cmd->index, MGMT_OP_REMOVE_KEYS, &rp,
2152                                                                 sizeof(rp));
2153
2154         mgmt_pending_remove(cmd);
2155 }
2156
2157 int mgmt_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
2158                                                                 u8 addr_type)
2159 {
2160         struct mgmt_addr_info ev;
2161         struct sock *sk = NULL;
2162         int err;
2163
2164         mgmt_pending_foreach(MGMT_OP_DISCONNECT, hdev, disconnect_rsp, &sk);
2165
2166         bacpy(&ev.bdaddr, bdaddr);
2167         ev.type = link_to_mgmt(link_type, addr_type);
2168
2169         err = mgmt_event(MGMT_EV_DISCONNECTED, hdev, &ev, sizeof(ev), sk);
2170
2171         if (sk)
2172                 sock_put(sk);
2173
2174         mgmt_pending_foreach(MGMT_OP_REMOVE_KEYS, hdev, remove_keys_rsp, NULL);
2175
2176         return err;
2177 }
2178
2179 int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 status)
2180 {
2181         struct pending_cmd *cmd;
2182         int err;
2183
2184         cmd = mgmt_pending_find(MGMT_OP_DISCONNECT, hdev);
2185         if (!cmd)
2186                 return -ENOENT;
2187
2188         if (bdaddr) {
2189                 struct mgmt_rp_disconnect rp;
2190
2191                 bacpy(&rp.bdaddr, bdaddr);
2192                 rp.status = status;
2193
2194                 err = cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT,
2195                                                         &rp, sizeof(rp));
2196         } else
2197                 err = cmd_status(cmd->sk, hdev->id, MGMT_OP_DISCONNECT,
2198                                                                 status);
2199
2200         mgmt_pending_remove(cmd);
2201
2202         return err;
2203 }
2204
2205 int mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
2206                                                 u8 addr_type, u8 status)
2207 {
2208         struct mgmt_ev_connect_failed ev;
2209
2210         bacpy(&ev.addr.bdaddr, bdaddr);
2211         ev.addr.type = link_to_mgmt(link_type, addr_type);
2212         ev.status = status;
2213
2214         return mgmt_event(MGMT_EV_CONNECT_FAILED, hdev, &ev, sizeof(ev), NULL);
2215 }
2216
2217 int mgmt_pin_code_request(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 secure)
2218 {
2219         struct mgmt_ev_pin_code_request ev;
2220
2221         bacpy(&ev.bdaddr, bdaddr);
2222         ev.secure = secure;
2223
2224         return mgmt_event(MGMT_EV_PIN_CODE_REQUEST, hdev, &ev, sizeof(ev),
2225                                                                         NULL);
2226 }
2227
2228 int mgmt_pin_code_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
2229                                                                 u8 status)
2230 {
2231         struct pending_cmd *cmd;
2232         struct mgmt_rp_pin_code_reply rp;
2233         int err;
2234
2235         cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_REPLY, hdev);
2236         if (!cmd)
2237                 return -ENOENT;
2238
2239         bacpy(&rp.bdaddr, bdaddr);
2240         rp.status = status;
2241
2242         err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, &rp,
2243                                                                 sizeof(rp));
2244
2245         mgmt_pending_remove(cmd);
2246
2247         return err;
2248 }
2249
2250 int mgmt_pin_code_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
2251                                                                 u8 status)
2252 {
2253         struct pending_cmd *cmd;
2254         struct mgmt_rp_pin_code_reply rp;
2255         int err;
2256
2257         cmd = mgmt_pending_find(MGMT_OP_PIN_CODE_NEG_REPLY, hdev);
2258         if (!cmd)
2259                 return -ENOENT;
2260
2261         bacpy(&rp.bdaddr, bdaddr);
2262         rp.status = status;
2263
2264         err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_PIN_CODE_NEG_REPLY, &rp,
2265                                                                 sizeof(rp));
2266
2267         mgmt_pending_remove(cmd);
2268
2269         return err;
2270 }
2271
2272 int mgmt_user_confirm_request(struct hci_dev *hdev, bdaddr_t *bdaddr,
2273                                                 __le32 value, u8 confirm_hint)
2274 {
2275         struct mgmt_ev_user_confirm_request ev;
2276
2277         BT_DBG("%s", hdev->name);
2278
2279         bacpy(&ev.bdaddr, bdaddr);
2280         ev.confirm_hint = confirm_hint;
2281         put_unaligned_le32(value, &ev.value);
2282
2283         return mgmt_event(MGMT_EV_USER_CONFIRM_REQUEST, hdev, &ev, sizeof(ev),
2284                                                                         NULL);
2285 }
2286
2287 static int confirm_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
2288                                                         u8 status, u8 opcode)
2289 {
2290         struct pending_cmd *cmd;
2291         struct mgmt_rp_user_confirm_reply rp;
2292         int err;
2293
2294         cmd = mgmt_pending_find(opcode, hdev);
2295         if (!cmd)
2296                 return -ENOENT;
2297
2298         bacpy(&rp.bdaddr, bdaddr);
2299         rp.status = status;
2300         err = cmd_complete(cmd->sk, hdev->id, opcode, &rp, sizeof(rp));
2301
2302         mgmt_pending_remove(cmd);
2303
2304         return err;
2305 }
2306
2307 int mgmt_user_confirm_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
2308                                                                 u8 status)
2309 {
2310         return confirm_reply_complete(hdev, bdaddr, status,
2311                                                 MGMT_OP_USER_CONFIRM_REPLY);
2312 }
2313
2314 int mgmt_user_confirm_neg_reply_complete(struct hci_dev *hdev,
2315                                                 bdaddr_t *bdaddr, u8 status)
2316 {
2317         return confirm_reply_complete(hdev, bdaddr, status,
2318                                         MGMT_OP_USER_CONFIRM_NEG_REPLY);
2319 }
2320
2321 int mgmt_auth_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 status)
2322 {
2323         struct mgmt_ev_auth_failed ev;
2324
2325         bacpy(&ev.bdaddr, bdaddr);
2326         ev.status = status;
2327
2328         return mgmt_event(MGMT_EV_AUTH_FAILED, hdev, &ev, sizeof(ev), NULL);
2329 }
2330
2331 int mgmt_set_local_name_complete(struct hci_dev *hdev, u8 *name, u8 status)
2332 {
2333         struct pending_cmd *cmd;
2334         struct mgmt_cp_set_local_name ev;
2335         int err;
2336
2337         memset(&ev, 0, sizeof(ev));
2338         memcpy(ev.name, name, HCI_MAX_NAME_LENGTH);
2339
2340         cmd = mgmt_pending_find(MGMT_OP_SET_LOCAL_NAME, hdev);
2341         if (!cmd)
2342                 goto send_event;
2343
2344         if (status) {
2345                 err = cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_LOCAL_NAME,
2346                                                                         EIO);
2347                 goto failed;
2348         }
2349
2350         update_eir(hdev);
2351
2352         err = cmd_complete(cmd->sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, &ev,
2353                                                                 sizeof(ev));
2354         if (err < 0)
2355                 goto failed;
2356
2357 send_event:
2358         err = mgmt_event(MGMT_EV_LOCAL_NAME_CHANGED, hdev, &ev, sizeof(ev),
2359                                                         cmd ? cmd->sk : NULL);
2360
2361 failed:
2362         if (cmd)
2363                 mgmt_pending_remove(cmd);
2364         return err;
2365 }
2366
2367 int mgmt_read_local_oob_data_reply_complete(struct hci_dev *hdev, u8 *hash,
2368                                                 u8 *randomizer, u8 status)
2369 {
2370         struct pending_cmd *cmd;
2371         int err;
2372
2373         BT_DBG("%s status %u", hdev->name, status);
2374
2375         cmd = mgmt_pending_find(MGMT_OP_READ_LOCAL_OOB_DATA, hdev);
2376         if (!cmd)
2377                 return -ENOENT;
2378
2379         if (status) {
2380                 err = cmd_status(cmd->sk, hdev->id,
2381                                         MGMT_OP_READ_LOCAL_OOB_DATA, EIO);
2382         } else {
2383                 struct mgmt_rp_read_local_oob_data rp;
2384
2385                 memcpy(rp.hash, hash, sizeof(rp.hash));
2386                 memcpy(rp.randomizer, randomizer, sizeof(rp.randomizer));
2387
2388                 err = cmd_complete(cmd->sk, hdev->id,
2389                                                 MGMT_OP_READ_LOCAL_OOB_DATA,
2390                                                 &rp, sizeof(rp));
2391         }
2392
2393         mgmt_pending_remove(cmd);
2394
2395         return err;
2396 }
2397
2398 int mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
2399                                 u8 addr_type, u8 *dev_class, s8 rssi, u8 *eir)
2400 {
2401         struct mgmt_ev_device_found ev;
2402
2403         memset(&ev, 0, sizeof(ev));
2404
2405         bacpy(&ev.addr.bdaddr, bdaddr);
2406         ev.addr.type = link_to_mgmt(link_type, addr_type);
2407         ev.rssi = rssi;
2408
2409         if (eir)
2410                 memcpy(ev.eir, eir, sizeof(ev.eir));
2411
2412         if (dev_class)
2413                 memcpy(ev.dev_class, dev_class, sizeof(ev.dev_class));
2414
2415         return mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, &ev, sizeof(ev), NULL);
2416 }
2417
2418 int mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 *name)
2419 {
2420         struct mgmt_ev_remote_name ev;
2421
2422         memset(&ev, 0, sizeof(ev));
2423
2424         bacpy(&ev.bdaddr, bdaddr);
2425         memcpy(ev.name, name, HCI_MAX_NAME_LENGTH);
2426
2427         return mgmt_event(MGMT_EV_REMOTE_NAME, hdev, &ev, sizeof(ev), NULL);
2428 }
2429
2430 int mgmt_start_discovery_failed(struct hci_dev *hdev, u8 status)
2431 {
2432         struct pending_cmd *cmd;
2433         int err;
2434
2435         cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev);
2436         if (!cmd)
2437                 return -ENOENT;
2438
2439         err = cmd_status(cmd->sk, hdev->id, cmd->opcode, status);
2440         mgmt_pending_remove(cmd);
2441
2442         return err;
2443 }
2444
2445 int mgmt_stop_discovery_failed(struct hci_dev *hdev, u8 status)
2446 {
2447         struct pending_cmd *cmd;
2448         int err;
2449
2450         cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev);
2451         if (!cmd)
2452                 return -ENOENT;
2453
2454         err = cmd_status(cmd->sk, hdev->id, cmd->opcode, status);
2455         mgmt_pending_remove(cmd);
2456
2457         return err;
2458 }
2459
2460 int mgmt_discovering(struct hci_dev *hdev, u8 discovering)
2461 {
2462         struct pending_cmd *cmd;
2463
2464         if (discovering)
2465                 cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev);
2466         else
2467                 cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev);
2468
2469         if (cmd != NULL) {
2470                 cmd_complete(cmd->sk, hdev->id, cmd->opcode, NULL, 0);
2471                 mgmt_pending_remove(cmd);
2472         }
2473
2474         return mgmt_event(MGMT_EV_DISCOVERING, hdev, &discovering,
2475                                                 sizeof(discovering), NULL);
2476 }
2477
2478 int mgmt_device_blocked(struct hci_dev *hdev, bdaddr_t *bdaddr)
2479 {
2480         struct pending_cmd *cmd;
2481         struct mgmt_ev_device_blocked ev;
2482
2483         cmd = mgmt_pending_find(MGMT_OP_BLOCK_DEVICE, hdev);
2484
2485         bacpy(&ev.bdaddr, bdaddr);
2486
2487         return mgmt_event(MGMT_EV_DEVICE_BLOCKED, hdev, &ev, sizeof(ev),
2488                                                         cmd ? cmd->sk : NULL);
2489 }
2490
2491 int mgmt_device_unblocked(struct hci_dev *hdev, bdaddr_t *bdaddr)
2492 {
2493         struct pending_cmd *cmd;
2494         struct mgmt_ev_device_unblocked ev;
2495
2496         cmd = mgmt_pending_find(MGMT_OP_UNBLOCK_DEVICE, hdev);
2497
2498         bacpy(&ev.bdaddr, bdaddr);
2499
2500         return mgmt_event(MGMT_EV_DEVICE_UNBLOCKED, hdev, &ev, sizeof(ev),
2501                                                         cmd ? cmd->sk : NULL);
2502 }