sys/netgraph/bluetooth/hci/ng_hci_main.c

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