lwbt/hci.c

   1 /*
   2  * Copyright (c) 2003 EISLAB, Lulea University of Technology.
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without modification,
   6  * are permitted provided that the following conditions are met:
   7  *
   8  * 1. Redistributions of source code must retain the above copyright notice,
   9  *    this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright notice,
  11  *    this list of conditions and the following disclaimer in the documentation
  12  *    and/or other materials provided with the distribution.
  13  * 3. The name of the author may not be used to endorse or promote products
  14  *    derived from this software without specific prior written permission.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
  17  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  18  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
  19  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  20  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
  21  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
  24  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
  25  * OF SUCH DAMAGE.
  26  *
  27  * This file is part of the lwBT Bluetooth stack.
  28  *
  29  * Author: Conny Ohult <conny@sm.luth.se>
  30  *
  31  */
  32
  33 /*-----------------------------------------------------------------------------------*/
  34 /* hci.c
  35  *
  36  * Implementation of the Host Controller Interface (HCI). A command interface to the
  37  * baseband controller and link manager, and gives access to hardware status and
  38  * control registers.
  39  *
  40  */
  41 /*-----------------------------------------------------------------------------------*/
  42
  43 #include <stdio.h>
  44 #include <stdlib.h>
  45 #include <string.h>
  46 #include <malloc.h>
  47 #include <ogcsys.h>
  48 #include <gccore.h>
  49
  50 #include "hci.h"
  51 #include "l2cap.h"
  52 #include "btmemr.h"
  53 #include "btmemb.h"
  54 #include "btpbuf.h"
  55 #include "physbusif.h"
  56
  57 struct hci_pcb *hci_dev = NULL;
  58 struct hci_link *hci_active_links = NULL;
  59 struct hci_link *hci_tmp_link = NULL;
  60 struct hci_link_key *hci_tmp_key = NULL;
  61
  62 MEMB(hci_pcbs,sizeof(struct hci_pcb),MEMB_NUM_HCI_PCB);
  63 MEMB(hci_links,sizeof(struct hci_link),MEMB_NUM_HCI_LINK);
  64 MEMB(hci_inq_results,sizeof(struct hci_inq_res),MEMB_NUM_HCI_INQ);
  65 MEMB(hci_link_key_results,sizeof(struct hci_link_key),MEMB_NUM_HCI_LINK_KEY);
  66
  67 err_t hci_init(void)
  68 {
  69         btmemr_init();
  70         btpbuf_init();
  71
  72         btmemb_init(&hci_pcbs);
  73         btmemb_init(&hci_links);
  74         btmemb_init(&hci_inq_results);
  75         btmemb_init(&hci_link_key_results);
  76
  77         if((hci_dev=btmemb_alloc(&hci_pcbs))==NULL) {
  78                 ERROR("hci_init: Could not allocate memory for hci_dev\n");
  79                 return ERR_MEM;
  80         }
  81         memset(hci_dev,0,sizeof(struct hci_pcb));
  82
  83         hci_active_links = NULL;
  84         hci_tmp_link = NULL;
  85
  86         return ERR_OK;
  87 }
  88
  89 struct hci_link* hci_new(void)
  90 {
  91         struct hci_link *link;
  92
  93         link = btmemb_alloc(&hci_links);
  94         if(link==NULL) return NULL;
  95
  96         memset(link,0,sizeof(struct hci_link));
  97         return link;
  98 }
  99
 100 struct hci_link* hci_get_link(struct bd_addr *bdaddr)
 101 {
 102         struct hci_link *link;
 103
 104         for(link=hci_active_links;link!=NULL;link=link->next) {
 105                 if(bd_addr_cmp(&(link->bdaddr),bdaddr)) break;
 106         }
 107         return link;
 108 }
 109
 110 /*-----------------------------------------------------------------------------------*/
 111 /*
 112  * hci_close():
 113  *
 114  * Close the link control block.
 115  */
 116 /*-----------------------------------------------------------------------------------*/
 117 err_t hci_close(struct hci_link *link)
 118 {
 119         if(link->p != NULL) {
 120                 btpbuf_free(link->p);
 121         }
 122
 123         HCI_RMV(&(hci_active_links), link);
 124         btmemb_free(&hci_links, link);
 125         link = NULL;
 126         return ERR_OK;
 127 }
 128
 129 /*-----------------------------------------------------------------------------------*/
 130 /*
 131  * hci_reset_all():
 132  *
 133  * Closes all active link control blocks.
 134  */
 135 /*-----------------------------------------------------------------------------------*/
 136 void hci_reset_all(void)
 137 {
 138         struct hci_link *link,*tlink;
 139         struct hci_inq_res *ires,*tires;
 140         struct hci_link_key *ikeys,*tikeys;
 141
 142         for(link=hci_active_links;link!=NULL;) {
 143                 tlink = link->next;
 144                 hci_close(link);
 145                 link = tlink;
 146         }
 147         hci_active_links = NULL;
 148
 149         for(ires=hci_dev->ires;ires!=NULL;) {
 150                 tires = ires->next;
 151                 btmemb_free(&hci_inq_results,ires);
 152                 ires = tires;
 153         }
 154
 155         for(ikeys=hci_dev->keyres;ikeys!=NULL;) {
 156                 tikeys = ikeys->next;
 157                 btmemb_free(&hci_inq_results,ikeys);
 158                 ikeys = tikeys;
 159         }
 160         btmemb_free(&hci_pcbs,hci_dev);
 161
 162         hci_init();
 163 }
 164
 165 void hci_arg(void *arg)
 166 {
 167         hci_dev->cbarg = arg;
 168 }
 169
 170 void hci_cmd_complete(err_t (*cmd_complete)(void *arg,struct hci_pcb *pcb,u8_t ogf,u8_t ocf,u8_t result))
 171 {
 172         hci_dev->cmd_complete = cmd_complete;
 173 }
 174
 175 /*-----------------------------------------------------------------------------------*/
 176 /*
 177  * hci_pin_req():
 178  *
 179  * Used to specify the function that should be called when HCI has received a
 180  * PIN code request event.
 181  */
 182 /*-----------------------------------------------------------------------------------*/
 183 void hci_pin_req(err_t (* pin_req)(void *arg, struct bd_addr *bdaddr))
 184 {
 185         hci_dev->pin_req = pin_req;
 186 }
 187 /*-----------------------------------------------------------------------------------*/
 188 /*
 189  * hci_link_key_not():
 190  *
 191  * Used to specify the function that should be called when HCI has received a
 192  * link key notification event.
 193  */
 194 /*-----------------------------------------------------------------------------------*/
 195 void hci_link_key_not(err_t (* link_key_not)(void *arg, struct bd_addr *bdaddr, u8_t *key))
 196 {
 197         hci_dev->link_key_not = link_key_not;
 198 }
 199
 200 /*-----------------------------------------------------------------------------------*/
 201 /*
 202  * hci_connection_complete():
 203  *
 204  * Used to specify the function that should be called when HCI has received a
 205  * connection complete event.
 206  */
 207 /*-----------------------------------------------------------------------------------*/
 208 void hci_connection_complete(err_t (* conn_complete)(void *arg, struct bd_addr *bdaddr))
 209 {
 210         hci_dev->conn_complete = conn_complete;
 211 }
 212
 213 /*-----------------------------------------------------------------------------------*/
 214 /*
 215  * hci_wlp_complete():
 216  *
 217  * Used to specify the function that should be called when HCI has received a
 218  * successful write link policy complete event.
 219  */
 220 /*-----------------------------------------------------------------------------------*/
 221 void  hci_wlp_complete(err_t (* wlp_complete)(void *arg, struct bd_addr *bdaddr))
 222 {
 223         hci_dev->wlp_complete = wlp_complete;
 224 }
 225
 226 void hci_conn_req(err_t (*conn_req)(void *arg,struct bd_addr *bdaddr,u8_t *cod,u8_t link_type))
 227 {
 228         hci_dev->conn_req = conn_req;
 229 }
 230
 231 err_t hci_reg_dev_info(struct bd_addr *bdaddr,u8_t *cod,u8_t psrm,u8_t psm,u16_t co)
 232 {
 233         struct hci_inq_res *ires;
 234
 235         if(hci_dev==NULL) return ERR_VAL;
 236
 237         if((ires=btmemb_alloc(&hci_inq_results))!=NULL) {
 238                 bd_addr_set(&(ires->bdaddr),bdaddr);
 239                 memcpy(ires->cod,cod,3);
 240                 ires->psrm = psrm;
 241                 ires->psm = psm;
 242                 ires->co = co;
 243                 ires->next = NULL;
 244
 245                 HCI_REG(&(hci_dev->ires),ires);
 246                 return ERR_OK;
 247         }
 248         return ERR_MEM;
 249 }
 250
 251 struct pbuf* hci_cmd_ass(struct pbuf *p,u8_t ocf,u8_t ogf,u8_t len)
 252 {
 253         ((u8_t*)p->payload)[0] = HCI_COMMAND_DATA_PACKET; /* cmd packet type */
 254         ((u8_t*)p->payload)[1] = (ocf&0xff); /* OCF & OGF */
 255         ((u8_t*)p->payload)[2] = ((ocf>>8)|(ogf<<2));
 256         ((u8_t*)p->payload)[3] = len-HCI_CMD_HDR_LEN-1; /* Param len = plen - cmd hdr - ptype */
 257
 258         if(hci_dev->num_cmd>0) hci_dev->num_cmd--;
 259         return p;
 260 }
 261
 262 err_t hci_reset(void)
 263 {
 264         struct pbuf *p = NULL;
 265
 266         if((p=btpbuf_alloc(PBUF_RAW,HCI_RESET_PLEN,PBUF_RAM))==NULL) {
 267                 ERROR("hci_reset: Could not allocate memory for pbuf\n");
 268                 return ERR_MEM;
 269         }
 270
 271         p = hci_cmd_ass(p,HCI_RESET_OCF,HCI_HC_BB_OGF,HCI_RESET_PLEN);
 272
 273         physbusif_output(p,p->tot_len);
 274         btpbuf_free(p);
 275
 276         return ERR_OK;
 277 }
 278
 279 err_t hci_read_buffer_size(void)
 280 {
 281         struct pbuf *p = NULL;
 282
 283         if((p=btpbuf_alloc(PBUF_RAW,HCI_R_BUF_SIZE_PLEN,PBUF_RAM))==NULL) {
 284                 ERROR("hci_read_buffer_size: Could not allocate memory for pbuf\n");
 285                 return ERR_MEM;
 286         }
 287
 288         p = hci_cmd_ass(p,HCI_R_BUF_SIZE_OCF,HCI_INFO_PARAM_OGF,HCI_R_BUF_SIZE_PLEN);
 289
 290         physbusif_output(p,p->tot_len);
 291         btpbuf_free(p);
 292
 293         return ERR_OK;
 294 }
 295
 296 err_t hci_read_bd_addr(void)
 297 {
 298         struct pbuf *p = NULL;
 299
 300         if((p=btpbuf_alloc(PBUF_RAW,HCI_R_BD_ADDR_PLEN,PBUF_RAM))==NULL) {
 301                 ERROR("hci_read_bd_addr: Could not allocate memory for pbuf\n");
 302                 return ERR_MEM;
 303         }
 304
 305         p = hci_cmd_ass(p,HCI_R_BD_ADDR_OCF,HCI_INFO_PARAM_OGF,HCI_R_BD_ADDR_PLEN);
 306
 307         physbusif_output(p,p->tot_len);
 308         btpbuf_free(p);
 309
 310         return ERR_OK;
 311 }
 312
 313 err_t hci_read_local_version(void)
 314 {
 315         struct pbuf *p = NULL;
 316
 317         if((p=btpbuf_alloc(PBUF_RAW,HCI_R_LOC_VERS_SIZE_PLEN,PBUF_RAM))==NULL) {
 318                 ERROR("hci_read_local_version: Could not allocate memory for pbuf\n");
 319                 return ERR_MEM;
 320         }
 321
 322         p = hci_cmd_ass(p,HCI_R_LOC_VERSION_OCF,HCI_INFO_PARAM_OGF,HCI_R_LOC_VERS_SIZE_PLEN);
 323
 324         physbusif_output(p,p->tot_len);
 325         btpbuf_free(p);
 326
 327         return ERR_OK;
 328 }
 329
 330 err_t hci_read_local_features(void)
 331 {
 332         struct pbuf *p = NULL;
 333
 334         if((p=btpbuf_alloc(PBUF_RAW,HCI_R_LOC_FEAT_SIZE_PLEN,PBUF_RAM))==NULL) {
 335                 ERROR("hci_read_local_features: Could not allocate memory for pbuf\n");
 336                 return ERR_MEM;
 337         }
 338
 339         p = hci_cmd_ass(p,HCI_R_LOC_FEATURES_OCF,HCI_INFO_PARAM_OGF,HCI_R_LOC_FEAT_SIZE_PLEN);
 340
 341         physbusif_output(p,p->tot_len);
 342         btpbuf_free(p);
 343
 344         return ERR_OK;
 345 }
 346
 347 err_t hci_read_stored_link_key()
 348 {
 349         struct pbuf *p = NULL;
 350         struct hci_link_key *tmpres;
 351
 352         /* Free any previous link key result list */
 353         while(hci_dev->keyres != NULL) {
 354                 tmpres = hci_dev->keyres;
 355                 hci_dev->keyres = hci_dev->keyres->next;
 356                 btmemb_free(&hci_link_key_results,tmpres);
 357         }
 358
 359
 360         if((p=btpbuf_alloc(PBUF_RAW,HCI_R_STORED_LINK_KEY_PLEN,PBUF_RAM))==NULL) {
 361                 ERROR("hci_read_stored_link_keys: Could not allocate memory for pbuf\n");
 362                 return ERR_MEM;
 363         }
 364
 365         p = hci_cmd_ass(p,HCI_R_STORED_LINK_KEY_OCF,HCI_HC_BB_OGF,HCI_R_STORED_LINK_KEY_PLEN);
 366
 367         memcpy((void*)((u8_t*)p->payload + 4),hci_dev->bdaddr.addr,6);
 368         ((u8_t*)p->payload)[10] = 1;
 369
 370         physbusif_output(p,p->tot_len);
 371         btpbuf_free(p);
 372
 373         return ERR_OK;
 374 }
 375
 376 err_t hci_set_event_filter(u8_t filter_type,u8_t filter_cond_type,u8_t *cond)
 377 {
 378         u32 cond_len = 0;
 379         struct pbuf *p = NULL;
 380
 381         switch(filter_type) {
 382                 case 0x00:
 383                         cond_len = 0x00;
 384                         break;
 385                 case 0x01:
 386                         switch(filter_cond_type) {
 387                                 case 0x00:
 388                                         cond_len = 0x00;
 389                                         break;
 390                                 case 0x01:
 391                                         cond_len = 0x06;
 392                                         break;
 393                                 case 0x02:
 394                                         cond_len = 0x06;
 395                                         break;
 396                                 default:
 397                                         break;
 398                         }
 399                         break;
 400                 case 0x02:
 401                         switch(filter_cond_type) {
 402                                 case 0x00:
 403                                         cond_len = 0x01;
 404                                         break;
 405                                 case 0x01:
 406                                         cond_len = 0x07;
 407                                         break;
 408                                 case 0x02:
 409                                         cond_len = 0x07;
 410                                         break;
 411                                 default:
 412                                         break;
 413                         }
 414                         break;
 415                 default:
 416                         break;
 417         }
 418
 419         if((p=btpbuf_alloc(PBUF_RAW,HCI_SET_EV_FILTER_PLEN+cond_len,PBUF_RAM))==NULL) {
 420                 ERROR("hci_set_event_filter: Could not allocate memory for pbuf\n");
 421                 return ERR_MEM;
 422         }
 423
 424         p = hci_cmd_ass(p,HCI_SET_EV_FILTER_OCF,HCI_HC_BB_OGF,HCI_SET_EV_FILTER_PLEN+cond_len);
 425         ((u8_t*)p->payload)[4] = filter_type;
 426         ((u8_t*)p->payload)[5] = filter_cond_type;
 427         if(cond_len>0) memcpy(p->payload+6,cond,cond_len);
 428
 429         physbusif_output(p,p->tot_len);
 430         btpbuf_free(p);
 431
 432         return ERR_OK;
 433 }
 434
 435 err_t hci_write_page_timeout(u16_t timeout)
 436 {
 437         struct pbuf *p = NULL;
 438
 439         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_PAGE_TIMEOUT_PLEN,PBUF_RAM))==NULL) {
 440                 ERROR("hci_set_write_page_timeout: Could not allocate memory for pbuf\n");
 441                 return ERR_MEM;
 442         }
 443
 444         p = hci_cmd_ass(p,HCI_W_PAGE_TIMEOUT_OCF,HCI_HC_BB_OGF,HCI_W_PAGE_TIMEOUT_PLEN);
 445         ((u16_t*)p->payload)[2] = htole16(timeout);
 446
 447         physbusif_output(p,p->tot_len);
 448         btpbuf_free(p);
 449
 450         return ERR_OK;
 451 }
 452
 453 err_t hci_write_scan_enable(u8_t scan_enable)
 454 {
 455         struct pbuf *p = NULL;
 456
 457         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_SCAN_EN_PLEN,PBUF_RAM))==NULL) {
 458                 ERROR("hci_set_write_page_timeout: Could not allocate memory for pbuf\n");
 459                 return ERR_MEM;
 460         }
 461
 462         p = hci_cmd_ass(p,HCI_W_SCAN_EN_OCF,HCI_HC_BB_OGF,HCI_W_SCAN_EN_PLEN);
 463         ((u8_t*)p->payload)[4] = scan_enable;
 464
 465         physbusif_output(p,p->tot_len);
 466         btpbuf_free(p);
 467
 468         return ERR_OK;
 469 }
 470
 471 err_t hci_inquiry(u32_t lap,u8_t inq_len,u8_t num_resp,err_t (*inq_complete)(void *arg,struct hci_pcb *pcb,struct hci_inq_res *ires,u16_t result))
 472 {
 473         struct pbuf *p = NULL;
 474         struct hci_inq_res *tmpres;
 475
 476         /* Free any previous inquiry result list */
 477         while(hci_dev->ires != NULL) {
 478                 tmpres = hci_dev->ires;
 479                 hci_dev->ires = hci_dev->ires->next;
 480                 btmemb_free(&hci_inq_results,tmpres);
 481         }
 482
 483         hci_dev->inq_complete = inq_complete;
 484         if((p=btpbuf_alloc(PBUF_RAW,HCI_INQUIRY_PLEN,PBUF_RAM))==NULL) {
 485                 ERROR("hci_inquiry: Could not allocate memory for pbuf\n");
 486                 return ERR_MEM;
 487         }
 488
 489         p = hci_cmd_ass(p,HCI_INQUIRY_OCF,HCI_LINK_CTRL_OGF,HCI_INQUIRY_PLEN);
 490         ((u8_t*)p->payload)[4] = (lap&0xff);
 491         ((u8_t*)p->payload)[5] = (lap>>8);
 492         ((u8_t*)p->payload)[6] = (lap>>16);
 493
 494         ((u8_t*)p->payload)[7] = inq_len;
 495         ((u8_t*)p->payload)[8] = num_resp;
 496
 497         physbusif_output(p,p->tot_len);
 498         btpbuf_free(p);
 499
 500         return ERR_OK;
 501 }
 502
 503 err_t hci_periodic_inquiry(u32_t lap,u16_t min_period,u16_t max_period,u8_t inq_len,u8_t num_resp,err_t (*inq_complete)(void *arg,struct hci_pcb *pcb,struct hci_inq_res *ires,u16_t result))
 504 {
 505         struct pbuf *p = NULL;
 506         struct hci_inq_res *tmpres;
 507
 508         /* Free any previous inquiry result list */
 509         while(hci_dev->ires != NULL) {
 510                 tmpres = hci_dev->ires;
 511                 hci_dev->ires = hci_dev->ires->next;
 512                 btmemb_free(&hci_inq_results,tmpres);
 513         }
 514
 515         hci_dev->inq_complete = inq_complete;
 516         if((p=btpbuf_alloc(PBUF_RAW,HCI_PERIODIC_INQUIRY_PLEN,PBUF_RAM))==NULL) {
 517                 ERROR("hci_periodic_inquiry: Could not allocate memory for pbuf\n");
 518                 return ERR_MEM;
 519         }
 520
 521         /* Assembling command packet */
 522         p = hci_cmd_ass(p,HCI_PERIODIC_INQUIRY_OCF,HCI_LINK_CTRL_OGF,HCI_PERIODIC_INQUIRY_PLEN);
 523
 524         /* Assembling cmd prameters */
 525         ((u16_t*)p->payload)[2] = htole16(max_period);
 526         ((u16_t*)p->payload)[3] = htole16(min_period);
 527         ((u8_t*)p->payload)[8] = (lap&0xff);
 528         ((u8_t*)p->payload)[9] = (lap>>8);
 529         ((u8_t*)p->payload)[10] = (lap>>16);
 530
 531         ((u8_t*)p->payload)[11] = inq_len;
 532         ((u8_t*)p->payload)[12] = num_resp;
 533
 534         physbusif_output(p,p->tot_len);
 535         btpbuf_free(p);
 536
 537         return ERR_OK;
 538 }
 539
 540 err_t hci_exit_periodic_inquiry()
 541 {
 542         struct pbuf *p = NULL;
 543
 544         if((p=btpbuf_alloc(PBUF_RAW,HCI_EXIT_PERIODIC_INQUIRY_PLEN,PBUF_RAM))==NULL) {
 545                 ERROR("hci_exit_periodic_inquiry: Could not allocate memory for pbuf\n");
 546                 return ERR_MEM;
 547         }
 548
 549         /* Assembling command packet */
 550         p = hci_cmd_ass(p,HCI_EXIT_PERIODIC_INQUIRY_OCF,HCI_LINK_CTRL_OGF,HCI_EXIT_PERIODIC_INQUIRY_PLEN);
 551
 552         physbusif_output(p,p->tot_len);
 553         btpbuf_free(p);
 554
 555         return ERR_OK;
 556 }
 557
 558 err_t hci_accecpt_conn_request(struct bd_addr *bdaddr,u8_t role)
 559 {
 560         struct pbuf *p = NULL;
 561
 562         if((p=btpbuf_alloc(PBUF_RAW,HCI_ACCEPT_CONN_REQ_PLEN,PBUF_RAM))==NULL) {
 563                 ERROR("hci_exit_periodic_inquiry: Could not allocate memory for pbuf\n");
 564                 return ERR_MEM;
 565         }
 566
 567         /* Assembling command packet */
 568         p = hci_cmd_ass(p,HCI_ACCEPT_CONN_REQ_OCF,HCI_LINK_CTRL_OGF,HCI_ACCEPT_CONN_REQ_PLEN);
 569
 570         /* Assembling cmd prameters */
 571         memcpy((void*)(((u8_t*)p->payload)+4),bdaddr,6);
 572         ((u8_t*)p->payload)[10] = role;
 573
 574         physbusif_output(p,p->tot_len);
 575         btpbuf_free(p);
 576
 577         return ERR_OK;
 578 }
 579
 580 err_t hci_set_event_mask(u64_t ev_mask)
 581 {
 582         u64_t mask;
 583         struct pbuf *p = NULL;
 584
 585         if((p=btpbuf_alloc(PBUF_RAW,HCI_SET_EV_MASK_PLEN,PBUF_RAM))==NULL) {
 586                 ERROR("hci_set_event_mask: Could not allocate memory for pbuf\n");
 587                 return ERR_MEM;
 588         }
 589
 590         /* Assembling command packet */
 591         p = hci_cmd_ass(p,HCI_SET_EV_MASK_OCF,HCI_HC_BB_OGF,HCI_SET_EV_MASK_PLEN);
 592
 593         mask = htole64(ev_mask);
 594         memcpy(((u8_t*)p->payload)+4,&mask,8);
 595
 596         physbusif_output(p,p->tot_len);
 597         btpbuf_free(p);
 598
 599         return ERR_OK;
 600 }
 601
 602 err_t hci_write_local_name(u8_t *name,u8_t len)
 603 {
 604         struct pbuf *p = NULL;
 605
 606         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_LOCAL_NAME_PLEN,PBUF_RAM))==NULL) {
 607                 ERROR("hci_write_local_name: Could not allocate memory for pbuf\n");
 608                 return ERR_MEM;
 609         }
 610
 611         /* Assembling command packet */
 612         p = hci_cmd_ass(p,HCI_W_LOCAL_NAME_OCF,HCI_HC_BB_OGF,HCI_W_LOCAL_NAME_PLEN);
 613         /* Assembling cmd prameters */
 614         memcpy(((u8_t *)p->payload) + 4, name, len);
 615
 616         physbusif_output(p, p->tot_len);
 617         btpbuf_free(p);
 618
 619         return ERR_OK;
 620 }
 621
 622 err_t hci_write_pin_type(u8_t type)
 623 {
 624         struct pbuf *p = NULL;
 625
 626         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_PIN_TYPE_PLEN,PBUF_RAM))==NULL) {
 627                 ERROR("hci_write_local_name: Could not allocate memory for pbuf\n");
 628                 return ERR_MEM;
 629         }
 630
 631         /* Assembling command packet */
 632         p = hci_cmd_ass(p,HCI_W_PIN_TYPE_OCF,HCI_HC_BB_OGF,HCI_W_PIN_TYPE_PLEN);
 633         /* Assembling cmd prameters */
 634         ((u8_t *)p->payload)[4] = type;
 635
 636         physbusif_output(p, p->tot_len);
 637         btpbuf_free(p);
 638
 639         return ERR_OK;
 640 }
 641
 642 err_t hci_read_remote_name(struct bd_addr *bdaddr)
 643 {
 644         u16_t clock_offset;
 645         struct pbuf *p = NULL;
 646         struct hci_inq_res *ires;
 647         u8_t page_scan_repetition_mode, page_scan_mode;
 648
 649         for(ires=hci_dev->ires;ires!=NULL;ires=ires->next) {
 650                 if(bd_addr_cmp(&(ires->bdaddr),bdaddr)) {
 651                         page_scan_repetition_mode = ires->psrm;
 652                         page_scan_mode = ires->psm;
 653                         clock_offset = ires->co;
 654                         break;
 655                 }
 656         }
 657
 658         if(ires==NULL) {
 659                 page_scan_repetition_mode = 0x01;
 660                 page_scan_mode = 0x00;
 661                 clock_offset = 0x00;
 662         }
 663
 664         if((p=btpbuf_alloc(PBUF_RAW,HCI_R_REMOTE_NAME_PLEN,PBUF_RAM))==NULL) {
 665                 ERROR("hci_read_remote_name: Could not allocate memory for pbuf\n");
 666                 return ERR_MEM;
 667         }
 668
 669         /* Assembling command packet */
 670         p = hci_cmd_ass(p,HCI_R_REMOTE_NAME_OCF,HCI_LINK_CTRL_OGF,HCI_R_REMOTE_NAME_PLEN);
 671         /* Assembling cmd prameters */
 672         memcpy(((u8_t *)p->payload+4),bdaddr->addr,6);
 673         ((u8_t*)p->payload)[10] = page_scan_repetition_mode;
 674         ((u8_t*)p->payload)[11] = page_scan_mode;
 675         ((u16_t*)p->payload)[6] = htole16(clock_offset);
 676
 677         physbusif_output(p, p->tot_len);
 678         btpbuf_free(p);
 679
 680         return ERR_OK;
 681
 682 }
 683
 684 err_t hci_write_inquiry_mode(u8_t mode)
 685 {
 686         struct pbuf *p = NULL;
 687
 688         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_INQUIRY_MODE_PLEN,PBUF_RAM))==NULL) {
 689                 ERROR("hci_write_inquiry_mode: Could not allocate memory for pbuf\n");
 690                 return ERR_MEM;
 691         }
 692
 693         /* Assembling command packet */
 694         p = hci_cmd_ass(p,HCI_W_INQUIRY_MODE_OCF,HCI_HC_BB_OGF,HCI_W_INQUIRY_MODE_PLEN);
 695         /* Assembling cmd prameters */
 696         ((u8_t*)p->payload)[4] = mode;
 697
 698         physbusif_output(p, p->tot_len);
 699         btpbuf_free(p);
 700
 701         return ERR_OK;
 702 }
 703
 704 err_t hci_write_page_scan_type(u8_t type)
 705 {
 706         struct pbuf *p = NULL;
 707
 708         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_PAGE_SCAN_TYPE_PLEN,PBUF_RAM))==NULL) {
 709                 ERROR("hci_write_inquiry_mode: Could not allocate memory for pbuf\n");
 710                 return ERR_MEM;
 711         }
 712
 713         /* Assembling command packet */
 714         p = hci_cmd_ass(p,HCI_W_PAGE_SCAN_TYPE_OCF,HCI_HC_BB_OGF,HCI_W_PAGE_SCAN_TYPE_PLEN);
 715         /* Assembling cmd prameters */
 716         ((u8_t*)p->payload)[4] = type;
 717
 718         physbusif_output(p, p->tot_len);
 719         btpbuf_free(p);
 720
 721         return ERR_OK;
 722 }
 723
 724 err_t hci_write_inquiry_scan_type(u8_t type)
 725 {
 726         struct pbuf *p = NULL;
 727
 728         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_INQUIRY_SCAN_TYPE_PLEN,PBUF_RAM))==NULL) {
 729                 ERROR("hci_write_inquiry_mode: Could not allocate memory for pbuf\n");
 730                 return ERR_MEM;
 731         }
 732
 733         /* Assembling command packet */
 734         p = hci_cmd_ass(p,HCI_W_INQUIRY_SCAN_TYPE_OCF,HCI_HC_BB_OGF,HCI_W_INQUIRY_SCAN_TYPE_PLEN);
 735         /* Assembling cmd prameters */
 736         ((u8_t*)p->payload)[4] = type;
 737
 738         physbusif_output(p, p->tot_len);
 739         btpbuf_free(p);
 740
 741         return ERR_OK;
 742 }
 743
 744 err_t hci_vendor_specific_command(u8_t ocf,u8_t ogf,void *data,u8_t len)
 745 {
 746         struct pbuf *p = NULL;
 747
 748         if((p=btpbuf_alloc(PBUF_RAW,HCI_W_VENDOR_CMD_PLEN + len,PBUF_RAM))==NULL) {
 749                 ERROR("hci_vendor_specific_patch: Could not allocate memory for pbuf\n");
 750                 return ERR_MEM;
 751         }
 752
 753         /* Assembling command packet */
 754         p = hci_cmd_ass(p,ocf,ogf,HCI_W_VENDOR_CMD_PLEN + len);
 755         /* Assembling cmd prameters */
 756         memcpy(((u8_t*)p->payload + 4),data,len);
 757
 758         physbusif_output(p, p->tot_len);
 759         btpbuf_free(p);
 760
 761         return ERR_OK;
 762 }
 763 /*-----------------------------------------------------------------------------------*/
 764 /* hci_sniff_mode():
 765  *
 766  * Sets an ACL connection to low power Sniff mode.
 767  */
 768 /*-----------------------------------------------------------------------------------*/
 769 err_t hci_sniff_mode(struct bd_addr *bdaddr, u16_t max_interval, u16_t min_interval, u16_t attempt, u16_t timeout)
 770 {
 771         struct pbuf *p;
 772         struct hci_link *link;
 773
 774         /* Check if an ACL connection exists */
 775         link = hci_get_link(bdaddr);
 776
 777         if(link == NULL) {
 778                 ERROR("hci_sniff_mode: ACL connection does not exist\n");
 779                 return ERR_CONN;
 780         }
 781
 782         if((p = btpbuf_alloc(PBUF_TRANSPORT, HCI_SNIFF_PLEN, PBUF_RAM)) == NULL) { /* Alloc len of packet */
 783                 ERROR("hci_sniff_mode: Could not allocate memory for pbuf\n");
 784                 return ERR_MEM;
 785         }
 786
 787         /* Assembling command packet */
 788         p = hci_cmd_ass(p, HCI_SNIFF_MODE_OCF, HCI_LINK_POLICY_OGF, HCI_SNIFF_PLEN);
 789         /* Assembling cmd prameters */
 790         ((u16_t *)p->payload)[2] = htole16(link->connhdl);
 791         ((u16_t *)p->payload)[3] = htole16(max_interval);
 792         ((u16_t *)p->payload)[4] = htole16(min_interval);
 793         ((u16_t *)p->payload)[5] = htole16(attempt);
 794         ((u16_t *)p->payload)[6] = htole16(timeout);
 795
 796         physbusif_output(p, p->tot_len);
 797         btpbuf_free(p);
 798
 799         return ERR_OK;
 800 }
 801 /*-----------------------------------------------------------------------------------*/
 802 /* hci_write_link_policy_settings():
 803  *
 804  * Control the modes (park, sniff, hold) that an ACL connection can take.
 805  *
 806  */
 807 /*-----------------------------------------------------------------------------------*/
 808 err_t hci_write_link_policy_settings(struct bd_addr *bdaddr, u16_t link_policy)
 809 {
 810         struct pbuf *p;
 811         struct hci_link *link;
 812
 813         /* Check if an ACL connection exists */
 814         link = hci_get_link(bdaddr);
 815
 816         if(link == NULL) {
 817                 ERROR("hci_write_link_policy_settings: ACL connection does not exist\n");
 818                 return ERR_CONN;
 819         }
 820
 821         if( (p = btpbuf_alloc(PBUF_TRANSPORT, HCI_W_LINK_POLICY_PLEN, PBUF_RAM)) == NULL) { /* Alloc len of packet */
 822                 ERROR("hci_write_link_policy_settings: Could not allocate memory for pbuf\n");
 823                 return ERR_MEM;
 824         }
 825         /* Assembling command packet */
 826         p = hci_cmd_ass(p, HCI_W_LINK_POLICY_OCF, HCI_LINK_POLICY_OGF, HCI_W_LINK_POLICY_PLEN);
 827
 828         /* Assembling cmd prameters */
 829         ((u16_t *)p->payload)[2] = htole16(link->connhdl);
 830         ((u16_t *)p->payload)[3] = htole16(link_policy);
 831
 832         physbusif_output(p, p->tot_len);
 833         btpbuf_free(p);
 834         return ERR_OK;
 835 }
 836
 837 /*-----------------------------------------------------------------------------------*/
 838 /* hci_pin_code_request_reply():
 839  *
 840  * Used to reply to a PIN Code Request event from the Host Controller and specifies
 841  * the PIN code to use for a connection.
 842  */
 843  /*-----------------------------------------------------------------------------------*/
 844 err_t hci_pin_code_request_reply(struct bd_addr *bdaddr, u8_t pinlen, u8_t *pincode)
 845 {
 846         struct pbuf *p;
 847
 848         if((p = btpbuf_alloc(PBUF_RAW, HCI_PIN_CODE_REQ_REP_PLEN, PBUF_RAM)) == NULL) {
 849                 ERROR("hci_pin_code_request_reply: Could not allocate memory for pbuf\n");
 850                 return ERR_MEM;
 851         }
 852
 853         /* Reset buffer content just to make sure */
 854         memset((u8_t *)p->payload, 0, HCI_PIN_CODE_REQ_REP_PLEN);
 855
 856         /* Assembling command packet */
 857         p = hci_cmd_ass(p, HCI_PIN_CODE_REQ_REP, HCI_LINK_CTRL_OGF, HCI_PIN_CODE_REQ_REP_PLEN);
 858         /* Assembling cmd prameters */
 859         memcpy(((u8_t *)p->payload) + 4, bdaddr->addr, 6);
 860         ((u8_t *)p->payload)[10] = pinlen;
 861         memcpy(((u8_t *)p->payload) + 11, pincode, pinlen);
 862
 863         physbusif_output(p, p->tot_len);
 864         btpbuf_free(p);
 865
 866         return ERR_OK;
 867 }
 868
 869 /*-----------------------------------------------------------------------------------*/
 870 /* hci_pin_code_request_neg_reply():
 871  *
 872  * Used to reply to a PIN Code Request event from the Host Controller when the Host
 873  * cannot specify a PIN code to use for a connection.
 874  */
 875  /*-----------------------------------------------------------------------------------*/
 876 err_t hci_pin_code_request_neg_reply(struct bd_addr *bdaddr)
 877 {
 878         struct pbuf *p;
 879
 880         if((p=btpbuf_alloc(PBUF_RAW,HCI_PIN_CODE_REQ_NEG_REP_PLEN,PBUF_RAM)) == NULL) {
 881                 ERROR("hci_pin_code_request_neg_reply: Could not allocate memory for pbuf\n");
 882                 return ERR_MEM;
 883         }
 884
 885         p = hci_cmd_ass(p,HCI_PIN_CODE_REQ_NEG_REP,HCI_LINK_CTRL_OGF,HCI_PIN_CODE_REQ_NEG_REP_PLEN);
 886         memcpy(((u8_t *)p->payload)+4, bdaddr->addr, 6);
 887
 888         physbusif_output(p,p->tot_len);
 889         btpbuf_free(p);
 890
 891         return ERR_OK;
 892 }
 893
 894 /*-----------------------------------------------------------------------------------*/
 895 /* hci_disconnect():
 896  *
 897  * Used to terminate an existing connection.
 898  */
 899  /*-----------------------------------------------------------------------------------*/
 900 err_t hci_disconnect(struct bd_addr *bdaddr, u8_t reason)
 901 {
 902         struct pbuf *p;
 903         struct hci_link *link;
 904
 905         link = hci_get_link(bdaddr);
 906
 907         if(link == NULL) {
 908                 ERROR("hci_disconnect: Connection does not exist\n");
 909                 return ERR_CONN; /* Connection does not exist */
 910         }
 911         if((p = btpbuf_alloc(PBUF_RAW, HCI_DISCONN_PLEN, PBUF_RAM)) == NULL) {
 912                 ERROR("hci_disconnect: Could not allocate memory for pbuf\n");
 913                 return ERR_MEM; /* Could not allocate memory for pbuf */
 914         }
 915         /* Assembling command packet */
 916         p = hci_cmd_ass(p, HCI_DISCONN_OCF, HCI_LINK_CTRL_OGF, HCI_DISCONN_PLEN);
 917
 918         /* Assembling cmd prameters */
 919         ((u16_t *)p->payload)[2] = htole16(link->connhdl);
 920         ((u8_t *)p->payload)[6] = reason;
 921
 922         physbusif_output(p, p->tot_len);
 923         btpbuf_free(p);
 924
 925         return ERR_OK;
 926 }
 927
 928 /*-----------------------------------------------------------------------------------*/
 929 /* hci_reject_connection_request():
 930  *
 931  * Used to decline a new incoming connection request.
 932  */
 933  /*-----------------------------------------------------------------------------------*/
 934 err_t hci_reject_connection_request(struct bd_addr *bdaddr, u8_t reason)
 935 {
 936         struct pbuf *p;
 937
 938         if((p = btpbuf_alloc(PBUF_RAW, HCI_REJECT_CONN_REQ_PLEN, PBUF_RAM)) == NULL) {
 939                 ERROR("hci_reject_connection_request: Could not allocate memory for pbuf\n");
 940                 return ERR_MEM;
 941         }
 942         /* Assembling command packet */
 943         p = hci_cmd_ass(p, HCI_REJECT_CONN_REQ_OCF, HCI_LINK_CTRL_OGF, HCI_REJECT_CONN_REQ_PLEN);
 944         /* Assembling cmd prameters */
 945         memcpy(((u8_t *)p->payload) + 4, bdaddr->addr, 6);
 946         ((u8_t *)p->payload)[10] = reason;
 947
 948         physbusif_output(p, p->tot_len);
 949         btpbuf_free(p);
 950
 951         return ERR_OK;
 952 }
 953
 954 /*-----------------------------------------------------------------------------------*/
 955 /* hci_write_stored_link_key():
 956  *
 957  * Writes a link key to be stored in the Bluetooth host controller.
 958  */
 959  /*-----------------------------------------------------------------------------------*/
 960 err_t hci_write_stored_link_key(struct bd_addr *bdaddr, u8_t *link)
 961 {
 962   struct pbuf *p;
 963
 964         if((p = btpbuf_alloc(PBUF_RAW, HCI_WRITE_STORED_LINK_KEY_PLEN, PBUF_RAM)) == NULL) {
 965                 ERROR("hci_write_stored_link_key: Could not allocate memory for pbuf\n");
 966                 return ERR_MEM;
 967         }
 968         /* Assembling command packet */
 969         p = hci_cmd_ass(p, HCI_WRITE_STORED_LINK_KEY, HCI_HC_BB_OGF, HCI_WRITE_STORED_LINK_KEY_PLEN);
 970         /* Assembling cmd prameters */
 971         ((u8_t *)p->payload)[4] = 0x01;
 972         memcpy(((u8_t *)p->payload) + 5, bdaddr->addr, 6);
 973         memcpy(((u8_t *)p->payload) + 11, link, 16);
 974
 975         physbusif_output(p, p->tot_len);
 976         btpbuf_free(p);
 977
 978         return ERR_OK;
 979 }
 980
 981 /*-----------------------------------------------------------------------------------*/
 982 /* hci_write_cod():
 983  *
 984  * Write the value for the Class_of_Device parameter, which is used to indicate its
 985  * capabilities to other devices.
 986  */
 987  /*-----------------------------------------------------------------------------------*/
 988 err_t hci_write_cod(u8_t *cod)
 989 {
 990         struct pbuf *p;
 991
 992         if((p = btpbuf_alloc(PBUF_RAW, HCI_W_COD_PLEN, PBUF_RAM)) == NULL) {
 993                 ERROR("hci_write_cod: Could not allocate memory for pbuf\n");
 994                 return ERR_MEM;
 995         }
 996         /* Assembling command packet */
 997         p = hci_cmd_ass(p, HCI_W_COD_OCF, HCI_HC_BB_OGF, HCI_W_COD_PLEN);
 998         /* Assembling cmd prameters */
 999         memcpy(((u8_t *)p->payload)+4, cod, 3);
1000
1001         physbusif_output(p, p->tot_len);
1002         btpbuf_free(p);
1003
1004         return ERR_OK;
1005 }
1006
1007 err_t hci_read_current_lap(void)
1008 {
1009         struct pbuf *p;
1010
1011         if((p = btpbuf_alloc(PBUF_RAW, HCI_R_CUR_IACLAP_PLEN, PBUF_RAM)) == NULL) {
1012                 ERROR("hci_read_current_lap: Could not allocate memory for pbuf\n");
1013                 return ERR_MEM;
1014         }
1015         /* Assembling command packet */
1016         p = hci_cmd_ass(p, HCI_R_CUR_IACLAP_OCF, HCI_HC_BB_OGF, HCI_R_CUR_IACLAP_PLEN);
1017
1018         physbusif_output(p, p->tot_len);
1019         btpbuf_free(p);
1020
1021         return ERR_OK;
1022 }
1023
1024 /*-----------------------------------------------------------------------------------*/
1025 /* hci_set_hc_to_h_fc():
1026  *
1027  * Used by the Host to turn flow control on or off in the direction from the Host
1028  * Controller to the Host.
1029  */
1030  /*-----------------------------------------------------------------------------------*/
1031 err_t hci_set_hc_to_h_fc(void)
1032 {
1033         struct pbuf *p;
1034
1035         if((p = btpbuf_alloc(PBUF_RAW, HCI_SET_HC_TO_H_FC_PLEN, PBUF_RAM)) == NULL) {
1036                 ERROR("hci_set_hc_to_h_fc: Could not allocate memory for pbuf\n");
1037                 return ERR_MEM;
1038         }
1039         /* Assembling command packet */
1040         p = hci_cmd_ass(p, HCI_SET_HC_TO_H_FC_OCF, HCI_HC_BB_OGF, HCI_SET_HC_TO_H_FC_PLEN);
1041         /* Assembling cmd prameters */
1042         ((u8_t *)p->payload)[4] = 0x01; /* Flow control on for HCI ACL Data Packets and off for HCI
1043                                                                          SCO Data Packets in direction from Host Controller to
1044                                  Host */
1045         physbusif_output(p, p->tot_len);
1046         btpbuf_free(p);
1047
1048         return ERR_OK;
1049 }
1050
1051 /*-----------------------------------------------------------------------------------*/
1052 /* hci_host_buffer_size():
1053  *
1054  * Used by the Host to notify the Host Controller about the maximum size of the data
1055  * portion of HCI ACL Data Packets sent from the Host Controller to the Host.
1056  */
1057  /*-----------------------------------------------------------------------------------*/
1058 err_t hci_host_buffer_size(void)
1059 {
1060         struct pbuf *p;
1061         if((p = btpbuf_alloc(PBUF_RAW, HCI_H_BUF_SIZE_PLEN, PBUF_RAM)) == NULL) {
1062                 ERROR("hci_host_buffer_size: Could not allocate memory for pbuf\n");
1063                 return ERR_MEM;
1064         }
1065         /* Assembling command packet */
1066         p = hci_cmd_ass(p, HCI_H_BUF_SIZE_OCF, HCI_HC_BB_OGF, HCI_H_BUF_SIZE_PLEN);
1067         ((u16_t *)p->payload)[2] = htole16(HCI_HOST_ACL_MAX_LEN); /* Host ACL data packet maximum length */
1068         ((u8_t *)p->payload)[6] = 255; /* Host SCO Data Packet Length */
1069         *((u16_t *)(((u8_t *)p->payload)+7)) = htole16(HCI_HOST_MAX_NUM_ACL); /* Host max total num ACL data packets */
1070         ((u16_t *)p->payload)[4] = htole16(1); /* Host Total Num SCO Data Packets */
1071         physbusif_output(p, p->tot_len);
1072         btpbuf_free(p);
1073
1074         hci_dev->host_num_acl = HCI_HOST_MAX_NUM_ACL;
1075
1076         return ERR_OK;
1077 }
1078
1079 /*-----------------------------------------------------------------------------------*/
1080 /* hci_host_num_comp_packets():
1081  *
1082  * Used by the Host to indicate to the Host Controller the number of HCI Data Packets
1083  * that have been completed for each Connection Handle since the previous
1084  * Host_Number_Of_Completed_Packets command was sent to the Host Controller.
1085  */
1086  /*-----------------------------------------------------------------------------------*/
1087 err_t hci_host_num_comp_packets(u16_t conhdl, u16_t num_complete)
1088 {
1089         struct pbuf *p;
1090
1091         if((p = btpbuf_alloc(PBUF_RAW, HCI_H_NUM_COMPL_PLEN, PBUF_RAM)) == NULL) {
1092                 ERROR("hci_host_num_comp_packets: Could not allocate memory for pbuf\n");
1093                 return ERR_MEM;
1094         }
1095         /* Assembling command packet */
1096         p = hci_cmd_ass(p, HCI_H_NUM_COMPL_OCF, HCI_HC_BB_OGF, HCI_H_NUM_COMPL_PLEN);
1097         ((u8_t*)p->payload)[4] = 1;
1098         *(u16_t*)(p->payload+5) = htole16(conhdl);
1099         *(u16_t*)(p->payload+7) = htole16(num_complete); /* Number of completed acl packets */
1100
1101         physbusif_output(p, p->tot_len);
1102         btpbuf_free(p);
1103
1104         hci_dev->host_num_acl += num_complete;
1105
1106         return ERR_OK;
1107 }
1108
1109 /*-----------------------------------------------------------------------------------*/
1110 /* lp_pdu_maxsize():
1111  *
1112  * Called by L2CAP to check the maxsize of the PDU. In this case it is the largest
1113  * ACL packet that the Host Controller can buffer.
1114  */
1115 /*-----------------------------------------------------------------------------------*/
1116 u16_t lp_pdu_maxsize()
1117 {
1118         return hci_dev->acl_mtu;
1119 }
1120
1121 /*-----------------------------------------------------------------------------------*/
1122 /* lp_is_connected():
1123  *
1124  * Called by L2CAP to check if an active ACL connection exists for the specified
1125  * Bluetooth address.
1126  */
1127 /*-----------------------------------------------------------------------------------*/
1128 u8_t lp_is_connected(struct bd_addr *bdaddr)
1129 {
1130         struct hci_link *link;
1131
1132         link = hci_get_link(bdaddr);
1133
1134         if(link == NULL) {
1135                 return 0;
1136         }
1137         return 1;
1138 }
1139
1140 /*-----------------------------------------------------------------------------------*/
1141 /* lp_acl_write():
1142  *
1143  * Called by L2CAP to send data to the Host Controller that will be transfered over
1144  * the ACL link from there.
1145  */
1146 /*-----------------------------------------------------------------------------------*/
1147 err_t lp_acl_write(struct bd_addr *bdaddr,struct pbuf *p,u16_t len,u8_t pb)
1148 {
1149         u16_t connhdlpbbc;
1150         struct hci_link *link;
1151         struct hci_acl_hdr *aclhdr;
1152         struct pbuf *q;
1153
1154         link = hci_get_link(bdaddr);
1155         if(link==NULL) {
1156                 ERROR("lp_acl_write: ACL connection does not exist\n");
1157                 return ERR_CONN;
1158         }
1159
1160         if(hci_dev->acl_max_pkt==0) {
1161                 if(p != NULL) {
1162                         /* Packet can be queued? */
1163                         if(link->p != NULL) {
1164                                 LOG("lp_acl_write: Host buffer full. Dropped packet\n");
1165                                 return ERR_OK; /* Drop packet */
1166                         } else {
1167                                 /* Copy PBUF_REF referenced payloads into PBUF_RAM */
1168                                 p = btpbuf_take(p);
1169                                 /* Remember pbuf to queue, if any */
1170                                 link->p = p;
1171                                 link->len = len;
1172                                 link->pb = pb;
1173                                 /* Pbufs are queued, increase the reference count */
1174                                 btpbuf_ref(p);
1175                                 LOG("lp_acl_write: Host queued packet %p\n", (void *)p);
1176                         }
1177                 }
1178         }
1179
1180         if((q=btpbuf_alloc(PBUF_RAW,HCI_ACL_HDR_LEN+1,PBUF_RAM))==NULL) {
1181                 ERROR("lp_acl_write: Could not allocate memory for pbuf\n");
1182                 return ERR_MEM;
1183         }
1184
1185         btpbuf_chain(q,p);
1186         ((u8_t*)q->payload)[0] = HCI_ACL_DATA_PACKET;
1187
1188         aclhdr = (void*)((u8_t*)q->payload+1);
1189         //aclhdr->connhdl_pb_bc = CONNPBBC(link->connhdl,pb,0);
1190         connhdlpbbc = link->connhdl; /* Received from connection complete event */
1191         connhdlpbbc |= (pb<<12); /* Packet boundary flag */
1192         connhdlpbbc &= 0x3FFF; /* Point-to-point */
1193         aclhdr->connhdl_pb_bc = htole16(connhdlpbbc);
1194         aclhdr->len = htole16(len);
1195
1196         physbusif_output(q,(q->len+len));
1197         --hci_dev->acl_max_pkt;
1198
1199         p = btpbuf_dechain(q);
1200         btpbuf_free(q);
1201         return ERR_OK;
1202 }
1203
1204 /*-----------------------------------------------------------------------------------*/
1205 /* lp_write_flush_timeout():
1206  *
1207  * Called by L2CAP to set the flush timeout for the ACL link.
1208  */
1209 /*-----------------------------------------------------------------------------------*/
1210 err_t lp_write_flush_timeout(struct bd_addr *bdaddr, u16_t flushto)
1211 {
1212         struct hci_link *link;
1213         struct pbuf *p;
1214
1215         /* Check if an ACL connection exists */
1216         link = hci_get_link(bdaddr);
1217
1218         if(link == NULL) {
1219                 ERROR("lp_write_flush_timeout: ACL connection does not exist\n");
1220                 return ERR_CONN;
1221         }
1222
1223         if((p = btpbuf_alloc(PBUF_TRANSPORT, HCI_W_FLUSHTO_PLEN, PBUF_RAM)) == NULL) { /* Alloc len of packet */
1224                 ERROR("lp_write_flush_timeout: Could not allocate memory for pbuf\n");
1225                 return ERR_MEM;
1226         }
1227
1228         /* Assembling command packet */
1229         p = hci_cmd_ass(p, HCI_W_FLUSHTO, HCI_HC_BB_OGF, HCI_W_FLUSHTO_PLEN);
1230         /* Assembling cmd prameters */
1231         ((u16_t *)p->payload)[2] = htole16(link->connhdl);
1232         ((u16_t *)p->payload)[3] = htole16(flushto);
1233
1234         physbusif_output(p, p->tot_len);
1235         btpbuf_free(p);
1236         return ERR_OK;
1237 }
1238
1239 /*-----------------------------------------------------------------------------------*/
1240 /* lp_connect_req():
1241  *
1242  * Called by L2CAP to cause the Link Manager to create a connection to the
1243  * Bluetooth device with the BD_ADDR specified by the command parameters.
1244  */
1245 /*-----------------------------------------------------------------------------------*/
1246 err_t lp_connect_req(struct bd_addr *bdaddr, u8_t allow_role_switch)
1247 {
1248         u8_t page_scan_repetition_mode, page_scan_mode;
1249         u16_t clock_offset;
1250         struct pbuf *p;
1251         struct hci_link *link = hci_new();
1252         struct hci_inq_res *inqres;
1253
1254         if(link == NULL) {
1255                 ERROR("lp_connect_req: Could not allocate memory for link\n");
1256                 return ERR_MEM; /* Could not allocate memory for link */
1257         }
1258
1259         bd_addr_set(&(link->bdaddr), bdaddr);
1260         HCI_REG(&(hci_active_links), link);
1261
1262
1263         /* Check if module has been discovered in a recent inquiry */
1264         for(inqres = hci_dev->ires; inqres != NULL; inqres = inqres->next) {
1265                 if(bd_addr_cmp(&inqres->bdaddr, bdaddr)) {
1266                         page_scan_repetition_mode = inqres->psrm;
1267                         page_scan_mode = inqres->psm;
1268                         clock_offset = inqres->co;
1269                         break;
1270                 }
1271         }
1272         if(inqres == NULL) {
1273                 /* No information on parameters from an inquiry. Using default values */
1274                 page_scan_repetition_mode = 0x01; /* Assuming worst case: time between
1275                                                                                          successive page scans starting
1276                                                                                          <= 2.56s */
1277                 page_scan_mode = 0x00; /* Assumes the device uses mandatory scanning, most
1278                 devices use this. If no conn is established, try
1279                 again w this parm set to optional page scanning */
1280                 clock_offset = 0x00; /* If the device was not found in a recent inquiry
1281                 this  information is irrelevant */
1282         }
1283
1284         if((p = btpbuf_alloc(PBUF_RAW, HCI_CREATE_CONN_PLEN, PBUF_RAM)) == NULL) {
1285                 ERROR("lp_connect_req: Could not allocate memory for pbuf\n");
1286                 return ERR_MEM; /* Could not allocate memory for pbuf */
1287         }
1288
1289         /* Assembling command packet */
1290         p = hci_cmd_ass(p, HCI_CREATE_CONN_OCF, HCI_LINK_CTRL_OGF, HCI_CREATE_CONN_PLEN);
1291         /* Assembling cmd prameters */
1292         memcpy(((u8_t *)p->payload)+4, bdaddr->addr, 6);
1293         ((u16_t *)p->payload)[5] = htole16(hci_dev->pkt_type);
1294         ((u8_t *)p->payload)[12] = page_scan_repetition_mode;
1295         ((u8_t *)p->payload)[13] = page_scan_mode;
1296         ((u16_t *)p->payload)[7] = htole16(clock_offset);
1297         ((u8_t *)p->payload)[16] = allow_role_switch;
1298
1299         physbusif_output(p, p->tot_len);
1300         btpbuf_free(p);
1301
1302         return ERR_OK;
1303 }
1304
1305 static void hci_cc_info_param(u8_t ocf,struct pbuf *p)
1306 {
1307         struct bd_addr *bdaddr;
1308
1309         switch(ocf) {
1310                 case HCI_READ_LOCAL_VERSION:
1311                         if(((u8_t*)p->payload)[0]==HCI_SUCCESS) {
1312                                 hci_dev->info.hci_version = *((u8_t*)p->payload + 1);
1313                                 hci_dev->info.hci_revision = le16toh(*(u16_t*)((u8_t*)p->payload + 2));
1314                                 hci_dev->info.lmp_version = *((u8_t*)p->payload + 4);
1315                                 hci_dev->info.manufacturer = le16toh(*(u16_t*)((u8_t*)p->payload + 5));
1316                                 hci_dev->info.lmp_subversion = le16toh(*(u16_t*)((u8_t*)p->payload + 7));
1317                                 LOG("hci_cc_info_param(HCI_READ_LOCAL_VERSION): hci_version = %02x, hci_revision = %04x, lmp_version = %02x, manufacturer = %04x, lmp_suversion = %04x\n",hci_dev->info.hci_version,hci_dev->info.hci_revision,hci_dev->info.lmp_version,hci_dev->info.manufacturer,hci_dev->info.lmp_subversion);
1318                         }
1319                         break;
1320                 case HCI_READ_LOCAL_FEATURES:
1321                         if(((u8_t*)p->payload)[0]==HCI_SUCCESS) {
1322                                 memcpy(hci_dev->features,(void*)((u8_t*)p->payload+1),sizeof(hci_dev->features));
1323
1324                                 if(hci_dev->features[0]&LMP_3SLOT)
1325                                         hci_dev->pkt_type |= (HCI_DM3|HCI_DH3);
1326                                 if(hci_dev->features[0]&LMP_5SLOT)
1327                                         hci_dev->pkt_type |= (HCI_DM5|HCI_DH5);
1328                                 if(hci_dev->features[1]&LMP_HV2)
1329                                         hci_dev->pkt_type |= HCI_HV2;
1330                                 if(hci_dev->features[1]&LMP_HV3)
1331                                         hci_dev->pkt_type |= HCI_HV3;
1332                                 LOG("hci_cc_info_param(HCI_READ_LOCAL_FEATURES): %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",hci_dev->features[0],hci_dev->features[1],hci_dev->features[2],hci_dev->features[3],
1333                                                                                                                                                                                                                            hci_dev->features[4],hci_dev->features[5],hci_dev->features[6],hci_dev->features[7]);
1334                         }
1335                         break;
1336                 case HCI_READ_BUFFER_SIZE:
1337                         if(((u8_t*)p->payload)[0]==HCI_SUCCESS) {
1338                                 hci_dev->acl_mtu = le16toh(*(u16_t*)(((u8_t*)p->payload)+1));
1339                                 hci_dev->sco_mtu = *((u8_t*)p->payload+3);
1340                                 hci_dev->acl_max_pkt = le16toh(*(u16_t*)(((u8_t*)p->payload)+4));
1341                                 hci_dev->sco_max_pkt = le16toh(*(u16_t*)(((u8_t*)p->payload)+5));
1342                                 LOG("hci_cc_info_param(HCI_READ_BUFFER_SIZE): acl_mt = %d, sco_mt = %d, acl_max_pkt = %d, sco_max_pkt = %d\n",hci_dev->acl_mtu,hci_dev->sco_mtu,hci_dev->acl_max_pkt,hci_dev->sco_max_pkt);
1343                         }
1344                         break;
1345                 case HCI_READ_BD_ADDR:
1346                         if(((u8_t*)p->payload)[0]==HCI_SUCCESS) {
1347                                 bdaddr = (void*)((u8_t*)p->payload+1);
1348                                 LOG("hci_cc_info_param(HCI_READ_BD_ADDR): %02x:%02x:%02x:%02x:%02x:%02x",bdaddr->addr[0],bdaddr->addr[1],bdaddr->addr[2],bdaddr->addr[3],bdaddr->addr[4],bdaddr->addr[5]);
1349                                 bd_addr_set(&(hci_dev->bdaddr),bdaddr);
1350                         }
1351                         break;
1352         }
1353 }
1354
1355 static void hci_cc_host_ctrl(u8_t ocf,struct pbuf *p)
1356 {
1357         u8_t *lap;
1358         u8_t i,resp_off;
1359
1360         //printf("hci_cc_host_ctrl(%02x)\n",ocf);
1361         switch(ocf) {
1362                 case HCI_SET_HC_TO_H_FC:
1363                         if(((u8_t*)p->payload)[0]==HCI_SUCCESS) hci_dev->flow = 1;
1364                         break;
1365                 case HCI_READ_CUR_IACLAP:
1366                         if(((u8_t*)p->payload)[0]==HCI_SUCCESS) {
1367                                 for(i=0;i<((u8_t*)p->payload)[1];i++) {
1368                                         resp_off = (i*3);
1369                                         lap = (void*)(((u8_t*)p->payload)+(2+resp_off));
1370                                         printf("lap = 00%02x%02x%02x\n",lap[2],lap[1],lap[0]);
1371                                 }
1372                         }
1373                         break;
1374         }
1375 }
1376
1377 static void hci_cc_link_policy(u8_t ocf,struct pbuf *p)
1378 {
1379         err_t ret;
1380         struct hci_link *link;
1381
1382         (void)ret;
1383
1384         switch(ocf) {
1385                 case HCI_W_LINK_POLICY:
1386                         if(((u8_t*)p->payload)[0]==HCI_SUCCESS) {
1387                                 for(link=hci_active_links;link!=NULL;link=link->next) {
1388                                         if(link->connhdl==le16toh(*((u16_t*)(((u8_t*)p->payload)+1)))) break;
1389                                 }
1390                                 if(link==NULL) {
1391                                         LOG("hci_cc_link_policy: Connection does not exist\n");
1392                                         break;
1393                                 }
1394                                 HCI_EVENT_WLP_COMPLETE(hci_dev,&link->bdaddr,ret);
1395                         } else {
1396                                 LOG("Unsuccessful HCI_W_LINK_POLICY.\n");
1397                         }
1398                         break;
1399         }
1400 }
1401
1402 static void hci_conn_request_evt(struct pbuf *p)
1403 {
1404         u8_t *cod;
1405         u8_t link_type;
1406         err_t ret = ERR_OK;
1407         struct bd_addr *bdaddr;
1408         struct hci_link *link;
1409
1410         LOG("hci_conn_request_evt()\n");
1411         bdaddr = (void*)((u8_t*)p->payload);
1412         cod = (((u8_t*)p->payload)+6);
1413         link_type = *(((u8_t*)p->payload)+9);
1414
1415         HCI_EVENT_CONN_REQ(hci_dev,bdaddr,cod,link_type,ret);
1416         if(ret==ERR_OK) {
1417                 link = hci_get_link(bdaddr);
1418                 if(link==NULL) {
1419                         if((link=hci_new())==NULL) {
1420                                 ERROR("hci_conn_request_evt: Could not allocate memory for link. Disconnect\n");
1421                                 return;
1422                         }
1423
1424                         bd_addr_set(&(link->bdaddr),bdaddr);
1425                         HCI_REG(&(hci_active_links),link);
1426                 }
1427                 hci_accecpt_conn_request(bdaddr,0x00);
1428         } else {
1429         }
1430 }
1431
1432 static void hci_conn_complete_evt(struct pbuf *p)
1433 {
1434         err_t ret;
1435         struct bd_addr *bdaddr;
1436         struct hci_link *link;
1437
1438         (void)ret;
1439
1440         bdaddr = (void*)(((u8_t*)p->payload)+3);
1441         link = hci_get_link(bdaddr);
1442         LOG("hci_conn_complete_evt(%p,%02x - %02x:%02x:%02x:%02x:%02x:%02x)\n",link,((u8_t*)p->payload)[0],bdaddr->addr[0],bdaddr->addr[1],bdaddr->addr[2],bdaddr->addr[3],bdaddr->addr[4],bdaddr->addr[5]);
1443         switch(((u8_t*)p->payload)[0]) {
1444                 case HCI_SUCCESS:
1445                         if(link==NULL) {
1446                                 if((link=hci_new())==NULL) {
1447                                         ERROR("hci_conn_complete_evt: Could not allocate memory for link. Disconnect\n");
1448                                         hci_disconnect(bdaddr, HCI_OTHER_END_TERMINATED_CONN_LOW_RESOURCES);
1449                                         lp_disconnect_ind(bdaddr,HCI_CONN_TERMINATED_BY_LOCAL_HOST);
1450                                         break;
1451                                 }
1452                                 bd_addr_set(&(link->bdaddr),bdaddr);
1453                                 link->connhdl = le16toh(*((u16_t*)(((u8_t*)p->payload)+1)));
1454                                 HCI_REG(&(hci_active_links),link);
1455                                 HCI_EVENT_CONN_COMPLETE(hci_dev,bdaddr,ret);
1456                                 lp_connect_ind(&(link->bdaddr));
1457                         } else {
1458                                 link->connhdl = le16toh(*((u16_t*)(((u8_t*)p->payload)+1)));
1459                                 HCI_EVENT_CONN_COMPLETE(hci_dev,bdaddr,ret);
1460                                 lp_connect_cfm(&(link->bdaddr),((u8_t*)p->payload)[10],ERR_OK);
1461                         }
1462                         break;
1463                 case HCI_PAGE_TIMEOUT:
1464                         break;
1465                 default:
1466                         if(link!=NULL) {
1467                                 hci_close(link);
1468                                 lp_connect_cfm(bdaddr,((u8_t*)p->payload)[10],ERR_CONN);
1469                         }
1470                         break;
1471         }
1472 }
1473
1474 static void hci_inquiry_result_evt(struct pbuf *p)
1475 {
1476         u8_t num_resp;
1477         u32_t i,resp_off;
1478         struct bd_addr *bdaddr;
1479         struct hci_inq_res *ires;
1480
1481         num_resp = ((u8_t*)p->payload)[0];
1482         //printf("hci_inquriy_result_evt(%d)\n",num_resp);
1483         for(i=0;i<num_resp && i<MEMB_NUM_HCI_INQ;i++) {
1484                 resp_off = (i*14);
1485                 bdaddr = (void*)(((u8_t*)p->payload)+(1+resp_off));
1486                 if((ires=btmemb_alloc(&hci_inq_results))!=NULL) {
1487                         bd_addr_set(&(ires->bdaddr),bdaddr);
1488                         ires->psrm = ((u8_t*)p->payload)[7+resp_off];
1489                         ires->psm = ((u8_t*)p->payload)[8+resp_off];
1490                         memcpy(ires->cod,((u8_t*)p->payload)+10+resp_off,3);
1491                         ires->co = le16toh(*((u16_t*)(((u8_t*)p->payload)+13+resp_off)));
1492                         ires->next = NULL;
1493
1494                         HCI_REG(&(hci_dev->ires),ires);
1495                 } else
1496                         ERROR("hci_inquriy_result_evt: Could not allocate memory for inquiry result\n");
1497         }
1498
1499 }
1500
1501 static void hci_return_link_key_evt(struct pbuf *p)
1502 {
1503         u8_t num_keys;
1504         u32_t i,resp_off;
1505         struct bd_addr *bdaddr;
1506         struct hci_link_key *keyres;
1507
1508         num_keys = ((u8_t*)p->payload)[0];
1509         //printf("hci_return_link_key_evt(%d)\n",num_keys);
1510         for(i=0;i<num_keys && i<MEMB_NUM_HCI_LINK_KEY;i++) {
1511                 resp_off = (i*22);
1512                 bdaddr = (void*)(((u8_t*)p->payload)+1+resp_off);
1513                 if((keyres=btmemb_alloc(&hci_link_key_results))!=NULL) {
1514                         bd_addr_set(&(keyres->bdaddr),bdaddr);
1515                         memcpy(keyres->key,((u8_t*)p->payload)+7+resp_off,16);
1516                         keyres->next = NULL;
1517
1518                         //printf("link key evt: %02x:%02x:%02x:%02x:%02x:%02x\n",bdaddr->addr[0],bdaddr->addr[1],bdaddr->addr[2],bdaddr->addr[3],bdaddr->addr[4],bdaddr->addr[5]);
1519                         HCI_REG(&(hci_dev->keyres),keyres);
1520                 } else
1521                         ERROR("hci_return_link_key_evt: Could not allocate memory for link key result\n");
1522         }
1523
1524 }
1525
1526 void hci_event_handler(struct pbuf *p)
1527 {
1528         err_t ret;
1529         u8_t i,resp_off;
1530         u16_t ogf,ocf,opc;
1531         u16_t connhdl;
1532         struct pbuf *q;
1533         struct hci_link *link;
1534         struct bd_addr *bdaddr;
1535         struct hci_evt_hdr *evthdr;
1536
1537         (void)ret;
1538
1539         evthdr = p->payload;
1540         btpbuf_header(p,-HCI_EVENT_HDR_LEN);
1541
1542         switch(evthdr->code) {
1543                 case HCI_INQUIRY_COMPLETE:
1544                         //printf("HCI_INQUIRY_COMPLETE\n");
1545                         HCI_EVENT_INQ_COMPLETE(hci_dev,((u8_t*)p->payload)[0],ret);
1546                         break;
1547                 case HCI_INQUIRY_RESULT:
1548                         hci_inquiry_result_evt(p);
1549                         break;
1550                 case HCI_CONNECTION_COMPLETE:
1551                         hci_conn_complete_evt(p);
1552                         break;
1553                 case HCI_CONNECTION_REQUEST:
1554                         hci_conn_request_evt(p);
1555                         break;
1556                 case HCI_DISCONNECTION_COMPLETE:
1557                         switch(((u8_t*)p->payload)[0]) {
1558                                 case HCI_SUCCESS:
1559                                         for(link=hci_active_links;link!=NULL;link=link->next) {
1560                                                 if(link->connhdl==le16toh(*((u16_t*)(((u8_t*)p->payload)+1)))) break;
1561                                         }
1562                                         if(link!=NULL) {
1563                                                 lp_disconnect_ind(&(link->bdaddr),((u8_t*)p->payload)[3]);
1564                                                 hci_close(link);
1565                                         }
1566                                         break;
1567                                 default:
1568                                         return;
1569                         }
1570                         break;
1571                 case HCI_ENCRYPTION_CHANGE:
1572                         break;
1573                 case HCI_QOS_SETUP_COMPLETE:
1574                         break;
1575                 case HCI_COMMAND_COMPLETE:
1576                         hci_dev->num_cmd += ((u8_t*)p->payload)[0];
1577                         btpbuf_header(p,-1);
1578
1579                         opc = le16toh(((u16_t*)p->payload)[0]);
1580                         ocf = (opc&0x03ff);
1581                         ogf = (opc>>10);
1582                         btpbuf_header(p,-2);
1583
1584                         switch(ogf) {
1585                                 case HCI_INFO_PARAM:
1586                                         hci_cc_info_param(ocf,p);
1587                                         break;
1588                                 case HCI_HOST_C_N_BB:
1589                                         hci_cc_host_ctrl(ocf,p);
1590                                         break;
1591                                 case HCI_LINK_POLICY:
1592                                         hci_cc_link_policy(ocf,p);
1593                                         break;
1594                         }
1595                         HCI_EVENT_CMD_COMPLETE(hci_dev,ogf,ocf,((u8_t*)p->payload)[0],ret);
1596                         break;
1597                 case HCI_COMMAND_STATUS:
1598                         if(((u8_t*)p->payload)[0]!=HCI_SUCCESS) {
1599                                 btpbuf_header(p,-2);
1600
1601                                 opc = le16toh(((u16_t*)p->payload)[0]);
1602                                 ocf = (opc&0x03ff);
1603                                 ogf = (opc>>10);
1604                                 btpbuf_header(p,-2);
1605
1606                                 HCI_EVENT_CMD_COMPLETE(hci_dev,ogf,ocf,((u8_t*)p->payload)[0],ret);
1607                                 btpbuf_header(p,4);
1608                         }
1609                         hci_dev->num_cmd += ((u8_t*)p->payload)[1];
1610                         break;
1611                 case HCI_HARDWARE_ERROR:
1612                         //TODO: IS THIS FATAL??
1613                         break;
1614                 case HCI_ROLE_CHANGE:
1615                         break;
1616                 case HCI_NBR_OF_COMPLETED_PACKETS:
1617                         for(i=0;i<((u8_t *)p->payload)[0];i++) {
1618                                 resp_off = i*4;
1619                                 hci_dev->acl_max_pkt += le16toh(*((u16_t *)(((u8_t *)p->payload) + 3 + resp_off)));
1620                                 connhdl = le16toh(*((u16_t *)(((u8_t *)p->payload) + 1 + resp_off)));
1621
1622                                 for(link = hci_active_links; link != NULL; link = link->next) {
1623                                         if(link->connhdl == connhdl) break;
1624                                 }
1625
1626                                 q = link == NULL ? NULL : link->p;
1627                                 /* Queued packet present? */
1628                                 if (q != NULL) {
1629                                         /* NULL attached buffer immediately */
1630                                         link->p = NULL;
1631                                         /* Send the queued packet */
1632                                         lp_acl_write(&link->bdaddr, q, link->len, link->pb);
1633                                         /* Free the queued packet */
1634                                         btpbuf_free(q);
1635                                 }
1636                         }
1637                         break;
1638                 case HCI_MODE_CHANGE:
1639                         printf("HCI_MODE_CHANGE\n");
1640                         break;
1641                 case HCI_DATA_BUFFER_OVERFLOW:
1642                         //TODO: IS THIS FATAL????
1643                         break;
1644                 case HCI_MAX_SLOTS_CHANGE:
1645                         break;
1646                 case HCI_RETURN_LINK_KEYS:
1647                         hci_return_link_key_evt(p);
1648                         break;
1649                 case HCI_PIN_CODE_REQUEST:
1650                         bdaddr = (void *)((u8_t *)p->payload); /* Get the Bluetooth address */
1651                         HCI_EVENT_PIN_REQ(hci_dev, bdaddr, ret); /* Notify application. If event is not registered,
1652                                                                                                         send a negative reply */
1653                         break;
1654                 case HCI_LINK_KEY_NOTIFICATION:
1655                         bdaddr = (void *)((u8_t *)p->payload); /* Get the Bluetooth address */
1656
1657                         HCI_EVENT_LINK_KEY_NOT(hci_dev, bdaddr, ((u8_t *)p->payload) + 6, ret); /* Notify application.*/
1658                         break;
1659                 default:
1660                         LOG("hci_event_input: Undefined event code 0x%x\n", evthdr->code);
1661                         break;
1662         }
1663 }
1664
1665 void hci_acldata_handler(struct pbuf *p)
1666 {
1667         struct hci_acl_hdr *aclhdr;
1668         struct hci_link *link;
1669         u16_t conhdl;
1670
1671         aclhdr = p->payload;
1672         btpbuf_header(p, -HCI_ACL_HDR_LEN);
1673
1674         conhdl = le16toh(aclhdr->connhdl_pb_bc) & 0x0FFF; /* Get the connection handle from the first
1675                                            12 bits */
1676         if(hci_dev->flow) {
1677                 //TODO: XXX??? DO WE SAVE NUMACL PACKETS COMPLETED IN LINKS LIST?? SHOULD WE CALL
1678                 //hci_host_num_comp_packets from the main loop when no data has been received from the
1679                 //serial port???
1680                 --hci_dev->host_num_acl;
1681                 if(hci_dev->host_num_acl == 0) {
1682                         hci_host_num_comp_packets(conhdl, HCI_HOST_MAX_NUM_ACL);
1683                         hci_dev->host_num_acl = HCI_HOST_MAX_NUM_ACL;
1684                 }
1685         }
1686
1687         for(link = hci_active_links; link != NULL; link = link->next) {
1688                 if(link->connhdl == conhdl) {
1689                         break;
1690                 }
1691         }
1692
1693         if(link != NULL) {
1694                 if(le16toh(aclhdr->len)) {
1695                         //LOG("hci_acl_input: Forward ACL packet to higher layer p->tot_len = %d\n", p->tot_len);
1696                         l2cap_input(p, &(link->bdaddr));
1697                 } else {
1698                         btpbuf_free(p); /* If length of ACL packet is zero, we silently discard it */
1699                 }
1700         } else {
1701                 btpbuf_free(p); /* If no acitve ACL link was found, we silently discard the packet */
1702         }
1703 }