net/bluetooth/hci_event.c

   1 /*
   2    BlueZ - Bluetooth protocol stack for Linux
   3    Copyright (C) 2000-2001 Qualcomm Incorporated
   4
   5    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
   6
   7    This program is free software; you can redistribute it and/or modify
   8    it under the terms of the GNU General Public License version 2 as
   9    published by the Free Software Foundation;
  10
  11    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  12    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  13    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
  14    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
  15    CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
  16    WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  17    ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  18    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  19
  20    ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
  21    COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
  22    SOFTWARE IS DISCLAIMED.
  23 */
  24
  25 /* Bluetooth HCI event handling. */
  26
  27 #include <linux/config.h>
  28 #include <linux/module.h>
  29
  30 #include <linux/types.h>
  31 #include <linux/errno.h>
  32 #include <linux/kernel.h>
  33 #include <linux/sched.h>
  34 #include <linux/slab.h>
  35 #include <linux/poll.h>
  36 #include <linux/fcntl.h>
  37 #include <linux/init.h>
  38 #include <linux/skbuff.h>
  39 #include <linux/interrupt.h>
  40 #include <linux/notifier.h>
  41 #include <net/sock.h>
  42
  43 #include <asm/system.h>
  44 #include <asm/uaccess.h>
  45 #include <asm/unaligned.h>
  46
  47 #include <net/bluetooth/bluetooth.h>
  48 #include <net/bluetooth/hci_core.h>
  49
  50 #ifndef CONFIG_BT_HCI_CORE_DEBUG
  51 #undef  BT_DBG
  52 #define BT_DBG(D...)
  53 #endif
  54
  55 /* Handle HCI Event packets */
  56
  57 /* Command Complete OGF LINK_CTL  */
  58 static void hci_cc_link_ctl(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
  59 {
  60         __u8 status;
  61
  62         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
  63
  64         switch (ocf) {
  65         case OCF_INQUIRY_CANCEL:
  66                 status = *((__u8 *) skb->data);
  67
  68                 if (status) {
  69                         BT_DBG("%s Inquiry cancel error: status 0x%x", hdev->name, status);
  70                 } else {
  71                         clear_bit(HCI_INQUIRY, &hdev->flags);
  72                         hci_req_complete(hdev, status);
  73                 }
  74                 break;
  75
  76         default:
  77                 BT_DBG("%s Command complete: ogf LINK_CTL ocf %x", hdev->name, ocf);
  78                 break;
  79         }
  80 }
  81
  82 /* Command Complete OGF LINK_POLICY  */
  83 static void hci_cc_link_policy(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
  84 {
  85         struct hci_conn *conn;
  86         struct hci_rp_role_discovery *rd;
  87
  88         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
  89
  90         switch (ocf) {
  91         case OCF_ROLE_DISCOVERY:
  92                 rd = (void *) skb->data;
  93
  94                 if (rd->status)
  95                         break;
  96
  97                 hci_dev_lock(hdev);
  98
  99                 conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rd->handle));
 100                 if (conn) {
 101                         if (rd->role)
 102                                 conn->link_mode &= ~HCI_LM_MASTER;
 103                         else
 104                                 conn->link_mode |= HCI_LM_MASTER;
 105                 }
 106
 107                 hci_dev_unlock(hdev);
 108                 break;
 109
 110         default:
 111                 BT_DBG("%s: Command complete: ogf LINK_POLICY ocf %x",
 112                                 hdev->name, ocf);
 113                 break;
 114         }
 115 }
 116
 117 /* Command Complete OGF HOST_CTL  */
 118 static void hci_cc_host_ctl(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
 119 {
 120         __u8 status, param;
 121         __u16 setting;
 122         struct hci_rp_read_voice_setting *vs;
 123         void *sent;
 124
 125         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
 126
 127         switch (ocf) {
 128         case OCF_RESET:
 129                 status = *((__u8 *) skb->data);
 130                 hci_req_complete(hdev, status);
 131                 break;
 132
 133         case OCF_SET_EVENT_FLT:
 134                 status = *((__u8 *) skb->data);
 135                 if (status) {
 136                         BT_DBG("%s SET_EVENT_FLT failed %d", hdev->name, status);
 137                 } else {
 138                         BT_DBG("%s SET_EVENT_FLT succeseful", hdev->name);
 139                 }
 140                 break;
 141
 142         case OCF_WRITE_AUTH_ENABLE:
 143                 sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_AUTH_ENABLE);
 144                 if (!sent)
 145                         break;
 146
 147                 status = *((__u8 *) skb->data);
 148                 param  = *((__u8 *) sent);
 149
 150                 if (!status) {
 151                         if (param == AUTH_ENABLED)
 152                                 set_bit(HCI_AUTH, &hdev->flags);
 153                         else
 154                                 clear_bit(HCI_AUTH, &hdev->flags);
 155                 }
 156                 hci_req_complete(hdev, status);
 157                 break;
 158
 159         case OCF_WRITE_ENCRYPT_MODE:
 160                 sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_ENCRYPT_MODE);
 161                 if (!sent)
 162                         break;
 163
 164                 status = *((__u8 *) skb->data);
 165                 param  = *((__u8 *) sent);
 166
 167                 if (!status) {
 168                         if (param)
 169                                 set_bit(HCI_ENCRYPT, &hdev->flags);
 170                         else
 171                                 clear_bit(HCI_ENCRYPT, &hdev->flags);
 172                 }
 173                 hci_req_complete(hdev, status);
 174                 break;
 175
 176         case OCF_WRITE_CA_TIMEOUT:
 177                 status = *((__u8 *) skb->data);
 178                 if (status) {
 179                         BT_DBG("%s OCF_WRITE_CA_TIMEOUT failed %d", hdev->name, status);
 180                 } else {
 181                         BT_DBG("%s OCF_WRITE_CA_TIMEOUT succeseful", hdev->name);
 182                 }
 183                 break;
 184
 185         case OCF_WRITE_PG_TIMEOUT:
 186                 status = *((__u8 *) skb->data);
 187                 if (status) {
 188                         BT_DBG("%s OCF_WRITE_PG_TIMEOUT failed %d", hdev->name, status);
 189                 } else {
 190                         BT_DBG("%s: OCF_WRITE_PG_TIMEOUT succeseful", hdev->name);
 191                 }
 192                 break;
 193
 194         case OCF_WRITE_SCAN_ENABLE:
 195                 sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE);
 196                 if (!sent)
 197                         break;
 198
 199                 status = *((__u8 *) skb->data);
 200                 param  = *((__u8 *) sent);
 201
 202                 BT_DBG("param 0x%x", param);
 203
 204                 if (!status) {
 205                         clear_bit(HCI_PSCAN, &hdev->flags);
 206                         clear_bit(HCI_ISCAN, &hdev->flags);
 207                         if (param & SCAN_INQUIRY)
 208                                 set_bit(HCI_ISCAN, &hdev->flags);
 209
 210                         if (param & SCAN_PAGE)
 211                                 set_bit(HCI_PSCAN, &hdev->flags);
 212                 }
 213                 hci_req_complete(hdev, status);
 214                 break;
 215
 216         case OCF_READ_VOICE_SETTING:
 217                 vs = (struct hci_rp_read_voice_setting *) skb->data;
 218
 219                 if (vs->status) {
 220                         BT_DBG("%s READ_VOICE_SETTING failed %d", hdev->name, vs->status);
 221                         break;
 222                 }
 223
 224                 setting = __le16_to_cpu(vs->voice_setting);
 225
 226                 if (hdev->voice_setting != setting ) {
 227                         hdev->voice_setting = setting;
 228
 229                         BT_DBG("%s: voice setting 0x%04x", hdev->name, setting);
 230
 231                         if (hdev->notify) {
 232                                 tasklet_disable(&hdev->tx_task);
 233                                 hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING);
 234                                 tasklet_enable(&hdev->tx_task);
 235                         }
 236                 }
 237                 break;
 238
 239         case OCF_WRITE_VOICE_SETTING:
 240                 sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_VOICE_SETTING);
 241                 if (!sent)
 242                         break;
 243
 244                 status = *((__u8 *) skb->data);
 245                 setting = __le16_to_cpu(get_unaligned((__le16 *) sent));
 246
 247                 if (!status && hdev->voice_setting != setting) {
 248                         hdev->voice_setting = setting;
 249
 250                         BT_DBG("%s: voice setting 0x%04x", hdev->name, setting);
 251
 252                         if (hdev->notify) {
 253                                 tasklet_disable(&hdev->tx_task);
 254                                 hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING);
 255                                 tasklet_enable(&hdev->tx_task);
 256                         }
 257                 }
 258                 hci_req_complete(hdev, status);
 259                 break;
 260
 261         case OCF_HOST_BUFFER_SIZE:
 262                 status = *((__u8 *) skb->data);
 263                 if (status) {
 264                         BT_DBG("%s OCF_BUFFER_SIZE failed %d", hdev->name, status);
 265                         hci_req_complete(hdev, status);
 266                 }
 267                 break;
 268
 269         default:
 270                 BT_DBG("%s Command complete: ogf HOST_CTL ocf %x", hdev->name, ocf);
 271                 break;
 272         }
 273 }
 274
 275 /* Command Complete OGF INFO_PARAM  */
 276 static void hci_cc_info_param(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
 277 {
 278         struct hci_rp_read_loc_features *lf;
 279         struct hci_rp_read_buffer_size *bs;
 280         struct hci_rp_read_bd_addr *ba;
 281
 282         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
 283
 284         switch (ocf) {
 285         case OCF_READ_LOCAL_FEATURES:
 286                 lf = (struct hci_rp_read_loc_features *) skb->data;
 287
 288                 if (lf->status) {
 289                         BT_DBG("%s READ_LOCAL_FEATURES failed %d", hdev->name, lf->status);
 290                         break;
 291                 }
 292
 293                 memcpy(hdev->features, lf->features, sizeof(hdev->features));
 294
 295                 /* Adjust default settings according to features
 296                  * supported by device. */
 297                 if (hdev->features[0] & LMP_3SLOT)
 298                         hdev->pkt_type |= (HCI_DM3 | HCI_DH3);
 299
 300                 if (hdev->features[0] & LMP_5SLOT)
 301                         hdev->pkt_type |= (HCI_DM5 | HCI_DH5);
 302
 303                 if (hdev->features[1] & LMP_HV2)
 304                         hdev->pkt_type |= (HCI_HV2);
 305
 306                 if (hdev->features[1] & LMP_HV3)
 307                         hdev->pkt_type |= (HCI_HV3);
 308
 309                 BT_DBG("%s: features 0x%x 0x%x 0x%x", hdev->name, lf->features[0], lf->features[1], lf->features[2]);
 310
 311                 break;
 312
 313         case OCF_READ_BUFFER_SIZE:
 314                 bs = (struct hci_rp_read_buffer_size *) skb->data;
 315
 316                 if (bs->status) {
 317                         BT_DBG("%s READ_BUFFER_SIZE failed %d", hdev->name, bs->status);
 318                         hci_req_complete(hdev, bs->status);
 319                         break;
 320                 }
 321
 322                 hdev->acl_mtu  = __le16_to_cpu(bs->acl_mtu);
 323                 hdev->sco_mtu  = bs->sco_mtu ? bs->sco_mtu : 64;
 324                 hdev->acl_pkts = hdev->acl_cnt = __le16_to_cpu(bs->acl_max_pkt);
 325                 hdev->sco_pkts = hdev->sco_cnt = __le16_to_cpu(bs->sco_max_pkt);
 326
 327                 BT_DBG("%s mtu: acl %d, sco %d max_pkt: acl %d, sco %d", hdev->name,
 328                         hdev->acl_mtu, hdev->sco_mtu, hdev->acl_pkts, hdev->sco_pkts);
 329                 break;
 330
 331         case OCF_READ_BD_ADDR:
 332                 ba = (struct hci_rp_read_bd_addr *) skb->data;
 333
 334                 if (!ba->status) {
 335                         bacpy(&hdev->bdaddr, &ba->bdaddr);
 336                 } else {
 337                         BT_DBG("%s: READ_BD_ADDR failed %d", hdev->name, ba->status);
 338                 }
 339
 340                 hci_req_complete(hdev, ba->status);
 341                 break;
 342
 343         default:
 344                 BT_DBG("%s Command complete: ogf INFO_PARAM ocf %x", hdev->name, ocf);
 345                 break;
 346         }
 347 }
 348
 349 /* Command Status OGF LINK_CTL  */
 350 static inline void hci_cs_create_conn(struct hci_dev *hdev, __u8 status)
 351 {
 352         struct hci_conn *conn;
 353         struct hci_cp_create_conn *cp = hci_sent_cmd_data(hdev, OGF_LINK_CTL, OCF_CREATE_CONN);
 354
 355         if (!cp)
 356                 return;
 357
 358         hci_dev_lock(hdev);
 359
 360         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
 361
 362         BT_DBG("%s status 0x%x bdaddr %s conn %p", hdev->name,
 363                         status, batostr(&cp->bdaddr), conn);
 364
 365         if (status) {
 366                 if (conn && conn->state == BT_CONNECT) {
 367                         conn->state = BT_CLOSED;
 368                         hci_proto_connect_cfm(conn, status);
 369                         hci_conn_del(conn);
 370                 }
 371         } else {
 372                 if (!conn) {
 373                         conn = hci_conn_add(hdev, ACL_LINK, &cp->bdaddr);
 374                         if (conn) {
 375                                 conn->out = 1;
 376                                 conn->link_mode |= HCI_LM_MASTER;
 377                         } else
 378                                 BT_ERR("No memmory for new connection");
 379                 }
 380         }
 381
 382         hci_dev_unlock(hdev);
 383 }
 384
 385 static void hci_cs_link_ctl(struct hci_dev *hdev, __u16 ocf, __u8 status)
 386 {
 387         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
 388
 389         switch (ocf) {
 390         case OCF_CREATE_CONN:
 391                 hci_cs_create_conn(hdev, status);
 392                 break;
 393
 394         case OCF_ADD_SCO:
 395                 if (status) {
 396                         struct hci_conn *acl, *sco;
 397                         struct hci_cp_add_sco *cp = hci_sent_cmd_data(hdev, OGF_LINK_CTL, OCF_ADD_SCO);
 398                         __u16 handle;
 399
 400                         if (!cp)
 401                                 break;
 402
 403                         handle = __le16_to_cpu(cp->handle);
 404
 405                         BT_DBG("%s Add SCO error: handle %d status 0x%x", hdev->name, handle, status);
 406
 407                         hci_dev_lock(hdev);
 408
 409                         acl = hci_conn_hash_lookup_handle(hdev, handle);
 410                         if (acl && (sco = acl->link)) {
 411                                 sco->state = BT_CLOSED;
 412
 413                                 hci_proto_connect_cfm(sco, status);
 414                                 hci_conn_del(sco);
 415                         }
 416
 417                         hci_dev_unlock(hdev);
 418                 }
 419                 break;
 420
 421         case OCF_INQUIRY:
 422                 if (status) {
 423                         BT_DBG("%s Inquiry error: status 0x%x", hdev->name, status);
 424                         hci_req_complete(hdev, status);
 425                 } else {
 426                         set_bit(HCI_INQUIRY, &hdev->flags);
 427                 }
 428                 break;
 429
 430         default:
 431                 BT_DBG("%s Command status: ogf LINK_CTL ocf %x status %d",
 432                         hdev->name, ocf, status);
 433                 break;
 434         }
 435 }
 436
 437 /* Command Status OGF LINK_POLICY */
 438 static void hci_cs_link_policy(struct hci_dev *hdev, __u16 ocf, __u8 status)
 439 {
 440         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
 441
 442         switch (ocf) {
 443         default:
 444                 BT_DBG("%s Command status: ogf HOST_POLICY ocf %x", hdev->name, ocf);
 445                 break;
 446         }
 447 }
 448
 449 /* Command Status OGF HOST_CTL */
 450 static void hci_cs_host_ctl(struct hci_dev *hdev, __u16 ocf, __u8 status)
 451 {
 452         BT_DBG("%s ocf 0x%x", hdev->name, ocf);
 453
 454         switch (ocf) {
 455         default:
 456                 BT_DBG("%s Command status: ogf HOST_CTL ocf %x", hdev->name, ocf);
 457                 break;
 458         }
 459 }
 460
 461 /* Command Status OGF INFO_PARAM  */
 462 static void hci_cs_info_param(struct hci_dev *hdev, __u16 ocf, __u8 status)
 463 {
 464         BT_DBG("%s: hci_cs_info_param: ocf 0x%x", hdev->name, ocf);
 465
 466         switch (ocf) {
 467         default:
 468                 BT_DBG("%s Command status: ogf INFO_PARAM ocf %x", hdev->name, ocf);
 469                 break;
 470         }
 471 }
 472
 473 /* Inquiry Complete */
 474 static inline void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 475 {
 476         __u8 status = *((__u8 *) skb->data);
 477
 478         BT_DBG("%s status %d", hdev->name, status);
 479
 480         clear_bit(HCI_INQUIRY, &hdev->flags);
 481         hci_req_complete(hdev, status);
 482 }
 483
 484 /* Inquiry Result */
 485 static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
 486 {
 487         struct inquiry_data data;
 488         struct inquiry_info *info = (struct inquiry_info *) (skb->data + 1);
 489         int num_rsp = *((__u8 *) skb->data);
 490
 491         BT_DBG("%s num_rsp %d", hdev->name, num_rsp);
 492
 493         if (!num_rsp)
 494                 return;
 495
 496         hci_dev_lock(hdev);
 497
 498         for (; num_rsp; num_rsp--) {
 499                 bacpy(&data.bdaddr, &info->bdaddr);
 500                 data.pscan_rep_mode     = info->pscan_rep_mode;
 501                 data.pscan_period_mode  = info->pscan_period_mode;
 502                 data.pscan_mode         = info->pscan_mode;
 503                 memcpy(data.dev_class, info->dev_class, 3);
 504                 data.clock_offset       = info->clock_offset;
 505                 data.rssi               = 0x00;
 506                 info++;
 507                 hci_inquiry_cache_update(hdev, &data);
 508         }
 509
 510         hci_dev_unlock(hdev);
 511 }
 512
 513 /* Inquiry Result With RSSI */
 514 static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct sk_buff *skb)
 515 {
 516         struct inquiry_data data;
 517         int num_rsp = *((__u8 *) skb->data);
 518
 519         BT_DBG("%s num_rsp %d", hdev->name, num_rsp);
 520
 521         if (!num_rsp)
 522                 return;
 523
 524         hci_dev_lock(hdev);
 525
 526         if ((skb->len - 1) / num_rsp != sizeof(struct inquiry_info_with_rssi)) {
 527                 struct inquiry_info_with_rssi_and_pscan_mode *info =
 528                         (struct inquiry_info_with_rssi_and_pscan_mode *) (skb->data + 1);
 529
 530                 for (; num_rsp; num_rsp--) {
 531                         bacpy(&data.bdaddr, &info->bdaddr);
 532                         data.pscan_rep_mode     = info->pscan_rep_mode;
 533                         data.pscan_period_mode  = info->pscan_period_mode;
 534                         data.pscan_mode         = info->pscan_mode;
 535                         memcpy(data.dev_class, info->dev_class, 3);
 536                         data.clock_offset       = info->clock_offset;
 537                         data.rssi               = info->rssi;
 538                         info++;
 539                         hci_inquiry_cache_update(hdev, &data);
 540                 }
 541         } else {
 542                 struct inquiry_info_with_rssi *info =
 543                         (struct inquiry_info_with_rssi *) (skb->data + 1);
 544
 545                 for (; num_rsp; num_rsp--) {
 546                         bacpy(&data.bdaddr, &info->bdaddr);
 547                         data.pscan_rep_mode     = info->pscan_rep_mode;
 548                         data.pscan_period_mode  = info->pscan_period_mode;
 549                         data.pscan_mode         = 0x00;
 550                         memcpy(data.dev_class, info->dev_class, 3);
 551                         data.clock_offset       = info->clock_offset;
 552                         data.rssi               = info->rssi;
 553                         info++;
 554                         hci_inquiry_cache_update(hdev, &data);
 555                 }
 556         }
 557
 558         hci_dev_unlock(hdev);
 559 }
 560
 561 /* Extended Inquiry Result */
 562 static inline void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
 563 {
 564         struct inquiry_data data;
 565         struct extended_inquiry_info *info = (struct extended_inquiry_info *) (skb->data + 1);
 566         int num_rsp = *((__u8 *) skb->data);
 567
 568         BT_DBG("%s num_rsp %d", hdev->name, num_rsp);
 569
 570         if (!num_rsp)
 571                 return;
 572
 573         hci_dev_lock(hdev);
 574
 575         for (; num_rsp; num_rsp--) {
 576                 bacpy(&data.bdaddr, &info->bdaddr);
 577                 data.pscan_rep_mode     = info->pscan_rep_mode;
 578                 data.pscan_period_mode  = info->pscan_period_mode;
 579                 data.pscan_mode         = 0x00;
 580                 memcpy(data.dev_class, info->dev_class, 3);
 581                 data.clock_offset       = info->clock_offset;
 582                 data.rssi               = info->rssi;
 583                 info++;
 584                 hci_inquiry_cache_update(hdev, &data);
 585         }
 586
 587         hci_dev_unlock(hdev);
 588 }
 589
 590 /* Connect Request */
 591 static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 592 {
 593         struct hci_ev_conn_request *ev = (struct hci_ev_conn_request *) skb->data;
 594         int mask = hdev->link_mode;
 595
 596         BT_DBG("%s Connection request: %s type 0x%x", hdev->name,
 597                         batostr(&ev->bdaddr), ev->link_type);
 598
 599         mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type);
 600
 601         if (mask & HCI_LM_ACCEPT) {
 602                 /* Connection accepted */
 603                 struct hci_conn *conn;
 604                 struct hci_cp_accept_conn_req cp;
 605
 606                 hci_dev_lock(hdev);
 607                 conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr);
 608                 if (!conn) {
 609                         if (!(conn = hci_conn_add(hdev, ev->link_type, &ev->bdaddr))) {
 610                                 BT_ERR("No memmory for new connection");
 611                                 hci_dev_unlock(hdev);
 612                                 return;
 613                         }
 614                 }
 615                 memcpy(conn->dev_class, ev->dev_class, 3);
 616                 conn->state = BT_CONNECT;
 617                 hci_dev_unlock(hdev);
 618
 619                 bacpy(&cp.bdaddr, &ev->bdaddr);
 620
 621                 if (lmp_rswitch_capable(hdev) && (mask & HCI_LM_MASTER))
 622                         cp.role = 0x00; /* Become master */
 623                 else
 624                         cp.role = 0x01; /* Remain slave */
 625
 626                 hci_send_cmd(hdev, OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ, sizeof(cp), &cp);
 627         } else {
 628                 /* Connection rejected */
 629                 struct hci_cp_reject_conn_req cp;
 630
 631                 bacpy(&cp.bdaddr, &ev->bdaddr);
 632                 cp.reason = 0x0f;
 633                 hci_send_cmd(hdev, OGF_LINK_CTL, OCF_REJECT_CONN_REQ, sizeof(cp), &cp);
 634         }
 635 }
 636
 637 /* Connect Complete */
 638 static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 639 {
 640         struct hci_ev_conn_complete *ev = (struct hci_ev_conn_complete *) skb->data;
 641         struct hci_conn *conn = NULL;
 642
 643         BT_DBG("%s", hdev->name);
 644
 645         hci_dev_lock(hdev);
 646
 647         conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr);
 648         if (!conn) {
 649                 hci_dev_unlock(hdev);
 650                 return;
 651         }
 652
 653         if (!ev->status) {
 654                 conn->handle = __le16_to_cpu(ev->handle);
 655                 conn->state  = BT_CONNECTED;
 656
 657                 if (test_bit(HCI_AUTH, &hdev->flags))
 658                         conn->link_mode |= HCI_LM_AUTH;
 659
 660                 if (test_bit(HCI_ENCRYPT, &hdev->flags))
 661                         conn->link_mode |= HCI_LM_ENCRYPT;
 662
 663                 /* Set link policy */
 664                 if (conn->type == ACL_LINK && hdev->link_policy) {
 665                         struct hci_cp_write_link_policy cp;
 666                         cp.handle = ev->handle;
 667                         cp.policy = __cpu_to_le16(hdev->link_policy);
 668                         hci_send_cmd(hdev, OGF_LINK_POLICY, OCF_WRITE_LINK_POLICY, sizeof(cp), &cp);
 669                 }
 670
 671                 /* Set packet type for incoming connection */
 672                 if (!conn->out) {
 673                         struct hci_cp_change_conn_ptype cp;
 674                         cp.handle = ev->handle;
 675                         cp.pkt_type = (conn->type == ACL_LINK) ?
 676                                 __cpu_to_le16(hdev->pkt_type & ACL_PTYPE_MASK):
 677                                 __cpu_to_le16(hdev->pkt_type & SCO_PTYPE_MASK);
 678
 679                         hci_send_cmd(hdev, OGF_LINK_CTL, OCF_CHANGE_CONN_PTYPE, sizeof(cp), &cp);
 680                 }
 681         } else
 682                 conn->state = BT_CLOSED;
 683
 684         if (conn->type == ACL_LINK) {
 685                 struct hci_conn *sco = conn->link;
 686                 if (sco) {
 687                         if (!ev->status)
 688                                 hci_add_sco(sco, conn->handle);
 689                         else {
 690                                 hci_proto_connect_cfm(sco, ev->status);
 691                                 hci_conn_del(sco);
 692                         }
 693                 }
 694         }
 695
 696         hci_proto_connect_cfm(conn, ev->status);
 697         if (ev->status)
 698                 hci_conn_del(conn);
 699
 700         hci_dev_unlock(hdev);
 701 }
 702
 703 /* Disconnect Complete */
 704 static inline void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 705 {
 706         struct hci_ev_disconn_complete *ev = (struct hci_ev_disconn_complete *) skb->data;
 707         struct hci_conn *conn = NULL;
 708         __u16 handle = __le16_to_cpu(ev->handle);
 709
 710         BT_DBG("%s status %d", hdev->name, ev->status);
 711
 712         if (ev->status)
 713                 return;
 714
 715         hci_dev_lock(hdev);
 716
 717         conn = hci_conn_hash_lookup_handle(hdev, handle);
 718         if (conn) {
 719                 conn->state = BT_CLOSED;
 720                 hci_proto_disconn_ind(conn, ev->reason);
 721                 hci_conn_del(conn);
 722         }
 723
 724         hci_dev_unlock(hdev);
 725 }
 726
 727 /* Number of completed packets */
 728 static inline void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb)
 729 {
 730         struct hci_ev_num_comp_pkts *ev = (struct hci_ev_num_comp_pkts *) skb->data;
 731         __le16 *ptr;
 732         int i;
 733
 734         skb_pull(skb, sizeof(*ev));
 735
 736         BT_DBG("%s num_hndl %d", hdev->name, ev->num_hndl);
 737
 738         if (skb->len < ev->num_hndl * 4) {
 739                 BT_DBG("%s bad parameters", hdev->name);
 740                 return;
 741         }
 742
 743         tasklet_disable(&hdev->tx_task);
 744
 745         for (i = 0, ptr = (__le16 *) skb->data; i < ev->num_hndl; i++) {
 746                 struct hci_conn *conn;
 747                 __u16  handle, count;
 748
 749                 handle = __le16_to_cpu(get_unaligned(ptr++));
 750                 count  = __le16_to_cpu(get_unaligned(ptr++));
 751
 752                 conn = hci_conn_hash_lookup_handle(hdev, handle);
 753                 if (conn) {
 754                         conn->sent -= count;
 755
 756                         if (conn->type == SCO_LINK) {
 757                                 if ((hdev->sco_cnt += count) > hdev->sco_pkts)
 758                                         hdev->sco_cnt = hdev->sco_pkts;
 759                         } else {
 760                                 if ((hdev->acl_cnt += count) > hdev->acl_pkts)
 761                                         hdev->acl_cnt = hdev->acl_pkts;
 762                         }
 763                 }
 764         }
 765         hci_sched_tx(hdev);
 766
 767         tasklet_enable(&hdev->tx_task);
 768 }
 769
 770 /* Role Change */
 771 static inline void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 772 {
 773         struct hci_ev_role_change *ev = (struct hci_ev_role_change *) skb->data;
 774         struct hci_conn *conn = NULL;
 775
 776         BT_DBG("%s status %d", hdev->name, ev->status);
 777
 778         hci_dev_lock(hdev);
 779
 780         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr);
 781         if (conn) {
 782                 if (!ev->status) {
 783                         if (ev->role)
 784                                 conn->link_mode &= ~HCI_LM_MASTER;
 785                         else
 786                                 conn->link_mode |= HCI_LM_MASTER;
 787                 }
 788
 789                 clear_bit(HCI_CONN_RSWITCH_PEND, &conn->pend);
 790
 791                 hci_role_switch_cfm(conn, ev->status, ev->role);
 792         }
 793
 794         hci_dev_unlock(hdev);
 795 }
 796
 797 /* Authentication Complete */
 798 static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 799 {
 800         struct hci_ev_auth_complete *ev = (struct hci_ev_auth_complete *) skb->data;
 801         struct hci_conn *conn = NULL;
 802         __u16 handle = __le16_to_cpu(ev->handle);
 803
 804         BT_DBG("%s status %d", hdev->name, ev->status);
 805
 806         hci_dev_lock(hdev);
 807
 808         conn = hci_conn_hash_lookup_handle(hdev, handle);
 809         if (conn) {
 810                 if (!ev->status)
 811                         conn->link_mode |= HCI_LM_AUTH;
 812
 813                 clear_bit(HCI_CONN_AUTH_PEND, &conn->pend);
 814
 815                 hci_auth_cfm(conn, ev->status);
 816
 817                 if (test_bit(HCI_CONN_ENCRYPT_PEND, &conn->pend)) {
 818                         if (!ev->status) {
 819                                 struct hci_cp_set_conn_encrypt cp;
 820                                 cp.handle  = __cpu_to_le16(conn->handle);
 821                                 cp.encrypt = 1;
 822                                 hci_send_cmd(conn->hdev, OGF_LINK_CTL,
 823                                                 OCF_SET_CONN_ENCRYPT,
 824                                                 sizeof(cp), &cp);
 825                         } else {
 826                                 clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->pend);
 827                                 hci_encrypt_cfm(conn, ev->status, 0x00);
 828                         }
 829                 }
 830         }
 831
 832         hci_dev_unlock(hdev);
 833 }
 834
 835 /* Encryption Change */
 836 static inline void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 837 {
 838         struct hci_ev_encrypt_change *ev = (struct hci_ev_encrypt_change *) skb->data;
 839         struct hci_conn *conn = NULL;
 840         __u16 handle = __le16_to_cpu(ev->handle);
 841
 842         BT_DBG("%s status %d", hdev->name, ev->status);
 843
 844         hci_dev_lock(hdev);
 845
 846         conn = hci_conn_hash_lookup_handle(hdev, handle);
 847         if (conn) {
 848                 if (!ev->status) {
 849                         if (ev->encrypt)
 850                                 conn->link_mode |= HCI_LM_ENCRYPT;
 851                         else
 852                                 conn->link_mode &= ~HCI_LM_ENCRYPT;
 853                 }
 854
 855                 clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->pend);
 856
 857                 hci_encrypt_cfm(conn, ev->status, ev->encrypt);
 858         }
 859
 860         hci_dev_unlock(hdev);
 861 }
 862
 863 /* Change Connection Link Key Complete */
 864 static inline void hci_change_conn_link_key_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 865 {
 866         struct hci_ev_change_conn_link_key_complete *ev = (struct hci_ev_change_conn_link_key_complete *) skb->data;
 867         struct hci_conn *conn = NULL;
 868         __u16 handle = __le16_to_cpu(ev->handle);
 869
 870         BT_DBG("%s status %d", hdev->name, ev->status);
 871
 872         hci_dev_lock(hdev);
 873
 874         conn = hci_conn_hash_lookup_handle(hdev, handle);
 875         if (conn) {
 876                 if (!ev->status)
 877                         conn->link_mode |= HCI_LM_SECURE;
 878
 879                 clear_bit(HCI_CONN_AUTH_PEND, &conn->pend);
 880
 881                 hci_key_change_cfm(conn, ev->status);
 882         }
 883
 884         hci_dev_unlock(hdev);
 885 }
 886
 887 /* Pin Code Request*/
 888 static inline void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 889 {
 890 }
 891
 892 /* Link Key Request */
 893 static inline void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 894 {
 895 }
 896
 897 /* Link Key Notification */
 898 static inline void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb)
 899 {
 900 }
 901
 902 /* Clock Offset */
 903 static inline void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb)
 904 {
 905         struct hci_ev_clock_offset *ev = (struct hci_ev_clock_offset *) skb->data;
 906         struct hci_conn *conn = NULL;
 907         __u16 handle = __le16_to_cpu(ev->handle);
 908
 909         BT_DBG("%s status %d", hdev->name, ev->status);
 910
 911         hci_dev_lock(hdev);
 912
 913         conn = hci_conn_hash_lookup_handle(hdev, handle);
 914         if (conn && !ev->status) {
 915                 struct inquiry_entry *ie;
 916
 917                 if ((ie = hci_inquiry_cache_lookup(hdev, &conn->dst))) {
 918                         ie->data.clock_offset = ev->clock_offset;
 919                         ie->timestamp = jiffies;
 920                 }
 921         }
 922
 923         hci_dev_unlock(hdev);
 924 }
 925
 926 /* Page Scan Repetition Mode */
 927 static inline void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb)
 928 {
 929         struct hci_ev_pscan_rep_mode *ev = (struct hci_ev_pscan_rep_mode *) skb->data;
 930         struct inquiry_entry *ie;
 931
 932         BT_DBG("%s", hdev->name);
 933
 934         hci_dev_lock(hdev);
 935
 936         if ((ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr))) {
 937                 ie->data.pscan_rep_mode = ev->pscan_rep_mode;
 938                 ie->timestamp = jiffies;
 939         }
 940
 941         hci_dev_unlock(hdev);
 942 }
 943
 944 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 945 {
 946         struct hci_event_hdr *hdr = (struct hci_event_hdr *) skb->data;
 947         struct hci_ev_cmd_complete *ec;
 948         struct hci_ev_cmd_status *cs;
 949         u16 opcode, ocf, ogf;
 950
 951         skb_pull(skb, HCI_EVENT_HDR_SIZE);
 952
 953         BT_DBG("%s evt 0x%x", hdev->name, hdr->evt);
 954
 955         switch (hdr->evt) {
 956         case HCI_EV_NUM_COMP_PKTS:
 957                 hci_num_comp_pkts_evt(hdev, skb);
 958                 break;
 959
 960         case HCI_EV_INQUIRY_COMPLETE:
 961                 hci_inquiry_complete_evt(hdev, skb);
 962                 break;
 963
 964         case HCI_EV_INQUIRY_RESULT:
 965                 hci_inquiry_result_evt(hdev, skb);
 966                 break;
 967
 968         case HCI_EV_INQUIRY_RESULT_WITH_RSSI:
 969                 hci_inquiry_result_with_rssi_evt(hdev, skb);
 970                 break;
 971
 972         case HCI_EV_EXTENDED_INQUIRY_RESULT:
 973                 hci_extended_inquiry_result_evt(hdev, skb);
 974                 break;
 975
 976         case HCI_EV_CONN_REQUEST:
 977                 hci_conn_request_evt(hdev, skb);
 978                 break;
 979
 980         case HCI_EV_CONN_COMPLETE:
 981                 hci_conn_complete_evt(hdev, skb);
 982                 break;
 983
 984         case HCI_EV_DISCONN_COMPLETE:
 985                 hci_disconn_complete_evt(hdev, skb);
 986                 break;
 987
 988         case HCI_EV_ROLE_CHANGE:
 989                 hci_role_change_evt(hdev, skb);
 990                 break;
 991
 992         case HCI_EV_AUTH_COMPLETE:
 993                 hci_auth_complete_evt(hdev, skb);
 994                 break;
 995
 996         case HCI_EV_ENCRYPT_CHANGE:
 997                 hci_encrypt_change_evt(hdev, skb);
 998                 break;
 999
1000         case HCI_EV_CHANGE_CONN_LINK_KEY_COMPLETE:
1001                 hci_change_conn_link_key_complete_evt(hdev, skb);
1002                 break;
1003
1004         case HCI_EV_PIN_CODE_REQ:
1005                 hci_pin_code_request_evt(hdev, skb);
1006                 break;
1007
1008         case HCI_EV_LINK_KEY_REQ:
1009                 hci_link_key_request_evt(hdev, skb);
1010                 break;
1011
1012         case HCI_EV_LINK_KEY_NOTIFY:
1013                 hci_link_key_notify_evt(hdev, skb);
1014                 break;
1015
1016         case HCI_EV_CLOCK_OFFSET:
1017                 hci_clock_offset_evt(hdev, skb);
1018                 break;
1019
1020         case HCI_EV_PSCAN_REP_MODE:
1021                 hci_pscan_rep_mode_evt(hdev, skb);
1022                 break;
1023
1024         case HCI_EV_CMD_STATUS:
1025                 cs = (struct hci_ev_cmd_status *) skb->data;
1026                 skb_pull(skb, sizeof(cs));
1027
1028                 opcode = __le16_to_cpu(cs->opcode);
1029                 ogf = hci_opcode_ogf(opcode);
1030                 ocf = hci_opcode_ocf(opcode);
1031
1032                 switch (ogf) {
1033                 case OGF_INFO_PARAM:
1034                         hci_cs_info_param(hdev, ocf, cs->status);
1035                         break;
1036
1037                 case OGF_HOST_CTL:
1038                         hci_cs_host_ctl(hdev, ocf, cs->status);
1039                         break;
1040
1041                 case OGF_LINK_CTL:
1042                         hci_cs_link_ctl(hdev, ocf, cs->status);
1043                         break;
1044
1045                 case OGF_LINK_POLICY:
1046                         hci_cs_link_policy(hdev, ocf, cs->status);
1047                         break;
1048
1049                 default:
1050                         BT_DBG("%s Command Status OGF %x", hdev->name, ogf);
1051                         break;
1052                 }
1053
1054                 if (cs->ncmd) {
1055                         atomic_set(&hdev->cmd_cnt, 1);
1056                         if (!skb_queue_empty(&hdev->cmd_q))
1057                                 hci_sched_cmd(hdev);
1058                 }
1059                 break;
1060
1061         case HCI_EV_CMD_COMPLETE:
1062                 ec = (struct hci_ev_cmd_complete *) skb->data;
1063                 skb_pull(skb, sizeof(*ec));
1064
1065                 opcode = __le16_to_cpu(ec->opcode);
1066                 ogf = hci_opcode_ogf(opcode);
1067                 ocf = hci_opcode_ocf(opcode);
1068
1069                 switch (ogf) {
1070                 case OGF_INFO_PARAM:
1071                         hci_cc_info_param(hdev, ocf, skb);
1072                         break;
1073
1074                 case OGF_HOST_CTL:
1075                         hci_cc_host_ctl(hdev, ocf, skb);
1076                         break;
1077
1078                 case OGF_LINK_CTL:
1079                         hci_cc_link_ctl(hdev, ocf, skb);
1080                         break;
1081
1082                 case OGF_LINK_POLICY:
1083                         hci_cc_link_policy(hdev, ocf, skb);
1084                         break;
1085
1086                 default:
1087                         BT_DBG("%s Command Completed OGF %x", hdev->name, ogf);
1088                         break;
1089                 }
1090
1091                 if (ec->ncmd) {
1092                         atomic_set(&hdev->cmd_cnt, 1);
1093                         if (!skb_queue_empty(&hdev->cmd_q))
1094                                 hci_sched_cmd(hdev);
1095                 }
1096                 break;
1097         }
1098
1099         kfree_skb(skb);
1100         hdev->stat.evt_rx++;
1101 }
1102
1103 /* Generate internal stack event */
1104 void hci_si_event(struct hci_dev *hdev, int type, int dlen, void *data)
1105 {
1106         struct hci_event_hdr *hdr;
1107         struct hci_ev_stack_internal *ev;
1108         struct sk_buff *skb;
1109
1110         skb = bt_skb_alloc(HCI_EVENT_HDR_SIZE + sizeof(*ev) + dlen, GFP_ATOMIC);
1111         if (!skb)
1112                 return;
1113
1114         hdr = (void *) skb_put(skb, HCI_EVENT_HDR_SIZE);
1115         hdr->evt  = HCI_EV_STACK_INTERNAL;
1116         hdr->plen = sizeof(*ev) + dlen;
1117
1118         ev  = (void *) skb_put(skb, sizeof(*ev) + dlen);
1119         ev->type = type;
1120         memcpy(ev->data, data, dlen);
1121
1122         bt_cb(skb)->incoming = 1;
1123         __net_timestamp(skb);
1124
1125         bt_cb(skb)->pkt_type = HCI_EVENT_PKT;
1126         skb->dev = (void *) hdev;
1127         hci_send_to_sock(hdev, skb);
1128         kfree_skb(skb);
1129 }