sys/netgraph/bluetooth/l2cap/ng_l2cap_llpi.c

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