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