sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c

   1 /*
   2  * ng_l2cap_llpi.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_l2cap_llpi.c,v 1.5 2003/09/08 19:11:45 max Exp $
  33  * $FreeBSD$
  34  */
  35
  36 #include <sys/param.h>
  37 #include <sys/systm.h>
  38 #include <sys/kernel.h>
  39 #include <sys/endian.h>
  40 #include <sys/malloc.h>
  41 #include <sys/mbuf.h>
  42 #include <sys/queue.h>
  43 #include <netgraph/ng_message.h>
  44 #include <netgraph/netgraph.h>
  45 #include <netgraph/bluetooth/include/ng_bluetooth.h>
  46 #include <netgraph/bluetooth/include/ng_hci.h>
  47 #include <netgraph/bluetooth/include/ng_l2cap.h>
  48 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h>
  49 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h>
  50 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h>
  51 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h>
  52 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h>
  53 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h>
  54
  55 /******************************************************************************
  56  ******************************************************************************
  57  **                 Lower Layer Protocol (HCI) Interface module
  58  ******************************************************************************
  59  ******************************************************************************/
  60
  61 /*
  62  * Send LP_ConnectReq event to the lower layer protocol. Create new connection
  63  * descriptor and initialize it. Create LP_ConnectReq event and send it to the
  64  * lower layer, then adjust connection state and start timer. The function WILL
  65  * FAIL if connection to the remote unit already exists.
  66  */
  67
  68 int
  69 ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr, int type)
  70 {
  71         struct ng_mesg          *msg = NULL;
  72         ng_hci_lp_con_req_ep    *ep = NULL;
  73         ng_l2cap_con_p           con = NULL;
  74         int                      error = 0;
  75
  76         /* Verify that we DO NOT have connection to the remote unit */
  77         con = ng_l2cap_con_by_addr(l2cap, bdaddr, type);
  78         if (con != NULL) {
  79                 NG_L2CAP_ALERT(
  80 "%s: %s - unexpected LP_ConnectReq event. " \
  81 "Connection already exists, state=%d, con_handle=%d\n",
  82                         __func__, NG_NODE_NAME(l2cap->node), con->state,
  83                         con->con_handle);
  84
  85                 return (EEXIST);
  86         }
  87
  88         /* Check if lower layer protocol is still connected */
  89         if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
  90                 NG_L2CAP_ERR(
  91 "%s: %s - hook \"%s\" is not connected or valid\n",
  92                         __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
  93
  94                 return (ENOTCONN);
  95         }
  96
  97         /* Create and intialize new connection descriptor */
  98         con = ng_l2cap_new_con(l2cap, bdaddr, type);
  99         if (con == NULL)
 100                 return (ENOMEM);
 101
 102         /* Create and send LP_ConnectReq event */
 103         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ,
 104                 sizeof(*ep), M_NOWAIT);
 105         if (msg == NULL) {
 106                 ng_l2cap_free_con(con);
 107
 108                 return (ENOMEM);
 109         }
 110
 111         ep = (ng_hci_lp_con_req_ep *) (msg->data);
 112         bcopy(bdaddr, &ep->bdaddr, sizeof(ep->bdaddr));
 113         ep->link_type = type;
 114
 115         con->flags |= NG_L2CAP_CON_OUTGOING;
 116         con->state = NG_L2CAP_W4_LP_CON_CFM;
 117         ng_l2cap_lp_timeout(con);
 118
 119         NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0);
 120         if (error != 0) {
 121                 if (ng_l2cap_lp_untimeout(con) == 0)
 122                         ng_l2cap_free_con(con);
 123
 124                 /*
 125                  * Do not free connection if ng_l2cap_lp_untimeout() failed
 126                  * let timeout handler deal with it. Always return error to
 127                  * the caller.
 128                  */
 129         }
 130
 131         return (error);
 132 } /* ng_l2cap_lp_con_req */
 133
 134 /*
 135  * Process LP_ConnectCfm event from the lower layer protocol. It could be
 136  * positive or negative. Verify remote unit address then stop the timer and
 137  * process event.
 138  */
 139
 140 int
 141 ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg)
 142 {
 143         ng_hci_lp_con_cfm_ep    *ep = NULL;
 144         ng_l2cap_con_p           con = NULL;
 145         int                      error = 0;
 146
 147         /* Check message */
 148         if (msg->header.arglen != sizeof(*ep)) {
 149                 NG_L2CAP_ALERT(
 150 "%s: %s - invalid LP_ConnectCfm[Neg] message size\n",
 151                         __func__, NG_NODE_NAME(l2cap->node));
 152                 error = EMSGSIZE;
 153                 goto out;
 154         }
 155
 156         ep = (ng_hci_lp_con_cfm_ep *) (msg->data);
 157         /* Check if we have requested/accepted this connection */
 158         con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr, ep->link_type);
 159         if (con == NULL) {
 160                 NG_L2CAP_ERR(
 161 "%s: %s - unexpected LP_ConnectCfm event. Connection does not exist\n",
 162                         __func__, NG_NODE_NAME(l2cap->node));
 163                 error = ENOENT;
 164                 goto out;
 165         }
 166
 167         /* Check connection state */
 168         if (con->state != NG_L2CAP_W4_LP_CON_CFM) {
 169                 NG_L2CAP_ALERT(
 170 "%s: %s - unexpected LP_ConnectCfm event. " \
 171 "Invalid connection state, state=%d, con_handle=%d\n",
 172                         __func__, NG_NODE_NAME(l2cap->node), con->state,
 173                         con->con_handle);
 174                 error = EINVAL;
 175                 goto out;
 176         }
 177
 178         /*
 179          * Looks like it is our confirmation. It is safe now to cancel
 180          * connection timer and notify upper layer. If timeout already
 181          * happened then ignore connection confirmation and let timeout
 182          * handle that.
 183          */
 184
 185         if ((error = ng_l2cap_lp_untimeout(con)) != 0)
 186                 goto out;
 187
 188         if (ep->status == 0) {
 189                 con->state = NG_L2CAP_CON_OPEN;
 190                 con->con_handle = ep->con_handle;
 191                 ng_l2cap_lp_deliver(con);
 192         } else /* Negative confirmation - remove connection descriptor */
 193                 ng_l2cap_con_fail(con, ep->status);
 194 out:
 195         return (error);
 196 } /* ng_l2cap_lp_con_cfm */
 197
 198 /*
 199  * Process LP_ConnectInd event from the lower layer protocol. This is a good
 200  * place to put some extra check on remote unit address and/or class. We could
 201  * even forward this information to control hook (or check against internal
 202  * black list) and thus implement some kind of firewall. But for now be simple
 203  * and create new connection descriptor, start timer and send LP_ConnectRsp
 204  * event (i.e. accept connection).
 205  */
 206
 207 int
 208 ng_l2cap_lp_con_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
 209 {
 210         ng_hci_lp_con_ind_ep    *ep = NULL;
 211         ng_hci_lp_con_rsp_ep    *rp = NULL;
 212         struct ng_mesg          *rsp = NULL;
 213         ng_l2cap_con_p           con = NULL;
 214         int                      error = 0;
 215
 216         /* Check message */
 217         if (msg->header.arglen != sizeof(*ep)) {
 218                 NG_L2CAP_ALERT(
 219 "%s: %s - invalid LP_ConnectInd message size\n",
 220                         __func__, NG_NODE_NAME(l2cap->node));
 221
 222                 return (EMSGSIZE);
 223         }
 224
 225         ep = (ng_hci_lp_con_ind_ep *) (msg->data);
 226
 227         /* Make sure we have only one connection to the remote unit */
 228         con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr, ep->link_type);
 229         if (con != NULL) {
 230                 NG_L2CAP_ALERT(
 231 "%s: %s - unexpected LP_ConnectInd event. " \
 232 "Connection already exists, state=%d, con_handle=%d\n",
 233                         __func__, NG_NODE_NAME(l2cap->node), con->state,
 234                         con->con_handle);
 235
 236                 return (EEXIST);
 237         }
 238
 239         /* Check if lower layer protocol is still connected */
 240         if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
 241                 NG_L2CAP_ERR(
 242 "%s: %s - hook \"%s\" is not connected or valid",
 243                         __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
 244
 245                 return (ENOTCONN);
 246         }
 247
 248         /* Create and intialize new connection descriptor */
 249         con = ng_l2cap_new_con(l2cap, &ep->bdaddr, ep->link_type);
 250         if (con == NULL)
 251                 return (ENOMEM);
 252
 253         /* Create and send LP_ConnectRsp event */
 254         NG_MKMESSAGE(rsp, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP,
 255                 sizeof(*rp), M_NOWAIT);
 256         if (rsp == NULL) {
 257                 ng_l2cap_free_con(con);
 258
 259                 return (ENOMEM);
 260         }
 261
 262         rp = (ng_hci_lp_con_rsp_ep *)(rsp->data);
 263         rp->status = 0x00; /* accept connection */
 264         rp->link_type = NG_HCI_LINK_ACL;
 265         bcopy(&ep->bdaddr, &rp->bdaddr, sizeof(rp->bdaddr));
 266
 267         con->state = NG_L2CAP_W4_LP_CON_CFM;
 268         ng_l2cap_lp_timeout(con);
 269
 270         NG_SEND_MSG_HOOK(error, l2cap->node, rsp, l2cap->hci, 0);
 271         if (error != 0) {
 272                 if (ng_l2cap_lp_untimeout(con) == 0)
 273                         ng_l2cap_free_con(con);
 274
 275                 /*
 276                  * Do not free connection if ng_l2cap_lp_untimeout() failed
 277                  * let timeout handler deal with it. Always return error to
 278                  * the caller.
 279                  */
 280         }
 281
 282         return (error);
 283 } /* ng_l2cap_lp_con_ind */
 284
 285 /*
 286  * Process LP_DisconnectInd event from the lower layer protocol. We have been
 287  * disconnected from the remote unit. So notify the upper layer protocol.
 288  */
 289
 290 int
 291 ng_l2cap_lp_discon_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
 292 {
 293         ng_hci_lp_discon_ind_ep *ep = NULL;
 294         ng_l2cap_con_p           con = NULL;
 295         int                      error = 0;
 296
 297         /* Check message */
 298         if (msg->header.arglen != sizeof(*ep)) {
 299                 NG_L2CAP_ALERT(
 300 "%s: %s - invalid LP_DisconnectInd message size\n",
 301                         __func__, NG_NODE_NAME(l2cap->node));
 302                 error = EMSGSIZE;
 303                 goto out;
 304         }
 305
 306         ep = (ng_hci_lp_discon_ind_ep *) (msg->data);
 307
 308         /* Check if we have this connection */
 309         con = ng_l2cap_con_by_handle(l2cap, ep->con_handle);
 310         if (con == NULL) {
 311                 NG_L2CAP_ERR(
 312 "%s: %s - unexpected LP_DisconnectInd event. " \
 313 "Connection does not exist, con_handle=%d\n",
 314                         __func__, NG_NODE_NAME(l2cap->node), ep->con_handle);
 315                 error = ENOENT;
 316                 goto out;
 317         }
 318
 319         /* XXX Verify connection state -- do we need to check this? */
 320         if (con->state != NG_L2CAP_CON_OPEN) {
 321                 NG_L2CAP_ERR(
 322 "%s: %s - unexpected LP_DisconnectInd event. " \
 323 "Invalid connection state, state=%d, con_handle=%d\n",
 324                         __func__, NG_NODE_NAME(l2cap->node), con->state,
 325                         con->con_handle);
 326                 error = EINVAL;
 327                 goto out;
 328         }
 329
 330         /*
 331          * Notify upper layer and remove connection
 332          * Note: The connection could have auto disconnect timeout set. Try
 333          * to remove it. If auto disconnect timeout happened then ignore
 334          * disconnect indication and let timeout handle that.
 335          */
 336
 337         if (con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)
 338                 if ((error = ng_l2cap_discon_untimeout(con)) != 0)
 339                         return (error);
 340
 341         ng_l2cap_con_fail(con, ep->reason);
 342 out:
 343         return (error);
 344 } /* ng_l2cap_lp_discon_ind */
 345
 346 /*
 347  * Send LP_QoSSetupReq event to the lower layer protocol
 348  */
 349
 350 int
 351 ng_l2cap_lp_qos_req(ng_l2cap_p l2cap, u_int16_t con_handle,
 352                 ng_l2cap_flow_p flow)
 353 {
 354         struct ng_mesg          *msg = NULL;
 355         ng_hci_lp_qos_req_ep    *ep = NULL;
 356         ng_l2cap_con_p           con = NULL;
 357         int                      error = 0;
 358
 359         /* Verify that we have this connection */
 360         con = ng_l2cap_con_by_handle(l2cap, con_handle);
 361         if (con == NULL) {
 362                 NG_L2CAP_ERR(
 363 "%s: %s - unexpected LP_QoSSetupReq event. " \
 364 "Connection does not exist, con_handle=%d\n",
 365                         __func__, NG_NODE_NAME(l2cap->node), con_handle);
 366
 367                 return (ENOENT);
 368         }
 369
 370         /* Verify connection state */
 371         if (con->state != NG_L2CAP_CON_OPEN) {
 372                 NG_L2CAP_ERR(
 373 "%s: %s - unexpected LP_QoSSetupReq event. " \
 374 "Invalid connection state, state=%d, con_handle=%d\n",
 375                         __func__, NG_NODE_NAME(l2cap->node), con->state,
 376                         con->con_handle);
 377
 378                 return (EINVAL);
 379         }
 380
 381         /* Check if lower layer protocol is still connected */
 382         if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
 383                 NG_L2CAP_ERR(
 384 "%s: %s - hook \"%s\" is not connected or valid",
 385                         __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
 386
 387                 return (ENOTCONN);
 388         }
 389
 390         /* Create and send LP_QoSSetupReq event */
 391         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_QOS_REQ,
 392                 sizeof(*ep), M_NOWAIT);
 393         if (msg == NULL)
 394                 return (ENOMEM);
 395
 396         ep = (ng_hci_lp_qos_req_ep *) (msg->data);
 397         ep->con_handle = con_handle;
 398         ep->flags = flow->flags;
 399         ep->service_type = flow->service_type;
 400         ep->token_rate = flow->token_rate;
 401         ep->peak_bandwidth = flow->peak_bandwidth;
 402         ep->latency = flow->latency;
 403         ep->delay_variation = flow->delay_variation;
 404
 405         NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0);
 406
 407         return (error);
 408 } /* ng_l2cap_lp_con_req */
 409
 410 /*
 411  * Process LP_QoSSetupCfm from the lower layer protocol
 412  */
 413
 414 int
 415 ng_l2cap_lp_qos_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg)
 416 {
 417         ng_hci_lp_qos_cfm_ep    *ep = NULL;
 418         int                      error = 0;
 419
 420         /* Check message */
 421         if (msg->header.arglen != sizeof(*ep)) {
 422                 NG_L2CAP_ALERT(
 423 "%s: %s - invalid LP_QoSSetupCfm[Neg] message size\n",
 424                         __func__, NG_NODE_NAME(l2cap->node));
 425                 error = EMSGSIZE;
 426                 goto out;
 427         }
 428
 429         ep = (ng_hci_lp_qos_cfm_ep *) (msg->data);
 430         /* XXX FIXME do something */
 431 out:
 432         return (error);
 433 } /* ng_l2cap_lp_qos_cfm */
 434
 435 /*
 436  * Process LP_QoSViolationInd event from the lower layer protocol. Lower
 437  * layer protocol has detected QoS Violation, so we MUST notify the
 438  * upper layer.
 439  */
 440
 441 int
 442 ng_l2cap_lp_qos_ind(ng_l2cap_p l2cap, struct ng_mesg *msg)
 443 {
 444         ng_hci_lp_qos_ind_ep    *ep = NULL;
 445         ng_l2cap_con_p           con = NULL;
 446         int                      error = 0;
 447
 448         /* Check message */
 449         if (msg->header.arglen != sizeof(*ep)) {
 450                 NG_L2CAP_ALERT(
 451 "%s: %s - invalid LP_QoSViolation message size\n",
 452                         __func__, NG_NODE_NAME(l2cap->node));
 453                 error = EMSGSIZE;
 454                 goto out;
 455         }
 456
 457         ep = (ng_hci_lp_qos_ind_ep *) (msg->data);
 458
 459         /* Check if we have this connection */
 460         con = ng_l2cap_con_by_handle(l2cap, ep->con_handle);
 461         if (con == NULL) {
 462                 NG_L2CAP_ERR(
 463 "%s: %s - unexpected LP_QoSViolationInd event. " \
 464 "Connection does not exist, con_handle=%d\n",
 465                         __func__, NG_NODE_NAME(l2cap->node), ep->con_handle);
 466                 error = ENOENT;
 467                 goto out;
 468         }
 469
 470         /* Verify connection state */
 471         if (con->state != NG_L2CAP_CON_OPEN) {
 472                 NG_L2CAP_ERR(
 473 "%s: %s - unexpected LP_QoSViolationInd event. " \
 474 "Invalid connection state, state=%d, con_handle=%d\n",
 475                         __func__, NG_NODE_NAME(l2cap->node), con->state,
 476                         con->con_handle);
 477                 error = EINVAL;
 478                 goto out;
 479         }
 480
 481         /* XXX FIXME Notify upper layer and terminate channels if required */
 482 out:
 483         return (error);
 484 } /* ng_l2cap_qos_ind */
 485
 486 int
 487 ng_l2cap_lp_enc_change(ng_l2cap_p l2cap, struct ng_mesg *msg)
 488 {
 489         ng_hci_lp_enc_change_ep *ep = NULL;
 490         ng_l2cap_con_p           con = NULL;
 491         int                      error = 0;
 492         ng_l2cap_chan_p          ch = NULL;
 493         /* Check message */
 494         if (msg->header.arglen != sizeof(*ep)) {
 495                 NG_L2CAP_ALERT(
 496 "%s: %s - invalid LP_ENCChange message size\n",
 497                         __func__, NG_NODE_NAME(l2cap->node));
 498                 error = EMSGSIZE;
 499                 goto out;
 500         }
 501
 502         ep = (ng_hci_lp_enc_change_ep *) (msg->data);
 503
 504         /* Check if we have this connection */
 505         con = ng_l2cap_con_by_handle(l2cap, ep->con_handle);
 506         if (con == NULL) {
 507                 NG_L2CAP_ERR(
 508 "%s: %s - unexpected LP_Enc Change Event. " \
 509 "Connection does not exist, con_handle=%d\n",
 510                         __func__, NG_NODE_NAME(l2cap->node), ep->con_handle);
 511                 error = ENOENT;
 512                 goto out;
 513         }
 514
 515         /* Verify connection state */
 516         if (con->state != NG_L2CAP_CON_OPEN) {
 517                 NG_L2CAP_ERR(
 518 "%s: %s - unexpected ENC_CHANGE event. " \
 519 "Invalid connection state, state=%d, con_handle=%d\n",
 520                         __func__, NG_NODE_NAME(l2cap->node), con->state,
 521                         con->con_handle);
 522                 error = EINVAL;
 523                 goto out;
 524         }
 525
 526         con->encryption = ep->status;
 527
 528         LIST_FOREACH(ch, &l2cap->chan_list, next){
 529                 if((ch->con->con_handle == ep->con_handle) &&
 530                    (ch->con->linktype == ep->link_type))
 531                         ng_l2cap_l2ca_encryption_change(ch, ep->status);
 532         }
 533
 534 out:
 535         return (error);
 536 } /* ng_l2cap_enc_change */
 537
 538 /*
 539  * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then
 540  * segment it according to HCI MTU.
 541  */
 542
 543 int
 544 ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0)
 545 {
 546         ng_l2cap_p               l2cap = con->l2cap;
 547         ng_l2cap_hdr_t          *l2cap_hdr = NULL;
 548         ng_hci_acldata_pkt_t    *acl_hdr = NULL;
 549         struct mbuf             *m_last = NULL, *m = NULL;
 550         int                      len, flag = NG_HCI_PACKET_START;
 551
 552         KASSERT((con->tx_pkt == NULL),
 553 ("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node)));
 554         KASSERT((l2cap->pkt_size > 0),
 555 ("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node)));
 556
 557         /* Prepend mbuf with L2CAP header */
 558         m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr));
 559         if (m0 == NULL) {
 560                 NG_L2CAP_ALERT(
 561 "%s: %s - ng_l2cap_prepend(%zd) failed\n",
 562                         __func__, NG_NODE_NAME(l2cap->node),
 563                         sizeof(*l2cap_hdr));
 564
 565                 goto fail;
 566         }
 567
 568         l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *);
 569         l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr));
 570         l2cap_hdr->dcid = htole16(dcid);
 571
 572         /*
 573          * Segment single L2CAP packet according to the HCI layer MTU. Convert
 574          * each segment into ACL data packet and prepend it with ACL data packet
 575          * header. Link all segments together via m_nextpkt link.
 576          *
 577          * XXX BC (Broadcast flag) will always be 0 (zero).
 578          */
 579
 580         while (m0 != NULL) {
 581                 /* Check length of the packet against HCI MTU */
 582                 len = m0->m_pkthdr.len;
 583                 if (len > l2cap->pkt_size) {
 584                         m = m_split(m0, l2cap->pkt_size, M_NOWAIT);
 585                         if (m == NULL) {
 586                                 NG_L2CAP_ALERT(
 587 "%s: %s - m_split(%d) failed\n",        __func__, NG_NODE_NAME(l2cap->node),
 588                                         l2cap->pkt_size);
 589                                 goto fail;
 590                         }
 591
 592                         len = l2cap->pkt_size;
 593                 }
 594
 595                 /* Convert packet fragment into ACL data packet */
 596                 m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr));
 597                 if (m0 == NULL) {
 598                         NG_L2CAP_ALERT(
 599 "%s: %s - ng_l2cap_prepend(%zd) failed\n",
 600                                 __func__, NG_NODE_NAME(l2cap->node),
 601                                 sizeof(*acl_hdr));
 602                         goto fail;
 603                 }
 604
 605                 acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *);
 606                 acl_hdr->type = NG_HCI_ACL_DATA_PKT;
 607                 acl_hdr->length = htole16(len);
 608                 acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(
 609                                         con->con_handle, flag, 0));
 610
 611                 /* Add fragment to the chain */
 612                 m0->m_nextpkt = NULL;
 613
 614                 if (con->tx_pkt == NULL)
 615                         con->tx_pkt = m_last = m0;
 616                 else {
 617                         m_last->m_nextpkt = m0;
 618                         m_last = m0;
 619                 }
 620
 621                 NG_L2CAP_INFO(
 622 "%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n",
 623                         __func__, NG_NODE_NAME(l2cap->node), con->con_handle,
 624                         flag, len);
 625
 626                 m0 = m;
 627                 m = NULL;
 628                 flag = NG_HCI_PACKET_FRAGMENT;
 629         }
 630
 631         return (0);
 632 fail:
 633         NG_FREE_M(m0);
 634         NG_FREE_M(m);
 635
 636         while (con->tx_pkt != NULL) {
 637                 m = con->tx_pkt->m_nextpkt;
 638                 m_freem(con->tx_pkt);
 639                 con->tx_pkt = m;
 640         }
 641
 642         return (ENOBUFS);
 643 } /* ng_l2cap_lp_send */
 644
 645 /*
 646  * Receive ACL data packet from the HCI layer. First strip ACL packet header
 647  * and get connection handle, PB (Packet Boundary) flag and payload length.
 648  * Then find connection descriptor and verify its state. Then process ACL
 649  * packet as follows.
 650  *
 651  * 1) If we got first segment (pb == NG_HCI_PACKET_START) then extract L2CAP
 652  *    header and get total length of the L2CAP packet. Then start new L2CAP
 653  *    packet.
 654  *
 655  * 2) If we got other (then first :) segment (pb == NG_HCI_PACKET_FRAGMENT)
 656  *    then add segment to the packet.
 657  */
 658
 659 int
 660 ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m)
 661 {
 662         ng_hci_acldata_pkt_t    *acl_hdr = NULL;
 663         ng_l2cap_hdr_t          *l2cap_hdr = NULL;
 664         ng_l2cap_con_p           con = NULL;
 665         u_int16_t                con_handle, length, pb;
 666         int                      error = 0;
 667
 668         /* Check ACL data packet */
 669         if (m->m_pkthdr.len < sizeof(*acl_hdr)) {
 670                 NG_L2CAP_ERR(
 671 "%s: %s - invalid ACL data packet. Packet too small, length=%d\n",
 672                         __func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len);
 673                 error = EMSGSIZE;
 674                 goto drop;
 675         }
 676
 677         /* Strip ACL data packet header */
 678         NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr));
 679         if (m == NULL)
 680                 return (ENOBUFS);
 681
 682         acl_hdr = mtod(m, ng_hci_acldata_pkt_t *);
 683         m_adj(m, sizeof(*acl_hdr));
 684
 685         /* Get ACL connection handle, PB flag and payload length */
 686         acl_hdr->con_handle = le16toh(acl_hdr->con_handle);
 687         con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle);
 688         pb = NG_HCI_PB_FLAG(acl_hdr->con_handle);
 689         length = le16toh(acl_hdr->length);
 690
 691         NG_L2CAP_INFO(
 692 "%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n",
 693                 __func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length);
 694
 695         /* Get connection descriptor */
 696         con = ng_l2cap_con_by_handle(l2cap, con_handle);
 697         if (con == NULL) {
 698                 NG_L2CAP_ERR(
 699 "%s: %s - unexpected ACL data packet. " \
 700 "Connection does not exist, con_handle=%d\n",
 701                         __func__, NG_NODE_NAME(l2cap->node), con_handle);
 702                 error = ENOENT;
 703                 goto drop;
 704         }
 705
 706         /* Verify connection state */
 707         if (con->state != NG_L2CAP_CON_OPEN) {
 708                 NG_L2CAP_ERR(
 709 "%s: %s - unexpected ACL data packet. Invalid connection state=%d\n",
 710                         __func__, NG_NODE_NAME(l2cap->node), con->state);
 711                 error = EHOSTDOWN;
 712                 goto drop;
 713         }
 714
 715         /* Process packet */
 716         if (pb == NG_HCI_PACKET_START) {
 717                 if (con->rx_pkt != NULL) {
 718                         NG_L2CAP_ERR(
 719 "%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n",
 720                                 __func__, NG_NODE_NAME(l2cap->node),
 721                                 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
 722                         NG_FREE_M(con->rx_pkt);
 723                         con->rx_pkt_len = 0;
 724                 }
 725
 726                 /* Get L2CAP header */
 727                 if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) {
 728                         NG_L2CAP_ERR(
 729 "%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n",
 730                                 __func__, NG_NODE_NAME(l2cap->node),
 731                                 m->m_pkthdr.len);
 732                         error = EMSGSIZE;
 733                         goto drop;
 734                 }
 735
 736                 NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr));
 737                 if (m == NULL)
 738                         return (ENOBUFS);
 739
 740                 l2cap_hdr = mtod(m, ng_l2cap_hdr_t *);
 741
 742                 NG_L2CAP_INFO(
 743 "%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n",
 744                         __func__, NG_NODE_NAME(l2cap->node), con_handle,
 745                         le16toh(l2cap_hdr->length));
 746
 747                 /* Start new L2CAP packet */
 748                 con->rx_pkt = m;
 749                 con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr);
 750         } else if (pb == NG_HCI_PACKET_FRAGMENT) {
 751                 if (con->rx_pkt == NULL) {
 752                         NG_L2CAP_ERR(
 753 "%s: %s - unexpected ACL data packet fragment, con_handle=%d\n",
 754                                 __func__, NG_NODE_NAME(l2cap->node),
 755                                 con->con_handle);
 756                         goto drop;
 757                 }
 758
 759                 /* Add fragment to the L2CAP packet */
 760                 m_cat(con->rx_pkt, m);
 761                 con->rx_pkt->m_pkthdr.len += length;
 762         } else {
 763                 NG_L2CAP_ERR(
 764 "%s: %s - invalid ACL data packet. Invalid PB flag=%#x\n",
 765                         __func__, NG_NODE_NAME(l2cap->node), pb);
 766                 error = EINVAL;
 767                 goto drop;
 768         }
 769
 770         con->rx_pkt_len -= length;
 771         if (con->rx_pkt_len < 0) {
 772                 NG_L2CAP_ALERT(
 773 "%s: %s - packet length mismatch. Got %d bytes, offset %d bytes\n",
 774                         __func__, NG_NODE_NAME(l2cap->node),
 775                         con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
 776                 NG_FREE_M(con->rx_pkt);
 777                 con->rx_pkt_len = 0;
 778         } else if (con->rx_pkt_len == 0) {
 779                 /* OK, we have got complete L2CAP packet, so process it */
 780                 error = ng_l2cap_receive(con);
 781                 con->rx_pkt = NULL;
 782                 con->rx_pkt_len = 0;
 783         }
 784
 785         return (error);
 786
 787 drop:
 788         NG_FREE_M(m);
 789
 790         return (error);
 791 } /* ng_l2cap_lp_receive */
 792
 793 /*
 794  * Send queued ACL packets to the HCI layer
 795  */
 796
 797 void
 798 ng_l2cap_lp_deliver(ng_l2cap_con_p con)
 799 {
 800         ng_l2cap_p       l2cap = con->l2cap;
 801         struct mbuf     *m = NULL;
 802         int              error;
 803
 804         /* Check connection */
 805         if (con->state != NG_L2CAP_CON_OPEN)
 806                 return;
 807
 808         if (con->tx_pkt == NULL)
 809                 ng_l2cap_con_wakeup(con);
 810
 811         if (con->tx_pkt == NULL)
 812                 return;
 813
 814         /* Check if lower layer protocol is still connected */
 815         if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
 816                 NG_L2CAP_ERR(
 817 "%s: %s - hook \"%s\" is not connected or valid",
 818                         __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
 819
 820                 goto drop; /* XXX what to do with "pending"? */
 821         }
 822
 823         /* Send ACL data packets */
 824         while (con->pending < con->l2cap->num_pkts && con->tx_pkt != NULL) {
 825                 m = con->tx_pkt;
 826                 con->tx_pkt = con->tx_pkt->m_nextpkt;
 827                 m->m_nextpkt = NULL;
 828
 829                 if(m->m_flags &M_PROTO2){
 830                         ng_l2cap_lp_receive(con->l2cap, m);
 831                         continue;
 832                 }
 833                 NG_L2CAP_INFO(
 834 "%s: %s - sending ACL packet, con_handle=%d, len=%d\n",
 835                         __func__, NG_NODE_NAME(l2cap->node), con->con_handle,
 836                         m->m_pkthdr.len);
 837
 838                 NG_SEND_DATA_ONLY(error, l2cap->hci, m);
 839                 if (error != 0) {
 840                         NG_L2CAP_ERR(
 841 "%s: %s - could not send ACL data packet, con_handle=%d, error=%d\n",
 842                                 __func__, NG_NODE_NAME(l2cap->node),
 843                                 con->con_handle, error);
 844
 845                         goto drop; /* XXX what to do with "pending"? */
 846                 }
 847
 848                 con->pending ++;
 849         }
 850
 851         NG_L2CAP_INFO(
 852 "%s: %s - %d ACL packets have been sent, con_handle=%d\n",
 853                 __func__, NG_NODE_NAME(l2cap->node), con->pending,
 854                 con->con_handle);
 855
 856         return;
 857
 858 drop:
 859         while (con->tx_pkt != NULL) {
 860                 m = con->tx_pkt->m_nextpkt;
 861                 m_freem(con->tx_pkt);
 862                 con->tx_pkt = m;
 863         }
 864 } /* ng_l2cap_lp_deliver */
 865
 866 /*
 867  * Process connection timeout. Remove connection from the list. If there
 868  * are any channels that wait for the connection then notify them. Free
 869  * connection descriptor.
 870  */
 871
 872 void
 873 ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int con_handle)
 874 {
 875         ng_l2cap_p      l2cap = NULL;
 876         ng_l2cap_con_p  con = NULL;
 877
 878         if (NG_NODE_NOT_VALID(node)) {
 879                 printf("%s: Netgraph node is not valid\n", __func__);
 880                 return;
 881         }
 882
 883         l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
 884         con = ng_l2cap_con_by_handle(l2cap, con_handle);
 885
 886         if (con == NULL) {
 887                 NG_L2CAP_ALERT(
 888 "%s: %s - could not find connection, con_handle=%d\n",
 889                         __func__, NG_NODE_NAME(node), con_handle);
 890                 return;
 891         }
 892
 893         if (!(con->flags & NG_L2CAP_CON_LP_TIMO)) {
 894                 NG_L2CAP_ALERT(
 895 "%s: %s - no pending LP timeout, con_handle=%d, state=%d, flags=%#x\n",
 896                         __func__, NG_NODE_NAME(node), con_handle, con->state,
 897                         con->flags);
 898                 return;
 899         }
 900
 901         /*
 902          * Notify channels that connection has timed out. This will remove
 903          * connection, channels and pending commands.
 904          */
 905
 906         con->flags &= ~NG_L2CAP_CON_LP_TIMO;
 907         ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT);
 908 } /* ng_l2cap_process_lp_timeout */
 909
 910 /*
 911  * Process auto disconnect timeout and send LP_DisconReq event to the
 912  * lower layer protocol
 913  */
 914
 915 void
 916 ng_l2cap_process_discon_timeout(node_p node, hook_p hook, void *arg1, int con_handle)
 917 {
 918         ng_l2cap_p               l2cap = NULL;
 919         ng_l2cap_con_p           con = NULL;
 920         struct ng_mesg          *msg = NULL;
 921         ng_hci_lp_discon_req_ep *ep = NULL;
 922         int                      error;
 923
 924         if (NG_NODE_NOT_VALID(node)) {
 925                 printf("%s: Netgraph node is not valid\n", __func__);
 926                 return;
 927         }
 928
 929         l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
 930         con = ng_l2cap_con_by_handle(l2cap, con_handle);
 931
 932         if (con == NULL) {
 933                 NG_L2CAP_ALERT(
 934 "%s: %s - could not find connection, con_handle=%d\n",
 935                         __func__, NG_NODE_NAME(node), con_handle);
 936                 return;
 937         }
 938
 939         if (!(con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)) {
 940                 NG_L2CAP_ALERT(
 941 "%s: %s - no pending disconnect timeout, con_handle=%d, state=%d, flags=%#x\n",
 942                         __func__, NG_NODE_NAME(node), con_handle, con->state,
 943                         con->flags);
 944                 return;
 945         }
 946
 947         con->flags &= ~NG_L2CAP_CON_AUTO_DISCON_TIMO;
 948
 949         /* Check if lower layer protocol is still connected */
 950         if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
 951                 NG_L2CAP_ERR(
 952 "%s: %s - hook \"%s\" is not connected or valid\n",
 953                         __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
 954                 return;
 955         }
 956
 957         /* Create and send LP_DisconReq event */
 958         NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ,
 959                 sizeof(*ep), M_NOWAIT);
 960         if (msg == NULL)
 961                 return;
 962
 963         ep = (ng_hci_lp_discon_req_ep *) (msg->data);
 964         ep->con_handle = con->con_handle;
 965         ep->reason = 0x13; /* User Ended Connection */
 966
 967         NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0);
 968 } /* ng_l2cap_process_discon_timeout */
 969