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