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