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