sys/netgraph/bluetooth/l2cap/ng_l2cap_misc.c

   1 /*
   2  * ng_l2cap_misc.c
   3  */
   4
   5 /*-
   6  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   7  *
   8  * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com>
   9  * All rights reserved.
  10  *
  11  * Redistribution and use in source and binary forms, with or without
  12  * modification, are permitted provided that the following conditions
  13  * are met:
  14  * 1. Redistributions of source code must retain the above copyright
  15  *    notice, this list of conditions and the following disclaimer.
  16  * 2. Redistributions in binary form must reproduce the above copyright
  17  *    notice, this list of conditions and the following disclaimer in the
  18  *    documentation and/or other materials provided with the distribution.
  19  *
  20  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  21  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  23  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  26  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  27  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  29  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  30  * SUCH DAMAGE.
  31  *
  32  * $Id: ng_l2cap_misc.c,v 1.5 2003/09/08 19:11:45 max Exp $
  33  * $FreeBSD$
  34  */
  35
  36 #include <sys/param.h>
  37 #include <sys/systm.h>
  38 #include <sys/kernel.h>
  39 #include <sys/malloc.h>
  40 #include <sys/mbuf.h>
  41 #include <sys/queue.h>
  42 #include <netgraph/ng_message.h>
  43 #include <netgraph/netgraph.h>
  44 #include <netgraph/bluetooth/include/ng_bluetooth.h>
  45 #include <netgraph/bluetooth/include/ng_hci.h>
  46 #include <netgraph/bluetooth/include/ng_l2cap.h>
  47 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h>
  48 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h>
  49 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h>
  50 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h>
  51 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h>
  52 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h>
  53
  54 static u_int16_t        ng_l2cap_get_cid        (ng_l2cap_p, int);
  55
  56 /******************************************************************************
  57  ******************************************************************************
  58  **                              Utility routines
  59  ******************************************************************************
  60  ******************************************************************************/
  61
  62 /*
  63  * Send hook information to the upper layer
  64  */
  65
  66 void
  67 ng_l2cap_send_hook_info(node_p node, hook_p hook, void *arg1, int arg2)
  68 {
  69         ng_l2cap_p       l2cap = NULL;
  70         struct ng_mesg  *msg = NULL;
  71         int              error = 0;
  72         ng_l2cap_node_hook_info_ep *ep ;
  73
  74         if (node == NULL || NG_NODE_NOT_VALID(node) ||
  75             hook == NULL || NG_HOOK_NOT_VALID(hook))
  76                 return;
  77
  78         l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
  79         if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci) ||
  80             bcmp(&l2cap->bdaddr, NG_HCI_BDADDR_ANY, sizeof(l2cap->bdaddr)) == 0)
  81                 return;
  82
  83         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_NODE_HOOK_INFO,
  84                      sizeof(*ep), M_NOWAIT);
  85
  86         if (msg != NULL) {
  87                 ep = (ng_l2cap_node_hook_info_ep *) &msg->data;
  88                 bcopy(&l2cap->bdaddr, &ep->addr, sizeof(bdaddr_t));
  89                 NG_SEND_MSG_HOOK(error, node, msg, hook, 0);
  90         } else
  91                 error = ENOMEM;
  92
  93         if (error != 0)
  94                 NG_L2CAP_INFO(
  95 "%s: %s - failed to send HOOK_INFO message to hook \"%s\", error=%d\n",
  96                         __func__, NG_NODE_NAME(l2cap->node), NG_HOOK_NAME(hook),
  97                         error);
  98 } /* ng_l2cap_send_hook_info */
  99
 100 /*
 101  * Create new connection descriptor for the "remote" unit.
 102  * Will link connection descriptor to the l2cap node.
 103  */
 104
 105 ng_l2cap_con_p
 106 ng_l2cap_new_con(ng_l2cap_p l2cap, bdaddr_p bdaddr, int type)
 107 {
 108         static int      fake_con_handle = 0x0f00;
 109         ng_l2cap_con_p  con = NULL;
 110
 111         /* Create new connection descriptor */
 112         con = malloc(sizeof(*con), M_NETGRAPH_L2CAP,
 113                 M_NOWAIT|M_ZERO);
 114         if (con == NULL)
 115                 return (NULL);
 116
 117         con->l2cap = l2cap;
 118         con->state = NG_L2CAP_CON_CLOSED;
 119         con->encryption = 0;
 120         /*
 121          * XXX
 122          *
 123          * Assign fake connection handle to the connection descriptor.
 124          * Bluetooth specification marks 0x0f00 - 0x0fff connection
 125          * handles as reserved. We need this fake connection handles
 126          * for timeouts. Connection handle will be passed as argument
 127          * to timeout so when timeout happens we can find the right
 128          * connection descriptor. We can not pass pointers, because
 129          * timeouts are external (to Netgraph) events and there might
 130          * be a race when node/hook goes down and timeout event already
 131          * went into node's queue
 132          */
 133
 134         con->con_handle = fake_con_handle ++;
 135         if (fake_con_handle > 0x0fff)
 136                 fake_con_handle = 0x0f00;
 137
 138         bcopy(bdaddr, &con->remote, sizeof(con->remote));
 139         con->linktype = type;
 140         ng_callout_init(&con->con_timo);
 141
 142         con->ident = NG_L2CAP_FIRST_IDENT - 1;
 143         TAILQ_INIT(&con->cmd_list);
 144
 145         /* Link connection */
 146         LIST_INSERT_HEAD(&l2cap->con_list, con, next);
 147
 148         return (con);
 149 } /* ng_l2cap_new_con */
 150
 151 /*
 152  * Add reference to the connection descriptor
 153  */
 154
 155 void
 156 ng_l2cap_con_ref(ng_l2cap_con_p con)
 157 {
 158         con->refcnt ++;
 159
 160         if (con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO) {
 161                 if ((con->state != NG_L2CAP_CON_OPEN) ||
 162                     (con->flags & NG_L2CAP_CON_OUTGOING) == 0)
 163                         panic(
 164 "%s: %s - bad auto disconnect timeout, state=%d, flags=%#x\n",
 165                                 __func__, NG_NODE_NAME(con->l2cap->node),
 166                                 con->state, con->flags);
 167
 168                 ng_l2cap_discon_untimeout(con);
 169         }
 170 } /* ng_l2cap_con_ref */
 171
 172 /*
 173  * Remove reference from the connection descriptor
 174  */
 175
 176 void
 177 ng_l2cap_con_unref(ng_l2cap_con_p con)
 178 {
 179         con->refcnt --;
 180
 181         if (con->refcnt < 0)
 182                 panic(
 183 "%s: %s - con->refcnt < 0\n", __func__, NG_NODE_NAME(con->l2cap->node));
 184
 185         /*
 186          * Set auto disconnect timer only if the following conditions are met:
 187          * 1) we have no reference on the connection
 188          * 2) connection is in OPEN state
 189          * 3) it is an outgoing connection
 190          * 4) disconnect timeout > 0
 191          * 5) connection is not dying
 192          */
 193
 194         if ((con->refcnt == 0) &&
 195             (con->state == NG_L2CAP_CON_OPEN) &&
 196             (con->flags & NG_L2CAP_CON_OUTGOING) &&
 197             (con->l2cap->discon_timo > 0) &&
 198             ((con->flags & NG_L2CAP_CON_DYING) == 0))
 199                 ng_l2cap_discon_timeout(con);
 200 } /* ng_l2cap_con_unref */
 201
 202 /*
 203  * Set auto disconnect timeout
 204  * XXX FIXME: check return code from ng_callout
 205  */
 206
 207 int
 208 ng_l2cap_discon_timeout(ng_l2cap_con_p con)
 209 {
 210         if (con->flags & (NG_L2CAP_CON_LP_TIMO|NG_L2CAP_CON_AUTO_DISCON_TIMO))
 211                 panic(
 212 "%s: %s - invalid timeout, state=%d, flags=%#x\n",
 213                         __func__, NG_NODE_NAME(con->l2cap->node),
 214                         con->state, con->flags);
 215
 216         con->flags |= NG_L2CAP_CON_AUTO_DISCON_TIMO;
 217         ng_callout(&con->con_timo, con->l2cap->node, NULL,
 218                                 con->l2cap->discon_timo * hz,
 219                                 ng_l2cap_process_discon_timeout, NULL,
 220                                 con->con_handle);
 221
 222         return (0);
 223 } /* ng_l2cap_discon_timeout */
 224
 225 /*
 226  * Unset auto disconnect timeout
 227  */
 228
 229 int
 230 ng_l2cap_discon_untimeout(ng_l2cap_con_p con)
 231 {
 232         if (!(con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO))
 233                 panic(
 234 "%s: %s - no disconnect timeout, state=%d, flags=%#x\n",
 235                         __func__,  NG_NODE_NAME(con->l2cap->node),
 236                         con->state, con->flags);
 237
 238         if (ng_uncallout(&con->con_timo, con->l2cap->node) == 0)
 239                 return (ETIMEDOUT);
 240
 241         con->flags &= ~NG_L2CAP_CON_AUTO_DISCON_TIMO;
 242
 243         return (0);
 244 } /* ng_l2cap_discon_untimeout */
 245
 246 /*
 247  * Free connection descriptor. Will unlink connection and free everything.
 248  */
 249
 250 void
 251 ng_l2cap_free_con(ng_l2cap_con_p con)
 252 {
 253         ng_l2cap_chan_p f = NULL, n = NULL;
 254
 255         con->state = NG_L2CAP_CON_CLOSED;
 256
 257         while (con->tx_pkt != NULL) {
 258                 struct mbuf     *m = con->tx_pkt->m_nextpkt;
 259
 260                 m_freem(con->tx_pkt);
 261                 con->tx_pkt = m;
 262         }
 263
 264         NG_FREE_M(con->rx_pkt);
 265
 266         for (f = LIST_FIRST(&con->l2cap->chan_list); f != NULL; ) {
 267                 n = LIST_NEXT(f, next);
 268
 269                 if (f->con == con)
 270                         ng_l2cap_free_chan(f);
 271
 272                 f = n;
 273         }
 274
 275         while (!TAILQ_EMPTY(&con->cmd_list)) {
 276                 ng_l2cap_cmd_p  cmd = TAILQ_FIRST(&con->cmd_list);
 277
 278                 ng_l2cap_unlink_cmd(cmd);
 279                 if (cmd->flags & NG_L2CAP_CMD_PENDING)
 280                         ng_l2cap_command_untimeout(cmd);
 281                 ng_l2cap_free_cmd(cmd);
 282         }
 283
 284         if (con->flags & (NG_L2CAP_CON_AUTO_DISCON_TIMO|NG_L2CAP_CON_LP_TIMO))
 285                 panic(
 286 "%s: %s - timeout pending! state=%d, flags=%#x\n",
 287                         __func__,  NG_NODE_NAME(con->l2cap->node),
 288                         con->state, con->flags);
 289
 290         LIST_REMOVE(con, next);
 291
 292         bzero(con, sizeof(*con));
 293         free(con, M_NETGRAPH_L2CAP);
 294 } /* ng_l2cap_free_con */
 295
 296 /*
 297  * Get connection by "remote" address
 298  */
 299
 300 ng_l2cap_con_p
 301 ng_l2cap_con_by_addr(ng_l2cap_p l2cap, bdaddr_p bdaddr, unsigned int type)
 302 {
 303         ng_l2cap_con_p  con = NULL;
 304
 305         LIST_FOREACH(con, &l2cap->con_list, next)
 306                 if ((bcmp(bdaddr, &con->remote, sizeof(con->remote)) == 0)&&
 307                     (con->linktype == type))
 308                         break;
 309
 310         return (con);
 311 } /* ng_l2cap_con_by_addr */
 312
 313 /*
 314  * Get connection by "handle"
 315  */
 316
 317 ng_l2cap_con_p
 318 ng_l2cap_con_by_handle(ng_l2cap_p l2cap, u_int16_t con_handle)
 319 {
 320         ng_l2cap_con_p  con = NULL;
 321
 322         LIST_FOREACH(con, &l2cap->con_list, next)
 323                 if (con->con_handle == con_handle)
 324                         break;
 325
 326         return (con);
 327 } /* ng_l2cap_con_by_handle */
 328
 329 /*
 330  * Allocate new L2CAP channel descriptor on "con" connection with "psm".
 331  * Will link the channel to the l2cap node
 332  */
 333
 334 ng_l2cap_chan_p
 335 ng_l2cap_new_chan(ng_l2cap_p l2cap, ng_l2cap_con_p con, u_int16_t psm, int idtype)
 336 {
 337         ng_l2cap_chan_p ch = NULL;
 338
 339         ch = malloc(sizeof(*ch), M_NETGRAPH_L2CAP,
 340                 M_NOWAIT|M_ZERO);
 341         if (ch == NULL)
 342                 return (NULL);
 343         if(idtype == NG_L2CAP_L2CA_IDTYPE_ATT){
 344                 ch->scid = ch->dcid = NG_L2CAP_ATT_CID;
 345         }else if(idtype == NG_L2CAP_L2CA_IDTYPE_SMP){
 346                 ch->scid = ch->dcid = NG_L2CAP_SMP_CID;
 347         }else{
 348                 ch->scid = ng_l2cap_get_cid(l2cap,
 349                                             (con->linktype!= NG_HCI_LINK_ACL));
 350         }
 351
 352         ch->idtype = idtype;
 353         if (ch->scid != NG_L2CAP_NULL_CID) {
 354                 /* Initialize channel */
 355                 ch->psm = psm;
 356                 ch->con = con;
 357                 ch->state = NG_L2CAP_CLOSED;
 358
 359                 /* Set MTU and flow control settings to defaults */
 360                 ch->imtu = NG_L2CAP_MTU_DEFAULT;
 361                 bcopy(ng_l2cap_default_flow(), &ch->iflow, sizeof(ch->iflow));
 362
 363                 ch->omtu = NG_L2CAP_MTU_DEFAULT;
 364                 bcopy(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow));
 365
 366                 ch->flush_timo = NG_L2CAP_FLUSH_TIMO_DEFAULT;
 367                 ch->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT;
 368
 369                 LIST_INSERT_HEAD(&l2cap->chan_list, ch, next);
 370
 371                 ng_l2cap_con_ref(con);
 372         } else {
 373                 bzero(ch, sizeof(*ch));
 374                 free(ch, M_NETGRAPH_L2CAP);
 375                 ch = NULL;
 376         }
 377
 378         return (ch);
 379 } /* ng_l2cap_new_chan */
 380
 381
 382 ng_l2cap_chan_p
 383 ng_l2cap_chan_by_scid(ng_l2cap_p l2cap, u_int16_t scid, int idtype)
 384 {
 385         ng_l2cap_chan_p ch = NULL;
 386
 387         if((idtype == NG_L2CAP_L2CA_IDTYPE_ATT)||
 388            (idtype == NG_L2CAP_L2CA_IDTYPE_SMP)){
 389                 return NULL;
 390         }
 391
 392         LIST_FOREACH(ch, &l2cap->chan_list, next){
 393                 if((idtype != NG_L2CAP_L2CA_IDTYPE_BREDR)&&
 394                    (ch->con->linktype == NG_HCI_LINK_ACL ))
 395                         continue;
 396                 if((idtype != NG_L2CAP_L2CA_IDTYPE_LE)&&
 397                    (ch->con->linktype != NG_HCI_LINK_ACL ))
 398                         continue;
 399                 if (ch->scid == scid)
 400                         break;
 401         }
 402         return (ch);
 403 } /* ng_l2cap_chan_by_scid */
 404
 405 ng_l2cap_chan_p
 406 ng_l2cap_chan_by_conhandle(ng_l2cap_p l2cap, uint16_t scid,
 407                            u_int16_t con_handle)
 408 {
 409         ng_l2cap_chan_p ch = NULL;
 410
 411
 412         LIST_FOREACH(ch, &l2cap->chan_list, next){
 413                 if ((ch->scid == scid) &&
 414                     (ch->con->con_handle == con_handle))
 415                         break;
 416         }
 417         return (ch);
 418 } /* ng_l2cap_chan_by_scid */
 419
 420 /*
 421  * Free channel descriptor.
 422  */
 423
 424 void
 425 ng_l2cap_free_chan(ng_l2cap_chan_p ch)
 426 {
 427         ng_l2cap_cmd_p  f = NULL, n = NULL;
 428
 429         f = TAILQ_FIRST(&ch->con->cmd_list);
 430
 431         while (f != NULL) {
 432                 n = TAILQ_NEXT(f, next);
 433
 434                 if (f->ch == ch) {
 435                         ng_l2cap_unlink_cmd(f);
 436                         if (f->flags & NG_L2CAP_CMD_PENDING)
 437                                 ng_l2cap_command_untimeout(f);
 438                         ng_l2cap_free_cmd(f);
 439                 }
 440
 441                 f = n;
 442         }
 443
 444         LIST_REMOVE(ch, next);
 445
 446         ng_l2cap_con_unref(ch->con);
 447
 448         bzero(ch, sizeof(*ch));
 449         free(ch, M_NETGRAPH_L2CAP);
 450 } /* ng_l2cap_free_chan */
 451
 452 /*
 453  * Create new L2CAP command descriptor. WILL NOT add command to the queue.
 454  */
 455
 456 ng_l2cap_cmd_p
 457 ng_l2cap_new_cmd(ng_l2cap_con_p con, ng_l2cap_chan_p ch, u_int8_t ident,
 458                 u_int8_t code, u_int32_t token)
 459 {
 460         ng_l2cap_cmd_p  cmd = NULL;
 461
 462         KASSERT((ch == NULL || ch->con == con),
 463 ("%s: %s - invalid channel pointer!\n",
 464                 __func__, NG_NODE_NAME(con->l2cap->node)));
 465
 466         cmd = malloc(sizeof(*cmd), M_NETGRAPH_L2CAP,
 467                 M_NOWAIT|M_ZERO);
 468         if (cmd == NULL)
 469                 return (NULL);
 470
 471         cmd->con = con;
 472         cmd->ch = ch;
 473         cmd->ident = ident;
 474         cmd->code = code;
 475         cmd->token = token;
 476         ng_callout_init(&cmd->timo);
 477
 478         return (cmd);
 479 } /* ng_l2cap_new_cmd */
 480
 481 /*
 482  * Get pending (i.e. initiated by local side) L2CAP command descriptor by ident
 483  */
 484
 485 ng_l2cap_cmd_p
 486 ng_l2cap_cmd_by_ident(ng_l2cap_con_p con, u_int8_t ident)
 487 {
 488         ng_l2cap_cmd_p  cmd = NULL;
 489
 490         TAILQ_FOREACH(cmd, &con->cmd_list, next) {
 491                 if ((cmd->flags & NG_L2CAP_CMD_PENDING) && cmd->ident == ident) {
 492                         KASSERT((cmd->con == con),
 493 ("%s: %s - invalid connection pointer!\n",
 494                                 __func__, NG_NODE_NAME(con->l2cap->node)));
 495
 496                         break;
 497                 }
 498         }
 499
 500         return (cmd);
 501 } /* ng_l2cap_cmd_by_ident */
 502
 503 /*
 504  * Set LP timeout
 505  * XXX FIXME: check return code from ng_callout
 506  */
 507
 508 int
 509 ng_l2cap_lp_timeout(ng_l2cap_con_p con)
 510 {
 511         if (con->flags & (NG_L2CAP_CON_LP_TIMO|NG_L2CAP_CON_AUTO_DISCON_TIMO))
 512                 panic(
 513 "%s: %s - invalid timeout, state=%d, flags=%#x\n",
 514                         __func__, NG_NODE_NAME(con->l2cap->node),
 515                         con->state, con->flags);
 516
 517         con->flags |= NG_L2CAP_CON_LP_TIMO;
 518         ng_callout(&con->con_timo, con->l2cap->node, NULL,
 519                                 bluetooth_hci_connect_timeout(),
 520                                 ng_l2cap_process_lp_timeout, NULL,
 521                                 con->con_handle);
 522
 523         return (0);
 524 } /* ng_l2cap_lp_timeout */
 525
 526 /*
 527  * Unset LP timeout
 528  */
 529
 530 int
 531 ng_l2cap_lp_untimeout(ng_l2cap_con_p con)
 532 {
 533         if (!(con->flags & NG_L2CAP_CON_LP_TIMO))
 534                 panic(
 535 "%s: %s - no LP connection timeout, state=%d, flags=%#x\n",
 536                         __func__,  NG_NODE_NAME(con->l2cap->node),
 537                         con->state, con->flags);
 538
 539         if (ng_uncallout(&con->con_timo, con->l2cap->node) == 0)
 540                 return (ETIMEDOUT);
 541
 542         con->flags &= ~NG_L2CAP_CON_LP_TIMO;
 543
 544         return (0);
 545 } /* ng_l2cap_lp_untimeout */
 546
 547 /*
 548  * Set L2CAP command timeout
 549  * XXX FIXME: check return code from ng_callout
 550  */
 551
 552 int
 553 ng_l2cap_command_timeout(ng_l2cap_cmd_p cmd, int timo)
 554 {
 555         int     arg;
 556
 557         if (cmd->flags & NG_L2CAP_CMD_PENDING)
 558                 panic(
 559 "%s: %s - duplicated command timeout, code=%#x, flags=%#x\n",
 560                         __func__, NG_NODE_NAME(cmd->con->l2cap->node),
 561                         cmd->code, cmd->flags);
 562
 563         arg = ((cmd->ident << 16) | cmd->con->con_handle);
 564         cmd->flags |= NG_L2CAP_CMD_PENDING;
 565         ng_callout(&cmd->timo, cmd->con->l2cap->node, NULL, timo,
 566                                 ng_l2cap_process_command_timeout, NULL, arg);
 567
 568         return (0);
 569 } /* ng_l2cap_command_timeout */
 570
 571 /*
 572  * Unset L2CAP command timeout
 573  */
 574
 575 int
 576 ng_l2cap_command_untimeout(ng_l2cap_cmd_p cmd)
 577 {
 578         if (!(cmd->flags & NG_L2CAP_CMD_PENDING))
 579                 panic(
 580 "%s: %s - no command timeout, code=%#x, flags=%#x\n",
 581                         __func__, NG_NODE_NAME(cmd->con->l2cap->node),
 582                         cmd->code, cmd->flags);
 583
 584         if (ng_uncallout(&cmd->timo, cmd->con->l2cap->node) == 0)
 585                 return (ETIMEDOUT);
 586
 587         cmd->flags &= ~NG_L2CAP_CMD_PENDING;
 588
 589         return (0);
 590 } /* ng_l2cap_command_untimeout */
 591
 592 /*
 593  * Prepend "m"buf with "size" bytes
 594  */
 595
 596 struct mbuf *
 597 ng_l2cap_prepend(struct mbuf *m, int size)
 598 {
 599         M_PREPEND(m, size, M_NOWAIT);
 600         if (m == NULL || (m->m_len < size && (m = m_pullup(m, size)) == NULL))
 601                 return (NULL);
 602
 603         return (m);
 604 } /* ng_l2cap_prepend */
 605
 606 /*
 607  * Default flow settings
 608  */
 609
 610 ng_l2cap_flow_p
 611 ng_l2cap_default_flow(void)
 612 {
 613         static ng_l2cap_flow_t  default_flow = {
 614                 /* flags */             0x0,
 615                 /* service_type */      NG_HCI_SERVICE_TYPE_BEST_EFFORT,
 616                 /* token_rate */        0xffffffff, /* maximum */
 617                 /* token_bucket_size */ 0xffffffff, /* maximum */
 618                 /* peak_bandwidth */    0x00000000, /* maximum */
 619                 /* latency */           0xffffffff, /* don't care */
 620                 /* delay_variation */   0xffffffff  /* don't care */
 621         };
 622
 623         return (&default_flow);
 624 } /* ng_l2cap_default_flow */
 625
 626 /*
 627  * Get next available channel ID
 628  * XXX FIXME this is *UGLY* but will do for now
 629  */
 630
 631 static u_int16_t
 632 ng_l2cap_get_cid(ng_l2cap_p l2cap,int isle)
 633 {
 634         u_int16_t       cid ;
 635         u_int16_t       endcid;
 636         uint16_t         mask;
 637         int idtype;
 638         if(isle){
 639                 endcid = l2cap->lecid;
 640                 /*Assume Last CID is 2^n-1 */
 641                 mask = NG_L2CAP_LELAST_CID;
 642                 idtype = NG_L2CAP_L2CA_IDTYPE_LE;
 643         }else{
 644                 endcid = l2cap->cid;
 645                 /*Assume Last CID is 2^n-1 */
 646                 mask = NG_L2CAP_LAST_CID;
 647                 idtype = NG_L2CAP_L2CA_IDTYPE_BREDR;
 648         }
 649         cid = (endcid+1) & mask;
 650
 651         if (cid < NG_L2CAP_FIRST_CID)
 652                 cid = NG_L2CAP_FIRST_CID;
 653
 654         while (cid != endcid) {
 655                 if (ng_l2cap_chan_by_scid(l2cap, cid, idtype) == NULL) {
 656                         if(!isle){
 657                                 l2cap->cid = cid;
 658                         }else{
 659                                 l2cap->lecid = cid;
 660                         }
 661                         return (cid);
 662                 }
 663
 664                 cid ++;
 665                 cid &= mask;
 666                 if (cid < NG_L2CAP_FIRST_CID)
 667                         cid = NG_L2CAP_FIRST_CID;
 668         }
 669
 670         return (NG_L2CAP_NULL_CID);
 671 } /* ng_l2cap_get_cid */
 672
 673 /*
 674  * Get next available command ident
 675  * XXX FIXME this is *UGLY* but will do for now
 676  */
 677
 678 u_int8_t
 679 ng_l2cap_get_ident(ng_l2cap_con_p con)
 680 {
 681         u_int8_t        ident = con->ident + 1;
 682
 683         if (ident < NG_L2CAP_FIRST_IDENT)
 684                 ident = NG_L2CAP_FIRST_IDENT;
 685
 686         while (ident != con->ident) {
 687                 if (ng_l2cap_cmd_by_ident(con, ident) == NULL) {
 688                         con->ident = ident;
 689
 690                         return (ident);
 691                 }
 692
 693                 ident ++;
 694                 if (ident < NG_L2CAP_FIRST_IDENT)
 695                         ident = NG_L2CAP_FIRST_IDENT;
 696         }
 697
 698         return (NG_L2CAP_NULL_IDENT);
 699 } /* ng_l2cap_get_ident */
 700