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