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