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