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