sys/netgraph/bluetooth/hci/ng_hci_main.c

   1 /*
   2  * ng_hci_main.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_main.c,v 1.2 2003/03/18 00:09:36 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/ng_parse.h>
  44 #include <netgraph/bluetooth/include/ng_bluetooth.h>
  45 #include <netgraph/bluetooth/include/ng_hci.h>
  46 #include <netgraph/bluetooth/hci/ng_hci_var.h>
  47 #include <netgraph/bluetooth/hci/ng_hci_prse.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  **     This node implements Bluetooth Host Controller Interface (HCI)
  56  ******************************************************************************
  57  ******************************************************************************/
  58
  59 /* MALLOC define */
  60 #ifdef NG_SEPARATE_MALLOC
  61 MALLOC_DEFINE(M_NETGRAPH_HCI, "netgraph_hci", "Netgraph Bluetooth HCI node");
  62 #else
  63 #define M_NETGRAPH_HCI M_NETGRAPH
  64 #endif /* NG_SEPARATE_MALLOC */
  65
  66 /* Netgraph node methods */
  67 static  ng_constructor_t        ng_hci_constructor;
  68 static  ng_shutdown_t           ng_hci_shutdown;
  69 static  ng_newhook_t            ng_hci_newhook;
  70 static  ng_connect_t            ng_hci_connect;
  71 static  ng_disconnect_t         ng_hci_disconnect;
  72 static  ng_rcvmsg_t             ng_hci_default_rcvmsg;
  73 static  ng_rcvmsg_t             ng_hci_upper_rcvmsg;
  74 static  ng_rcvdata_t            ng_hci_drv_rcvdata;
  75 static  ng_rcvdata_t            ng_hci_acl_rcvdata;
  76 static  ng_rcvdata_t            ng_hci_sco_rcvdata;
  77 static  ng_rcvdata_t            ng_hci_raw_rcvdata;
  78
  79 /* Netgraph node type descriptor */
  80 static  struct ng_type          typestruct = {
  81         .version =      NG_ABI_VERSION,
  82         .name =         NG_HCI_NODE_TYPE,
  83         .constructor =  ng_hci_constructor,
  84         .rcvmsg =       ng_hci_default_rcvmsg,
  85         .shutdown =     ng_hci_shutdown,
  86         .newhook =      ng_hci_newhook,
  87         .connect =      ng_hci_connect,
  88         .rcvdata =      ng_hci_drv_rcvdata,
  89         .disconnect =   ng_hci_disconnect,
  90         .cmdlist =      ng_hci_cmdlist,
  91 };
  92 NETGRAPH_INIT(hci, &typestruct);
  93 MODULE_VERSION(ng_hci, NG_BLUETOOTH_VERSION);
  94 MODULE_DEPEND(ng_hci, ng_bluetooth, NG_BLUETOOTH_VERSION,
  95         NG_BLUETOOTH_VERSION, NG_BLUETOOTH_VERSION);
  96 static int ng_hci_linktype_to_addrtype(int linktype);
  97
  98 static int ng_hci_linktype_to_addrtype(int linktype)
  99 {
 100         switch(linktype){
 101         case NG_HCI_LINK_LE_PUBLIC:
 102                 return BDADDR_LE_PUBLIC;
 103         case NG_HCI_LINK_LE_RANDOM:
 104                 return BDADDR_LE_RANDOM;
 105         case NG_HCI_LINK_ACL:
 106                 /*FALLTHROUGH*/
 107         default:
 108                 return BDADDR_BREDR;
 109         }
 110         return BDADDR_BREDR;
 111 }
 112 /*****************************************************************************
 113  *****************************************************************************
 114  **                   Netgraph methods implementation
 115  *****************************************************************************
 116  *****************************************************************************/
 117
 118 /*
 119  * Create new instance of HCI node (new unit)
 120  */
 121
 122 static int
 123 ng_hci_constructor(node_p node)
 124 {
 125         ng_hci_unit_p   unit = NULL;
 126
 127         unit = malloc(sizeof(*unit), M_NETGRAPH_HCI, M_WAITOK | M_ZERO);
 128
 129         unit->node = node;
 130         unit->debug = NG_HCI_WARN_LEVEL;
 131
 132         unit->link_policy_mask = 0xffff; /* Enable all supported modes */
 133         unit->packet_mask = 0xffff; /* Enable all packet types */
 134         unit->role_switch = 1; /* Enable role switch (if device supports it) */
 135
 136         /*
 137          * Set default buffer info
 138          *
 139          * One HCI command
 140          * One ACL packet with max. size of 17 bytes (1 DM1 packet)
 141          * One SCO packet with max. size of 10 bytes (1 HV1 packet)
 142          */
 143
 144         NG_HCI_BUFF_CMD_SET(unit->buffer, 1);
 145         NG_HCI_BUFF_ACL_SET(unit->buffer, 1, 17, 1);
 146         NG_HCI_BUFF_SCO_SET(unit->buffer, 1, 10, 1);
 147
 148         /* Init command queue & command timeout handler */
 149         ng_callout_init(&unit->cmd_timo);
 150         NG_BT_MBUFQ_INIT(&unit->cmdq, NG_HCI_CMD_QUEUE_LEN);
 151
 152         /* Init lists */
 153         LIST_INIT(&unit->con_list);
 154         LIST_INIT(&unit->neighbors);
 155
 156         /*
 157          * This node has to be a WRITER because both data and messages
 158          * can change node state.
 159          */
 160
 161         NG_NODE_FORCE_WRITER(node);
 162         NG_NODE_SET_PRIVATE(node, unit);
 163
 164         return (0);
 165 } /* ng_hci_constructor */
 166
 167 /*
 168  * Destroy the node
 169  */
 170
 171 static int
 172 ng_hci_shutdown(node_p node)
 173 {
 174         ng_hci_unit_p   unit = (ng_hci_unit_p) NG_NODE_PRIVATE(node);
 175
 176         NG_NODE_SET_PRIVATE(node, NULL);
 177         NG_NODE_UNREF(node);
 178
 179         unit->node = NULL;
 180         ng_hci_unit_clean(unit, 0x16 /* Connection terminated by local host */);
 181
 182         NG_BT_MBUFQ_DESTROY(&unit->cmdq);
 183
 184         bzero(unit, sizeof(*unit));
 185         free(unit, M_NETGRAPH_HCI);
 186
 187         return (0);
 188 } /* ng_hci_shutdown */
 189
 190 /*
 191  * Give our OK for a hook to be added. Unit driver is connected to the driver
 192  * (NG_HCI_HOOK_DRV) hook. Upper layer protocols are connected to appropriate
 193  * (NG_HCI_HOOK_ACL or NG_HCI_HOOK_SCO) hooks.
 194  */
 195
 196 static int
 197 ng_hci_newhook(node_p node, hook_p hook, char const *name)
 198 {
 199         ng_hci_unit_p    unit = (ng_hci_unit_p) NG_NODE_PRIVATE(node);
 200         hook_p          *h = NULL;
 201
 202         if (strcmp(name, NG_HCI_HOOK_DRV) == 0)
 203                 h = &unit->drv;
 204         else if (strcmp(name, NG_HCI_HOOK_ACL) == 0)
 205                 h = &unit->acl;
 206         else if (strcmp(name, NG_HCI_HOOK_SCO) == 0)
 207                 h = &unit->sco;
 208         else if (strcmp(name, NG_HCI_HOOK_RAW) == 0)
 209                 h = &unit->raw;
 210         else
 211                 return (EINVAL);
 212
 213         if (*h != NULL)
 214                 return (EISCONN);
 215
 216         *h = hook;
 217
 218         return (0);
 219 } /* ng_hci_newhook */
 220
 221 /*
 222  * Give our final OK to connect hook
 223  */
 224
 225 static int
 226 ng_hci_connect(hook_p hook)
 227 {
 228         ng_hci_unit_p   unit = (ng_hci_unit_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 229
 230         if (hook != unit->drv) {
 231                 if (hook == unit->acl) {
 232                         NG_HOOK_SET_RCVMSG(hook, ng_hci_upper_rcvmsg);
 233                         NG_HOOK_SET_RCVDATA(hook, ng_hci_acl_rcvdata);
 234                 } else if (hook == unit->sco) {
 235                         NG_HOOK_SET_RCVMSG(hook, ng_hci_upper_rcvmsg);
 236                         NG_HOOK_SET_RCVDATA(hook, ng_hci_sco_rcvdata);
 237                 } else
 238                         NG_HOOK_SET_RCVDATA(hook, ng_hci_raw_rcvdata);
 239
 240                 /* Send delayed notification to the upper layers */
 241                 if (hook != unit->raw)
 242                         ng_send_fn(unit->node, hook, ng_hci_node_is_up, NULL,0);
 243         } else
 244                 unit->state |= NG_HCI_UNIT_CONNECTED;
 245
 246         return (0);
 247 } /* ng_hci_connect */
 248
 249 /*
 250  * Disconnect the hook
 251  */
 252
 253 static int
 254 ng_hci_disconnect(hook_p hook)
 255 {
 256         ng_hci_unit_p    unit = (ng_hci_unit_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 257
 258         if (hook == unit->acl)
 259                 unit->acl = NULL;
 260         else if (hook == unit->sco)
 261                 unit->sco = NULL;
 262         else if (hook == unit->raw)
 263                 unit->raw = NULL;
 264         else if (hook == unit->drv) {
 265                 unit->drv = NULL;
 266
 267                 /* Connection terminated by local host */
 268                 ng_hci_unit_clean(unit, 0x16);
 269                 unit->state &= ~(NG_HCI_UNIT_CONNECTED|NG_HCI_UNIT_INITED);
 270         } else
 271                 return (EINVAL);
 272
 273         /* Shutdown when all hooks are disconnected */
 274         if ((NG_NODE_NUMHOOKS(NG_HOOK_NODE(hook)) == 0) &&
 275             (NG_NODE_IS_VALID(NG_HOOK_NODE(hook))))
 276                 ng_rmnode_self(NG_HOOK_NODE(hook));
 277
 278         return (0);
 279 } /* ng_hci_disconnect */
 280
 281 /*
 282  * Default control message processing routine. Control message could be:
 283  *
 284  * 1) GENERIC Netgraph messages
 285  *
 286  * 2) Control message directed to the node itself.
 287  */
 288
 289 static int
 290 ng_hci_default_rcvmsg(node_p node, item_p item, hook_p lasthook)
 291 {
 292         ng_hci_unit_p    unit = (ng_hci_unit_p) NG_NODE_PRIVATE(node);
 293         struct ng_mesg  *msg = NULL, *rsp = NULL;
 294         int              error = 0;
 295
 296         NGI_GET_MSG(item, msg);
 297
 298         switch (msg->header.typecookie) {
 299         case NGM_GENERIC_COOKIE:
 300                 switch (msg->header.cmd) {
 301                 case NGM_TEXT_STATUS: {
 302                         int     cmd_avail,
 303                                 acl_total, acl_avail, acl_size,
 304                                 sco_total, sco_avail, sco_size;
 305
 306                         NG_MKRESPONSE(rsp, msg, NG_TEXTRESPONSE, M_NOWAIT);
 307                         if (rsp == NULL) {
 308                                 error = ENOMEM;
 309                                 break;
 310                         }
 311
 312                         NG_HCI_BUFF_CMD_GET(unit->buffer, cmd_avail);
 313
 314                         NG_HCI_BUFF_ACL_AVAIL(unit->buffer, acl_avail);
 315                         NG_HCI_BUFF_ACL_TOTAL(unit->buffer, acl_total);
 316                         NG_HCI_BUFF_ACL_SIZE(unit->buffer, acl_size);
 317
 318                         NG_HCI_BUFF_SCO_AVAIL(unit->buffer, sco_avail);
 319                         NG_HCI_BUFF_SCO_TOTAL(unit->buffer, sco_total);
 320                         NG_HCI_BUFF_SCO_SIZE(unit->buffer, sco_size);
 321
 322                         snprintf(rsp->data, NG_TEXTRESPONSE,
 323                                 "bdaddr %x:%x:%x:%x:%x:%x\n" \
 324                                 "Hooks  %s %s %s %s\n" \
 325                                 "State  %#x\n" \
 326                                 "Queue  cmd:%d\n" \
 327                                 "Buffer cmd:%d,acl:%d,%d,%d,sco:%d,%d,%d",
 328                                 unit->bdaddr.b[5], unit->bdaddr.b[4],
 329                                 unit->bdaddr.b[3], unit->bdaddr.b[2],
 330                                 unit->bdaddr.b[1], unit->bdaddr.b[0],
 331                                 (unit->drv != NULL)? NG_HCI_HOOK_DRV : "",
 332                                 (unit->acl != NULL)? NG_HCI_HOOK_ACL : "",
 333                                 (unit->sco != NULL)? NG_HCI_HOOK_SCO : "",
 334                                 (unit->raw != NULL)? NG_HCI_HOOK_RAW : "",
 335                                 unit->state,
 336                                 NG_BT_MBUFQ_LEN(&unit->cmdq),
 337                                 cmd_avail,
 338                                 acl_avail, acl_total, acl_size,
 339                                 sco_avail, sco_total, sco_size);
 340                         } break;
 341
 342                 default:
 343                         error = EINVAL;
 344                         break;
 345                 }
 346                 break;
 347
 348         case NGM_HCI_COOKIE:
 349                 switch (msg->header.cmd) {
 350                 /* Get current node state */
 351                 case NGM_HCI_NODE_GET_STATE:
 352                         NG_MKRESPONSE(rsp, msg, sizeof(unit->state), M_NOWAIT);
 353                         if (rsp == NULL) {
 354                                 error = ENOMEM;
 355                                 break;
 356                         }
 357
 358                         *((ng_hci_node_state_ep *)(rsp->data)) = unit->state;
 359                         break;
 360
 361                 /* Turn INITED bit - node initialized */
 362                 case NGM_HCI_NODE_INIT:
 363                         if (bcmp(&unit->bdaddr, NG_HCI_BDADDR_ANY,
 364                                         sizeof(bdaddr_t)) == 0) {
 365                                 error = ENXIO;
 366                                 break;
 367                         }
 368
 369                         unit->state |= NG_HCI_UNIT_INITED;
 370
 371                         ng_hci_node_is_up(unit->node, unit->acl, NULL, 0);
 372                         ng_hci_node_is_up(unit->node, unit->sco, NULL, 0);
 373                         break;
 374
 375                 /* Get node debug level */
 376                 case NGM_HCI_NODE_GET_DEBUG:
 377                         NG_MKRESPONSE(rsp, msg, sizeof(unit->debug), M_NOWAIT);
 378                         if (rsp == NULL) {
 379                                 error = ENOMEM;
 380                                 break;
 381                         }
 382
 383                         *((ng_hci_node_debug_ep *)(rsp->data)) = unit->debug;
 384                         break;
 385
 386                 /* Set node debug level */
 387                 case NGM_HCI_NODE_SET_DEBUG:
 388                         if (msg->header.arglen != sizeof(ng_hci_node_debug_ep)){
 389                                 error = EMSGSIZE;
 390                                 break;
 391                         }
 392
 393                         unit->debug = *((ng_hci_node_debug_ep *)(msg->data));
 394                         break;
 395
 396                 /* Get buffer info */
 397                 case NGM_HCI_NODE_GET_BUFFER: {
 398                         ng_hci_node_buffer_ep   *ep = NULL;
 399
 400                         NG_MKRESPONSE(rsp, msg, sizeof(ng_hci_node_buffer_ep),
 401                                 M_NOWAIT);
 402                         if (rsp == NULL) {
 403                                 error = ENOMEM;
 404                                 break;
 405                         }
 406
 407                         ep = (ng_hci_node_buffer_ep *)(rsp->data);
 408
 409                         NG_HCI_BUFF_CMD_GET(unit->buffer, ep->cmd_free);
 410                         NG_HCI_BUFF_ACL_AVAIL(unit->buffer, ep->acl_free);
 411                         NG_HCI_BUFF_ACL_TOTAL(unit->buffer, ep->acl_pkts);
 412                         NG_HCI_BUFF_ACL_SIZE(unit->buffer, ep->acl_size);
 413                         NG_HCI_BUFF_SCO_AVAIL(unit->buffer, ep->sco_free);
 414                         NG_HCI_BUFF_SCO_TOTAL(unit->buffer, ep->sco_pkts);
 415                         NG_HCI_BUFF_SCO_SIZE(unit->buffer, ep->sco_size);
 416                         } break;
 417
 418                 /* Get BDADDR */
 419                 case NGM_HCI_NODE_GET_BDADDR:
 420                         NG_MKRESPONSE(rsp, msg, sizeof(bdaddr_t), M_NOWAIT);
 421                         if (rsp == NULL) {
 422                                 error = ENOMEM;
 423                                 break;
 424                         }
 425
 426                         bcopy(&unit->bdaddr, rsp->data, sizeof(bdaddr_t));
 427                         break;
 428
 429                 /* Get features */
 430                 case NGM_HCI_NODE_GET_FEATURES:
 431                         NG_MKRESPONSE(rsp,msg,sizeof(unit->features),M_NOWAIT);
 432                         if (rsp == NULL) {
 433                                 error = ENOMEM;
 434                                 break;
 435                         }
 436
 437                         bcopy(&unit->features,rsp->data,sizeof(unit->features));
 438                         break;
 439
 440                 /* Get stat */
 441                 case NGM_HCI_NODE_GET_STAT:
 442                         NG_MKRESPONSE(rsp, msg, sizeof(unit->stat), M_NOWAIT);
 443                         if (rsp == NULL) {
 444                                 error = ENOMEM;
 445                                 break;
 446                         }
 447
 448                         bcopy(&unit->stat, rsp->data, sizeof(unit->stat));
 449                         break;
 450
 451                 /* Reset stat */
 452                 case NGM_HCI_NODE_RESET_STAT:
 453                         NG_HCI_STAT_RESET(unit->stat);
 454                         break;
 455
 456                 /* Clean up neighbors list */
 457                 case NGM_HCI_NODE_FLUSH_NEIGHBOR_CACHE:
 458                         ng_hci_flush_neighbor_cache(unit);
 459                         break;
 460
 461                 /* Get neighbor cache entries */
 462                 case NGM_HCI_NODE_GET_NEIGHBOR_CACHE: {
 463                         ng_hci_neighbor_p                        n = NULL;
 464                         ng_hci_node_get_neighbor_cache_ep       *e1 = NULL;
 465                         ng_hci_node_neighbor_cache_entry_ep     *e2 = NULL;
 466                         int                                      s = 0;
 467
 468                         /* Look for the fresh entries in the cache */
 469                         for (n = LIST_FIRST(&unit->neighbors); n != NULL; ) {
 470                                 ng_hci_neighbor_p       nn = LIST_NEXT(n, next);
 471
 472                                 if (ng_hci_neighbor_stale(n))
 473                                         ng_hci_free_neighbor(n);
 474                                 else
 475                                         s ++;
 476
 477                                 n = nn;
 478                         }
 479                         if (s > NG_HCI_MAX_NEIGHBOR_NUM)
 480                                 s = NG_HCI_MAX_NEIGHBOR_NUM;
 481
 482                         /* Prepare response */
 483                         NG_MKRESPONSE(rsp, msg, sizeof(*e1) + s * sizeof(*e2),
 484                                 M_NOWAIT);
 485                         if (rsp == NULL) {
 486                                 error = ENOMEM;
 487                                 break;
 488                         }
 489
 490                         e1 = (ng_hci_node_get_neighbor_cache_ep *)(rsp->data);
 491                         e2 = (ng_hci_node_neighbor_cache_entry_ep *)(e1 + 1);
 492
 493                         e1->num_entries = s;
 494
 495                         LIST_FOREACH(n, &unit->neighbors, next) {
 496                                 e2->page_scan_rep_mode = n->page_scan_rep_mode;
 497                                 e2->page_scan_mode = n->page_scan_mode;
 498                                 e2->clock_offset = n->clock_offset;
 499                                 e2->addrtype =
 500                                         ng_hci_linktype_to_addrtype(n->addrtype);
 501                                 e2->extinq_size = n->extinq_size;
 502                                 bcopy(&n->bdaddr, &e2->bdaddr,
 503                                         sizeof(e2->bdaddr));
 504                                 bcopy(&n->features, &e2->features,
 505                                         sizeof(e2->features));
 506                                 bcopy(&n->extinq_data, &e2->extinq_data,
 507                                       n->extinq_size);
 508                                 e2 ++;
 509                                 if (--s <= 0)
 510                                         break;
 511                         }
 512                         } break;
 513
 514                 /* Get connection list */
 515                 case NGM_HCI_NODE_GET_CON_LIST: {
 516                         ng_hci_unit_con_p        c = NULL;
 517                         ng_hci_node_con_list_ep *e1 = NULL;
 518                         ng_hci_node_con_ep      *e2 = NULL;
 519                         int                      s = 0;
 520
 521                         /* Count number of connections in the list */
 522                         LIST_FOREACH(c, &unit->con_list, next)
 523                                 s ++;
 524                         if (s > NG_HCI_MAX_CON_NUM)
 525                                 s = NG_HCI_MAX_CON_NUM;
 526
 527                         /* Prepare response */
 528                         NG_MKRESPONSE(rsp, msg, sizeof(*e1) + s * sizeof(*e2),
 529                                 M_NOWAIT);
 530                         if (rsp == NULL) {
 531                                 error = ENOMEM;
 532                                 break;
 533                         }
 534
 535                         e1 = (ng_hci_node_con_list_ep *)(rsp->data);
 536                         e2 = (ng_hci_node_con_ep *)(e1 + 1);
 537
 538                         e1->num_connections = s;
 539
 540                         LIST_FOREACH(c, &unit->con_list, next) {
 541                                 e2->link_type = c->link_type;
 542                                 e2->encryption_mode= c->encryption_mode;
 543                                 e2->mode = c->mode;
 544                                 e2->role = c->role;
 545
 546                                 e2->state = c->state;
 547
 548                                 e2->pending = c->pending;
 549                                 e2->queue_len = NG_BT_ITEMQ_LEN(&c->conq);
 550
 551                                 e2->con_handle = c->con_handle;
 552                                 bcopy(&c->bdaddr, &e2->bdaddr,
 553                                         sizeof(e2->bdaddr));
 554
 555                                 e2 ++;
 556                                 if (--s <= 0)
 557                                         break;
 558                         }
 559                         } break;
 560
 561                 /* Get link policy settings mask */
 562                 case NGM_HCI_NODE_GET_LINK_POLICY_SETTINGS_MASK:
 563                         NG_MKRESPONSE(rsp, msg, sizeof(unit->link_policy_mask),
 564                                 M_NOWAIT);
 565                         if (rsp == NULL) {
 566                                 error = ENOMEM;
 567                                 break;
 568                         }
 569
 570                         *((ng_hci_node_link_policy_mask_ep *)(rsp->data)) =
 571                                 unit->link_policy_mask;
 572                         break;
 573
 574                 /* Set link policy settings mask */
 575                 case NGM_HCI_NODE_SET_LINK_POLICY_SETTINGS_MASK:
 576                         if (msg->header.arglen !=
 577                                 sizeof(ng_hci_node_link_policy_mask_ep)) {
 578                                 error = EMSGSIZE;
 579                                 break;
 580                         }
 581
 582                         unit->link_policy_mask =
 583                                 *((ng_hci_node_link_policy_mask_ep *)
 584                                         (msg->data));
 585                         break;
 586
 587                 /* Get packet mask */
 588                 case NGM_HCI_NODE_GET_PACKET_MASK:
 589                         NG_MKRESPONSE(rsp, msg, sizeof(unit->packet_mask),
 590                                 M_NOWAIT);
 591                         if (rsp == NULL) {
 592                                 error = ENOMEM;
 593                                 break;
 594                         }
 595
 596                         *((ng_hci_node_packet_mask_ep *)(rsp->data)) =
 597                                 unit->packet_mask;
 598                         break;
 599
 600                 /* Set packet mask */
 601                 case NGM_HCI_NODE_SET_PACKET_MASK:
 602                         if (msg->header.arglen !=
 603                                         sizeof(ng_hci_node_packet_mask_ep)) {
 604                                 error = EMSGSIZE;
 605                                 break;
 606                         }
 607
 608                         unit->packet_mask =
 609                                 *((ng_hci_node_packet_mask_ep *)(msg->data));
 610                         break;
 611
 612                 /* Get role switch */
 613                 case NGM_HCI_NODE_GET_ROLE_SWITCH:
 614                         NG_MKRESPONSE(rsp, msg, sizeof(unit->role_switch),
 615                                 M_NOWAIT);
 616                         if (rsp == NULL) {
 617                                 error = ENOMEM;
 618                                 break;
 619                         }
 620
 621                         *((ng_hci_node_role_switch_ep *)(rsp->data)) =
 622                                 unit->role_switch;
 623                         break;
 624
 625                 /* Set role switch */
 626                 case NGM_HCI_NODE_SET_ROLE_SWITCH:
 627                         if (msg->header.arglen !=
 628                                         sizeof(ng_hci_node_role_switch_ep)) {
 629                                 error = EMSGSIZE;
 630                                 break;
 631                         }
 632
 633                         unit->role_switch =
 634                                 *((ng_hci_node_role_switch_ep *)(msg->data));
 635                         break;
 636
 637                 default:
 638                         error = EINVAL;
 639                         break;
 640                 }
 641                 break;
 642
 643         default:
 644                 error = EINVAL;
 645                 break;
 646         }
 647
 648         /* NG_RESPOND_MSG should take care of "item" and "rsp" */
 649         NG_RESPOND_MSG(error, node, item, rsp);
 650         NG_FREE_MSG(msg);
 651
 652         return (error);
 653 } /* ng_hci_default_rcvmsg */
 654
 655 /*
 656  * Process control message from upstream hooks (ACL and SCO).
 657  * Handle LP_xxx messages here, give everything else to default routine.
 658  */
 659
 660 static int
 661 ng_hci_upper_rcvmsg(node_p node, item_p item, hook_p lasthook)
 662 {
 663         ng_hci_unit_p   unit = (ng_hci_unit_p) NG_NODE_PRIVATE(node);
 664         int             error = 0;
 665
 666         switch (NGI_MSG(item)->header.typecookie) {
 667         case NGM_HCI_COOKIE:
 668                 switch (NGI_MSG(item)->header.cmd) {
 669                 case NGM_HCI_LP_CON_REQ:
 670                         error = ng_hci_lp_con_req(unit, item, lasthook);
 671                         break;
 672
 673                 case NGM_HCI_LP_DISCON_REQ: /* XXX not defined by specs */
 674                         error = ng_hci_lp_discon_req(unit, item, lasthook);
 675                         break;
 676
 677                 case NGM_HCI_LP_CON_RSP:
 678                         error = ng_hci_lp_con_rsp(unit, item, lasthook);
 679                         break;
 680
 681                 case NGM_HCI_LP_QOS_REQ:
 682                         error = ng_hci_lp_qos_req(unit, item, lasthook);
 683                         break;
 684
 685                 default:
 686                         error = ng_hci_default_rcvmsg(node, item, lasthook);
 687                         break;
 688                 }
 689                 break;
 690
 691         default:
 692                 error = ng_hci_default_rcvmsg(node, item, lasthook);
 693                 break;
 694         }
 695
 696         return (error);
 697 } /* ng_hci_upper_rcvmsg */
 698
 699 /*
 700  * Process data packet from the driver hook.
 701  * We expect HCI events, ACL or SCO data packets.
 702  */
 703
 704 static int
 705 ng_hci_drv_rcvdata(hook_p hook, item_p item)
 706 {
 707         ng_hci_unit_p    unit = (ng_hci_unit_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 708         struct mbuf     *m = NULL;
 709         int              error = 0;
 710
 711         /* Process packet */
 712         m = NGI_M(item); /* item still has mbuf, just peeking */
 713         m->m_flags |= M_PROTO1; /* mark as incoming packet */
 714
 715         NG_HCI_STAT_BYTES_RECV(unit->stat, m->m_pkthdr.len);
 716
 717         /* Give copy packet to RAW hook */
 718         ng_hci_mtap(unit, m);
 719
 720         /*
 721          * XXX XXX XXX
 722          * Lower layer drivers MUST NOT send mbuf chain with empty mbuf at
 723          * the beginning of the chain. HCI layer WILL NOT call m_pullup() here.
 724          */
 725
 726         switch (*mtod(m, u_int8_t *)) {
 727         case NG_HCI_ACL_DATA_PKT:
 728                 NG_HCI_STAT_ACL_RECV(unit->stat);
 729
 730                 if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY ||
 731                     unit->acl == NULL || NG_HOOK_NOT_VALID(unit->acl)) {
 732                         NG_HCI_WARN(
 733 "%s: %s - could not forward HCI ACL data packet, state=%#x, hook=%p\n",
 734                                 __func__, NG_NODE_NAME(unit->node),
 735                                 unit->state, unit->acl);
 736
 737                         NG_FREE_ITEM(item);
 738                 } else
 739                         NG_FWD_ITEM_HOOK(error, item, unit->acl);
 740                 break;
 741
 742         case NG_HCI_SCO_DATA_PKT:
 743                 NG_HCI_STAT_SCO_RECV(unit->stat);
 744
 745                 if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY ||
 746                     unit->sco == NULL || NG_HOOK_NOT_VALID(unit->sco)) {
 747                         NG_HCI_INFO(
 748 "%s: %s - could not forward HCI SCO data packet, state=%#x, hook=%p\n",
 749                                 __func__, NG_NODE_NAME(unit->node),
 750                                 unit->state, unit->sco);
 751
 752                         NG_FREE_ITEM(item);
 753                 } else
 754                         NG_FWD_ITEM_HOOK(error, item, unit->sco);
 755                 break;
 756
 757         case NG_HCI_EVENT_PKT:
 758                 NG_HCI_STAT_EVNT_RECV(unit->stat);
 759
 760                 /* Detach mbuf, discard item and process event */
 761                 NGI_GET_M(item, m);
 762                 NG_FREE_ITEM(item);
 763
 764                 error = ng_hci_process_event(unit, m);
 765                 break;
 766
 767         default:
 768                 NG_HCI_ALERT(
 769 "%s: %s - got unknown HCI packet type=%#x\n",
 770                         __func__, NG_NODE_NAME(unit->node),
 771                         *mtod(m, u_int8_t *));
 772
 773                 NG_FREE_ITEM(item);
 774
 775                 error = EINVAL;
 776                 break;
 777         }
 778
 779         return (error);
 780 } /* ng_hci_drv_rcvdata */
 781
 782 /*
 783  * Process data packet from ACL upstream hook.
 784  * We expect valid HCI ACL data packets.
 785  */
 786
 787 static int
 788 ng_hci_acl_rcvdata(hook_p hook, item_p item)
 789 {
 790         ng_hci_unit_p            unit = (ng_hci_unit_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 791         struct mbuf             *m = NULL;
 792         ng_hci_unit_con_p        con = NULL;
 793         u_int16_t                con_handle;
 794         int                      size, error = 0;
 795
 796         NG_HCI_BUFF_ACL_SIZE(unit->buffer, size);
 797         /* Check packet */
 798         NGI_GET_M(item, m);
 799
 800         if (*mtod(m, u_int8_t *) != NG_HCI_ACL_DATA_PKT) {
 801                 NG_HCI_ALERT(
 802 "%s: %s - invalid HCI data packet type=%#x\n",
 803                         __func__, NG_NODE_NAME(unit->node),
 804                         *mtod(m, u_int8_t *));
 805
 806                 error = EINVAL;
 807                 goto drop;
 808         }
 809         if (m->m_pkthdr.len < sizeof(ng_hci_acldata_pkt_t) ||
 810             m->m_pkthdr.len > sizeof(ng_hci_acldata_pkt_t) + size) {
 811                 NG_HCI_ALERT(
 812 "%s: %s - invalid HCI ACL data packet, len=%d, mtu=%d\n",
 813                         __func__, NG_NODE_NAME(unit->node),
 814                         m->m_pkthdr.len, size);
 815
 816                 error = EMSGSIZE;
 817                 goto drop;
 818         }
 819
 820         NG_HCI_M_PULLUP(m, sizeof(ng_hci_acldata_pkt_t));
 821         if (m == NULL) {
 822                 error = ENOBUFS;
 823                 goto drop;
 824         }
 825
 826         con_handle = NG_HCI_CON_HANDLE(le16toh(
 827                         mtod(m, ng_hci_acldata_pkt_t *)->con_handle));
 828         size = le16toh(mtod(m, ng_hci_acldata_pkt_t *)->length);
 829
 830         if (m->m_pkthdr.len != sizeof(ng_hci_acldata_pkt_t) + size) {
 831                 NG_HCI_ALERT(
 832 "%s: %s - invalid HCI ACL data packet size, len=%d, length=%d\n",
 833                         __func__, NG_NODE_NAME(unit->node),
 834                         m->m_pkthdr.len, size);
 835
 836                 error = EMSGSIZE;
 837                 goto drop;
 838         }
 839
 840         /* Queue packet */
 841         con = ng_hci_con_by_handle(unit, con_handle);
 842         if (con == NULL) {
 843                 NG_HCI_ERR(
 844 "%s: %s - unexpected HCI ACL data packet. Connection does not exists, " \
 845 "con_handle=%d\n",      __func__, NG_NODE_NAME(unit->node), con_handle);
 846
 847                 error = ENOENT;
 848                 goto drop;
 849         }
 850
 851         if (con->link_type == NG_HCI_LINK_SCO) {
 852                 NG_HCI_ERR(
 853 "%s: %s - unexpected HCI ACL data packet. Not ACL link, con_handle=%d, " \
 854 "link_type=%d\n",       __func__, NG_NODE_NAME(unit->node),
 855                         con_handle, con->link_type);
 856
 857                 error = EINVAL;
 858                 goto drop;
 859         }
 860
 861         if (con->state != NG_HCI_CON_OPEN) {
 862                 NG_HCI_ERR(
 863 "%s: %s - unexpected HCI ACL data packet. Invalid connection state=%d, " \
 864 "con_handle=%d\n",       __func__, NG_NODE_NAME(unit->node),
 865                         con->state, con_handle);
 866
 867                 error = EHOSTDOWN;
 868                 goto drop;
 869         }
 870
 871         if (NG_BT_ITEMQ_FULL(&con->conq)) {
 872                 NG_HCI_ALERT(
 873 "%s: %s - dropping HCI ACL data packet, con_handle=%d, len=%d, queue_len=%d\n",
 874                          __func__, NG_NODE_NAME(unit->node), con_handle,
 875                         m->m_pkthdr.len, NG_BT_ITEMQ_LEN(&con->conq));
 876
 877                 NG_BT_ITEMQ_DROP(&con->conq);
 878
 879                 error = ENOBUFS;
 880                 goto drop;
 881         }
 882
 883         /* Queue item and schedule data transfer */
 884         NGI_M(item) = m;
 885         NG_BT_ITEMQ_ENQUEUE(&con->conq, item);
 886         item = NULL;
 887         m = NULL;
 888
 889         ng_hci_send_data(unit);
 890 drop:
 891         if (item != NULL)
 892                 NG_FREE_ITEM(item);
 893
 894         NG_FREE_M(m); /* NG_FREE_M() checks for m != NULL */
 895
 896         return (error);
 897 } /* ng_hci_acl_rcvdata */
 898
 899 /*
 900  * Process data packet from SCO upstream hook.
 901  * We expect valid HCI SCO data packets
 902  */
 903
 904 static int
 905 ng_hci_sco_rcvdata(hook_p hook, item_p item)
 906 {
 907         ng_hci_unit_p            unit = (ng_hci_unit_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
 908         struct mbuf             *m = NULL;
 909         ng_hci_unit_con_p        con = NULL;
 910         u_int16_t                con_handle;
 911         int                      size, error = 0;
 912
 913         NG_HCI_BUFF_SCO_SIZE(unit->buffer, size);
 914
 915         /* Check packet */
 916         NGI_GET_M(item, m);
 917
 918         if (*mtod(m, u_int8_t *) != NG_HCI_SCO_DATA_PKT) {
 919                 NG_HCI_ALERT(
 920 "%s: %s - invalid HCI data packet type=%#x\n",
 921                         __func__, NG_NODE_NAME(unit->node),
 922                         *mtod(m, u_int8_t *));
 923
 924                 error = EINVAL;
 925                 goto drop;
 926         }
 927
 928         if (m->m_pkthdr.len < sizeof(ng_hci_scodata_pkt_t) ||
 929             m->m_pkthdr.len > sizeof(ng_hci_scodata_pkt_t) + size) {
 930                 NG_HCI_ALERT(
 931 "%s: %s - invalid HCI SCO data packet, len=%d, mtu=%d\n",
 932                         __func__, NG_NODE_NAME(unit->node),
 933                         m->m_pkthdr.len, size);
 934
 935                 error = EMSGSIZE;
 936                 goto drop;
 937         }
 938
 939         NG_HCI_M_PULLUP(m, sizeof(ng_hci_scodata_pkt_t));
 940         if (m == NULL) {
 941                 error = ENOBUFS;
 942                 goto drop;
 943         }
 944
 945         con_handle = NG_HCI_CON_HANDLE(le16toh(
 946                         mtod(m, ng_hci_scodata_pkt_t *)->con_handle));
 947         size = mtod(m, ng_hci_scodata_pkt_t *)->length;
 948
 949         if (m->m_pkthdr.len != sizeof(ng_hci_scodata_pkt_t) + size) {
 950                 NG_HCI_ALERT(
 951 "%s: %s - invalid HCI SCO data packet size, len=%d, length=%d\n",
 952                         __func__, NG_NODE_NAME(unit->node),
 953                         m->m_pkthdr.len, size);
 954
 955                 error = EMSGSIZE;
 956                 goto drop;
 957         }
 958
 959         /* Queue packet */
 960         con = ng_hci_con_by_handle(unit, con_handle);
 961         if (con == NULL) {
 962                 NG_HCI_ERR(
 963 "%s: %s - unexpected HCI SCO data packet. Connection does not exists, " \
 964 "con_handle=%d\n",      __func__, NG_NODE_NAME(unit->node), con_handle);
 965
 966                 error = ENOENT;
 967                 goto drop;
 968         }
 969
 970         if (con->link_type != NG_HCI_LINK_SCO) {
 971                 NG_HCI_ERR(
 972 "%s: %s - unexpected HCI SCO data packet. Not SCO link, con_handle=%d, " \
 973 "link_type=%d\n",       __func__, NG_NODE_NAME(unit->node),
 974                         con_handle, con->link_type);
 975
 976                 error = EINVAL;
 977                 goto drop;
 978         }
 979
 980         if (con->state != NG_HCI_CON_OPEN) {
 981                 NG_HCI_ERR(
 982 "%s: %s - unexpected HCI SCO data packet. Invalid connection state=%d, " \
 983 "con_handle=%d\n",      __func__, NG_NODE_NAME(unit->node),
 984                         con->state, con_handle);
 985
 986                 error = EHOSTDOWN;
 987                 goto drop;
 988         }
 989
 990         if (NG_BT_ITEMQ_FULL(&con->conq)) {
 991                 NG_HCI_ALERT(
 992 "%s: %s - dropping HCI SCO data packet, con_handle=%d, len=%d, queue_len=%d\n",
 993                         __func__, NG_NODE_NAME(unit->node), con_handle,
 994                         m->m_pkthdr.len, NG_BT_ITEMQ_LEN(&con->conq));
 995
 996                 NG_BT_ITEMQ_DROP(&con->conq);
 997
 998                 error = ENOBUFS;
 999                 goto drop;
1000         }
1001
1002         /* Queue item and schedule data transfer */
1003         NGI_M(item) = m;
1004         NG_BT_ITEMQ_ENQUEUE(&con->conq, item);
1005         item = NULL;
1006         m = NULL;
1007
1008         ng_hci_send_data(unit);
1009 drop:
1010         if (item != NULL)
1011                 NG_FREE_ITEM(item);
1012
1013         NG_FREE_M(m); /* NG_FREE_M() checks for m != NULL */
1014
1015         return (error);
1016 } /* ng_hci_sco_rcvdata */
1017
1018 /*
1019  * Process data packet from uptream RAW hook.
1020  * We expect valid HCI command packets.
1021  */
1022
1023 static int
1024 ng_hci_raw_rcvdata(hook_p hook, item_p item)
1025 {
1026         ng_hci_unit_p    unit = (ng_hci_unit_p) NG_NODE_PRIVATE(NG_HOOK_NODE(hook));
1027         struct mbuf     *m = NULL;
1028         int              error = 0;
1029
1030         NGI_GET_M(item, m);
1031         NG_FREE_ITEM(item);
1032
1033         /* Check packet */
1034         if (*mtod(m, u_int8_t *) != NG_HCI_CMD_PKT) {
1035                 NG_HCI_ALERT(
1036 "%s: %s - invalid HCI command packet type=%#x\n",
1037                         __func__, NG_NODE_NAME(unit->node),
1038                         *mtod(m, u_int8_t *));
1039
1040                 error = EINVAL;
1041                 goto drop;
1042         }
1043
1044         if (m->m_pkthdr.len < sizeof(ng_hci_cmd_pkt_t)) {
1045                 NG_HCI_ALERT(
1046 "%s: %s - invalid HCI command packet len=%d\n",
1047                         __func__, NG_NODE_NAME(unit->node), m->m_pkthdr.len);
1048
1049                 error = EMSGSIZE;
1050                 goto drop;
1051         }
1052
1053         NG_HCI_M_PULLUP(m, sizeof(ng_hci_cmd_pkt_t));
1054         if (m == NULL) {
1055                 error = ENOBUFS;
1056                 goto drop;
1057         }
1058
1059         if (m->m_pkthdr.len !=
1060             mtod(m, ng_hci_cmd_pkt_t *)->length + sizeof(ng_hci_cmd_pkt_t)) {
1061                 NG_HCI_ALERT(
1062 "%s: %s - invalid HCI command packet size, len=%d, length=%d\n",
1063                         __func__, NG_NODE_NAME(unit->node), m->m_pkthdr.len,
1064                         mtod(m, ng_hci_cmd_pkt_t *)->length);
1065
1066                 error = EMSGSIZE;
1067                 goto drop;
1068         }
1069
1070         if (mtod(m, ng_hci_cmd_pkt_t *)->opcode == 0) {
1071                 NG_HCI_ALERT(
1072 "%s: %s - invalid HCI command opcode\n",
1073                         __func__, NG_NODE_NAME(unit->node));
1074
1075                 error = EINVAL;
1076                 goto drop;
1077         }
1078
1079         if (NG_BT_MBUFQ_FULL(&unit->cmdq)) {
1080                 NG_HCI_ALERT(
1081 "%s: %s - dropping HCI command packet, len=%d, queue_len=%d\n",
1082                         __func__, NG_NODE_NAME(unit->node), m->m_pkthdr.len,
1083                         NG_BT_MBUFQ_LEN(&unit->cmdq));
1084
1085                 NG_BT_MBUFQ_DROP(&unit->cmdq);
1086
1087                 error = ENOBUFS;
1088                 goto drop;
1089         }
1090
1091         /* Queue and send command */
1092         NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
1093         m = NULL;
1094
1095         if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
1096                 error = ng_hci_send_command(unit);
1097 drop:
1098         NG_FREE_M(m); /* NG_FREE_M() checks for m != NULL */
1099
1100         return (error);
1101 } /* ng_hci_raw_rcvdata */
1102