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