sys/netgraph/bluetooth/hci/ng_hci_ulpi.c

   1 /*
   2  * ng_hci_ulpi.c
   3  */
   4
   5 /*-
   6  * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
   7  * All rights reserved.
   8  *
   9  * Redistribution and use in source and binary forms, with or without
  10  * modification, are permitted provided that the following conditions
  11  * are met:
  12  * 1. Redistributions of source code must retain the above copyright
  13  *    notice, this list of conditions and the following disclaimer.
  14  * 2. Redistributions in binary form must reproduce the above copyright
  15  *    notice, this list of conditions and the following disclaimer in the
  16  *    documentation and/or other materials provided with the distribution.
  17  *
  18  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  19  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  21  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  22  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  23  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  24  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  25  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  26  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  27  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  28  * SUCH DAMAGE.
  29  *
  30  * $Id: ng_hci_ulpi.c,v 1.7 2003/09/08 18:57:51 max Exp $
  31  * $FreeBSD$
  32  */
  33
  34 #include <sys/param.h>
  35 #include <sys/systm.h>
  36 #include <sys/kernel.h>
  37 #include <sys/endian.h>
  38 #include <sys/malloc.h>
  39 #include <sys/mbuf.h>
  40 #include <sys/queue.h>
  41 #include <netgraph/ng_message.h>
  42 #include <netgraph/netgraph.h>
  43 #include <netgraph/bluetooth/include/ng_bluetooth.h>
  44 #include <netgraph/bluetooth/include/ng_hci.h>
  45 #include <netgraph/bluetooth/hci/ng_hci_var.h>
  46 #include <netgraph/bluetooth/hci/ng_hci_cmds.h>
  47 #include <netgraph/bluetooth/hci/ng_hci_evnt.h>
  48 #include <netgraph/bluetooth/hci/ng_hci_ulpi.h>
  49 #include <netgraph/bluetooth/hci/ng_hci_misc.h>
  50
  51 /******************************************************************************
  52  ******************************************************************************
  53  **                 Upper Layer Protocol Interface module
  54  ******************************************************************************
  55  ******************************************************************************/
  56
  57 static int ng_hci_lp_acl_con_req (ng_hci_unit_p, item_p, hook_p);
  58 static int ng_hci_lp_sco_con_req (ng_hci_unit_p, item_p, hook_p);
  59 static int ng_hci_lp_le_con_req (ng_hci_unit_p, item_p, hook_p, int);
  60
  61 /*
  62  * Process LP_ConnectReq event from the upper layer protocol
  63  */
  64
  65 int
  66 ng_hci_lp_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
  67 {
  68         int link_type;
  69
  70         if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
  71                 NG_HCI_WARN(
  72 "%s: %s - unit is not ready, state=%#x\n",
  73                         __func__, NG_NODE_NAME(unit->node), unit->state);
  74
  75                 NG_FREE_ITEM(item);
  76
  77                 return (ENXIO);
  78         }
  79
  80         if (NGI_MSG(item)->header.arglen != sizeof(ng_hci_lp_con_req_ep)) {
  81                 NG_HCI_ALERT(
  82 "%s: %s - invalid LP_ConnectReq message size=%d\n",
  83                         __func__, NG_NODE_NAME(unit->node),
  84                         NGI_MSG(item)->header.arglen);
  85
  86                 NG_FREE_ITEM(item);
  87
  88                 return (EMSGSIZE);
  89         }
  90         link_type = ((ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data))->link_type;
  91         switch(link_type){
  92         case NG_HCI_LINK_ACL:
  93                 return (ng_hci_lp_acl_con_req(unit, item, hook));
  94         case NG_HCI_LINK_SCO:
  95                 if (hook != unit->sco ) {
  96                         NG_HCI_WARN(
  97                                 "%s: %s - LP_ConnectReq for SCO connection came from wrong hook=%p\n",
  98                                 __func__, NG_NODE_NAME(unit->node), hook);
  99
 100                         NG_FREE_ITEM(item);
 101
 102                         return (EINVAL);
 103                 }
 104
 105                 return (ng_hci_lp_sco_con_req(unit, item, hook));
 106         case NG_HCI_LINK_LE_PUBLIC:
 107         case NG_HCI_LINK_LE_RANDOM:
 108                 return (ng_hci_lp_le_con_req(unit, item, hook, link_type));
 109         default:
 110                 panic("%s: link_type invalid.", __func__);
 111         }
 112
 113         return (EINVAL);
 114 } /* ng_hci_lp_con_req */
 115
 116 /*
 117  * Request to create new ACL connection
 118  */
 119
 120 static int
 121 ng_hci_lp_acl_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 122 {
 123         struct acl_con_req {
 124                 ng_hci_cmd_pkt_t         hdr;
 125                 ng_hci_create_con_cp     cp;
 126         } __attribute__ ((packed))      *req = NULL;
 127         ng_hci_lp_con_req_ep            *ep = NULL;
 128         ng_hci_unit_con_p                con = NULL;
 129         ng_hci_neighbor_t               *n = NULL;
 130         struct mbuf                     *m = NULL;
 131         int                              error = 0;
 132
 133         ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
 134
 135         /*
 136          * Only one ACL connection can exist between each pair of units.
 137          * So try to find ACL connection descriptor (in any state) that
 138          * has requested remote BD_ADDR.
 139          *
 140          * Two cases:
 141          *
 142          * 1) We do not have connection to the remote unit. This is simple.
 143          *    Just create new connection descriptor and send HCI command to
 144          *    create new connection.
 145          *
 146          * 2) We do have connection descriptor. We need to check connection
 147          *    state:
 148          *
 149          * 2.1) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
 150          *      accepting connection from the remote unit. This is a race
 151          *      condition. We will ignore this message.
 152          *
 153          * 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
 154          *      requested connection or we just accepted it. In any case
 155          *      all we need to do here is set appropriate notification bit
 156          *      and wait.
 157          *
 158          * 2.3) NG_HCI_CON_OPEN means connection is open. Just reply back
 159          *      and let upper layer know that we have connection already.
 160          */
 161
 162         con = ng_hci_con_by_bdaddr(unit, &ep->bdaddr, NG_HCI_LINK_ACL);
 163         if (con != NULL) {
 164                 switch (con->state) {
 165                 case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
 166                         error = EALREADY;
 167                         break;
 168
 169                 case NG_HCI_CON_W4_CONN_COMPLETE:
 170                         if (hook == unit->acl)
 171                                 con->flags |= NG_HCI_CON_NOTIFY_ACL;
 172                         else
 173                                 con->flags |= NG_HCI_CON_NOTIFY_SCO;
 174                         break;
 175
 176                 case NG_HCI_CON_OPEN: {
 177                         struct ng_mesg          *msg = NULL;
 178                         ng_hci_lp_con_cfm_ep    *cfm = NULL;
 179
 180                         if (hook != NULL && NG_HOOK_IS_VALID(hook)) {
 181                                 NGI_GET_MSG(item, msg);
 182                                 NG_FREE_MSG(msg);
 183
 184                                 NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
 185                                         NGM_HCI_LP_CON_CFM, sizeof(*cfm),
 186                                         M_NOWAIT);
 187                                 if (msg != NULL) {
 188                                         cfm = (ng_hci_lp_con_cfm_ep *)msg->data;
 189                                         cfm->status = 0;
 190                                         cfm->link_type = con->link_type;
 191                                         cfm->con_handle = con->con_handle;
 192                                         bcopy(&con->bdaddr, &cfm->bdaddr,
 193                                                 sizeof(cfm->bdaddr));
 194
 195                                         /*
 196                                          * This will forward item back to
 197                                          * sender and set item to NULL
 198                                          */
 199
 200                                         _NGI_MSG(item) = msg;
 201                                         NG_FWD_ITEM_HOOK(error, item, hook);
 202                                 } else
 203                                         error = ENOMEM;
 204                         } else
 205                                 NG_HCI_INFO(
 206 "%s: %s - Source hook is not valid, hook=%p\n",
 207                                         __func__, NG_NODE_NAME(unit->node),
 208                                         hook);
 209                         } break;
 210
 211                 default:
 212                         panic(
 213 "%s: %s - Invalid connection state=%d\n",
 214                                 __func__, NG_NODE_NAME(unit->node), con->state);
 215                         break;
 216                 }
 217
 218                 goto out;
 219         }
 220
 221         /*
 222          * If we got here then we need to create new ACL connection descriptor
 223          * and submit HCI command. First create new connection desriptor, set
 224          * bdaddr and notification flags.
 225          */
 226
 227         con = ng_hci_new_con(unit, NG_HCI_LINK_ACL);
 228         if (con == NULL) {
 229                 error = ENOMEM;
 230                 goto out;
 231         }
 232
 233         bcopy(&ep->bdaddr, &con->bdaddr, sizeof(con->bdaddr));
 234
 235         /*
 236          * Create HCI command
 237          */
 238
 239         MGETHDR(m, M_NOWAIT, MT_DATA);
 240         if (m == NULL) {
 241                 ng_hci_free_con(con);
 242                 error = ENOBUFS;
 243                 goto out;
 244         }
 245
 246         m->m_pkthdr.len = m->m_len = sizeof(*req);
 247         req = mtod(m, struct acl_con_req *);
 248         req->hdr.type = NG_HCI_CMD_PKT;
 249         req->hdr.length = sizeof(req->cp);
 250         req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
 251                                         NG_HCI_OCF_CREATE_CON));
 252
 253         bcopy(&ep->bdaddr, &req->cp.bdaddr, sizeof(req->cp.bdaddr));
 254
 255         req->cp.pkt_type = (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1);
 256         if (unit->features[0] & NG_HCI_LMP_3SLOT)
 257                 req->cp.pkt_type |= (NG_HCI_PKT_DM3|NG_HCI_PKT_DH3);
 258         if (unit->features[0] & NG_HCI_LMP_5SLOT)
 259                 req->cp.pkt_type |= (NG_HCI_PKT_DM5|NG_HCI_PKT_DH5);
 260
 261         req->cp.pkt_type &= unit->packet_mask;
 262         if ((req->cp.pkt_type & (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1|
 263                                  NG_HCI_PKT_DM3|NG_HCI_PKT_DH3|
 264                                  NG_HCI_PKT_DM5|NG_HCI_PKT_DH5)) == 0)
 265                 req->cp.pkt_type = (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1);
 266
 267         req->cp.pkt_type = htole16(req->cp.pkt_type);
 268
 269         if ((unit->features[0] & NG_HCI_LMP_SWITCH) && unit->role_switch)
 270                 req->cp.accept_role_switch = 1;
 271         else
 272                 req->cp.accept_role_switch = 0;
 273
 274         /*
 275          * We may speed up connect by specifying valid parameters.
 276          * So check the neighbor cache.
 277          */
 278
 279         n = ng_hci_get_neighbor(unit, &ep->bdaddr, NG_HCI_LINK_ACL);
 280         if (n == NULL) {
 281                 req->cp.page_scan_rep_mode = 0;
 282                 req->cp.page_scan_mode = 0;
 283                 req->cp.clock_offset = 0;
 284         } else {
 285                 req->cp.page_scan_rep_mode = n->page_scan_rep_mode;
 286                 req->cp.page_scan_mode = n->page_scan_mode;
 287                 req->cp.clock_offset = htole16(n->clock_offset);
 288         }
 289
 290         /*
 291          * Adust connection state
 292          */
 293
 294         if (hook == unit->acl)
 295                 con->flags |= NG_HCI_CON_NOTIFY_ACL;
 296         else
 297                 con->flags |= NG_HCI_CON_NOTIFY_SCO;
 298
 299         con->state = NG_HCI_CON_W4_CONN_COMPLETE;
 300         ng_hci_con_timeout(con);
 301
 302         /*
 303          * Queue and send HCI command
 304          */
 305
 306         NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
 307         if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
 308                 error = ng_hci_send_command(unit);
 309 out:
 310         if (item != NULL)
 311                 NG_FREE_ITEM(item);
 312
 313         return (error);
 314 } /* ng_hci_lp_acl_con_req */
 315
 316 /*
 317  * Request to create new SCO connection
 318  */
 319
 320 static int
 321 ng_hci_lp_sco_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 322 {
 323         struct sco_con_req {
 324                 ng_hci_cmd_pkt_t         hdr;
 325                 ng_hci_add_sco_con_cp    cp;
 326         } __attribute__ ((packed))      *req = NULL;
 327         ng_hci_lp_con_req_ep            *ep = NULL;
 328         ng_hci_unit_con_p                acl_con = NULL, sco_con = NULL;
 329         struct mbuf                     *m = NULL;
 330         int                              error = 0;
 331
 332         ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
 333
 334         /*
 335          * SCO connection without ACL link
 336          *
 337          * If upper layer requests SCO connection and there is no open ACL
 338          * connection to the desired remote unit, we will reject the request.
 339          */
 340
 341         LIST_FOREACH(acl_con, &unit->con_list, next)
 342                 if (acl_con->link_type == NG_HCI_LINK_ACL &&
 343                     acl_con->state == NG_HCI_CON_OPEN &&
 344                     bcmp(&acl_con->bdaddr, &ep->bdaddr, sizeof(bdaddr_t)) == 0)
 345                         break;
 346
 347         if (acl_con == NULL) {
 348                 NG_HCI_INFO(
 349 "%s: %s - No open ACL connection to bdaddr=%x:%x:%x:%x:%x:%x\n",
 350                         __func__, NG_NODE_NAME(unit->node),
 351                         ep->bdaddr.b[5], ep->bdaddr.b[4], ep->bdaddr.b[3],
 352                         ep->bdaddr.b[2], ep->bdaddr.b[1], ep->bdaddr.b[0]);
 353
 354                 error = ENOENT;
 355                 goto out;
 356         }
 357
 358         /*
 359          * Multiple SCO connections can exist between the same pair of units.
 360          * We assume that multiple SCO connections have to be opened one after
 361          * another.
 362          *
 363          * Try to find SCO connection descriptor that matches the following:
 364          *
 365          * 1) sco_con->link_type == NG_HCI_LINK_SCO
 366          *
 367          * 2) sco_con->state == NG_HCI_CON_W4_LP_CON_RSP ||
 368          *    sco_con->state == NG_HCI_CON_W4_CONN_COMPLETE
 369          *
 370          * 3) sco_con->bdaddr == ep->bdaddr
 371          *
 372          * Two cases:
 373          *
 374          * 1) We do not have connection descriptor. This is simple. Just
 375          *    create new connection and submit Add_SCO_Connection command.
 376          *
 377          * 2) We do have connection descriptor. We need to check the state.
 378          *
 379          * 2.1) NG_HCI_CON_W4_LP_CON_RSP means we in the middle of accepting
 380          *      connection from the remote unit. This is a race condition and
 381          *      we will ignore the request.
 382          *
 383          * 2.2) NG_HCI_CON_W4_CONN_COMPLETE means upper layer already requested
 384          *      connection or we just accepted it.
 385          */
 386
 387         LIST_FOREACH(sco_con, &unit->con_list, next)
 388                 if (sco_con->link_type == NG_HCI_LINK_SCO &&
 389                     (sco_con->state == NG_HCI_CON_W4_LP_CON_RSP ||
 390                      sco_con->state == NG_HCI_CON_W4_CONN_COMPLETE) &&
 391                     bcmp(&sco_con->bdaddr, &ep->bdaddr, sizeof(bdaddr_t)) == 0)
 392                         break;
 393
 394         if (sco_con != NULL) {
 395                 switch (sco_con->state) {
 396                 case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
 397                         error = EALREADY;
 398                         break;
 399
 400                 case NG_HCI_CON_W4_CONN_COMPLETE:
 401                         sco_con->flags |= NG_HCI_CON_NOTIFY_SCO;
 402                         break;
 403
 404                 default:
 405                         panic(
 406 "%s: %s - Invalid connection state=%d\n",
 407                                 __func__, NG_NODE_NAME(unit->node),
 408                                 sco_con->state);
 409                         break;
 410                 }
 411
 412                 goto out;
 413         }
 414
 415         /*
 416          * If we got here then we need to create new SCO connection descriptor
 417          * and submit HCI command.
 418          */
 419
 420         sco_con = ng_hci_new_con(unit, NG_HCI_LINK_SCO);
 421         if (sco_con == NULL) {
 422                 error = ENOMEM;
 423                 goto out;
 424         }
 425
 426         bcopy(&ep->bdaddr, &sco_con->bdaddr, sizeof(sco_con->bdaddr));
 427
 428         /*
 429          * Create HCI command
 430          */
 431
 432         MGETHDR(m, M_NOWAIT, MT_DATA);
 433         if (m == NULL) {
 434                 ng_hci_free_con(sco_con);
 435                 error = ENOBUFS;
 436                 goto out;
 437         }
 438
 439         m->m_pkthdr.len = m->m_len = sizeof(*req);
 440         req = mtod(m, struct sco_con_req *);
 441         req->hdr.type = NG_HCI_CMD_PKT;
 442         req->hdr.length = sizeof(req->cp);
 443         req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
 444                                         NG_HCI_OCF_ADD_SCO_CON));
 445
 446         req->cp.con_handle = htole16(acl_con->con_handle);
 447
 448         req->cp.pkt_type = NG_HCI_PKT_HV1;
 449         if (unit->features[1] & NG_HCI_LMP_HV2_PKT)
 450                 req->cp.pkt_type |= NG_HCI_PKT_HV2;
 451         if (unit->features[1] & NG_HCI_LMP_HV3_PKT)
 452                 req->cp.pkt_type |= NG_HCI_PKT_HV3;
 453
 454         req->cp.pkt_type &= unit->packet_mask;
 455         if ((req->cp.pkt_type & (NG_HCI_PKT_HV1|
 456                                  NG_HCI_PKT_HV2|
 457                                  NG_HCI_PKT_HV3)) == 0)
 458                 req->cp.pkt_type = NG_HCI_PKT_HV1;
 459
 460         req->cp.pkt_type = htole16(req->cp.pkt_type);
 461
 462         /*
 463          * Adust connection state
 464          */
 465
 466         sco_con->flags |= NG_HCI_CON_NOTIFY_SCO;
 467
 468         sco_con->state = NG_HCI_CON_W4_CONN_COMPLETE;
 469         ng_hci_con_timeout(sco_con);
 470
 471         /*
 472          * Queue and send HCI command
 473          */
 474
 475         NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
 476         if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
 477                 error = ng_hci_send_command(unit);
 478 out:
 479         NG_FREE_ITEM(item);
 480
 481         return (error);
 482 } /* ng_hci_lp_sco_con_req */
 483
 484 static int
 485 ng_hci_lp_le_con_req(ng_hci_unit_p unit, item_p item, hook_p hook, int link_type)
 486 {
 487         struct acl_con_req {
 488                 ng_hci_cmd_pkt_t         hdr;
 489                 ng_hci_le_create_connection_cp   cp;
 490         } __attribute__ ((packed))      *req = NULL;
 491         ng_hci_lp_con_req_ep            *ep = NULL;
 492         ng_hci_unit_con_p                con = NULL;
 493         struct mbuf                     *m = NULL;
 494         int                              error = 0;
 495
 496         ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
 497         if((link_type != NG_HCI_LINK_LE_PUBLIC)&&
 498            (link_type != NG_HCI_LINK_LE_RANDOM)){
 499                 printf("%s: Link type %d Cannot be here \n", __func__,
 500                        link_type);
 501         }
 502         /*
 503          * Only one ACL connection can exist between each pair of units.
 504          * So try to find ACL connection descriptor (in any state) that
 505          * has requested remote BD_ADDR.
 506          *
 507          * Two cases:
 508          *
 509          * 1) We do not have connection to the remote unit. This is simple.
 510          *    Just create new connection descriptor and send HCI command to
 511          *    create new connection.
 512          *
 513          * 2) We do have connection descriptor. We need to check connection
 514          *    state:
 515          *
 516          * 2.1) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
 517          *      accepting connection from the remote unit. This is a race
 518          *      condition. We will ignore this message.
 519          *
 520          * 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
 521          *      requested connection or we just accepted it. In any case
 522          *      all we need to do here is set appropriate notification bit
 523          *      and wait.
 524          *
 525          * 2.3) NG_HCI_CON_OPEN means connection is open. Just reply back
 526          *      and let upper layer know that we have connection already.
 527          */
 528
 529         con = ng_hci_con_by_bdaddr(unit, &ep->bdaddr, link_type);
 530         if (con != NULL) {
 531                 switch (con->state) {
 532                 case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
 533                         error = EALREADY;
 534                         break;
 535
 536                 case NG_HCI_CON_W4_CONN_COMPLETE:
 537                         if (hook != unit->sco)
 538                                 con->flags |= NG_HCI_CON_NOTIFY_ACL;
 539                         else
 540                                 con->flags |= NG_HCI_CON_NOTIFY_SCO;
 541                         break;
 542
 543                 case NG_HCI_CON_OPEN: {
 544                         struct ng_mesg          *msg = NULL;
 545                         ng_hci_lp_con_cfm_ep    *cfm = NULL;
 546
 547                         if (hook != NULL && NG_HOOK_IS_VALID(hook)) {
 548                                 NGI_GET_MSG(item, msg);
 549                                 NG_FREE_MSG(msg);
 550
 551                                 NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
 552                                         NGM_HCI_LP_CON_CFM, sizeof(*cfm),
 553                                         M_NOWAIT);
 554                                 if (msg != NULL) {
 555                                         cfm = (ng_hci_lp_con_cfm_ep *)msg->data;
 556                                         cfm->status = 0;
 557                                         cfm->link_type = con->link_type;
 558                                         cfm->con_handle = con->con_handle;
 559                                         bcopy(&con->bdaddr, &cfm->bdaddr,
 560                                                 sizeof(cfm->bdaddr));
 561
 562                                         /*
 563                                          * This will forward item back to
 564                                          * sender and set item to NULL
 565                                          */
 566
 567                                         _NGI_MSG(item) = msg;
 568                                         NG_FWD_ITEM_HOOK(error, item, hook);
 569                                 } else
 570                                         error = ENOMEM;
 571                         } else
 572                                 NG_HCI_INFO(
 573 "%s: %s - Source hook is not valid, hook=%p\n",
 574                                         __func__, NG_NODE_NAME(unit->node),
 575                                         hook);
 576                         } break;
 577
 578                 default:
 579                         panic(
 580 "%s: %s - Invalid connection state=%d\n",
 581                                 __func__, NG_NODE_NAME(unit->node), con->state);
 582                         break;
 583                 }
 584
 585                 goto out;
 586         }
 587
 588         /*
 589          * If we got here then we need to create new ACL connection descriptor
 590          * and submit HCI command. First create new connection desriptor, set
 591          * bdaddr and notification flags.
 592          */
 593
 594         con = ng_hci_new_con(unit, link_type);
 595         if (con == NULL) {
 596                 error = ENOMEM;
 597                 goto out;
 598         }
 599
 600         bcopy(&ep->bdaddr, &con->bdaddr, sizeof(con->bdaddr));
 601
 602         /*
 603          * Create HCI command
 604          */
 605
 606         MGETHDR(m, M_NOWAIT, MT_DATA);
 607         if (m == NULL) {
 608                 ng_hci_free_con(con);
 609                 error = ENOBUFS;
 610                 goto out;
 611         }
 612
 613         m->m_pkthdr.len = m->m_len = sizeof(*req);
 614         req = mtod(m, struct acl_con_req *);
 615         req->hdr.type = NG_HCI_CMD_PKT;
 616         req->hdr.length = sizeof(req->cp);
 617         req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LE,
 618                                         NG_HCI_OCF_LE_CREATE_CONNECTION));
 619
 620         bcopy(&ep->bdaddr, &req->cp.peer_addr, sizeof(req->cp.peer_addr));
 621         req->cp.own_address_type = 0;
 622         req->cp.peer_addr_type = (link_type == NG_HCI_LINK_LE_RANDOM)? 1:0;
 623         req->cp.scan_interval = htole16(4);
 624         req->cp.scan_window = htole16(4);
 625         req->cp.filter_policy = 0;
 626         req->cp.conn_interval_min = htole16(0xf);
 627         req->cp.conn_interval_max = htole16(0xf);
 628         req->cp.conn_latency = htole16(0);
 629         req->cp.supervision_timeout = htole16(0xc80);
 630         req->cp.min_ce_length = htole16(1);
 631         req->cp.max_ce_length = htole16(1);
 632         /*
 633          * Adust connection state
 634          */
 635
 636         if (hook != unit->sco)
 637                 con->flags |= NG_HCI_CON_NOTIFY_ACL;
 638         else
 639                 con->flags |= NG_HCI_CON_NOTIFY_SCO;
 640
 641         con->state = NG_HCI_CON_W4_CONN_COMPLETE;
 642         ng_hci_con_timeout(con);
 643
 644         /*
 645          * Queue and send HCI command
 646          */
 647
 648         NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
 649         if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
 650                 error = ng_hci_send_command(unit);
 651 out:
 652         if (item != NULL)
 653                 NG_FREE_ITEM(item);
 654
 655         return (error);
 656 } /* ng_hci_lp_acl_con_req */
 657
 658 /*
 659  * Process LP_DisconnectReq event from the upper layer protocol
 660  */
 661
 662 int
 663 ng_hci_lp_discon_req(ng_hci_unit_p unit, item_p item, hook_p hook)
 664 {
 665         struct discon_req {
 666                 ng_hci_cmd_pkt_t         hdr;
 667                 ng_hci_discon_cp         cp;
 668         } __attribute__ ((packed))      *req = NULL;
 669         ng_hci_lp_discon_req_ep         *ep = NULL;
 670         ng_hci_unit_con_p                con = NULL;
 671         struct mbuf                     *m = NULL;
 672         int                              error = 0;
 673
 674         /* Check if unit is ready */
 675         if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
 676                 NG_HCI_WARN(
 677 "%s: %s - unit is not ready, state=%#x\n",
 678                         __func__, NG_NODE_NAME(unit->node), unit->state);
 679
 680                 error = ENXIO;
 681                 goto out;
 682         }
 683
 684         if (NGI_MSG(item)->header.arglen != sizeof(*ep)) {
 685                 NG_HCI_ALERT(
 686 "%s: %s - invalid LP_DisconnectReq message size=%d\n",
 687                         __func__, NG_NODE_NAME(unit->node),
 688                         NGI_MSG(item)->header.arglen);
 689
 690                 error = EMSGSIZE;
 691                 goto out;
 692         }
 693
 694         ep = (ng_hci_lp_discon_req_ep *)(NGI_MSG(item)->data);
 695
 696         con = ng_hci_con_by_handle(unit, ep->con_handle);
 697         if (con == NULL) {
 698                 NG_HCI_ERR(
 699 "%s: %s - invalid connection handle=%d\n",
 700                         __func__, NG_NODE_NAME(unit->node), ep->con_handle);
 701
 702                 error = ENOENT;
 703                 goto out;
 704         }
 705
 706         if (con->state != NG_HCI_CON_OPEN) {
 707                 NG_HCI_ERR(
 708 "%s: %s - invalid connection state=%d, handle=%d\n",
 709                         __func__, NG_NODE_NAME(unit->node), con->state,
 710                         ep->con_handle);
 711
 712                 error = EINVAL;
 713                 goto out;
 714         }
 715
 716         /*
 717          * Create HCI command
 718          */
 719
 720         MGETHDR(m, M_NOWAIT, MT_DATA);
 721         if (m == NULL) {
 722                 error = ENOBUFS;
 723                 goto out;
 724         }
 725
 726         m->m_pkthdr.len = m->m_len = sizeof(*req);
 727         req = mtod(m, struct discon_req *);
 728         req->hdr.type = NG_HCI_CMD_PKT;
 729         req->hdr.length = sizeof(req->cp);
 730         req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
 731                                                         NG_HCI_OCF_DISCON));
 732
 733         req->cp.con_handle = htole16(ep->con_handle);
 734         req->cp.reason = ep->reason;
 735
 736         /*
 737          * Queue and send HCI command
 738          */
 739
 740         NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
 741         if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
 742                 error = ng_hci_send_command(unit);
 743 out:
 744         NG_FREE_ITEM(item);
 745
 746         return (error);
 747 } /* ng_hci_lp_discon_req */
 748
 749 /*
 750  * Send LP_ConnectCfm event to the upper layer protocol
 751  */
 752
 753 int
 754 ng_hci_lp_con_cfm(ng_hci_unit_con_p con, int status)
 755 {
 756         ng_hci_unit_p            unit = con->unit;
 757         struct ng_mesg          *msg = NULL;
 758         ng_hci_lp_con_cfm_ep    *ep = NULL;
 759         int                      error;
 760
 761         /*
 762          * Check who wants to be notified. For ACL links both ACL and SCO
 763          * upstream hooks will be notified (if required). For SCO links
 764          * only SCO upstream hook will receive notification
 765          */
 766
 767         if (con->link_type != NG_HCI_LINK_SCO &&
 768             con->flags & NG_HCI_CON_NOTIFY_ACL) {
 769                 if (unit->acl != NULL && NG_HOOK_IS_VALID(unit->acl)) {
 770                         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_CFM,
 771                                 sizeof(*ep), M_NOWAIT);
 772                         if (msg != NULL) {
 773                                 ep = (ng_hci_lp_con_cfm_ep *) msg->data;
 774                                 ep->status = status;
 775                                 ep->link_type = con->link_type;
 776                                 ep->con_handle = con->con_handle;
 777                                 bcopy(&con->bdaddr, &ep->bdaddr,
 778                                         sizeof(ep->bdaddr));
 779
 780                                 NG_SEND_MSG_HOOK(error, unit->node, msg,
 781                                         unit->acl, 0);
 782                         }
 783                 } else
 784                         NG_HCI_INFO(
 785 "%s: %s - ACL hook not valid, hook=%p\n",
 786                                 __func__, NG_NODE_NAME(unit->node), unit->acl);
 787
 788                 con->flags &= ~NG_HCI_CON_NOTIFY_ACL;
 789         }
 790
 791         if (con->flags & NG_HCI_CON_NOTIFY_SCO) {
 792                 if (unit->sco != NULL && NG_HOOK_IS_VALID(unit->sco)) {
 793                         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_CFM,
 794                                 sizeof(*ep), M_NOWAIT);
 795                         if (msg != NULL) {
 796                                 ep = (ng_hci_lp_con_cfm_ep *) msg->data;
 797                                 ep->status = status;
 798                                 ep->link_type = con->link_type;
 799                                 ep->con_handle = con->con_handle;
 800                                 bcopy(&con->bdaddr, &ep->bdaddr,
 801                                         sizeof(ep->bdaddr));
 802
 803                                 NG_SEND_MSG_HOOK(error, unit->node, msg,
 804                                         unit->sco, 0);
 805                         }
 806                 } else
 807                         NG_HCI_INFO(
 808 "%s: %s - SCO hook not valid, hook=%p\n",
 809                                 __func__, NG_NODE_NAME(unit->node), unit->acl);
 810
 811                 con->flags &= ~NG_HCI_CON_NOTIFY_SCO;
 812         }
 813
 814         return (0);
 815 } /* ng_hci_lp_con_cfm */
 816
 817 int
 818 ng_hci_lp_enc_change(ng_hci_unit_con_p con, int status)
 819 {
 820         ng_hci_unit_p            unit = con->unit;
 821         struct ng_mesg          *msg = NULL;
 822         ng_hci_lp_enc_change_ep *ep = NULL;
 823         int                      error;
 824
 825
 826         if (con->link_type != NG_HCI_LINK_SCO) {
 827                 if (unit->acl != NULL && NG_HOOK_IS_VALID(unit->acl)) {
 828                         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_ENC_CHG,
 829                                 sizeof(*ep), M_NOWAIT);
 830                         if (msg != NULL) {
 831                                 ep = (ng_hci_lp_enc_change_ep *) msg->data;
 832                                 ep->status = status;
 833                                 ep->link_type = con->link_type;
 834                                 ep->con_handle = con->con_handle;
 835
 836                                 NG_SEND_MSG_HOOK(error, unit->node, msg,
 837                                         unit->acl, 0);
 838                         }
 839                 } else
 840                         NG_HCI_INFO(
 841 "%s: %s - ACL hook not valid, hook=%p\n",
 842                                 __func__, NG_NODE_NAME(unit->node), unit->acl);
 843
 844         }
 845         return (0);
 846 } /* ng_hci_lp_con_cfm */
 847
 848 /*
 849  * Send LP_ConnectInd event to the upper layer protocol
 850  */
 851
 852 int
 853 ng_hci_lp_con_ind(ng_hci_unit_con_p con, u_int8_t *uclass)
 854 {
 855         ng_hci_unit_p            unit = con->unit;
 856         struct ng_mesg          *msg = NULL;
 857         ng_hci_lp_con_ind_ep    *ep = NULL;
 858         hook_p                   hook = NULL;
 859         int                      error = 0;
 860
 861         /*
 862          * Connection_Request event is generated for specific link type.
 863          * Use link_type to select upstream hook.
 864          */
 865
 866         if (con->link_type != NG_HCI_LINK_SCO)
 867                 hook = unit->acl;
 868         else
 869                 hook = unit->sco;
 870
 871         if (hook != NULL && NG_HOOK_IS_VALID(hook)) {
 872                 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_IND,
 873                         sizeof(*ep), M_NOWAIT);
 874                 if (msg == NULL)
 875                         return (ENOMEM);
 876
 877                 ep = (ng_hci_lp_con_ind_ep *)(msg->data);
 878                 ep->link_type = con->link_type;
 879                 bcopy(uclass, ep->uclass, sizeof(ep->uclass));
 880                 bcopy(&con->bdaddr, &ep->bdaddr, sizeof(ep->bdaddr));
 881
 882                 NG_SEND_MSG_HOOK(error, unit->node, msg, hook, 0);
 883         } else {
 884                 NG_HCI_WARN(
 885 "%s: %s - Upstream hook is not connected or not valid, hook=%p\n",
 886                         __func__, NG_NODE_NAME(unit->node), hook);
 887
 888                 error = ENOTCONN;
 889         }
 890
 891         return (error);
 892 } /* ng_hci_lp_con_ind */
 893
 894 /*
 895  * Process LP_ConnectRsp event from the upper layer protocol
 896  */
 897
 898 int
 899 ng_hci_lp_con_rsp(ng_hci_unit_p unit, item_p item, hook_p hook)
 900 {
 901         struct con_rsp_req {
 902                 ng_hci_cmd_pkt_t                 hdr;
 903                 union {
 904                         ng_hci_accept_con_cp     acc;
 905                         ng_hci_reject_con_cp     rej;
 906                 } __attribute__ ((packed))       cp;
 907         } __attribute__ ((packed))              *req = NULL;
 908         ng_hci_lp_con_rsp_ep                    *ep = NULL;
 909         ng_hci_unit_con_p                        con = NULL;
 910         struct mbuf                             *m = NULL;
 911         int                                      error = 0;
 912
 913         /* Check if unit is ready */
 914         if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
 915                 NG_HCI_WARN(
 916 "%s: %s - unit is not ready, state=%#x\n",
 917                         __func__, NG_NODE_NAME(unit->node), unit->state);
 918
 919                 error = ENXIO;
 920                 goto out;
 921         }
 922
 923         if (NGI_MSG(item)->header.arglen != sizeof(*ep)) {
 924                 NG_HCI_ALERT(
 925 "%s: %s - invalid LP_ConnectRsp message size=%d\n",
 926                         __func__, NG_NODE_NAME(unit->node),
 927                         NGI_MSG(item)->header.arglen);
 928
 929                 error = EMSGSIZE;
 930                 goto out;
 931         }
 932
 933         ep = (ng_hci_lp_con_rsp_ep *)(NGI_MSG(item)->data);
 934
 935         /*
 936          * Here we have to deal with race. Upper layers might send conflicting
 937          * requests. One might send Accept and other Reject. We will not try
 938          * to solve all the problems, so first request will always win.
 939          *
 940          * Try to find connection that matches the following:
 941          *
 942          * 1) con->link_type == ep->link_type
 943          *
 944          * 2) con->state == NG_HCI_CON_W4_LP_CON_RSP ||
 945          *    con->state == NG_HCI_CON_W4_CONN_COMPLETE
 946          *
 947          * 3) con->bdaddr == ep->bdaddr
 948          *
 949          * Two cases:
 950          *
 951          * 1) We do not have connection descriptor. Could be bogus request or
 952          *    we have rejected connection already.
 953          *
 954          * 2) We do have connection descriptor. Then we need to check state:
 955          *
 956          * 2.1) NG_HCI_CON_W4_LP_CON_RSP means upper layer has requested
 957          *      connection and it is a first response from the upper layer.
 958          *      if "status == 0" (Accept) then we will send Accept_Connection
 959          *      command and change connection state to W4_CONN_COMPLETE, else
 960          *      send reject and delete connection.
 961          *
 962          * 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that we already accepted
 963          *      connection. If "status == 0" we just need to link request
 964          *      and wait, else ignore Reject request.
 965          */
 966
 967         LIST_FOREACH(con, &unit->con_list, next)
 968                 if (con->link_type == ep->link_type &&
 969                     (con->state == NG_HCI_CON_W4_LP_CON_RSP ||
 970                      con->state == NG_HCI_CON_W4_CONN_COMPLETE) &&
 971                     bcmp(&con->bdaddr, &ep->bdaddr, sizeof(bdaddr_t)) == 0)
 972                         break;
 973
 974         if (con == NULL) {
 975                 /* Reject for non-existing connection is fine */
 976                 error = (ep->status == 0)? ENOENT : 0;
 977                 goto out;
 978         }
 979
 980         /*
 981          * Remove connection timeout and check connection state.
 982          * Note: if ng_hci_con_untimeout() fails (returns non-zero value) then
 983          * timeout already happened and event went into node's queue.
 984          */
 985
 986         if ((error = ng_hci_con_untimeout(con)) != 0)
 987                 goto out;
 988
 989         switch (con->state) {
 990         case NG_HCI_CON_W4_LP_CON_RSP:
 991
 992                 /*
 993                  * Create HCI command
 994                  */
 995
 996                 MGETHDR(m, M_NOWAIT, MT_DATA);
 997                 if (m == NULL) {
 998                         error = ENOBUFS;
 999                         goto out;
1000                 }
1001
1002                 req = mtod(m, struct con_rsp_req *);
1003                 req->hdr.type = NG_HCI_CMD_PKT;
1004
1005                 if (ep->status == 0) {
1006                         req->hdr.length = sizeof(req->cp.acc);
1007                         req->hdr.opcode = htole16(NG_HCI_OPCODE(
1008                                                         NG_HCI_OGF_LINK_CONTROL,
1009                                                         NG_HCI_OCF_ACCEPT_CON));
1010
1011                         bcopy(&ep->bdaddr, &req->cp.acc.bdaddr,
1012                                 sizeof(req->cp.acc.bdaddr));
1013
1014                         /*
1015                          * We are accepting connection, so if we support role
1016                          * switch and role switch was enabled then set role to
1017                          * NG_HCI_ROLE_MASTER and let LM peform role switch.
1018                          * Otherwise we remain slave. In this case LM WILL NOT
1019                          * perform role switch.
1020                          */
1021
1022                         if ((unit->features[0] & NG_HCI_LMP_SWITCH) &&
1023                             unit->role_switch)
1024                                 req->cp.acc.role = NG_HCI_ROLE_MASTER;
1025                         else
1026                                 req->cp.acc.role = NG_HCI_ROLE_SLAVE;
1027
1028                         /*
1029                          * Adjust connection state
1030                          */
1031
1032                         if (hook == unit->acl)
1033                                 con->flags |= NG_HCI_CON_NOTIFY_ACL;
1034                         else
1035                                 con->flags |= NG_HCI_CON_NOTIFY_SCO;
1036
1037                         con->state = NG_HCI_CON_W4_CONN_COMPLETE;
1038                         ng_hci_con_timeout(con);
1039                 } else {
1040                         req->hdr.length = sizeof(req->cp.rej);
1041                         req->hdr.opcode = htole16(NG_HCI_OPCODE(
1042                                                         NG_HCI_OGF_LINK_CONTROL,
1043                                                         NG_HCI_OCF_REJECT_CON));
1044
1045                         bcopy(&ep->bdaddr, &req->cp.rej.bdaddr,
1046                                 sizeof(req->cp.rej.bdaddr));
1047
1048                         req->cp.rej.reason = ep->status;
1049
1050                         /*
1051                          * Free connection descritor
1052                          * Item will be deleted just before return.
1053                          */
1054
1055                         ng_hci_free_con(con);
1056                 }
1057
1058                 m->m_pkthdr.len = m->m_len = sizeof(req->hdr) + req->hdr.length;
1059
1060                 /* Queue and send HCI command */
1061                 NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
1062                 if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
1063                         error = ng_hci_send_command(unit);
1064                 break;
1065
1066         case NG_HCI_CON_W4_CONN_COMPLETE:
1067                 if (ep->status == 0) {
1068                         if (hook == unit->acl)
1069                                 con->flags |= NG_HCI_CON_NOTIFY_ACL;
1070                         else
1071                                 con->flags |= NG_HCI_CON_NOTIFY_SCO;
1072                 } else
1073                         error = EPERM;
1074                 break;
1075
1076         default:
1077                 panic(
1078 "%s: %s - Invalid connection state=%d\n",
1079                         __func__, NG_NODE_NAME(unit->node), con->state);
1080                 break;
1081         }
1082 out:
1083         NG_FREE_ITEM(item);
1084
1085         return (error);
1086 } /* ng_hci_lp_con_rsp */
1087
1088 /*
1089  * Send LP_DisconnectInd to the upper layer protocol
1090  */
1091
1092 int
1093 ng_hci_lp_discon_ind(ng_hci_unit_con_p con, int reason)
1094 {
1095         ng_hci_unit_p            unit = con->unit;
1096         struct ng_mesg          *msg = NULL;
1097         ng_hci_lp_discon_ind_ep *ep = NULL;
1098         int                      error = 0;
1099
1100         /*
1101          * Disconnect_Complete event is generated for specific connection
1102          * handle. For ACL connection handles both ACL and SCO upstream
1103          * hooks will receive notification. For SCO connection handles
1104          * only SCO upstream hook will receive notification.
1105          */
1106
1107         if (con->link_type != NG_HCI_LINK_SCO) {
1108                 if (unit->acl != NULL && NG_HOOK_IS_VALID(unit->acl)) {
1109                         NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
1110                                 NGM_HCI_LP_DISCON_IND, sizeof(*ep), M_NOWAIT);
1111                         if (msg == NULL)
1112                                 return (ENOMEM);
1113
1114                         ep = (ng_hci_lp_discon_ind_ep *) msg->data;
1115                         ep->reason = reason;
1116                         ep->link_type = con->link_type;
1117                         ep->con_handle = con->con_handle;
1118
1119                         NG_SEND_MSG_HOOK(error,unit->node,msg,unit->acl,0);
1120                 } else
1121                         NG_HCI_INFO(
1122 "%s: %s - ACL hook is not connected or not valid, hook=%p\n",
1123                                 __func__, NG_NODE_NAME(unit->node), unit->acl);
1124         }
1125
1126         if (unit->sco != NULL && NG_HOOK_IS_VALID(unit->sco)) {
1127                 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_IND,
1128                         sizeof(*ep), M_NOWAIT);
1129                 if (msg == NULL)
1130                         return (ENOMEM);
1131
1132                 ep = (ng_hci_lp_discon_ind_ep *) msg->data;
1133                 ep->reason = reason;
1134                 ep->link_type = con->link_type;
1135                 ep->con_handle = con->con_handle;
1136
1137                 NG_SEND_MSG_HOOK(error, unit->node, msg, unit->sco, 0);
1138         } else
1139                 NG_HCI_INFO(
1140 "%s: %s - SCO hook is not connected or not valid, hook=%p\n",
1141                         __func__, NG_NODE_NAME(unit->node), unit->sco);
1142
1143         return (0);
1144 } /* ng_hci_lp_discon_ind */
1145
1146 /*
1147  * Process LP_QoSReq action from the upper layer protocol
1148  */
1149
1150 int
1151 ng_hci_lp_qos_req(ng_hci_unit_p unit, item_p item, hook_p hook)
1152 {
1153         struct qos_setup_req {
1154                 ng_hci_cmd_pkt_t         hdr;
1155                 ng_hci_qos_setup_cp      cp;
1156         } __attribute__ ((packed))      *req = NULL;
1157         ng_hci_lp_qos_req_ep            *ep = NULL;
1158         ng_hci_unit_con_p                con = NULL;
1159         struct mbuf                     *m = NULL;
1160         int                              error = 0;
1161
1162         /* Check if unit is ready */
1163         if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
1164                 NG_HCI_WARN(
1165 "%s: %s - unit is not ready, state=%#x\n",
1166                         __func__, NG_NODE_NAME(unit->node), unit->state);
1167
1168                 error = ENXIO;
1169                 goto out;
1170         }
1171
1172         if (NGI_MSG(item)->header.arglen != sizeof(*ep)) {
1173                 NG_HCI_ALERT(
1174 "%s: %s - invalid LP_QoSSetupReq message size=%d\n",
1175                         __func__, NG_NODE_NAME(unit->node),
1176                         NGI_MSG(item)->header.arglen);
1177
1178                 error = EMSGSIZE;
1179                 goto out;
1180         }
1181
1182         ep = (ng_hci_lp_qos_req_ep *)(NGI_MSG(item)->data);
1183
1184         con = ng_hci_con_by_handle(unit, ep->con_handle);
1185         if (con == NULL) {
1186                 NG_HCI_ERR(
1187 "%s: %s - invalid connection handle=%d\n",
1188                         __func__, NG_NODE_NAME(unit->node), ep->con_handle);
1189
1190                 error = EINVAL;
1191                 goto out;
1192         }
1193
1194         if (con->link_type != NG_HCI_LINK_ACL) {
1195                 NG_HCI_ERR("%s: %s - invalid link type=%d\n",
1196                         __func__, NG_NODE_NAME(unit->node), con->link_type);
1197
1198                 error = EINVAL;
1199                 goto out;
1200         }
1201
1202         if (con->state != NG_HCI_CON_OPEN) {
1203                 NG_HCI_ERR(
1204 "%s: %s - invalid connection state=%d, handle=%d\n",
1205                         __func__, NG_NODE_NAME(unit->node), con->state,
1206                         con->con_handle);
1207
1208                 error = EINVAL;
1209                 goto out;
1210         }
1211
1212         /*
1213          * Create HCI command
1214          */
1215
1216         MGETHDR(m, M_NOWAIT, MT_DATA);
1217         if (m == NULL) {
1218                 error = ENOBUFS;
1219                 goto out;
1220         }
1221
1222         m->m_pkthdr.len = m->m_len = sizeof(*req);
1223         req = mtod(m, struct qos_setup_req *);
1224         req->hdr.type = NG_HCI_CMD_PKT;
1225         req->hdr.length = sizeof(req->cp);
1226         req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_POLICY,
1227                         NG_HCI_OCF_QOS_SETUP));
1228
1229         req->cp.con_handle = htole16(ep->con_handle);
1230         req->cp.flags = ep->flags;
1231         req->cp.service_type = ep->service_type;
1232         req->cp.token_rate = htole32(ep->token_rate);
1233         req->cp.peak_bandwidth = htole32(ep->peak_bandwidth);
1234         req->cp.latency = htole32(ep->latency);
1235         req->cp.delay_variation = htole32(ep->delay_variation);
1236
1237         /*
1238          * Adjust connection state
1239          */
1240
1241         if (hook == unit->acl)
1242                 con->flags |= NG_HCI_CON_NOTIFY_ACL;
1243         else
1244                 con->flags |= NG_HCI_CON_NOTIFY_SCO;
1245
1246         /*
1247          * Queue and send HCI command
1248          */
1249
1250         NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
1251         if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
1252                 error = ng_hci_send_command(unit);
1253 out:
1254         NG_FREE_ITEM(item);
1255
1256         return (error);
1257 } /* ng_hci_lp_qos_req */
1258
1259 /*
1260  * Send LP_QoSCfm event to the upper layer protocol
1261  */
1262
1263 int
1264 ng_hci_lp_qos_cfm(ng_hci_unit_con_p con, int status)
1265 {
1266         ng_hci_unit_p            unit = con->unit;
1267         struct ng_mesg          *msg = NULL;
1268         ng_hci_lp_qos_cfm_ep    *ep = NULL;
1269         int                      error;
1270
1271         if (con->flags & NG_HCI_CON_NOTIFY_ACL) {
1272                 if (unit->acl != NULL && NG_HOOK_IS_VALID(unit->acl)) {
1273                         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_CFM,
1274                                 sizeof(*ep), M_NOWAIT);
1275                         if (msg != NULL) {
1276                                 ep = (ng_hci_lp_qos_cfm_ep *) msg->data;
1277                                 ep->status = status;
1278                                 ep->con_handle = con->con_handle;
1279
1280                                 NG_SEND_MSG_HOOK(error, unit->node, msg,
1281                                         unit->acl, 0);
1282                         }
1283                 } else
1284                         NG_HCI_INFO(
1285 "%s: %s - ACL hook not valid, hook=%p\n",
1286                                 __func__, NG_NODE_NAME(unit->node), unit->acl);
1287
1288                 con->flags &= ~NG_HCI_CON_NOTIFY_ACL;
1289         }
1290
1291         if (con->flags & NG_HCI_CON_NOTIFY_SCO) {
1292                 if (unit->sco != NULL && NG_HOOK_IS_VALID(unit->sco)) {
1293                         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_CFM,
1294                                 sizeof(*ep), M_NOWAIT);
1295                         if (msg != NULL) {
1296                                 ep = (ng_hci_lp_qos_cfm_ep *) msg->data;
1297                                 ep->status = status;
1298                                 ep->con_handle = con->con_handle;
1299
1300                                 NG_SEND_MSG_HOOK(error, unit->node, msg,
1301                                         unit->sco, 0);
1302                         }
1303                 } else
1304                         NG_HCI_INFO(
1305 "%s: %s - SCO hook not valid, hook=%p\n",
1306                                  __func__, NG_NODE_NAME(unit->node), unit->sco);
1307
1308                 con->flags &= ~NG_HCI_CON_NOTIFY_SCO;
1309         }
1310
1311         return (0);
1312 } /* ng_hci_lp_qos_cfm */
1313
1314 /*
1315  * Send LP_QoSViolationInd event to the upper layer protocol
1316  */
1317
1318 int
1319 ng_hci_lp_qos_ind(ng_hci_unit_con_p con)
1320 {
1321         ng_hci_unit_p            unit = con->unit;
1322         struct ng_mesg          *msg = NULL;
1323         ng_hci_lp_qos_ind_ep    *ep = NULL;
1324         int                      error;
1325
1326         /*
1327          * QoS Violation can only be generated for ACL connection handles.
1328          * Both ACL and SCO upstream hooks will receive notification.
1329          */
1330
1331         if (unit->acl != NULL && NG_HOOK_IS_VALID(unit->acl)) {
1332                 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_IND,
1333                         sizeof(*ep), M_NOWAIT);
1334                 if (msg == NULL)
1335                         return (ENOMEM);
1336
1337                 ep = (ng_hci_lp_qos_ind_ep *) msg->data;
1338                 ep->con_handle = con->con_handle;
1339
1340                 NG_SEND_MSG_HOOK(error, unit->node, msg, unit->acl, 0);
1341         } else
1342                 NG_HCI_INFO(
1343 "%s: %s - ACL hook is not connected or not valid, hook=%p\n",
1344                         __func__, NG_NODE_NAME(unit->node), unit->acl);
1345
1346         if (unit->sco != NULL && NG_HOOK_IS_VALID(unit->sco)) {
1347                 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_IND,
1348                         sizeof(*ep), M_NOWAIT);
1349                 if (msg == NULL)
1350                         return (ENOMEM);
1351
1352                 ep = (ng_hci_lp_qos_ind_ep *) msg->data;
1353                 ep->con_handle = con->con_handle;
1354
1355                 NG_SEND_MSG_HOOK(error, unit->node, msg, unit->sco, 0);
1356         } else
1357                 NG_HCI_INFO(
1358 "%s: %s - SCO hook is not connected or not valid, hook=%p\n",
1359                         __func__, NG_NODE_NAME(unit->node), unit->sco);
1360
1361         return (0);
1362 } /* ng_hci_lp_qos_ind */
1363
1364 /*
1365  * Process connection timeout
1366  */
1367
1368 void
1369 ng_hci_process_con_timeout(node_p node, hook_p hook, void *arg1, int con_handle)
1370 {
1371         ng_hci_unit_p           unit = NULL;
1372         ng_hci_unit_con_p       con = NULL;
1373
1374         if (NG_NODE_NOT_VALID(node)) {
1375                 printf("%s: Netgraph node is not valid\n", __func__);
1376                 return;
1377         }
1378
1379         unit = (ng_hci_unit_p) NG_NODE_PRIVATE(node);
1380         con = ng_hci_con_by_handle(unit, con_handle);
1381
1382         if (con == NULL) {
1383                 NG_HCI_ALERT(
1384 "%s: %s - could not find connection, handle=%d\n",
1385                         __func__, NG_NODE_NAME(node), con_handle);
1386                 return;
1387         }
1388
1389         if (!(con->flags & NG_HCI_CON_TIMEOUT_PENDING)) {
1390                 NG_HCI_ALERT(
1391 "%s: %s - no pending connection timeout, handle=%d, state=%d, flags=%#x\n",
1392                         __func__, NG_NODE_NAME(node), con_handle, con->state,
1393                         con->flags);
1394                 return;
1395         }
1396
1397         con->flags &= ~NG_HCI_CON_TIMEOUT_PENDING;
1398
1399         /*
1400          * We expect to receive connection timeout in one of the following
1401          * states:
1402          *
1403          * 1) NG_HCI_CON_W4_LP_CON_RSP means that upper layer has not responded
1404          *    to our LP_CON_IND. Do nothing and destroy connection. Remote peer
1405          *    most likely already gave up on us.
1406          *
1407          * 2) NG_HCI_CON_W4_CONN_COMPLETE means upper layer requested connection
1408          *    (or we in the process of accepting it) and baseband has timedout
1409          *    on us. Inform upper layers and send LP_CON_CFM.
1410          */
1411
1412         switch (con->state) {
1413         case NG_HCI_CON_W4_LP_CON_RSP:
1414                 break;
1415
1416         case NG_HCI_CON_W4_CONN_COMPLETE:
1417                 ng_hci_lp_con_cfm(con, 0xee);
1418                 break;
1419
1420         default:
1421                 panic(
1422 "%s: %s - Invalid connection state=%d\n",
1423                         __func__, NG_NODE_NAME(node), con->state);
1424                 break;
1425         }
1426
1427         ng_hci_free_con(con);
1428 } /* ng_hci_process_con_timeout */
1429