sys/netgraph/bluetooth/hci/ng_hci_ulpi.c

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