1 /*
2 * ng_l2cap_llpi.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_llpi.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/endian.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 /******************************************************************************
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
ng_l2cap_lp_con_req(ng_l2cap_p l2cap,bdaddr_p bdaddr,int type)68 ng_l2cap_lp_con_req(ng_l2cap_p l2cap, bdaddr_p bdaddr, int type)
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, type);
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, type);
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_NOWAIT);
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 = type;
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 (ng_l2cap_lp_untimeout(con) == 0)
121 ng_l2cap_free_con(con);
122
123 /*
124 * Do not free connection if ng_l2cap_lp_untimeout() failed
125 * let timeout handler deal with it. Always return error to
126 * the caller.
127 */
128 }
129
130 return (error);
131 } /* ng_l2cap_lp_con_req */
132
133 /*
134 * Process LP_ConnectCfm event from the lower layer protocol. It could be
135 * positive or negative. Verify remote unit address then stop the timer and
136 * process event.
137 */
138
139 int
ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap,struct ng_mesg * msg)140 ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg)
141 {
142 ng_hci_lp_con_cfm_ep *ep = NULL;
143 ng_l2cap_con_p con = NULL;
144 int error = 0;
145
146 /* Check message */
147 if (msg->header.arglen != sizeof(*ep)) {
148 NG_L2CAP_ALERT(
149 "%s: %s - invalid LP_ConnectCfm[Neg] message size\n",
150 __func__, NG_NODE_NAME(l2cap->node));
151 error = EMSGSIZE;
152 goto out;
153 }
154
155 ep = (ng_hci_lp_con_cfm_ep *) (msg->data);
156 /* Check if we have requested/accepted this connection */
157 con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr, ep->link_type);
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
ng_l2cap_lp_con_ind(ng_l2cap_p l2cap,struct ng_mesg * msg)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, ep->link_type);
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, ep->link_type);
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
ng_l2cap_lp_discon_ind(ng_l2cap_p l2cap,struct ng_mesg * msg)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
ng_l2cap_lp_qos_req(ng_l2cap_p l2cap,u_int16_t con_handle,ng_l2cap_flow_p flow)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
ng_l2cap_lp_qos_cfm(ng_l2cap_p l2cap,struct ng_mesg * msg)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
ng_l2cap_lp_qos_ind(ng_l2cap_p l2cap,struct ng_mesg * msg)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 int
ng_l2cap_lp_enc_change(ng_l2cap_p l2cap,struct ng_mesg * msg)486 ng_l2cap_lp_enc_change(ng_l2cap_p l2cap, struct ng_mesg *msg)
487 {
488 ng_hci_lp_enc_change_ep *ep = NULL;
489 ng_l2cap_con_p con = NULL;
490 int error = 0;
491 ng_l2cap_chan_p ch = NULL;
492 /* Check message */
493 if (msg->header.arglen != sizeof(*ep)) {
494 NG_L2CAP_ALERT(
495 "%s: %s - invalid LP_ENCChange message size\n",
496 __func__, NG_NODE_NAME(l2cap->node));
497 error = EMSGSIZE;
498 goto out;
499 }
500
501 ep = (ng_hci_lp_enc_change_ep *) (msg->data);
502
503 /* Check if we have this connection */
504 con = ng_l2cap_con_by_handle(l2cap, ep->con_handle);
505 if (con == NULL) {
506 NG_L2CAP_ERR(
507 "%s: %s - unexpected LP_Enc Change Event. " \
508 "Connection does not exist, con_handle=%d\n",
509 __func__, NG_NODE_NAME(l2cap->node), ep->con_handle);
510 error = ENOENT;
511 goto out;
512 }
513
514 /* Verify connection state */
515 if (con->state != NG_L2CAP_CON_OPEN) {
516 NG_L2CAP_ERR(
517 "%s: %s - unexpected ENC_CHANGE event. " \
518 "Invalid connection state, state=%d, con_handle=%d\n",
519 __func__, NG_NODE_NAME(l2cap->node), con->state,
520 con->con_handle);
521 error = EINVAL;
522 goto out;
523 }
524
525 con->encryption = ep->status;
526
527 LIST_FOREACH(ch, &l2cap->chan_list, next){
528 if((ch->con->con_handle == ep->con_handle) &&
529 (ch->con->linktype == ep->link_type))
530 ng_l2cap_l2ca_encryption_change(ch, ep->status);
531 }
532
533 out:
534 return (error);
535 } /* ng_l2cap_enc_change */
536
537 /*
538 * Prepare L2CAP packet. Prepend packet with L2CAP packet header and then
539 * segment it according to HCI MTU.
540 */
541
542 int
ng_l2cap_lp_send(ng_l2cap_con_p con,u_int16_t dcid,struct mbuf * m0)543 ng_l2cap_lp_send(ng_l2cap_con_p con, u_int16_t dcid, struct mbuf *m0)
544 {
545 ng_l2cap_p l2cap = con->l2cap;
546 ng_l2cap_hdr_t *l2cap_hdr = NULL;
547 ng_hci_acldata_pkt_t *acl_hdr = NULL;
548 struct mbuf *m_last = NULL, *m = NULL;
549 int len, flag = (con->linktype == NG_HCI_LINK_ACL) ? NG_HCI_PACKET_START : NG_HCI_LE_PACKET_START;
550
551 KASSERT((con->tx_pkt == NULL),
552 ("%s: %s - another packet pending?!\n", __func__, NG_NODE_NAME(l2cap->node)));
553 KASSERT((l2cap->pkt_size > 0),
554 ("%s: %s - invalid l2cap->pkt_size?!\n", __func__, NG_NODE_NAME(l2cap->node)));
555
556 /* Prepend mbuf with L2CAP header */
557 m0 = ng_l2cap_prepend(m0, sizeof(*l2cap_hdr));
558 if (m0 == NULL) {
559 NG_L2CAP_ALERT(
560 "%s: %s - ng_l2cap_prepend(%zd) failed\n",
561 __func__, NG_NODE_NAME(l2cap->node),
562 sizeof(*l2cap_hdr));
563
564 goto fail;
565 }
566
567 l2cap_hdr = mtod(m0, ng_l2cap_hdr_t *);
568 l2cap_hdr->length = htole16(m0->m_pkthdr.len - sizeof(*l2cap_hdr));
569 l2cap_hdr->dcid = htole16(dcid);
570
571 /*
572 * Segment single L2CAP packet according to the HCI layer MTU. Convert
573 * each segment into ACL data packet and prepend it with ACL data packet
574 * header. Link all segments together via m_nextpkt link.
575 *
576 * XXX BC (Broadcast flag) will always be 0 (zero).
577 */
578
579 while (m0 != NULL) {
580 /* Check length of the packet against HCI MTU */
581 len = m0->m_pkthdr.len;
582 if (len > l2cap->pkt_size) {
583 m = m_split(m0, l2cap->pkt_size, M_NOWAIT);
584 if (m == NULL) {
585 NG_L2CAP_ALERT(
586 "%s: %s - m_split(%d) failed\n", __func__, NG_NODE_NAME(l2cap->node),
587 l2cap->pkt_size);
588 goto fail;
589 }
590
591 len = l2cap->pkt_size;
592 }
593
594 /* Convert packet fragment into ACL data packet */
595 m0 = ng_l2cap_prepend(m0, sizeof(*acl_hdr));
596 if (m0 == NULL) {
597 NG_L2CAP_ALERT(
598 "%s: %s - ng_l2cap_prepend(%zd) failed\n",
599 __func__, NG_NODE_NAME(l2cap->node),
600 sizeof(*acl_hdr));
601 goto fail;
602 }
603
604 acl_hdr = mtod(m0, ng_hci_acldata_pkt_t *);
605 acl_hdr->type = NG_HCI_ACL_DATA_PKT;
606 acl_hdr->length = htole16(len);
607 acl_hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(
608 con->con_handle, flag, 0));
609
610 /* Add fragment to the chain */
611 m0->m_nextpkt = NULL;
612
613 if (con->tx_pkt == NULL)
614 con->tx_pkt = m_last = m0;
615 else {
616 m_last->m_nextpkt = m0;
617 m_last = m0;
618 }
619
620 NG_L2CAP_INFO(
621 "%s: %s - attaching ACL packet, con_handle=%d, PB=%#x, length=%d\n",
622 __func__, NG_NODE_NAME(l2cap->node), con->con_handle,
623 flag, len);
624
625 m0 = m;
626 m = NULL;
627 flag = NG_HCI_PACKET_FRAGMENT;
628 }
629
630 return (0);
631 fail:
632 NG_FREE_M(m0);
633 NG_FREE_M(m);
634
635 while (con->tx_pkt != NULL) {
636 m = con->tx_pkt->m_nextpkt;
637 m_freem(con->tx_pkt);
638 con->tx_pkt = m;
639 }
640
641 return (ENOBUFS);
642 } /* ng_l2cap_lp_send */
643
644 /*
645 * Receive ACL data packet from the HCI layer. First strip ACL packet header
646 * and get connection handle, PB (Packet Boundary) flag and payload length.
647 * Then find connection descriptor and verify its state. Then process ACL
648 * packet as follows.
649 *
650 * 1) If we got first segment (pb == NG_HCI_PACKET_START) then extract L2CAP
651 * header and get total length of the L2CAP packet. Then start new L2CAP
652 * packet.
653 *
654 * 2) If we got other (then first :) segment (pb == NG_HCI_PACKET_FRAGMENT)
655 * then add segment to the packet.
656 */
657
658 int
ng_l2cap_lp_receive(ng_l2cap_p l2cap,struct mbuf * m)659 ng_l2cap_lp_receive(ng_l2cap_p l2cap, struct mbuf *m)
660 {
661 ng_hci_acldata_pkt_t *acl_hdr = NULL;
662 ng_l2cap_hdr_t *l2cap_hdr = NULL;
663 ng_l2cap_con_p con = NULL;
664 u_int16_t con_handle, length, pb;
665 int error = 0;
666
667 /* Check ACL data packet */
668 if (m->m_pkthdr.len < sizeof(*acl_hdr)) {
669 NG_L2CAP_ERR(
670 "%s: %s - invalid ACL data packet. Packet too small, length=%d\n",
671 __func__, NG_NODE_NAME(l2cap->node), m->m_pkthdr.len);
672 error = EMSGSIZE;
673 goto drop;
674 }
675
676 /* Strip ACL data packet header */
677 NG_L2CAP_M_PULLUP(m, sizeof(*acl_hdr));
678 if (m == NULL)
679 return (ENOBUFS);
680
681 acl_hdr = mtod(m, ng_hci_acldata_pkt_t *);
682 m_adj(m, sizeof(*acl_hdr));
683
684 /* Get ACL connection handle, PB flag and payload length */
685 acl_hdr->con_handle = le16toh(acl_hdr->con_handle);
686 con_handle = NG_HCI_CON_HANDLE(acl_hdr->con_handle);
687 pb = NG_HCI_PB_FLAG(acl_hdr->con_handle);
688 length = le16toh(acl_hdr->length);
689
690 NG_L2CAP_INFO(
691 "%s: %s - got ACL data packet, con_handle=%d, PB=%#x, length=%d\n",
692 __func__, NG_NODE_NAME(l2cap->node), con_handle, pb, length);
693
694 /* Get connection descriptor */
695 con = ng_l2cap_con_by_handle(l2cap, con_handle);
696 if (con == NULL) {
697 NG_L2CAP_ERR(
698 "%s: %s - unexpected ACL data packet. " \
699 "Connection does not exist, con_handle=%d\n",
700 __func__, NG_NODE_NAME(l2cap->node), con_handle);
701 error = ENOENT;
702 goto drop;
703 }
704
705 /* Verify connection state */
706 if (con->state != NG_L2CAP_CON_OPEN) {
707 NG_L2CAP_ERR(
708 "%s: %s - unexpected ACL data packet. Invalid connection state=%d\n",
709 __func__, NG_NODE_NAME(l2cap->node), con->state);
710 error = EHOSTDOWN;
711 goto drop;
712 }
713
714 /* Process packet */
715 if ((pb == NG_HCI_PACKET_START) || (pb == NG_HCI_LE_PACKET_START))
716 {
717 if (con->rx_pkt != NULL) {
718 NG_L2CAP_ERR(
719 "%s: %s - dropping incomplete L2CAP packet, got %d bytes, want %d bytes\n",
720 __func__, NG_NODE_NAME(l2cap->node),
721 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
722 NG_FREE_M(con->rx_pkt);
723 con->rx_pkt_len = 0;
724 }
725
726 /* Get L2CAP header */
727 if (m->m_pkthdr.len < sizeof(*l2cap_hdr)) {
728 NG_L2CAP_ERR(
729 "%s: %s - invalid L2CAP packet start fragment. Packet too small, length=%d\n",
730 __func__, NG_NODE_NAME(l2cap->node),
731 m->m_pkthdr.len);
732 error = EMSGSIZE;
733 goto drop;
734 }
735
736 NG_L2CAP_M_PULLUP(m, sizeof(*l2cap_hdr));
737 if (m == NULL)
738 return (ENOBUFS);
739
740 l2cap_hdr = mtod(m, ng_l2cap_hdr_t *);
741
742 NG_L2CAP_INFO(
743 "%s: %s - staring new L2CAP packet, con_handle=%d, length=%d\n",
744 __func__, NG_NODE_NAME(l2cap->node), con_handle,
745 le16toh(l2cap_hdr->length));
746
747 /* Start new L2CAP packet */
748 con->rx_pkt = m;
749 con->rx_pkt_len = le16toh(l2cap_hdr->length)+sizeof(*l2cap_hdr);
750 } else if (pb == NG_HCI_PACKET_FRAGMENT) {
751 if (con->rx_pkt == NULL) {
752 NG_L2CAP_ERR(
753 "%s: %s - unexpected ACL data packet fragment, con_handle=%d\n",
754 __func__, NG_NODE_NAME(l2cap->node),
755 con->con_handle);
756 goto drop;
757 }
758
759 /* Add fragment to the L2CAP packet */
760 m_cat(con->rx_pkt, m);
761 con->rx_pkt->m_pkthdr.len += length;
762 } else {
763 NG_L2CAP_ERR(
764 "%s: %s - invalid ACL data packet. Invalid PB flag=%#x\n",
765 __func__, NG_NODE_NAME(l2cap->node), pb);
766 error = EINVAL;
767 goto drop;
768 }
769
770 con->rx_pkt_len -= length;
771 if (con->rx_pkt_len < 0) {
772 NG_L2CAP_ALERT(
773 "%s: %s - packet length mismatch. Got %d bytes, offset %d bytes\n",
774 __func__, NG_NODE_NAME(l2cap->node),
775 con->rx_pkt->m_pkthdr.len, con->rx_pkt_len);
776 NG_FREE_M(con->rx_pkt);
777 con->rx_pkt_len = 0;
778 } else if (con->rx_pkt_len == 0) {
779 /* OK, we have got complete L2CAP packet, so process it */
780 error = ng_l2cap_receive(con);
781 con->rx_pkt = NULL;
782 con->rx_pkt_len = 0;
783 }
784
785 return (error);
786
787 drop:
788 NG_FREE_M(m);
789
790 return (error);
791 } /* ng_l2cap_lp_receive */
792
793 /*
794 * Send queued ACL packets to the HCI layer
795 */
796
797 void
ng_l2cap_lp_deliver(ng_l2cap_con_p con)798 ng_l2cap_lp_deliver(ng_l2cap_con_p con)
799 {
800 ng_l2cap_p l2cap = con->l2cap;
801 struct mbuf *m = NULL;
802 int error;
803
804 /* Check connection */
805 if (con->state != NG_L2CAP_CON_OPEN)
806 return;
807
808 if (con->tx_pkt == NULL)
809 ng_l2cap_con_wakeup(con);
810
811 if (con->tx_pkt == NULL)
812 return;
813
814 /* Check if lower layer protocol is still connected */
815 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
816 NG_L2CAP_ERR(
817 "%s: %s - hook \"%s\" is not connected or valid",
818 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
819
820 goto drop; /* XXX what to do with "pending"? */
821 }
822
823 /* Send ACL data packets */
824 while (con->pending < con->l2cap->num_pkts && con->tx_pkt != NULL) {
825 m = con->tx_pkt;
826 con->tx_pkt = con->tx_pkt->m_nextpkt;
827 m->m_nextpkt = NULL;
828
829 if(m->m_flags &M_PROTO2){
830 ng_l2cap_lp_receive(con->l2cap, m);
831 continue;
832 }
833 NG_L2CAP_INFO(
834 "%s: %s - sending ACL packet, con_handle=%d, len=%d\n",
835 __func__, NG_NODE_NAME(l2cap->node), con->con_handle,
836 m->m_pkthdr.len);
837
838 NG_SEND_DATA_ONLY(error, l2cap->hci, m);
839 if (error != 0) {
840 NG_L2CAP_ERR(
841 "%s: %s - could not send ACL data packet, con_handle=%d, error=%d\n",
842 __func__, NG_NODE_NAME(l2cap->node),
843 con->con_handle, error);
844
845 goto drop; /* XXX what to do with "pending"? */
846 }
847
848 con->pending ++;
849 }
850
851 NG_L2CAP_INFO(
852 "%s: %s - %d ACL packets have been sent, con_handle=%d\n",
853 __func__, NG_NODE_NAME(l2cap->node), con->pending,
854 con->con_handle);
855
856 return;
857
858 drop:
859 while (con->tx_pkt != NULL) {
860 m = con->tx_pkt->m_nextpkt;
861 m_freem(con->tx_pkt);
862 con->tx_pkt = m;
863 }
864 } /* ng_l2cap_lp_deliver */
865
866 /*
867 * Process connection timeout. Remove connection from the list. If there
868 * are any channels that wait for the connection then notify them. Free
869 * connection descriptor.
870 */
871
872 void
ng_l2cap_process_lp_timeout(node_p node,hook_p hook,void * arg1,int con_handle)873 ng_l2cap_process_lp_timeout(node_p node, hook_p hook, void *arg1, int con_handle)
874 {
875 ng_l2cap_p l2cap = NULL;
876 ng_l2cap_con_p con = NULL;
877
878 if (NG_NODE_NOT_VALID(node)) {
879 printf("%s: Netgraph node is not valid\n", __func__);
880 return;
881 }
882
883 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
884 con = ng_l2cap_con_by_handle(l2cap, con_handle);
885
886 if (con == NULL) {
887 NG_L2CAP_ALERT(
888 "%s: %s - could not find connection, con_handle=%d\n",
889 __func__, NG_NODE_NAME(node), con_handle);
890 return;
891 }
892
893 if (!(con->flags & NG_L2CAP_CON_LP_TIMO)) {
894 NG_L2CAP_ALERT(
895 "%s: %s - no pending LP timeout, con_handle=%d, state=%d, flags=%#x\n",
896 __func__, NG_NODE_NAME(node), con_handle, con->state,
897 con->flags);
898 return;
899 }
900
901 /*
902 * Notify channels that connection has timed out. This will remove
903 * connection, channels and pending commands.
904 */
905
906 con->flags &= ~NG_L2CAP_CON_LP_TIMO;
907 ng_l2cap_con_fail(con, NG_L2CAP_TIMEOUT);
908 } /* ng_l2cap_process_lp_timeout */
909
910 /*
911 * Process auto disconnect timeout and send LP_DisconReq event to the
912 * lower layer protocol
913 */
914
915 void
ng_l2cap_process_discon_timeout(node_p node,hook_p hook,void * arg1,int con_handle)916 ng_l2cap_process_discon_timeout(node_p node, hook_p hook, void *arg1, int con_handle)
917 {
918 ng_l2cap_p l2cap = NULL;
919 ng_l2cap_con_p con = NULL;
920 struct ng_mesg *msg = NULL;
921 ng_hci_lp_discon_req_ep *ep = NULL;
922 int error;
923
924 if (NG_NODE_NOT_VALID(node)) {
925 printf("%s: Netgraph node is not valid\n", __func__);
926 return;
927 }
928
929 l2cap = (ng_l2cap_p) NG_NODE_PRIVATE(node);
930 con = ng_l2cap_con_by_handle(l2cap, con_handle);
931
932 if (con == NULL) {
933 NG_L2CAP_ALERT(
934 "%s: %s - could not find connection, con_handle=%d\n",
935 __func__, NG_NODE_NAME(node), con_handle);
936 return;
937 }
938
939 if (!(con->flags & NG_L2CAP_CON_AUTO_DISCON_TIMO)) {
940 NG_L2CAP_ALERT(
941 "%s: %s - no pending disconnect timeout, con_handle=%d, state=%d, flags=%#x\n",
942 __func__, NG_NODE_NAME(node), con_handle, con->state,
943 con->flags);
944 return;
945 }
946
947 con->flags &= ~NG_L2CAP_CON_AUTO_DISCON_TIMO;
948
949 /* Check if lower layer protocol is still connected */
950 if (l2cap->hci == NULL || NG_HOOK_NOT_VALID(l2cap->hci)) {
951 NG_L2CAP_ERR(
952 "%s: %s - hook \"%s\" is not connected or valid\n",
953 __func__, NG_NODE_NAME(l2cap->node), NG_L2CAP_HOOK_HCI);
954 return;
955 }
956
957 /* Create and send LP_DisconReq event */
958 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ,
959 sizeof(*ep), M_NOWAIT);
960 if (msg == NULL)
961 return;
962
963 ep = (ng_hci_lp_discon_req_ep *) (msg->data);
964 ep->con_handle = con->con_handle;
965 ep->reason = 0x13; /* User Ended Connection */
966
967 NG_SEND_MSG_HOOK(error, l2cap->node, msg, l2cap->hci, 0);
968 } /* ng_l2cap_process_discon_timeout */
969