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