xref: /freebsd/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c (revision 59e2ff550c448126b988150ce800cdf73bb5103e)
1 /*
2  * ng_btsocket_l2cap.c
3  */
4 
5 /*-
6  * Copyright (c) 2001-2002 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_btsocket_l2cap.c,v 1.16 2003/09/14 23:29:06 max Exp $
31  * $FreeBSD$
32  */
33 
34 #include <sys/param.h>
35 #include <sys/systm.h>
36 #include <sys/bitstring.h>
37 #include <sys/domain.h>
38 #include <sys/endian.h>
39 #include <sys/errno.h>
40 #include <sys/filedesc.h>
41 #include <sys/ioccom.h>
42 #include <sys/kernel.h>
43 #include <sys/lock.h>
44 #include <sys/malloc.h>
45 #include <sys/mbuf.h>
46 #include <sys/mutex.h>
47 #include <sys/protosw.h>
48 #include <sys/queue.h>
49 #include <sys/socket.h>
50 #include <sys/socketvar.h>
51 #include <sys/sysctl.h>
52 #include <sys/taskqueue.h>
53 
54 #include <net/vnet.h>
55 
56 #include <netgraph/ng_message.h>
57 #include <netgraph/netgraph.h>
58 #include <netgraph/bluetooth/include/ng_bluetooth.h>
59 #include <netgraph/bluetooth/include/ng_hci.h>
60 #include <netgraph/bluetooth/include/ng_l2cap.h>
61 #include <netgraph/bluetooth/include/ng_btsocket.h>
62 #include <netgraph/bluetooth/include/ng_btsocket_l2cap.h>
63 
64 /* MALLOC define */
65 #ifdef NG_SEPARATE_MALLOC
66 static MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_L2CAP, "netgraph_btsocks_l2cap",
67 		"Netgraph Bluetooth L2CAP sockets");
68 #else
69 #define M_NETGRAPH_BTSOCKET_L2CAP M_NETGRAPH
70 #endif /* NG_SEPARATE_MALLOC */
71 
72 /* Netgraph node methods */
73 static ng_constructor_t	ng_btsocket_l2cap_node_constructor;
74 static ng_rcvmsg_t	ng_btsocket_l2cap_node_rcvmsg;
75 static ng_shutdown_t	ng_btsocket_l2cap_node_shutdown;
76 static ng_newhook_t	ng_btsocket_l2cap_node_newhook;
77 static ng_connect_t	ng_btsocket_l2cap_node_connect;
78 static ng_rcvdata_t	ng_btsocket_l2cap_node_rcvdata;
79 static ng_disconnect_t	ng_btsocket_l2cap_node_disconnect;
80 
81 static void		ng_btsocket_l2cap_input   (void *, int);
82 static void		ng_btsocket_l2cap_rtclean (void *, int);
83 
84 /* Netgraph type descriptor */
85 static struct ng_type	typestruct = {
86 	.version =	NG_ABI_VERSION,
87 	.name =		NG_BTSOCKET_L2CAP_NODE_TYPE,
88 	.constructor =	ng_btsocket_l2cap_node_constructor,
89 	.rcvmsg =	ng_btsocket_l2cap_node_rcvmsg,
90 	.shutdown =	ng_btsocket_l2cap_node_shutdown,
91 	.newhook =	ng_btsocket_l2cap_node_newhook,
92 	.connect =	ng_btsocket_l2cap_node_connect,
93 	.rcvdata =	ng_btsocket_l2cap_node_rcvdata,
94 	.disconnect =	ng_btsocket_l2cap_node_disconnect,
95 };
96 
97 /* Globals */
98 extern int					ifqmaxlen;
99 static u_int32_t				ng_btsocket_l2cap_debug_level;
100 static node_p					ng_btsocket_l2cap_node;
101 static struct ng_bt_itemq			ng_btsocket_l2cap_queue;
102 static struct mtx				ng_btsocket_l2cap_queue_mtx;
103 static struct task				ng_btsocket_l2cap_queue_task;
104 static LIST_HEAD(, ng_btsocket_l2cap_pcb)	ng_btsocket_l2cap_sockets;
105 static struct mtx				ng_btsocket_l2cap_sockets_mtx;
106 static LIST_HEAD(, ng_btsocket_l2cap_rtentry)	ng_btsocket_l2cap_rt;
107 static struct mtx				ng_btsocket_l2cap_rt_mtx;
108 static struct task				ng_btsocket_l2cap_rt_task;
109 static struct timeval				ng_btsocket_l2cap_lasttime;
110 static int					ng_btsocket_l2cap_curpps;
111 
112 /* Sysctl tree */
113 SYSCTL_DECL(_net_bluetooth_l2cap_sockets);
114 static SYSCTL_NODE(_net_bluetooth_l2cap_sockets, OID_AUTO, seq, CTLFLAG_RW,
115 	0, "Bluetooth SEQPACKET L2CAP sockets family");
116 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, debug_level,
117 	CTLFLAG_RW,
118 	&ng_btsocket_l2cap_debug_level, NG_BTSOCKET_WARN_LEVEL,
119 	"Bluetooth SEQPACKET L2CAP sockets debug level");
120 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_len,
121 	CTLFLAG_RD,
122 	&ng_btsocket_l2cap_queue.len, 0,
123 	"Bluetooth SEQPACKET L2CAP sockets input queue length");
124 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_maxlen,
125 	CTLFLAG_RD,
126 	&ng_btsocket_l2cap_queue.maxlen, 0,
127 	"Bluetooth SEQPACKET L2CAP sockets input queue max. length");
128 SYSCTL_UINT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_drops,
129 	CTLFLAG_RD,
130 	&ng_btsocket_l2cap_queue.drops, 0,
131 	"Bluetooth SEQPACKET L2CAP sockets input queue drops");
132 
133 /* Debug */
134 #define NG_BTSOCKET_L2CAP_INFO \
135 	if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_INFO_LEVEL && \
136 	    ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
137 		printf
138 
139 #define NG_BTSOCKET_L2CAP_WARN \
140 	if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_WARN_LEVEL && \
141 	    ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
142 		printf
143 
144 #define NG_BTSOCKET_L2CAP_ERR \
145 	if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_ERR_LEVEL && \
146 	    ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
147 		printf
148 
149 #define NG_BTSOCKET_L2CAP_ALERT \
150 	if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_ALERT_LEVEL && \
151 	    ppsratecheck(&ng_btsocket_l2cap_lasttime, &ng_btsocket_l2cap_curpps, 1)) \
152 		printf
153 
154 /*
155  * Netgraph message processing routines
156  */
157 
158 static int ng_btsocket_l2cap_process_l2ca_con_req_rsp
159 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
160 static int ng_btsocket_l2cap_process_l2ca_con_rsp_rsp
161 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
162 static int ng_btsocket_l2cap_process_l2ca_con_ind
163 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
164 
165 static int ng_btsocket_l2cap_process_l2ca_cfg_req_rsp
166 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
167 static int ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp
168 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
169 static int ng_btsocket_l2cap_process_l2ca_cfg_ind
170 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
171 
172 static int ng_btsocket_l2cap_process_l2ca_discon_rsp
173 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
174 static int ng_btsocket_l2cap_process_l2ca_discon_ind
175 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
176 
177 static int ng_btsocket_l2cap_process_l2ca_write_rsp
178 	(struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
179 
180 /*
181  * Send L2CA_xxx messages to the lower layer
182  */
183 
184 static int  ng_btsocket_l2cap_send_l2ca_con_req
185 	(ng_btsocket_l2cap_pcb_p);
186 static int  ng_btsocket_l2cap_send_l2ca_con_rsp_req
187 	(u_int32_t, ng_btsocket_l2cap_rtentry_p, bdaddr_p, int, int, int, int);
188 static int  ng_btsocket_l2cap_send_l2ca_cfg_req
189 	(ng_btsocket_l2cap_pcb_p);
190 static int  ng_btsocket_l2cap_send_l2ca_cfg_rsp
191 	(ng_btsocket_l2cap_pcb_p);
192 static int  ng_btsocket_l2cap_send_l2ca_discon_req
193 	(u_int32_t, ng_btsocket_l2cap_pcb_p);
194 
195 static int ng_btsocket_l2cap_send2
196 	(ng_btsocket_l2cap_pcb_p);
197 
198 /*
199  * Timeout processing routines
200  */
201 
202 static void ng_btsocket_l2cap_timeout         (ng_btsocket_l2cap_pcb_p);
203 static void ng_btsocket_l2cap_untimeout       (ng_btsocket_l2cap_pcb_p);
204 static void ng_btsocket_l2cap_process_timeout (void *);
205 
206 /*
207  * Other stuff
208  */
209 
210 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_addr(bdaddr_p, int);
211 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_token(u_int32_t);
212 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_cid (bdaddr_p, int,int);
213 static int                         ng_btsocket_l2cap_result2errno(int);
214 
215 static int ng_btsock_l2cap_addrtype_to_linktype(int addrtype);
216 static int ng_btsock_l2cap_pcb_to_idtype(struct ng_btsocket_l2cap_pcb *);
217 #define ng_btsocket_l2cap_wakeup_input_task() \
218 	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_queue_task)
219 
220 #define ng_btsocket_l2cap_wakeup_route_task() \
221 	taskqueue_enqueue(taskqueue_swi_giant, &ng_btsocket_l2cap_rt_task)
222 
223 
224 int ng_btsock_l2cap_pcb_to_idtype(struct ng_btsocket_l2cap_pcb *pcb)
225 {
226 	if(pcb->dsttype == BDADDR_BREDR){
227 		return NG_L2CAP_L2CA_IDTYPE_BREDR;
228 	}else if(pcb->psm == 0){
229 		return NG_L2CAP_L2CA_IDTYPE_ATT;
230 	}else{
231 		return NG_L2CAP_L2CA_IDTYPE_LE;
232 	}
233 }
234 
235 int ng_btsock_l2cap_addrtype_to_linktype(int addrtype)
236 {
237 	switch(addrtype){
238 	case BDADDR_LE_PUBLIC:
239 		return NG_HCI_LINK_LE_PUBLIC;
240 	case BDADDR_LE_RANDOM:
241 		return NG_HCI_LINK_LE_RANDOM;
242 	default:
243 		return NG_HCI_LINK_ACL;
244 	}
245 }
246 
247 
248 /*****************************************************************************
249  *****************************************************************************
250  **                        Netgraph node interface
251  *****************************************************************************
252  *****************************************************************************/
253 
254 /*
255  * Netgraph node constructor. Do not allow to create node of this type.
256  */
257 
258 static int
259 ng_btsocket_l2cap_node_constructor(node_p node)
260 {
261 	return (EINVAL);
262 } /* ng_btsocket_l2cap_node_constructor */
263 
264 /*
265  * Do local shutdown processing. Let old node go and create new fresh one.
266  */
267 
268 static int
269 ng_btsocket_l2cap_node_shutdown(node_p node)
270 {
271 	int	error = 0;
272 
273 	NG_NODE_UNREF(node);
274 
275 	/* Create new node */
276 	error = ng_make_node_common(&typestruct, &ng_btsocket_l2cap_node);
277 	if (error != 0) {
278 		NG_BTSOCKET_L2CAP_ALERT(
279 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
280 
281 		ng_btsocket_l2cap_node = NULL;
282 
283 		return (error);
284 	}
285 
286 	error = ng_name_node(ng_btsocket_l2cap_node,
287 				NG_BTSOCKET_L2CAP_NODE_TYPE);
288 	if (error != 0) {
289 		NG_BTSOCKET_L2CAP_ALERT(
290 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
291 
292 		NG_NODE_UNREF(ng_btsocket_l2cap_node);
293 		ng_btsocket_l2cap_node = NULL;
294 
295 		return (error);
296 	}
297 
298 	return (0);
299 } /* ng_btsocket_l2cap_node_shutdown */
300 
301 /*
302  * We allow any hook to be connected to the node.
303  */
304 
305 static int
306 ng_btsocket_l2cap_node_newhook(node_p node, hook_p hook, char const *name)
307 {
308 	return (0);
309 } /* ng_btsocket_l2cap_node_newhook */
310 
311 /*
312  * Just say "YEP, that's OK by me!"
313  */
314 
315 static int
316 ng_btsocket_l2cap_node_connect(hook_p hook)
317 {
318 	NG_HOOK_SET_PRIVATE(hook, NULL);
319 	NG_HOOK_REF(hook); /* Keep extra reference to the hook */
320 
321 #if 0
322 	NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
323 	NG_HOOK_FORCE_QUEUE(hook);
324 #endif
325 
326 	return (0);
327 } /* ng_btsocket_l2cap_node_connect */
328 
329 /*
330  * Hook disconnection. Schedule route cleanup task
331  */
332 
333 static int
334 ng_btsocket_l2cap_node_disconnect(hook_p hook)
335 {
336 	/*
337 	 * If hook has private information than we must have this hook in
338 	 * the routing table and must schedule cleaning for the routing table.
339 	 * Otherwise hook was connected but we never got "hook_info" message,
340 	 * so we have never added this hook to the routing table and it save
341 	 * to just delete it.
342 	 */
343 
344 	if (NG_HOOK_PRIVATE(hook) != NULL)
345 		return (ng_btsocket_l2cap_wakeup_route_task());
346 
347 	NG_HOOK_UNREF(hook); /* Remove extra reference */
348 
349 	return (0);
350 } /* ng_btsocket_l2cap_node_disconnect */
351 
352 /*
353  * Process incoming messages
354  */
355 
356 static int
357 ng_btsocket_l2cap_node_rcvmsg(node_p node, item_p item, hook_p hook)
358 {
359 	struct ng_mesg	*msg = NGI_MSG(item); /* item still has message */
360 	int		 error = 0;
361 
362 	if (msg != NULL && msg->header.typecookie == NGM_L2CAP_COOKIE) {
363 		mtx_lock(&ng_btsocket_l2cap_queue_mtx);
364 		if (NG_BT_ITEMQ_FULL(&ng_btsocket_l2cap_queue)) {
365 			NG_BTSOCKET_L2CAP_ERR(
366 "%s: Input queue is full (msg)\n", __func__);
367 
368 			NG_BT_ITEMQ_DROP(&ng_btsocket_l2cap_queue);
369 			NG_FREE_ITEM(item);
370 			error = ENOBUFS;
371 		} else {
372 			if (hook != NULL) {
373 				NG_HOOK_REF(hook);
374 				NGI_SET_HOOK(item, hook);
375 			}
376 
377 			NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
378 			error = ng_btsocket_l2cap_wakeup_input_task();
379 		}
380 		mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
381 	} else {
382 		NG_FREE_ITEM(item);
383 		error = EINVAL;
384 	}
385 
386 	return (error);
387 } /* ng_btsocket_l2cap_node_rcvmsg */
388 
389 /*
390  * Receive data on a hook
391  */
392 
393 static int
394 ng_btsocket_l2cap_node_rcvdata(hook_p hook, item_p item)
395 {
396 	int	error = 0;
397 
398 	mtx_lock(&ng_btsocket_l2cap_queue_mtx);
399 	if (NG_BT_ITEMQ_FULL(&ng_btsocket_l2cap_queue)) {
400 		NG_BTSOCKET_L2CAP_ERR(
401 "%s: Input queue is full (data)\n", __func__);
402 
403 		NG_BT_ITEMQ_DROP(&ng_btsocket_l2cap_queue);
404 		NG_FREE_ITEM(item);
405 		error = ENOBUFS;
406 	} else {
407 		NG_HOOK_REF(hook);
408 		NGI_SET_HOOK(item, hook);
409 
410 		NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
411 		error = ng_btsocket_l2cap_wakeup_input_task();
412 	}
413 	mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
414 
415 	return (error);
416 } /* ng_btsocket_l2cap_node_rcvdata */
417 
418 /*
419  * Process L2CA_Connect respose. Socket layer must have initiated connection,
420  * so we have to have a socket associated with message token.
421  */
422 
423 static int
424 ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
425 		ng_btsocket_l2cap_rtentry_p rt)
426 {
427 	ng_l2cap_l2ca_con_op	*op = NULL;
428 	ng_btsocket_l2cap_pcb_t	*pcb = NULL;
429 	int			 error = 0;
430 
431 	if (msg->header.arglen != sizeof(*op))
432 		return (EMSGSIZE);
433 
434 	op = (ng_l2cap_l2ca_con_op *)(msg->data);
435 
436 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
437 
438 	/* Look for the socket with the token */
439 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
440 	if (pcb == NULL) {
441 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
442 		return (ENOENT);
443 	}
444 
445 	mtx_lock(&pcb->pcb_mtx);
446 
447 	NG_BTSOCKET_L2CAP_INFO(
448 "%s: Got L2CA_Connect response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
449 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, status=%d, " \
450 "state=%d\n",	__func__, msg->header.token,
451 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
452 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
453 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
454 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
455 		pcb->psm, op->lcid, op->result, op->status,
456 		pcb->state);
457 
458 	if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
459 		mtx_unlock(&pcb->pcb_mtx);
460 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
461 
462 		return (ENOENT);
463 	}
464 
465 	ng_btsocket_l2cap_untimeout(pcb);
466 
467 	if (op->result == NG_L2CAP_PENDING) {
468 		ng_btsocket_l2cap_timeout(pcb);
469 		mtx_unlock(&pcb->pcb_mtx);
470 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
471 
472 		return (0);
473 	}
474 
475 	if (op->result == NG_L2CAP_SUCCESS){
476 		if(ng_btsock_l2cap_pcb_to_idtype(pcb) ==
477 		   NG_L2CAP_L2CA_IDTYPE_ATT){
478 			pcb->state = NG_BTSOCKET_L2CAP_OPEN;
479 			soisconnected(pcb->so);
480 			pcb->cid = op->lcid;
481 		}else{
482 			/*
483 			 * Channel is now open, so update local channel ID and
484 			 * start configuration process. Source and destination
485 			 * addresses as well as route must be already set.
486 			 */
487 
488 			pcb->cid = op->lcid;
489 
490 			error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb);
491 			if (error != 0) {
492 				/* Send disconnect request with "zero" token */
493 				ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
494 
495 				/* ... and close the socket */
496 				pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
497 				soisdisconnected(pcb->so);
498 			} else {
499 				pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT;
500 				pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING;
501 
502 				ng_btsocket_l2cap_timeout(pcb);
503 			}
504 		}
505 	} else {
506 		/*
507 		 * We have failed to open connection, so convert result
508 		 * code to "errno" code and disconnect the socket. Channel
509 		 * already has been closed.
510 		 */
511 
512 		pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
513 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
514 		soisdisconnected(pcb->so);
515 	}
516 
517 	mtx_unlock(&pcb->pcb_mtx);
518 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
519 
520 	return (error);
521 } /* ng_btsocket_l2cap_process_l2ca_con_req_rsp */
522 
523 /*
524  * Process L2CA_ConnectRsp response
525  */
526 
527 static int
528 ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
529 		ng_btsocket_l2cap_rtentry_p rt)
530 {
531 	ng_l2cap_l2ca_con_rsp_op	*op = NULL;
532 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
533 
534 	if (msg->header.arglen != sizeof(*op))
535 		return (EMSGSIZE);
536 
537 	op = (ng_l2cap_l2ca_con_rsp_op *)(msg->data);
538 
539 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
540 
541 	/* Look for the socket with the token */
542 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
543 	if (pcb == NULL) {
544 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
545 		return (ENOENT);
546 	}
547 
548 	mtx_lock(&pcb->pcb_mtx);
549 
550 	NG_BTSOCKET_L2CAP_INFO(
551 "%s: Got L2CA_ConnectRsp response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
552 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d\n",
553 		__func__, msg->header.token,
554 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
555 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
556 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
557 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
558 		pcb->psm, pcb->cid, op->result, pcb->state);
559 
560 	if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
561 		mtx_unlock(&pcb->pcb_mtx);
562 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
563 
564 		return (ENOENT);
565 	}
566 
567 	ng_btsocket_l2cap_untimeout(pcb);
568 
569 	/* Check the result and disconnect the socket on failure */
570 	if (op->result != NG_L2CAP_SUCCESS) {
571 		/* Close the socket - channel already closed */
572 		pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
573 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
574 		soisdisconnected(pcb->so);
575 	} else {
576 		/* Move to CONFIGURING state and wait for CONFIG_IND */
577 		pcb->cfg_state = 0;
578 		pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING;
579 		ng_btsocket_l2cap_timeout(pcb);
580 	}
581 
582 	mtx_unlock(&pcb->pcb_mtx);
583 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
584 
585 	return (0);
586 } /* ng_btsocket_process_l2ca_con_rsp_rsp */
587 
588 /*
589  * Process L2CA_Connect indicator. Find socket that listens on address
590  * and PSM. Find exact or closest match. Create new socket and initiate
591  * connection.
592  */
593 
594 static int
595 ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg *msg,
596 		ng_btsocket_l2cap_rtentry_p rt)
597 {
598 	ng_l2cap_l2ca_con_ind_ip	*ip = NULL;
599 	ng_btsocket_l2cap_pcb_t		*pcb = NULL, *pcb1 = NULL;
600 	int				 error = 0;
601 	u_int32_t			 token = 0;
602 	u_int16_t			 result = 0;
603 
604 	if (msg->header.arglen != sizeof(*ip))
605 		return (EMSGSIZE);
606 
607 	ip = (ng_l2cap_l2ca_con_ind_ip *)(msg->data);
608 
609 	NG_BTSOCKET_L2CAP_INFO(
610 "%s: Got L2CA_Connect indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
611 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, ident=%d\n",
612 		__func__,
613 		rt->src.b[5], rt->src.b[4], rt->src.b[3],
614 		rt->src.b[2], rt->src.b[1], rt->src.b[0],
615 		ip->bdaddr.b[5], ip->bdaddr.b[4], ip->bdaddr.b[3],
616 		ip->bdaddr.b[2], ip->bdaddr.b[1], ip->bdaddr.b[0],
617 		ip->psm, ip->lcid, ip->ident);
618 
619 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
620 
621 	pcb = ng_btsocket_l2cap_pcb_by_addr(&rt->src, ip->psm);
622 	if (pcb != NULL) {
623 		struct socket	*so1 = NULL;
624 
625 		mtx_lock(&pcb->pcb_mtx);
626 
627 		/*
628 		 * First check the pending connections queue and if we have
629 		 * space then create new socket and set proper source address.
630 		 */
631 
632 		if (pcb->so->so_qlen <= pcb->so->so_qlimit) {
633 			CURVNET_SET(pcb->so->so_vnet);
634 			so1 = sonewconn(pcb->so, 0);
635 			CURVNET_RESTORE();
636 		}
637 
638 		if (so1 == NULL) {
639 			result = NG_L2CAP_NO_RESOURCES;
640 			goto respond;
641 		}
642 
643 		/*
644 		 * If we got here than we have created new socket. So complete
645 		 * connection. If we we listening on specific address then copy
646 		 * source address from listening socket, otherwise copy source
647 		 * address from hook's routing information.
648 		 */
649 
650 		pcb1 = so2l2cap_pcb(so1);
651 		KASSERT((pcb1 != NULL),
652 ("%s: pcb1 == NULL\n", __func__));
653 
654  		mtx_lock(&pcb1->pcb_mtx);
655 
656 		if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)) != 0)
657 			bcopy(&pcb->src, &pcb1->src, sizeof(pcb1->src));
658 		else
659 			bcopy(&rt->src, &pcb1->src, sizeof(pcb1->src));
660 
661 		pcb1->flags &= ~NG_BTSOCKET_L2CAP_CLIENT;
662 
663 		bcopy(&ip->bdaddr, &pcb1->dst, sizeof(pcb1->dst));
664 		pcb1->psm = ip->psm;
665 		pcb1->cid = ip->lcid;
666 		pcb1->rt = rt;
667 
668 		/* Copy socket settings */
669 		pcb1->imtu = pcb->imtu;
670 		bcopy(&pcb->oflow, &pcb1->oflow, sizeof(pcb1->oflow));
671 		pcb1->flush_timo = pcb->flush_timo;
672 
673 		token = pcb1->token;
674 	} else
675 		/* Nobody listens on requested BDADDR/PSM */
676 		result = NG_L2CAP_PSM_NOT_SUPPORTED;
677 
678 respond:
679 	error = ng_btsocket_l2cap_send_l2ca_con_rsp_req(token, rt,
680 							&ip->bdaddr,
681 							ip->ident, ip->lcid,
682 							result,ip->linktype);
683 	if (pcb1 != NULL) {
684 		if (error != 0) {
685 			pcb1->so->so_error = error;
686 			pcb1->state = NG_BTSOCKET_L2CAP_CLOSED;
687 			soisdisconnected(pcb1->so);
688 		} else {
689 			pcb1->state = NG_BTSOCKET_L2CAP_CONNECTING;
690 			soisconnecting(pcb1->so);
691 
692 			ng_btsocket_l2cap_timeout(pcb1);
693 		}
694 
695 		mtx_unlock(&pcb1->pcb_mtx);
696 	}
697 
698 	if (pcb != NULL)
699 		mtx_unlock(&pcb->pcb_mtx);
700 
701 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
702 
703 	return (error);
704 } /* ng_btsocket_l2cap_process_l2ca_con_ind */
705 
706 /*
707  * Process L2CA_Config response
708  */
709 
710 static int
711 ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
712 		ng_btsocket_l2cap_rtentry_p rt)
713 {
714 	ng_l2cap_l2ca_cfg_op	*op = NULL;
715 	ng_btsocket_l2cap_pcb_p	 pcb = NULL;
716 
717 	if (msg->header.arglen != sizeof(*op))
718 		return (EMSGSIZE);
719 
720 	op = (ng_l2cap_l2ca_cfg_op *)(msg->data);
721 
722 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
723 
724 	/*
725 	 * Socket must have issued a Configure request, so we must have a
726 	 * socket that wants to be configured. Use Netgraph message token
727 	 * to find it
728 	 */
729 
730 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
731 	if (pcb == NULL) {
732 		/*
733 		 * XXX FIXME what to do here? We could not find a
734 		 * socket with requested token. We even can not send
735 		 * Disconnect, because we do not know channel ID
736 		 */
737 
738 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
739 		return (ENOENT);
740 	}
741 
742 	mtx_lock(&pcb->pcb_mtx);
743 
744         NG_BTSOCKET_L2CAP_INFO(
745 "%s: Got L2CA_Config response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
746 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d, " \
747 "cfg_state=%x\n",
748 		__func__, msg->header.token,
749 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
750 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
751 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
752 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
753 		pcb->psm, pcb->cid, op->result, pcb->state, pcb->cfg_state);
754 
755 	if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
756 		mtx_unlock(&pcb->pcb_mtx);
757 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
758 
759 		return (ENOENT);
760 	}
761 
762 	if (op->result == NG_L2CAP_SUCCESS) {
763 		/*
764 		 * XXX FIXME Actually set flush and link timeout.
765 		 * Set QoS here if required. Resolve conficts (flush_timo).
766 		 * Save incoming MTU (peer's outgoing MTU) and outgoing flow
767 		 * spec.
768 		 */
769 
770 		pcb->imtu = op->imtu;
771 		bcopy(&op->oflow, &pcb->oflow, sizeof(pcb->oflow));
772 		pcb->flush_timo = op->flush_timo;
773 
774 		/*
775 		 * We have configured incoming side, so record it and check
776 		 * if configuration is complete. If complete then mark socket
777 		 * as connected, otherwise wait for the peer.
778 		 */
779 
780 		pcb->cfg_state &= ~NG_BTSOCKET_L2CAP_CFG_IN_SENT;
781 		pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_IN;
782 
783 		if (pcb->cfg_state == NG_BTSOCKET_L2CAP_CFG_BOTH) {
784 			/* Configuration complete - mark socket as open */
785 			ng_btsocket_l2cap_untimeout(pcb);
786 			pcb->state = NG_BTSOCKET_L2CAP_OPEN;
787 			soisconnected(pcb->so);
788 		}
789 	} else {
790 		/*
791 		 * Something went wrong. Could be unacceptable parameters,
792 		 * reject or unknown option. That's too bad, but we will
793 		 * not negotiate. Send Disconnect and close the channel.
794 		 */
795 
796 		ng_btsocket_l2cap_untimeout(pcb);
797 
798 		switch (op->result) {
799 		case NG_L2CAP_UNACCEPTABLE_PARAMS:
800 		case NG_L2CAP_UNKNOWN_OPTION:
801 			pcb->so->so_error = EINVAL;
802 			break;
803 
804 		default:
805 			pcb->so->so_error = ECONNRESET;
806 			break;
807 		}
808 
809 		/* Send disconnect with "zero" token */
810 		ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
811 
812 		/* ... and close the socket */
813 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
814 		soisdisconnected(pcb->so);
815 	}
816 
817 	mtx_unlock(&pcb->pcb_mtx);
818 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
819 
820 	return (0);
821 } /* ng_btsocket_l2cap_process_l2ca_cfg_req_rsp */
822 
823 /*
824  * Process L2CA_ConfigRsp response
825  */
826 
827 static int
828 ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg *msg,
829 		ng_btsocket_l2cap_rtentry_p rt)
830 {
831 	ng_l2cap_l2ca_cfg_rsp_op	*op = NULL;
832 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
833 	int				 error = 0;
834 
835 	if (msg->header.arglen != sizeof(*op))
836 		return (EMSGSIZE);
837 
838 	op = (ng_l2cap_l2ca_cfg_rsp_op *)(msg->data);
839 
840 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
841 
842 	/* Look for the socket with the token */
843 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
844 	if (pcb == NULL) {
845 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
846 		return (ENOENT);
847 	}
848 
849 	mtx_lock(&pcb->pcb_mtx);
850 
851         NG_BTSOCKET_L2CAP_INFO(
852 "%s: Got L2CA_ConfigRsp response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
853 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d, " \
854 "cfg_state=%x\n",
855 		__func__, msg->header.token,
856 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
857 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
858 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
859 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
860 		pcb->psm, pcb->cid, op->result, pcb->state, pcb->cfg_state);
861 
862 	if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
863 		mtx_unlock(&pcb->pcb_mtx);
864 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
865 
866 		return (ENOENT);
867 	}
868 
869 	/* Check the result and disconnect socket of failure */
870 	if (op->result != NG_L2CAP_SUCCESS)
871 		goto disconnect;
872 
873 	/*
874 	 * Now we done with remote side configuration. Configure local
875 	 * side if we have not done it yet.
876 	 */
877 
878 	pcb->cfg_state &= ~NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
879 	pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT;
880 
881 	if (pcb->cfg_state == NG_BTSOCKET_L2CAP_CFG_BOTH) {
882 		/* Configuration complete - mask socket as open */
883 		ng_btsocket_l2cap_untimeout(pcb);
884 		pcb->state = NG_BTSOCKET_L2CAP_OPEN;
885 		soisconnected(pcb->so);
886 	} else {
887 		if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_IN_SENT)) {
888 			/* Send L2CA_Config request - incoming path */
889 			error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb);
890 			if (error != 0)
891 				goto disconnect;
892 
893 			pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_IN_SENT;
894 		}
895 	}
896 
897 	mtx_unlock(&pcb->pcb_mtx);
898 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
899 
900 	return (error);
901 
902 disconnect:
903 	ng_btsocket_l2cap_untimeout(pcb);
904 
905 	/* Send disconnect with "zero" token */
906 	ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
907 
908 	/* ... and close the socket */
909 	pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
910 	soisdisconnected(pcb->so);
911 
912 	mtx_unlock(&pcb->pcb_mtx);
913 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
914 
915 	return (error);
916 } /* ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp */
917 
918 /*
919  * Process L2CA_Config indicator
920  */
921 
922 static int
923 ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
924 		ng_btsocket_l2cap_rtentry_p rt)
925 {
926 	ng_l2cap_l2ca_cfg_ind_ip	*ip = NULL;
927 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
928 	int				 error = 0;
929 
930 	if (msg->header.arglen != sizeof(*ip))
931 		return (EMSGSIZE);
932 
933 	ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data);
934 
935 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
936 
937 	/* Check for the open socket that has given channel ID */
938 	pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid,
939 					   NG_L2CAP_L2CA_IDTYPE_BREDR);
940 	if (pcb == NULL) {
941 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
942 		return (ENOENT);
943 	}
944 
945 	mtx_lock(&pcb->pcb_mtx);
946 
947         NG_BTSOCKET_L2CAP_INFO(
948 "%s: Got L2CA_Config indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
949 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, state=%d, cfg_state=%x\n",
950 		__func__,
951 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
952 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
953 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
954 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
955 		pcb->psm, pcb->cid, pcb->state, pcb->cfg_state);
956 
957 	/* XXX FIXME re-configuration on open socket */
958  	if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
959 		mtx_unlock(&pcb->pcb_mtx);
960 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
961 
962 		return (ENOENT);
963 	}
964 
965 	/*
966 	 * XXX FIXME Actually set flush and link timeout. Set QoS here if
967 	 * required. Resolve conficts (flush_timo). Note outgoing MTU (peer's
968 	 * incoming MTU) and incoming flow spec.
969 	 */
970 
971 	pcb->omtu = ip->omtu;
972 	bcopy(&ip->iflow, &pcb->iflow, sizeof(pcb->iflow));
973 	pcb->flush_timo = ip->flush_timo;
974 
975 	/*
976 	 * Send L2CA_Config response to our peer and check for the errors,
977 	 * if any send disconnect to close the channel.
978 	 */
979 
980 	if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_OUT_SENT)) {
981 		error = ng_btsocket_l2cap_send_l2ca_cfg_rsp(pcb);
982 		if (error != 0) {
983 			ng_btsocket_l2cap_untimeout(pcb);
984 
985 			pcb->so->so_error = error;
986 
987 			/* Send disconnect with "zero" token */
988 			ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
989 
990 			/* ... and close the socket */
991 			pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
992 			soisdisconnected(pcb->so);
993 		} else
994 			pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
995 	}
996 
997 	mtx_unlock(&pcb->pcb_mtx);
998 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
999 
1000 	return (error);
1001 } /* ng_btsocket_l2cap_process_l2cap_cfg_ind */
1002 
1003 /*
1004  * Process L2CA_Disconnect response
1005  */
1006 
1007 static int
1008 ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
1009 		ng_btsocket_l2cap_rtentry_p rt)
1010 {
1011 	ng_l2cap_l2ca_discon_op	*op = NULL;
1012 	ng_btsocket_l2cap_pcb_t	*pcb = NULL;
1013 
1014 	/* Check message */
1015 	if (msg->header.arglen != sizeof(*op))
1016 		return (EMSGSIZE);
1017 
1018 	op = (ng_l2cap_l2ca_discon_op *)(msg->data);
1019 
1020 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1021 
1022 	/*
1023 	 * Socket layer must have issued L2CA_Disconnect request, so there
1024 	 * must be a socket that wants to be disconnected. Use Netgraph
1025 	 * message token to find it.
1026 	 */
1027 
1028 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
1029 	if (pcb == NULL) {
1030 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1031 		return (0);
1032 	}
1033 
1034 	mtx_lock(&pcb->pcb_mtx);
1035 
1036 	/* XXX Close socket no matter what op->result says */
1037 	if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
1038        		NG_BTSOCKET_L2CAP_INFO(
1039 "%s: Got L2CA_Disconnect response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1040 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d\n",
1041 			__func__, msg->header.token,
1042 			pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1043 			pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1044 			pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1045 			pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1046 			pcb->psm, pcb->cid, op->result, pcb->state);
1047 
1048 		ng_btsocket_l2cap_untimeout(pcb);
1049 
1050 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1051 		soisdisconnected(pcb->so);
1052 	}
1053 
1054 	mtx_unlock(&pcb->pcb_mtx);
1055 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1056 
1057 	return (0);
1058 } /* ng_btsocket_l2cap_process_l2ca_discon_rsp */
1059 
1060 /*
1061  * Process L2CA_Disconnect indicator
1062  */
1063 
1064 static int
1065 ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg,
1066 		ng_btsocket_l2cap_rtentry_p rt)
1067 {
1068 	ng_l2cap_l2ca_discon_ind_ip	*ip = NULL;
1069 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
1070 
1071 	/* Check message */
1072 	if (msg->header.arglen != sizeof(*ip))
1073 		return (EMSGSIZE);
1074 
1075 	ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data);
1076 
1077 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1078 
1079 	/* Look for the socket with given channel ID */
1080 	pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid,
1081 					   NG_L2CAP_L2CA_IDTYPE_BREDR);
1082 	if (pcb == NULL) {
1083 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1084 		return (0);
1085 	}
1086 
1087 	/*
1088 	 * Channel has already been destroyed, so disconnect the socket
1089 	 * and be done with it. If there was any pending request we can
1090 	 * not do anything here anyway.
1091 	 */
1092 
1093 	mtx_lock(&pcb->pcb_mtx);
1094 
1095        	NG_BTSOCKET_L2CAP_INFO(
1096 "%s: Got L2CA_Disconnect indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1097 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, state=%d\n",
1098 		__func__,
1099 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1100 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1101 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1102 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1103 		pcb->psm, pcb->cid, pcb->state);
1104 
1105 	if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
1106 		ng_btsocket_l2cap_untimeout(pcb);
1107 
1108 	pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1109 	soisdisconnected(pcb->so);
1110 
1111 	mtx_unlock(&pcb->pcb_mtx);
1112 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1113 
1114 	return (0);
1115 } /* ng_btsocket_l2cap_process_l2ca_discon_ind */
1116 
1117 /*
1118  * Process L2CA_Write response
1119  */
1120 
1121 static int
1122 ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg *msg,
1123 		ng_btsocket_l2cap_rtentry_p rt)
1124 {
1125 	ng_l2cap_l2ca_write_op	*op = NULL;
1126 	ng_btsocket_l2cap_pcb_t	*pcb = NULL;
1127 
1128 	/* Check message */
1129 	if (msg->header.arglen != sizeof(*op))
1130 		return (EMSGSIZE);
1131 
1132 	op = (ng_l2cap_l2ca_write_op *)(msg->data);
1133 
1134 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1135 
1136 	/* Look for the socket with given token */
1137 	pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
1138 	if (pcb == NULL) {
1139 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1140 		return (ENOENT);
1141 	}
1142 
1143 	mtx_lock(&pcb->pcb_mtx);
1144 
1145        	NG_BTSOCKET_L2CAP_INFO(
1146 "%s: Got L2CA_Write response, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1147 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, length=%d, " \
1148 "state=%d\n",		__func__,
1149 			pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1150 			pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1151 			pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1152 			pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1153 			pcb->psm, pcb->cid, op->result, op->length,
1154 			pcb->state);
1155 
1156 	if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
1157 		mtx_unlock(&pcb->pcb_mtx);
1158 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1159 
1160 		return (ENOENT);
1161 	}
1162 
1163 	ng_btsocket_l2cap_untimeout(pcb);
1164 
1165 	/*
1166  	 * Check if we have more data to send
1167  	 */
1168 	sbdroprecord(&pcb->so->so_snd);
1169 	if (sbavail(&pcb->so->so_snd) > 0) {
1170 		if (ng_btsocket_l2cap_send2(pcb) == 0)
1171 			ng_btsocket_l2cap_timeout(pcb);
1172 		else
1173 			sbdroprecord(&pcb->so->so_snd); /* XXX */
1174 	}
1175 
1176 	/*
1177 	 * Now set the result, drop packet from the socket send queue and
1178 	 * ask for more (wakeup sender)
1179 	 */
1180 
1181 	pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
1182 	sowwakeup(pcb->so);
1183 
1184 	mtx_unlock(&pcb->pcb_mtx);
1185 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1186 
1187 	return (0);
1188 } /* ng_btsocket_l2cap_process_l2ca_write_rsp */
1189 
1190 /*
1191  * Send L2CA_Connect request
1192  */
1193 
1194 static int
1195 ng_btsocket_l2cap_send_l2ca_con_req(ng_btsocket_l2cap_pcb_p pcb)
1196 {
1197 	struct ng_mesg		*msg = NULL;
1198 	ng_l2cap_l2ca_con_ip	*ip = NULL;
1199 	int			 error = 0;
1200 
1201 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1202 
1203 	if (pcb->rt == NULL ||
1204 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1205 		return (ENETDOWN);
1206 
1207 	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON,
1208 		sizeof(*ip), M_NOWAIT);
1209 	if (msg == NULL)
1210 		return (ENOMEM);
1211 
1212 	msg->header.token = pcb->token;
1213 
1214 	ip = (ng_l2cap_l2ca_con_ip *)(msg->data);
1215 	bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr));
1216 	ip->psm = pcb->psm;
1217 	ip->linktype = ng_btsock_l2cap_addrtype_to_linktype(pcb->dsttype);
1218 	ip->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb);
1219 	NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1220 
1221 	return (error);
1222 } /* ng_btsocket_l2cap_send_l2ca_con_req */
1223 
1224 /*
1225  * Send L2CA_Connect response
1226  */
1227 
1228 static int
1229 ng_btsocket_l2cap_send_l2ca_con_rsp_req(u_int32_t token,
1230 		ng_btsocket_l2cap_rtentry_p rt, bdaddr_p dst, int ident,
1231 					int lcid, int result, int linktype)
1232 {
1233 	struct ng_mesg			*msg = NULL;
1234 	ng_l2cap_l2ca_con_rsp_ip	*ip = NULL;
1235 	int				 error = 0;
1236 
1237 	if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
1238 		return (ENETDOWN);
1239 
1240 	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP,
1241 		sizeof(*ip), M_NOWAIT);
1242 	if (msg == NULL)
1243 		return (ENOMEM);
1244 
1245 	msg->header.token = token;
1246 
1247 	ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data);
1248 	bcopy(dst, &ip->bdaddr, sizeof(ip->bdaddr));
1249 	ip->ident = ident;
1250 	ip->lcid = lcid;
1251 	ip->linktype = linktype;
1252 	ip->result = result;
1253 	ip->status = 0;
1254 
1255 	NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, rt->hook, 0);
1256 
1257 	return (error);
1258 } /* ng_btsocket_l2cap_send_l2ca_con_rsp_req */
1259 
1260 /*
1261  * Send L2CA_Config request
1262  */
1263 
1264 static int
1265 ng_btsocket_l2cap_send_l2ca_cfg_req(ng_btsocket_l2cap_pcb_p pcb)
1266 {
1267 	struct ng_mesg		*msg = NULL;
1268 	ng_l2cap_l2ca_cfg_ip	*ip = NULL;
1269 	int			 error = 0;
1270 
1271 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1272 
1273 	if (pcb->rt == NULL ||
1274 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1275 		return (ENETDOWN);
1276 
1277 	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG,
1278 		sizeof(*ip), M_NOWAIT);
1279 	if (msg == NULL)
1280 		return (ENOMEM);
1281 
1282 	msg->header.token = pcb->token;
1283 
1284 	ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data);
1285 	ip->lcid = pcb->cid;
1286 	ip->imtu = pcb->imtu;
1287 	bcopy(&pcb->oflow, &ip->oflow, sizeof(ip->oflow));
1288 	ip->flush_timo = pcb->flush_timo;
1289 	ip->link_timo = pcb->link_timo;
1290 
1291 	NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1292 
1293 	return (error);
1294 } /* ng_btsocket_l2cap_send_l2ca_cfg_req */
1295 
1296 /*
1297  * Send L2CA_Config response
1298  */
1299 
1300 static int
1301 ng_btsocket_l2cap_send_l2ca_cfg_rsp(ng_btsocket_l2cap_pcb_p pcb)
1302 {
1303 	struct ng_mesg			*msg = NULL;
1304 	ng_l2cap_l2ca_cfg_rsp_ip	*ip = NULL;
1305 	int				 error = 0;
1306 
1307 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1308 
1309 	if (pcb->rt == NULL ||
1310 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1311 		return (ENETDOWN);
1312 
1313 	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_RSP,
1314 		sizeof(*ip), M_NOWAIT);
1315 	if (msg == NULL)
1316 		return (ENOMEM);
1317 
1318 	msg->header.token = pcb->token;
1319 
1320 	ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data);
1321 	ip->lcid = pcb->cid;
1322 	ip->omtu = pcb->omtu;
1323 	bcopy(&pcb->iflow, &ip->iflow, sizeof(ip->iflow));
1324 
1325 	NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, pcb->rt->hook, 0);
1326 
1327 	return (error);
1328 } /* ng_btsocket_l2cap_send_l2ca_cfg_rsp */
1329 
1330 /*
1331  * Send L2CA_Disconnect request
1332  */
1333 
1334 static int
1335 ng_btsocket_l2cap_send_l2ca_discon_req(u_int32_t token,
1336 		ng_btsocket_l2cap_pcb_p pcb)
1337 {
1338 	struct ng_mesg		*msg = NULL;
1339 	ng_l2cap_l2ca_discon_ip	*ip = NULL;
1340 	int			 error = 0;
1341 
1342 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1343 
1344 	if (pcb->rt == NULL ||
1345 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1346 		return (ENETDOWN);
1347 
1348 	NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON,
1349 		sizeof(*ip), M_NOWAIT);
1350 	if (msg == NULL)
1351 		return (ENOMEM);
1352 
1353 	msg->header.token = token;
1354 
1355 	ip = (ng_l2cap_l2ca_discon_ip *)(msg->data);
1356 	ip->lcid = pcb->cid;
1357 	ip->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb);
1358 
1359 	NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1360 
1361 	return (error);
1362 } /* ng_btsocket_l2cap_send_l2ca_discon_req */
1363 
1364 /*****************************************************************************
1365  *****************************************************************************
1366  **                              Socket interface
1367  *****************************************************************************
1368  *****************************************************************************/
1369 
1370 /*
1371  * L2CAP sockets data input routine
1372  */
1373 
1374 static void
1375 ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
1376 {
1377 	ng_l2cap_hdr_t			*hdr = NULL;
1378 	ng_l2cap_clt_hdr_t		*clt_hdr = NULL;
1379 	ng_btsocket_l2cap_pcb_t		*pcb = NULL;
1380 	ng_btsocket_l2cap_rtentry_t	*rt = NULL;
1381 	uint16_t idtype;
1382 
1383 	if (hook == NULL) {
1384 		NG_BTSOCKET_L2CAP_ALERT(
1385 "%s: Invalid source hook for L2CAP data packet\n", __func__);
1386 		goto drop;
1387 	}
1388 
1389 	rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook);
1390 	if (rt == NULL) {
1391 		NG_BTSOCKET_L2CAP_ALERT(
1392 "%s: Could not find out source bdaddr for L2CAP data packet\n", __func__);
1393 		goto drop;
1394 	}
1395 
1396 	m = m_pullup(m, sizeof(uint16_t));
1397 	idtype = *mtod(m, uint16_t *);
1398 	m_adj(m, sizeof(uint16_t));
1399 
1400 	/* Make sure we can access header */
1401 	if (m->m_pkthdr.len < sizeof(*hdr)) {
1402 		NG_BTSOCKET_L2CAP_ERR(
1403 "%s: L2CAP data packet too small, len=%d\n", __func__, m->m_pkthdr.len);
1404 		goto drop;
1405 	}
1406 
1407 	if (m->m_len < sizeof(*hdr)) {
1408 		m = m_pullup(m, sizeof(*hdr));
1409 		if (m == NULL)
1410 			goto drop;
1411 	}
1412 
1413 	/* Strip L2CAP packet header and verify packet length */
1414 	hdr = mtod(m, ng_l2cap_hdr_t *);
1415 	m_adj(m, sizeof(*hdr));
1416 
1417 	if (hdr->length != m->m_pkthdr.len) {
1418 		NG_BTSOCKET_L2CAP_ERR(
1419 "%s: Bad L2CAP data packet length, len=%d, length=%d\n",
1420 			__func__, m->m_pkthdr.len, hdr->length);
1421 		goto drop;
1422 	}
1423 
1424 	/*
1425 	 * Now process packet. Two cases:
1426 	 *
1427 	 * 1) Normal packet (cid != 2) then find connected socket and append
1428 	 *    mbuf to the socket queue. Wakeup socket.
1429 	 *
1430 	 * 2) Broadcast packet (cid == 2) then find all sockets that connected
1431 	 *    to the given PSM and have SO_BROADCAST bit set and append mbuf
1432 	 *    to the socket queue. Wakeup socket.
1433 	 */
1434 
1435 	NG_BTSOCKET_L2CAP_INFO(
1436 "%s: Received L2CAP data packet: src bdaddr=%x:%x:%x:%x:%x:%x, " \
1437 "dcid=%d, length=%d\n",
1438 		__func__,
1439 		rt->src.b[5], rt->src.b[4], rt->src.b[3],
1440 		rt->src.b[2], rt->src.b[1], rt->src.b[0],
1441 		hdr->dcid, hdr->length);
1442 
1443 	if ((hdr->dcid >= NG_L2CAP_FIRST_CID) ||
1444 	    (idtype == NG_L2CAP_L2CA_IDTYPE_ATT)){
1445 
1446 		mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1447 
1448 		/* Normal packet: find connected socket */
1449 		pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, hdr->dcid,idtype);
1450 		if (pcb == NULL) {
1451 			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1452 			goto drop;
1453 		}
1454 
1455 		mtx_lock(&pcb->pcb_mtx);
1456 
1457 		if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
1458 			NG_BTSOCKET_L2CAP_ERR(
1459 "%s: No connected socket found, src bdaddr=%x:%x:%x:%x:%x:%x, dcid=%d, " \
1460 "state=%d\n",			__func__,
1461 				rt->src.b[5], rt->src.b[4], rt->src.b[3],
1462 				rt->src.b[2], rt->src.b[1], rt->src.b[0],
1463 				hdr->dcid, pcb->state);
1464 
1465 			mtx_unlock(&pcb->pcb_mtx);
1466 			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1467 			goto drop;
1468 		}
1469 
1470 		/* Check packet size against socket's incoming MTU */
1471 		if (hdr->length > pcb->imtu) {
1472 			NG_BTSOCKET_L2CAP_ERR(
1473 "%s: L2CAP data packet too big, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1474 "dcid=%d, length=%d, imtu=%d\n",
1475 				__func__,
1476 				rt->src.b[5], rt->src.b[4], rt->src.b[3],
1477 				rt->src.b[2], rt->src.b[1], rt->src.b[0],
1478 				hdr->dcid, hdr->length, pcb->imtu);
1479 
1480 			mtx_unlock(&pcb->pcb_mtx);
1481 			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1482 			goto drop;
1483 		}
1484 
1485 		/* Check if we have enough space in socket receive queue */
1486 		if (m->m_pkthdr.len > sbspace(&pcb->so->so_rcv)) {
1487 
1488 			/*
1489 			 * This is really bad. Receive queue on socket does
1490 			 * not have enough space for the packet. We do not
1491 			 * have any other choice but drop the packet. L2CAP
1492 			 * does not provide any flow control.
1493 			 */
1494 
1495 			NG_BTSOCKET_L2CAP_ERR(
1496 "%s: Not enough space in socket receive queue. Dropping L2CAP data packet, " \
1497 "src bdaddr=%x:%x:%x:%x:%x:%x, dcid=%d, len=%d, space=%ld\n",
1498 				__func__,
1499 				rt->src.b[5], rt->src.b[4], rt->src.b[3],
1500 				rt->src.b[2], rt->src.b[1], rt->src.b[0],
1501 				hdr->dcid, m->m_pkthdr.len,
1502 				sbspace(&pcb->so->so_rcv));
1503 
1504 			mtx_unlock(&pcb->pcb_mtx);
1505 			mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1506 			goto drop;
1507 		}
1508 
1509 		/* Append packet to the socket receive queue and wakeup */
1510 		sbappendrecord(&pcb->so->so_rcv, m);
1511 		m = NULL;
1512 
1513 		sorwakeup(pcb->so);
1514 
1515 		mtx_unlock(&pcb->pcb_mtx);
1516 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1517 	} else if (hdr->dcid == NG_L2CAP_CLT_CID) {
1518 		/* Broadcast packet: give packet to all sockets  */
1519 
1520 		/* Check packet size against connectionless MTU */
1521 		if (hdr->length > NG_L2CAP_MTU_DEFAULT) {
1522 			NG_BTSOCKET_L2CAP_ERR(
1523 "%s: Connectionless L2CAP data packet too big, " \
1524 "src bdaddr=%x:%x:%x:%x:%x:%x, length=%d\n",
1525 				__func__,
1526 				rt->src.b[5], rt->src.b[4], rt->src.b[3],
1527 				rt->src.b[2], rt->src.b[1], rt->src.b[0],
1528 				hdr->length);
1529 			goto drop;
1530 		}
1531 
1532 		/* Make sure we can access connectionless header */
1533 		if (m->m_pkthdr.len < sizeof(*clt_hdr)) {
1534 			NG_BTSOCKET_L2CAP_ERR(
1535 "%s: Can not get L2CAP connectionless packet header, " \
1536 "src bdaddr=%x:%x:%x:%x:%x:%x, length=%d\n",
1537 				__func__,
1538 				rt->src.b[5], rt->src.b[4], rt->src.b[3],
1539 				rt->src.b[2], rt->src.b[1], rt->src.b[0],
1540 				hdr->length);
1541 			goto drop;
1542 		}
1543 
1544 		if (m->m_len < sizeof(*clt_hdr)) {
1545 			m = m_pullup(m, sizeof(*clt_hdr));
1546 			if (m == NULL)
1547 				goto drop;
1548 		}
1549 
1550 		/* Strip connectionless header and deliver packet */
1551 		clt_hdr = mtod(m, ng_l2cap_clt_hdr_t *);
1552 		m_adj(m, sizeof(*clt_hdr));
1553 
1554 		NG_BTSOCKET_L2CAP_INFO(
1555 "%s: Got L2CAP connectionless data packet, " \
1556 "src bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, length=%d\n",
1557 			__func__,
1558 			rt->src.b[5], rt->src.b[4], rt->src.b[3],
1559 			rt->src.b[2], rt->src.b[1], rt->src.b[0],
1560 			clt_hdr->psm, hdr->length);
1561 
1562 		mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1563 
1564 		LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next) {
1565 			struct mbuf	*copy = NULL;
1566 
1567 			mtx_lock(&pcb->pcb_mtx);
1568 
1569 			if (bcmp(&rt->src, &pcb->src, sizeof(pcb->src)) != 0 ||
1570 			    pcb->psm != clt_hdr->psm ||
1571 			    pcb->state != NG_BTSOCKET_L2CAP_OPEN ||
1572 			    (pcb->so->so_options & SO_BROADCAST) == 0 ||
1573 			    m->m_pkthdr.len > sbspace(&pcb->so->so_rcv))
1574 				goto next;
1575 
1576 			/*
1577 			 * Create a copy of the packet and append it to the
1578 			 * socket's queue. If m_dup() failed - no big deal
1579 			 * it is a broadcast traffic after all
1580 			 */
1581 
1582 			copy = m_dup(m, M_NOWAIT);
1583 			if (copy != NULL) {
1584 				sbappendrecord(&pcb->so->so_rcv, copy);
1585 				sorwakeup(pcb->so);
1586 			}
1587 next:
1588 			mtx_unlock(&pcb->pcb_mtx);
1589 		}
1590 
1591 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1592 	}
1593 drop:
1594 	NG_FREE_M(m); /* checks for m != NULL */
1595 } /* ng_btsocket_l2cap_data_input */
1596 
1597 /*
1598  * L2CAP sockets default message input routine
1599  */
1600 
1601 static void
1602 ng_btsocket_l2cap_default_msg_input(struct ng_mesg *msg, hook_p hook)
1603 {
1604 	switch (msg->header.cmd) {
1605 	case NGM_L2CAP_NODE_HOOK_INFO: {
1606 		ng_btsocket_l2cap_rtentry_t	*rt = NULL;
1607 		ng_l2cap_node_hook_info_ep *ep =
1608 		  (ng_l2cap_node_hook_info_ep *)msg->data;
1609 		if (hook == NULL || msg->header.arglen != sizeof(*ep))
1610 			break;
1611 
1612 		if (bcmp(&ep->addr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
1613 			break;
1614 
1615 		mtx_lock(&ng_btsocket_l2cap_rt_mtx);
1616 
1617 		rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook);
1618 		if (rt == NULL) {
1619 			rt = malloc(sizeof(*rt),
1620 				M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT|M_ZERO);
1621 			if (rt == NULL) {
1622 				mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
1623 				break;
1624 			}
1625 
1626 			LIST_INSERT_HEAD(&ng_btsocket_l2cap_rt, rt, next);
1627 
1628 			NG_HOOK_SET_PRIVATE(hook, rt);
1629 		}
1630 
1631 		bcopy(&ep->addr, &rt->src, sizeof(rt->src));
1632 		rt->hook = hook;
1633 
1634 		mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
1635 
1636 		NG_BTSOCKET_L2CAP_INFO(
1637 "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x\n",
1638 			__func__, NG_HOOK_NAME(hook),
1639 			rt->src.b[5], rt->src.b[4], rt->src.b[3],
1640 			rt->src.b[2], rt->src.b[1], rt->src.b[0]);
1641 		} break;
1642 
1643 	default:
1644 		NG_BTSOCKET_L2CAP_WARN(
1645 "%s: Unknown message, cmd=%d\n", __func__, msg->header.cmd);
1646 		break;
1647 	}
1648 
1649 	NG_FREE_MSG(msg); /* Checks for msg != NULL */
1650 } /* ng_btsocket_l2cap_default_msg_input */
1651 
1652 /*
1653  * L2CAP sockets L2CA message input routine
1654  */
1655 
1656 static void
1657 ng_btsocket_l2cap_l2ca_msg_input(struct ng_mesg *msg, hook_p hook)
1658 {
1659 	ng_btsocket_l2cap_rtentry_p	rt = NULL;
1660 
1661 	if (hook == NULL) {
1662 		NG_BTSOCKET_L2CAP_ALERT(
1663 "%s: Invalid source hook for L2CA message\n", __func__);
1664 		goto drop;
1665 	}
1666 
1667 	rt = (ng_btsocket_l2cap_rtentry_p) NG_HOOK_PRIVATE(hook);
1668 	if (rt == NULL) {
1669 		NG_BTSOCKET_L2CAP_ALERT(
1670 "%s: Could not find out source bdaddr for L2CA message\n", __func__);
1671 		goto drop;
1672 	}
1673 
1674 	switch (msg->header.cmd) {
1675 	case NGM_L2CAP_L2CA_CON: /* L2CA_Connect response */
1676 		ng_btsocket_l2cap_process_l2ca_con_req_rsp(msg, rt);
1677 		break;
1678 
1679 	case NGM_L2CAP_L2CA_CON_RSP: /* L2CA_ConnectRsp response */
1680 		ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(msg, rt);
1681 		break;
1682 
1683 	case NGM_L2CAP_L2CA_CON_IND: /* L2CA_Connect indicator */
1684 		ng_btsocket_l2cap_process_l2ca_con_ind(msg, rt);
1685 		break;
1686 
1687 	case NGM_L2CAP_L2CA_CFG: /* L2CA_Config response */
1688 		ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(msg, rt);
1689 		break;
1690 
1691 	case NGM_L2CAP_L2CA_CFG_RSP: /* L2CA_ConfigRsp response */
1692 		ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(msg, rt);
1693 		break;
1694 
1695 	case NGM_L2CAP_L2CA_CFG_IND: /* L2CA_Config indicator */
1696 		ng_btsocket_l2cap_process_l2ca_cfg_ind(msg, rt);
1697 		break;
1698 
1699 	case NGM_L2CAP_L2CA_DISCON: /* L2CA_Disconnect response */
1700 		ng_btsocket_l2cap_process_l2ca_discon_rsp(msg, rt);
1701 		break;
1702 
1703 	case NGM_L2CAP_L2CA_DISCON_IND: /* L2CA_Disconnect indicator */
1704 		ng_btsocket_l2cap_process_l2ca_discon_ind(msg, rt);
1705 		break;
1706 
1707 	case NGM_L2CAP_L2CA_WRITE: /* L2CA_Write response */
1708 		ng_btsocket_l2cap_process_l2ca_write_rsp(msg, rt);
1709 		break;
1710 
1711 	/* XXX FIXME add other L2CA messages */
1712 
1713 	default:
1714 		NG_BTSOCKET_L2CAP_WARN(
1715 "%s: Unknown L2CA message, cmd=%d\n", __func__, msg->header.cmd);
1716 		break;
1717 	}
1718 drop:
1719 	NG_FREE_MSG(msg);
1720 } /* ng_btsocket_l2cap_l2ca_msg_input */
1721 
1722 /*
1723  * L2CAP sockets input routine
1724  */
1725 
1726 static void
1727 ng_btsocket_l2cap_input(void *context, int pending)
1728 {
1729 	item_p	item = NULL;
1730 	hook_p	hook = NULL;
1731 
1732 	for (;;) {
1733 		mtx_lock(&ng_btsocket_l2cap_queue_mtx);
1734 		NG_BT_ITEMQ_DEQUEUE(&ng_btsocket_l2cap_queue, item);
1735 		mtx_unlock(&ng_btsocket_l2cap_queue_mtx);
1736 
1737 		if (item == NULL)
1738 			break;
1739 
1740 		NGI_GET_HOOK(item, hook);
1741 		if (hook != NULL && NG_HOOK_NOT_VALID(hook))
1742 			goto drop;
1743 
1744 		switch(item->el_flags & NGQF_TYPE) {
1745 		case NGQF_DATA: {
1746 			struct mbuf     *m = NULL;
1747 
1748 			NGI_GET_M(item, m);
1749 			ng_btsocket_l2cap_data_input(m, hook);
1750 			} break;
1751 
1752 		case NGQF_MESG: {
1753 			struct ng_mesg  *msg = NULL;
1754 
1755 			NGI_GET_MSG(item, msg);
1756 
1757 			switch (msg->header.cmd) {
1758 			case NGM_L2CAP_L2CA_CON:
1759 			case NGM_L2CAP_L2CA_CON_RSP:
1760 			case NGM_L2CAP_L2CA_CON_IND:
1761 			case NGM_L2CAP_L2CA_CFG:
1762 			case NGM_L2CAP_L2CA_CFG_RSP:
1763 			case NGM_L2CAP_L2CA_CFG_IND:
1764 			case NGM_L2CAP_L2CA_DISCON:
1765 			case NGM_L2CAP_L2CA_DISCON_IND:
1766 			case NGM_L2CAP_L2CA_WRITE:
1767 			/* XXX FIXME add other L2CA messages */
1768 				ng_btsocket_l2cap_l2ca_msg_input(msg, hook);
1769 				break;
1770 
1771 			default:
1772 				ng_btsocket_l2cap_default_msg_input(msg, hook);
1773 				break;
1774 			}
1775 			} break;
1776 
1777 		default:
1778 			KASSERT(0,
1779 ("%s: invalid item type=%ld\n", __func__, (item->el_flags & NGQF_TYPE)));
1780 			break;
1781 		}
1782 drop:
1783 		if (hook != NULL)
1784 			NG_HOOK_UNREF(hook);
1785 
1786 		NG_FREE_ITEM(item);
1787 	}
1788 } /* ng_btsocket_l2cap_input */
1789 
1790 /*
1791  * Route cleanup task. Gets scheduled when hook is disconnected. Here we
1792  * will find all sockets that use "invalid" hook and disconnect them.
1793  */
1794 
1795 static void
1796 ng_btsocket_l2cap_rtclean(void *context, int pending)
1797 {
1798 	ng_btsocket_l2cap_pcb_p		pcb = NULL, pcb_next = NULL;
1799 	ng_btsocket_l2cap_rtentry_p	rt = NULL;
1800 
1801 	mtx_lock(&ng_btsocket_l2cap_rt_mtx);
1802 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
1803 
1804 	/*
1805 	 * First disconnect all sockets that use "invalid" hook
1806 	 */
1807 
1808 	for (pcb = LIST_FIRST(&ng_btsocket_l2cap_sockets); pcb != NULL; ) {
1809 		mtx_lock(&pcb->pcb_mtx);
1810 		pcb_next = LIST_NEXT(pcb, next);
1811 
1812 		if (pcb->rt != NULL &&
1813 		    pcb->rt->hook != NULL && NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1814 			if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
1815 				ng_btsocket_l2cap_untimeout(pcb);
1816 
1817 			pcb->so->so_error = ENETDOWN;
1818 			pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1819 			soisdisconnected(pcb->so);
1820 
1821 			pcb->token = 0;
1822 			pcb->cid = 0;
1823 			pcb->rt = NULL;
1824 		}
1825 
1826 		mtx_unlock(&pcb->pcb_mtx);
1827 		pcb = pcb_next;
1828 	}
1829 
1830 	/*
1831 	 * Now cleanup routing table
1832 	 */
1833 
1834 	for (rt = LIST_FIRST(&ng_btsocket_l2cap_rt); rt != NULL; ) {
1835 		ng_btsocket_l2cap_rtentry_p	rt_next = LIST_NEXT(rt, next);
1836 
1837 		if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
1838 			LIST_REMOVE(rt, next);
1839 
1840 			NG_HOOK_SET_PRIVATE(rt->hook, NULL);
1841 			NG_HOOK_UNREF(rt->hook); /* Remove extra reference */
1842 
1843 			bzero(rt, sizeof(*rt));
1844 			free(rt, M_NETGRAPH_BTSOCKET_L2CAP);
1845 		}
1846 
1847 		rt = rt_next;
1848 	}
1849 
1850 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
1851 	mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
1852 } /* ng_btsocket_l2cap_rtclean */
1853 
1854 /*
1855  * Initialize everything
1856  */
1857 
1858 void
1859 ng_btsocket_l2cap_init(void)
1860 {
1861 	int	error = 0;
1862 
1863 	/* Skip initialization of globals for non-default instances. */
1864 	if (!IS_DEFAULT_VNET(curvnet))
1865 		return;
1866 
1867 	ng_btsocket_l2cap_node = NULL;
1868 	ng_btsocket_l2cap_debug_level = NG_BTSOCKET_WARN_LEVEL;
1869 
1870 	/* Register Netgraph node type */
1871 	error = ng_newtype(&typestruct);
1872 	if (error != 0) {
1873 		NG_BTSOCKET_L2CAP_ALERT(
1874 "%s: Could not register Netgraph node type, error=%d\n", __func__, error);
1875 
1876                 return;
1877 	}
1878 
1879 	/* Create Netgrapg node */
1880 	error = ng_make_node_common(&typestruct, &ng_btsocket_l2cap_node);
1881 	if (error != 0) {
1882 		NG_BTSOCKET_L2CAP_ALERT(
1883 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
1884 
1885 		ng_btsocket_l2cap_node = NULL;
1886 
1887 		return;
1888 	}
1889 
1890 	error = ng_name_node(ng_btsocket_l2cap_node,
1891 				NG_BTSOCKET_L2CAP_NODE_TYPE);
1892 	if (error != 0) {
1893 		NG_BTSOCKET_L2CAP_ALERT(
1894 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
1895 
1896 		NG_NODE_UNREF(ng_btsocket_l2cap_node);
1897 		ng_btsocket_l2cap_node = NULL;
1898 
1899 		return;
1900 	}
1901 
1902 	/* Create input queue */
1903 	NG_BT_ITEMQ_INIT(&ng_btsocket_l2cap_queue, ifqmaxlen);
1904 	mtx_init(&ng_btsocket_l2cap_queue_mtx,
1905 		"btsocks_l2cap_queue_mtx", NULL, MTX_DEF);
1906 	TASK_INIT(&ng_btsocket_l2cap_queue_task, 0,
1907 		ng_btsocket_l2cap_input, NULL);
1908 
1909 	/* Create list of sockets */
1910 	LIST_INIT(&ng_btsocket_l2cap_sockets);
1911 	mtx_init(&ng_btsocket_l2cap_sockets_mtx,
1912 		"btsocks_l2cap_sockets_mtx", NULL, MTX_DEF);
1913 
1914 	/* Routing table */
1915 	LIST_INIT(&ng_btsocket_l2cap_rt);
1916 	mtx_init(&ng_btsocket_l2cap_rt_mtx,
1917 		"btsocks_l2cap_rt_mtx", NULL, MTX_DEF);
1918 	TASK_INIT(&ng_btsocket_l2cap_rt_task, 0,
1919 		ng_btsocket_l2cap_rtclean, NULL);
1920 } /* ng_btsocket_l2cap_init */
1921 
1922 /*
1923  * Abort connection on socket
1924  */
1925 
1926 void
1927 ng_btsocket_l2cap_abort(struct socket *so)
1928 {
1929 	so->so_error = ECONNABORTED;
1930 
1931 	(void)ng_btsocket_l2cap_disconnect(so);
1932 } /* ng_btsocket_l2cap_abort */
1933 
1934 void
1935 ng_btsocket_l2cap_close(struct socket *so)
1936 {
1937 
1938 	(void)ng_btsocket_l2cap_disconnect(so);
1939 } /* ng_btsocket_l2cap_close */
1940 
1941 /*
1942  * Accept connection on socket. Nothing to do here, socket must be connected
1943  * and ready, so just return peer address and be done with it.
1944  */
1945 
1946 int
1947 ng_btsocket_l2cap_accept(struct socket *so, struct sockaddr **nam)
1948 {
1949 	if (ng_btsocket_l2cap_node == NULL)
1950 		return (EINVAL);
1951 
1952 	return (ng_btsocket_l2cap_peeraddr(so, nam));
1953 } /* ng_btsocket_l2cap_accept */
1954 
1955 /*
1956  * Create and attach new socket
1957  */
1958 
1959 int
1960 ng_btsocket_l2cap_attach(struct socket *so, int proto, struct thread *td)
1961 {
1962 	static u_int32_t	token = 0;
1963 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
1964 	int			error;
1965 
1966 	/* Check socket and protocol */
1967 	if (ng_btsocket_l2cap_node == NULL)
1968 		return (EPROTONOSUPPORT);
1969 	if (so->so_type != SOCK_SEQPACKET)
1970 		return (ESOCKTNOSUPPORT);
1971 
1972 #if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
1973 	if (proto != 0)
1974 		if (proto != BLUETOOTH_PROTO_L2CAP)
1975 			return (EPROTONOSUPPORT);
1976 #endif /* XXX */
1977 
1978 	if (pcb != NULL)
1979 		return (EISCONN);
1980 
1981 	/* Reserve send and receive space if it is not reserved yet */
1982 	if ((so->so_snd.sb_hiwat == 0) || (so->so_rcv.sb_hiwat == 0)) {
1983 		error = soreserve(so, NG_BTSOCKET_L2CAP_SENDSPACE,
1984 					NG_BTSOCKET_L2CAP_RECVSPACE);
1985 		if (error != 0)
1986 			return (error);
1987 	}
1988 
1989 	/* Allocate the PCB */
1990         pcb = malloc(sizeof(*pcb),
1991 		M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT | M_ZERO);
1992         if (pcb == NULL)
1993                 return (ENOMEM);
1994 
1995 	/* Link the PCB and the socket */
1996 	so->so_pcb = (caddr_t) pcb;
1997 	pcb->so = so;
1998 	pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1999 
2000 	/* Initialize PCB */
2001 	pcb->imtu = pcb->omtu = NG_L2CAP_MTU_DEFAULT;
2002 
2003 	/* Default flow */
2004 	pcb->iflow.flags = 0x0;
2005 	pcb->iflow.service_type = NG_HCI_SERVICE_TYPE_BEST_EFFORT;
2006 	pcb->iflow.token_rate = 0xffffffff; /* maximum */
2007 	pcb->iflow.token_bucket_size = 0xffffffff; /* maximum */
2008 	pcb->iflow.peak_bandwidth = 0x00000000; /* maximum */
2009 	pcb->iflow.latency = 0xffffffff; /* don't care */
2010 	pcb->iflow.delay_variation = 0xffffffff; /* don't care */
2011 
2012 	bcopy(&pcb->iflow, &pcb->oflow, sizeof(pcb->oflow));
2013 
2014 	pcb->flush_timo = NG_L2CAP_FLUSH_TIMO_DEFAULT;
2015 	pcb->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT;
2016 
2017 	/*
2018 	 * XXX Mark PCB mutex as DUPOK to prevent "duplicated lock of
2019 	 * the same type" message. When accepting new L2CAP connection
2020 	 * ng_btsocket_l2cap_process_l2ca_con_ind() holds both PCB mutexes
2021 	 * for "old" (accepting) PCB and "new" (created) PCB.
2022 	 */
2023 
2024 	mtx_init(&pcb->pcb_mtx, "btsocks_l2cap_pcb_mtx", NULL,
2025 		MTX_DEF|MTX_DUPOK);
2026 	callout_init_mtx(&pcb->timo, &pcb->pcb_mtx, 0);
2027 
2028         /*
2029 	 * Add the PCB to the list
2030 	 *
2031 	 * XXX FIXME VERY IMPORTANT!
2032 	 *
2033 	 * This is totally FUBAR. We could get here in two cases:
2034 	 *
2035 	 * 1) When user calls socket()
2036 	 * 2) When we need to accept new incomming connection and call
2037 	 *    sonewconn()
2038 	 *
2039 	 * In the first case we must acquire ng_btsocket_l2cap_sockets_mtx.
2040 	 * In the second case we hold ng_btsocket_l2cap_sockets_mtx already.
2041 	 * So we now need to distinguish between these cases. From reading
2042 	 * /sys/kern/uipc_socket.c we can find out that sonewconn() calls
2043 	 * pru_attach with proto == 0 and td == NULL. For now use this fact
2044 	 * to figure out if we were called from socket() or from sonewconn().
2045 	 */
2046 
2047 	if (td != NULL)
2048 		mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2049 	else
2050 		mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2051 
2052 	/* Set PCB token. Use ng_btsocket_l2cap_sockets_mtx for protection */
2053 	if (++ token == 0)
2054 		token ++;
2055 
2056 	pcb->token = token;
2057 
2058 	LIST_INSERT_HEAD(&ng_btsocket_l2cap_sockets, pcb, next);
2059 
2060 	if (td != NULL)
2061 		mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2062 
2063         return (0);
2064 } /* ng_btsocket_l2cap_attach */
2065 
2066 /*
2067  * Bind socket
2068  */
2069 
2070 int
2071 ng_btsocket_l2cap_bind(struct socket *so, struct sockaddr *nam,
2072 		struct thread *td)
2073 {
2074 	ng_btsocket_l2cap_pcb_t	*pcb = NULL;
2075 	struct sockaddr_l2cap	*sa = (struct sockaddr_l2cap *) nam;
2076 	int			 psm, error = 0;
2077 
2078 	if (ng_btsocket_l2cap_node == NULL)
2079 		return (EINVAL);
2080 
2081 	/* Verify address */
2082 	if (sa == NULL)
2083 		return (EINVAL);
2084 	if (sa->l2cap_family != AF_BLUETOOTH)
2085 		return (EAFNOSUPPORT);
2086 	/*For the time being, Not support LE binding.*/
2087 	if ((sa->l2cap_len != sizeof(*sa))&&
2088 	    (sa->l2cap_len != sizeof(struct sockaddr_l2cap_compat)))
2089 		return (EINVAL);
2090 
2091 	psm = le16toh(sa->l2cap_psm);
2092 
2093 	/*
2094 	 * Check if other socket has this address already (look for exact
2095 	 * match PSM and bdaddr) and assign socket address if it's available.
2096 	 *
2097 	 * Note: socket can be bound to ANY PSM (zero) thus allowing several
2098 	 * channels with the same PSM between the same pair of BD_ADDR'es.
2099 	 */
2100 
2101 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2102 
2103 	LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next)
2104 		if (psm != 0 && psm == pcb->psm &&
2105 		    bcmp(&pcb->src, &sa->l2cap_bdaddr, sizeof(bdaddr_t)) == 0)
2106 			break;
2107 
2108 	if (pcb == NULL) {
2109 		/* Set socket address */
2110 		pcb = so2l2cap_pcb(so);
2111 		if (pcb != NULL) {
2112 			bcopy(&sa->l2cap_bdaddr, &pcb->src, sizeof(pcb->src));
2113 			pcb->psm = psm;
2114 		} else
2115 			error = EINVAL;
2116 	} else
2117 		error = EADDRINUSE;
2118 
2119 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2120 
2121 	return (error);
2122 } /* ng_btsocket_l2cap_bind */
2123 
2124 /*
2125  * Connect socket
2126  */
2127 
2128 int
2129 ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam,
2130 		struct thread *td)
2131 {
2132 	ng_btsocket_l2cap_pcb_t		*pcb = so2l2cap_pcb(so);
2133 	struct sockaddr_l2cap_compat	*sal = (struct sockaddr_l2cap_compat *) nam;
2134 	struct sockaddr_l2cap *sa  = (struct sockaddr_l2cap *)nam;
2135 	struct sockaddr_l2cap  ba;
2136 	ng_btsocket_l2cap_rtentry_t	*rt = NULL;
2137 	int				 have_src, error = 0;
2138 
2139 	/* Check socket */
2140 	if (pcb == NULL)
2141 		return (EINVAL);
2142 	if (ng_btsocket_l2cap_node == NULL)
2143 		return (EINVAL);
2144 	if (pcb->state == NG_BTSOCKET_L2CAP_CONNECTING)
2145 		return (EINPROGRESS);
2146 
2147 	/* Verify address */
2148 	if (sa == NULL)
2149 		return (EINVAL);
2150 	if (sa->l2cap_family != AF_BLUETOOTH)
2151 		return (EAFNOSUPPORT);
2152 	if (sa->l2cap_len == sizeof(*sal)){
2153 		bcopy(sal, &ba, sizeof(*sal));
2154 		sa = &ba;
2155 		sa->l2cap_len = sizeof(*sa);
2156 		sa->l2cap_bdaddr_type = BDADDR_BREDR;
2157 	}
2158 	if (sa->l2cap_len != sizeof(*sa))
2159 		return (EINVAL);
2160 	if ((sa->l2cap_psm &&  sa->l2cap_cid))
2161 		return EINVAL;
2162 	if (bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
2163 		return (EDESTADDRREQ);
2164 	if((sa->l2cap_bdaddr_type == BDADDR_BREDR)&&
2165 	   (sa->l2cap_psm == 0))
2166 		return EDESTADDRREQ;
2167 	if((sa->l2cap_bdaddr_type != BDADDR_BREDR)&&
2168 	   (sa->l2cap_cid != NG_L2CAP_ATT_CID)){
2169 		return EINVAL;
2170 	}
2171 	if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm))
2172 		return (EINVAL);
2173 	/*
2174 	 * Routing. Socket should be bound to some source address. The source
2175 	 * address can be ANY. Destination address must be set and it must not
2176 	 * be ANY. If source address is ANY then find first rtentry that has
2177 	 * src != dst.
2178 	 */
2179 
2180 	mtx_lock(&ng_btsocket_l2cap_rt_mtx);
2181 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2182 	mtx_lock(&pcb->pcb_mtx);
2183 
2184 	/* Send destination address and PSM */
2185 	bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst));
2186 	pcb->psm = le16toh(sa->l2cap_psm);
2187 	pcb->dsttype = sa->l2cap_bdaddr_type;
2188 	pcb->cid = sa->l2cap_cid;
2189 
2190 	pcb->rt = NULL;
2191 	have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
2192 
2193 	LIST_FOREACH(rt, &ng_btsocket_l2cap_rt, next) {
2194 		if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
2195 			continue;
2196 
2197 		/* Match src and dst */
2198 		if (have_src) {
2199 			if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
2200 				break;
2201 		} else {
2202 			if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
2203 				break;
2204 		}
2205 	}
2206 
2207 	if (rt != NULL) {
2208 		pcb->rt = rt;
2209 
2210 		if (!have_src){
2211 			bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
2212 			pcb->srctype =
2213 			  (sa->l2cap_bdaddr_type == BDADDR_BREDR)?
2214 			  BDADDR_BREDR : BDADDR_LE_RANDOM;
2215 		}
2216 	} else
2217 		error = EHOSTUNREACH;
2218 
2219 	/*
2220 	 * Send L2CA_Connect request
2221 	 */
2222 
2223 	if (error == 0) {
2224 		error = ng_btsocket_l2cap_send_l2ca_con_req(pcb);
2225 		if (error == 0) {
2226 			pcb->flags |= NG_BTSOCKET_L2CAP_CLIENT;
2227 			pcb->state = NG_BTSOCKET_L2CAP_CONNECTING;
2228 			soisconnecting(pcb->so);
2229 
2230 			ng_btsocket_l2cap_timeout(pcb);
2231 		}
2232 	}
2233 
2234 	mtx_unlock(&pcb->pcb_mtx);
2235 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2236 	mtx_unlock(&ng_btsocket_l2cap_rt_mtx);
2237 
2238 	return (error);
2239 } /* ng_btsocket_l2cap_connect */
2240 
2241 /*
2242  * Process ioctl's calls on socket
2243  */
2244 
2245 int
2246 ng_btsocket_l2cap_control(struct socket *so, u_long cmd, caddr_t data,
2247 		struct ifnet *ifp, struct thread *td)
2248 {
2249 	return (EINVAL);
2250 } /* ng_btsocket_l2cap_control */
2251 
2252 /*
2253  * Process getsockopt/setsockopt system calls
2254  */
2255 
2256 int
2257 ng_btsocket_l2cap_ctloutput(struct socket *so, struct sockopt *sopt)
2258 {
2259 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
2260 	int			error = 0;
2261 	ng_l2cap_cfg_opt_val_t	v;
2262 
2263 	if (pcb == NULL)
2264 		return (EINVAL);
2265 	if (ng_btsocket_l2cap_node == NULL)
2266 		return (EINVAL);
2267 
2268 	if (sopt->sopt_level != SOL_L2CAP)
2269 		return (0);
2270 
2271 	mtx_lock(&pcb->pcb_mtx);
2272 
2273 	switch (sopt->sopt_dir) {
2274 	case SOPT_GET:
2275 		switch (sopt->sopt_name) {
2276 		case SO_L2CAP_IMTU: /* get incoming MTU */
2277 			error = sooptcopyout(sopt, &pcb->imtu,
2278 						sizeof(pcb->imtu));
2279 			break;
2280 
2281 		case SO_L2CAP_OMTU: /* get outgoing (peer incoming) MTU */
2282 			error = sooptcopyout(sopt, &pcb->omtu,
2283 						sizeof(pcb->omtu));
2284 			break;
2285 
2286 		case SO_L2CAP_IFLOW: /* get incoming flow spec. */
2287 			error = sooptcopyout(sopt, &pcb->iflow,
2288 						sizeof(pcb->iflow));
2289 			break;
2290 
2291 		case SO_L2CAP_OFLOW: /* get outgoing flow spec. */
2292 			error = sooptcopyout(sopt, &pcb->oflow,
2293 						sizeof(pcb->oflow));
2294 			break;
2295 
2296 		case SO_L2CAP_FLUSH: /* get flush timeout */
2297 			error = sooptcopyout(sopt, &pcb->flush_timo,
2298 						sizeof(pcb->flush_timo));
2299 			break;
2300 
2301 		default:
2302 			error = ENOPROTOOPT;
2303 			break;
2304 		}
2305 		break;
2306 
2307 	case SOPT_SET:
2308 		/*
2309 		 * XXX
2310 		 * We do not allow to change these parameters while socket is
2311 		 * connected or we are in the process of creating a connection.
2312 		 * May be this should indicate re-configuration of the open
2313 		 * channel?
2314 		 */
2315 
2316 		if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
2317 			error = EACCES;
2318 			break;
2319 		}
2320 
2321 		switch (sopt->sopt_name) {
2322 		case SO_L2CAP_IMTU: /* set incoming MTU */
2323 			error = sooptcopyin(sopt, &v, sizeof(v), sizeof(v.mtu));
2324 			if (error == 0)
2325 				pcb->imtu = v.mtu;
2326 			break;
2327 
2328 		case SO_L2CAP_OFLOW: /* set outgoing flow spec. */
2329 			error = sooptcopyin(sopt, &v, sizeof(v),sizeof(v.flow));
2330 			if (error == 0)
2331 				bcopy(&v.flow, &pcb->oflow, sizeof(pcb->oflow));
2332 			break;
2333 
2334 		case SO_L2CAP_FLUSH: /* set flush timeout */
2335 			error = sooptcopyin(sopt, &v, sizeof(v),
2336 						sizeof(v.flush_timo));
2337 			if (error == 0)
2338 				pcb->flush_timo = v.flush_timo;
2339 			break;
2340 
2341 		default:
2342 			error = ENOPROTOOPT;
2343 			break;
2344 		}
2345 		break;
2346 
2347 	default:
2348 		error = EINVAL;
2349 		break;
2350 	}
2351 
2352 	mtx_unlock(&pcb->pcb_mtx);
2353 
2354 	return (error);
2355 } /* ng_btsocket_l2cap_ctloutput */
2356 
2357 /*
2358  * Detach and destroy socket
2359  */
2360 
2361 void
2362 ng_btsocket_l2cap_detach(struct socket *so)
2363 {
2364 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
2365 
2366 	KASSERT(pcb != NULL, ("ng_btsocket_l2cap_detach: pcb == NULL"));
2367 
2368 	if (ng_btsocket_l2cap_node == NULL)
2369 		return;
2370 
2371 	mtx_lock(&ng_btsocket_l2cap_sockets_mtx);
2372 	mtx_lock(&pcb->pcb_mtx);
2373 
2374 	/* XXX what to do with pending request? */
2375 	if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
2376 		ng_btsocket_l2cap_untimeout(pcb);
2377 
2378 	if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED &&
2379 	    pcb->state != NG_BTSOCKET_L2CAP_DISCONNECTING)
2380 		/* Send disconnect request with "zero" token */
2381 		ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
2382 
2383 	pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2384 
2385 	LIST_REMOVE(pcb, next);
2386 
2387 	mtx_unlock(&pcb->pcb_mtx);
2388 	mtx_unlock(&ng_btsocket_l2cap_sockets_mtx);
2389 
2390 	mtx_destroy(&pcb->pcb_mtx);
2391 	bzero(pcb, sizeof(*pcb));
2392 	free(pcb, M_NETGRAPH_BTSOCKET_L2CAP);
2393 
2394 	soisdisconnected(so);
2395 	so->so_pcb = NULL;
2396 } /* ng_btsocket_l2cap_detach */
2397 
2398 /*
2399  * Disconnect socket
2400  */
2401 
2402 int
2403 ng_btsocket_l2cap_disconnect(struct socket *so)
2404 {
2405 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
2406 	int			error = 0;
2407 
2408 	if (pcb == NULL)
2409 		return (EINVAL);
2410 	if (ng_btsocket_l2cap_node == NULL)
2411 		return (EINVAL);
2412 
2413 	mtx_lock(&pcb->pcb_mtx);
2414 
2415 	if (pcb->state == NG_BTSOCKET_L2CAP_DISCONNECTING) {
2416 		mtx_unlock(&pcb->pcb_mtx);
2417 		return (EINPROGRESS);
2418 	}
2419 
2420 	if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
2421 		/* XXX FIXME what to do with pending request? */
2422 		if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
2423 			ng_btsocket_l2cap_untimeout(pcb);
2424 
2425 		error = ng_btsocket_l2cap_send_l2ca_discon_req(pcb->token, pcb);
2426 		if (error == 0) {
2427 			pcb->state = NG_BTSOCKET_L2CAP_DISCONNECTING;
2428 			soisdisconnecting(so);
2429 
2430 			ng_btsocket_l2cap_timeout(pcb);
2431 		}
2432 
2433 		/* XXX FIXME what to do if error != 0 */
2434 	}
2435 
2436 	mtx_unlock(&pcb->pcb_mtx);
2437 
2438 	return (error);
2439 } /* ng_btsocket_l2cap_disconnect */
2440 
2441 /*
2442  * Listen on socket
2443  */
2444 
2445 int
2446 ng_btsocket_l2cap_listen(struct socket *so, int backlog, struct thread *td)
2447 {
2448 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
2449 	int error;
2450 
2451 	SOCK_LOCK(so);
2452 	error = solisten_proto_check(so);
2453 	if (error != 0)
2454 		goto out;
2455 	if (pcb == NULL) {
2456 		error = EINVAL;
2457 		goto out;
2458 	}
2459 	if (ng_btsocket_l2cap_node == NULL) {
2460 		error = EINVAL;
2461 		goto out;
2462 	}
2463 	if (pcb->psm == 0) {
2464 		error = EADDRNOTAVAIL;
2465 		goto out;
2466 	}
2467 	solisten_proto(so, backlog);
2468 out:
2469 	SOCK_UNLOCK(so);
2470 	return (error);
2471 } /* ng_btsocket_listen */
2472 
2473 /*
2474  * Get peer address
2475  */
2476 
2477 int
2478 ng_btsocket_l2cap_peeraddr(struct socket *so, struct sockaddr **nam)
2479 {
2480 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
2481 	struct sockaddr_l2cap	sa;
2482 
2483 	if (pcb == NULL)
2484 		return (EINVAL);
2485 	if (ng_btsocket_l2cap_node == NULL)
2486 		return (EINVAL);
2487 
2488 	bcopy(&pcb->dst, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr));
2489 	sa.l2cap_psm = htole16(pcb->psm);
2490 	sa.l2cap_len = sizeof(sa);
2491 	sa.l2cap_family = AF_BLUETOOTH;
2492 	sa.l2cap_cid = 0;
2493 	sa.l2cap_bdaddr_type = pcb->dsttype;
2494 	*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
2495 
2496 	return ((*nam == NULL)? ENOMEM : 0);
2497 } /* ng_btsocket_l2cap_peeraddr */
2498 
2499 /*
2500  * Send data to socket
2501  */
2502 
2503 int
2504 ng_btsocket_l2cap_send(struct socket *so, int flags, struct mbuf *m,
2505 		struct sockaddr *nam, struct mbuf *control, struct thread *td)
2506 {
2507 	ng_btsocket_l2cap_pcb_t	*pcb = so2l2cap_pcb(so);
2508 	int			 error = 0;
2509 
2510 	if (ng_btsocket_l2cap_node == NULL) {
2511 		error = ENETDOWN;
2512 		goto drop;
2513 	}
2514 
2515 	/* Check socket and input */
2516 	if (pcb == NULL || m == NULL || control != NULL) {
2517 		error = EINVAL;
2518 		goto drop;
2519 	}
2520 
2521 	mtx_lock(&pcb->pcb_mtx);
2522 
2523 	/* Make sure socket is connected */
2524 	if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
2525 		mtx_unlock(&pcb->pcb_mtx);
2526 		error = ENOTCONN;
2527 		goto drop;
2528 	}
2529 
2530 	/* Check route */
2531 	if (pcb->rt == NULL ||
2532 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) {
2533 		mtx_unlock(&pcb->pcb_mtx);
2534 		error = ENETDOWN;
2535 		goto drop;
2536 	}
2537 
2538 	/* Check packet size agains outgoing (peer's incoming) MTU) */
2539 	if (m->m_pkthdr.len > pcb->omtu) {
2540 		NG_BTSOCKET_L2CAP_ERR(
2541 "%s: Packet too big, len=%d, omtu=%d\n", __func__, m->m_pkthdr.len, pcb->omtu);
2542 
2543 		mtx_unlock(&pcb->pcb_mtx);
2544 		error = EMSGSIZE;
2545 		goto drop;
2546 	}
2547 
2548 	/*
2549 	 * First put packet on socket send queue. Then check if we have
2550 	 * pending timeout. If we do not have timeout then we must send
2551 	 * packet and schedule timeout. Otherwise do nothing and wait for
2552 	 * L2CA_WRITE_RSP.
2553 	 */
2554 
2555 	sbappendrecord(&pcb->so->so_snd, m);
2556 	m = NULL;
2557 
2558 	if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) {
2559 		error = ng_btsocket_l2cap_send2(pcb);
2560 		if (error == 0)
2561 			ng_btsocket_l2cap_timeout(pcb);
2562 		else
2563 			sbdroprecord(&pcb->so->so_snd); /* XXX */
2564 	}
2565 
2566 	mtx_unlock(&pcb->pcb_mtx);
2567 drop:
2568 	NG_FREE_M(m); /* checks for != NULL */
2569 	NG_FREE_M(control);
2570 
2571 	return (error);
2572 } /* ng_btsocket_l2cap_send */
2573 
2574 /*
2575  * Send first packet in the socket queue to the L2CAP layer
2576  */
2577 
2578 static int
2579 ng_btsocket_l2cap_send2(ng_btsocket_l2cap_pcb_p pcb)
2580 {
2581 	struct	mbuf		*m = NULL;
2582 	ng_l2cap_l2ca_hdr_t	*hdr = NULL;
2583 	int			 error = 0;
2584 
2585 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2586 
2587 	if (sbavail(&pcb->so->so_snd) == 0)
2588 		return (EINVAL); /* XXX */
2589 
2590 	m = m_dup(pcb->so->so_snd.sb_mb, M_NOWAIT);
2591 	if (m == NULL)
2592 		return (ENOBUFS);
2593 
2594 	/* Create L2CA packet header */
2595 	M_PREPEND(m, sizeof(*hdr), M_NOWAIT);
2596 	if (m != NULL)
2597 		if (m->m_len < sizeof(*hdr))
2598 			m = m_pullup(m, sizeof(*hdr));
2599 
2600 	if (m == NULL) {
2601 		NG_BTSOCKET_L2CAP_ERR(
2602 "%s: Failed to create L2CA packet header\n", __func__);
2603 
2604 		return (ENOBUFS);
2605 	}
2606 
2607 	hdr = mtod(m, ng_l2cap_l2ca_hdr_t *);
2608 	hdr->token = pcb->token;
2609 	hdr->length = m->m_pkthdr.len - sizeof(*hdr);
2610 	hdr->lcid = pcb->cid;
2611 	hdr->idtype = ng_btsock_l2cap_pcb_to_idtype(pcb);
2612 	NG_BTSOCKET_L2CAP_INFO(
2613 "%s: Sending packet: len=%d, length=%d, lcid=%d, token=%d, state=%d\n",
2614 		__func__, m->m_pkthdr.len, hdr->length, hdr->lcid,
2615 		hdr->token, pcb->state);
2616 
2617 	/*
2618 	 * If we got here than we have successfuly creates new L2CAP
2619 	 * data packet and now we can send it to the L2CAP layer
2620 	 */
2621 
2622 	NG_SEND_DATA_ONLY(error, pcb->rt->hook, m);
2623 
2624 	return (error);
2625 } /* ng_btsocket_l2cap_send2 */
2626 
2627 /*
2628  * Get socket address
2629  */
2630 
2631 int
2632 ng_btsocket_l2cap_sockaddr(struct socket *so, struct sockaddr **nam)
2633 {
2634 	ng_btsocket_l2cap_pcb_p	pcb = so2l2cap_pcb(so);
2635 	struct sockaddr_l2cap	sa;
2636 
2637 	if (pcb == NULL)
2638 		return (EINVAL);
2639 	if (ng_btsocket_l2cap_node == NULL)
2640 		return (EINVAL);
2641 
2642 	bcopy(&pcb->src, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr));
2643 	sa.l2cap_psm = htole16(pcb->psm);
2644 	sa.l2cap_len = sizeof(sa);
2645 	sa.l2cap_family = AF_BLUETOOTH;
2646 	sa.l2cap_cid = 0;
2647 	sa.l2cap_bdaddr_type = pcb->srctype;
2648 
2649 	*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
2650 
2651 	return ((*nam == NULL)? ENOMEM : 0);
2652 } /* ng_btsocket_l2cap_sockaddr */
2653 
2654 /*****************************************************************************
2655  *****************************************************************************
2656  **                              Misc. functions
2657  *****************************************************************************
2658  *****************************************************************************/
2659 
2660 /*
2661  * Look for the socket that listens on given PSM and bdaddr. Returns exact or
2662  * close match (if any). Caller must hold ng_btsocket_l2cap_sockets_mtx.
2663  */
2664 
2665 static ng_btsocket_l2cap_pcb_p
2666 ng_btsocket_l2cap_pcb_by_addr(bdaddr_p bdaddr, int psm)
2667 {
2668 	ng_btsocket_l2cap_pcb_p	p = NULL, p1 = NULL;
2669 
2670 	mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2671 
2672 	LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) {
2673 		if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN) ||
2674 		    p->psm != psm)
2675 			continue;
2676 
2677 		if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0)
2678 			break;
2679 
2680 		if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0)
2681 			p1 = p;
2682 	}
2683 
2684 	return ((p != NULL)? p : p1);
2685 } /* ng_btsocket_l2cap_pcb_by_addr */
2686 
2687 /*
2688  * Look for the socket that has given token.
2689  * Caller must hold ng_btsocket_l2cap_sockets_mtx.
2690  */
2691 
2692 static ng_btsocket_l2cap_pcb_p
2693 ng_btsocket_l2cap_pcb_by_token(u_int32_t token)
2694 {
2695 	ng_btsocket_l2cap_pcb_p	p = NULL;
2696 
2697 	if (token == 0)
2698 		return (NULL);
2699 
2700 	mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2701 
2702 	LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next)
2703 		if (p->token == token)
2704 			break;
2705 
2706 	return (p);
2707 } /* ng_btsocket_l2cap_pcb_by_token */
2708 
2709 /*
2710  * Look for the socket that assigned to given source address and channel ID.
2711  * Caller must hold ng_btsocket_l2cap_sockets_mtx
2712  */
2713 
2714 static ng_btsocket_l2cap_pcb_p
2715 ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src, int cid, int idtype)
2716 {
2717 	ng_btsocket_l2cap_pcb_p	p = NULL;
2718 
2719 	mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED);
2720 
2721 	LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next){
2722 		if (p->cid == cid &&
2723 		    bcmp(src, &p->src, sizeof(p->src)) == 0&&
2724 		    ng_btsock_l2cap_pcb_to_idtype(p) == idtype)
2725 			break;
2726 
2727 	}
2728 	return (p);
2729 } /* ng_btsocket_l2cap_pcb_by_cid */
2730 
2731 /*
2732  * Set timeout on socket
2733  */
2734 
2735 static void
2736 ng_btsocket_l2cap_timeout(ng_btsocket_l2cap_pcb_p pcb)
2737 {
2738 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2739 
2740 	if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) {
2741 		pcb->flags |= NG_BTSOCKET_L2CAP_TIMO;
2742 		callout_reset(&pcb->timo, bluetooth_l2cap_ertx_timeout(),
2743 		    ng_btsocket_l2cap_process_timeout, pcb);
2744 	} else
2745 		KASSERT(0,
2746 ("%s: Duplicated socket timeout?!\n", __func__));
2747 } /* ng_btsocket_l2cap_timeout */
2748 
2749 /*
2750  * Unset timeout on socket
2751  */
2752 
2753 static void
2754 ng_btsocket_l2cap_untimeout(ng_btsocket_l2cap_pcb_p pcb)
2755 {
2756 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2757 
2758 	if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO) {
2759 		callout_stop(&pcb->timo);
2760 		pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO;
2761 	} else
2762 		KASSERT(0,
2763 ("%s: No socket timeout?!\n", __func__));
2764 } /* ng_btsocket_l2cap_untimeout */
2765 
2766 /*
2767  * Process timeout on socket
2768  */
2769 
2770 static void
2771 ng_btsocket_l2cap_process_timeout(void *xpcb)
2772 {
2773 	ng_btsocket_l2cap_pcb_p	pcb = (ng_btsocket_l2cap_pcb_p) xpcb;
2774 
2775 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
2776 
2777 	pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO;
2778 	pcb->so->so_error = ETIMEDOUT;
2779 
2780 	switch (pcb->state) {
2781 	case NG_BTSOCKET_L2CAP_CONNECTING:
2782 	case NG_BTSOCKET_L2CAP_CONFIGURING:
2783 		/* Send disconnect request with "zero" token */
2784 		if (pcb->cid != 0)
2785 			ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
2786 
2787 		/* ... and close the socket */
2788 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2789 		soisdisconnected(pcb->so);
2790 		break;
2791 
2792 	case NG_BTSOCKET_L2CAP_OPEN:
2793 		/* Send timeout - drop packet and wakeup sender */
2794 		sbdroprecord(&pcb->so->so_snd);
2795 		sowwakeup(pcb->so);
2796 		break;
2797 
2798 	case NG_BTSOCKET_L2CAP_DISCONNECTING:
2799 		/* Disconnect timeout - disconnect the socket anyway */
2800 		pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2801 		soisdisconnected(pcb->so);
2802 		break;
2803 
2804 	default:
2805 		NG_BTSOCKET_L2CAP_ERR(
2806 "%s: Invalid socket state=%d\n", __func__, pcb->state);
2807 		break;
2808 	}
2809 } /* ng_btsocket_l2cap_process_timeout */
2810 
2811 /*
2812  * Translate HCI/L2CAP error code into "errno" code
2813  * XXX Note: Some L2CAP and HCI error codes have the same value, but
2814  *     different meaning
2815  */
2816 
2817 static int
2818 ng_btsocket_l2cap_result2errno(int result)
2819 {
2820 	switch (result) {
2821 	case 0x00: /* No error */
2822 		return (0);
2823 
2824 	case 0x01: /* Unknown HCI command */
2825 		return (ENODEV);
2826 
2827 	case 0x02: /* No connection */
2828 		return (ENOTCONN);
2829 
2830 	case 0x03: /* Hardware failure */
2831 		return (EIO);
2832 
2833 	case 0x04: /* Page timeout */
2834 		return (EHOSTDOWN);
2835 
2836 	case 0x05: /* Authentication failure */
2837 	case 0x06: /* Key missing */
2838 	case 0x18: /* Pairing not allowed */
2839 	case 0x21: /* Role change not allowed */
2840 	case 0x24: /* LMP PSU not allowed */
2841 	case 0x25: /* Encryption mode not acceptable */
2842 	case 0x26: /* Unit key used */
2843 		return (EACCES);
2844 
2845 	case 0x07: /* Memory full */
2846 		return (ENOMEM);
2847 
2848 	case 0x08:   /* Connection timeout */
2849 	case 0x10:   /* Host timeout */
2850 	case 0x22:   /* LMP response timeout */
2851 	case 0xee:   /* HCI timeout */
2852 	case 0xeeee: /* L2CAP timeout */
2853 		return (ETIMEDOUT);
2854 
2855 	case 0x09: /* Max number of connections */
2856 	case 0x0a: /* Max number of SCO connections to a unit */
2857 		return (EMLINK);
2858 
2859 	case 0x0b: /* ACL connection already exists */
2860 		return (EEXIST);
2861 
2862 	case 0x0c: /* Command disallowed */
2863 		return (EBUSY);
2864 
2865 	case 0x0d: /* Host rejected due to limited resources */
2866 	case 0x0e: /* Host rejected due to securiity reasons */
2867 	case 0x0f: /* Host rejected due to remote unit is a personal unit */
2868 	case 0x1b: /* SCO offset rejected */
2869 	case 0x1c: /* SCO interval rejected */
2870 	case 0x1d: /* SCO air mode rejected */
2871 		return (ECONNREFUSED);
2872 
2873 	case 0x11: /* Unsupported feature or parameter value */
2874 	case 0x19: /* Unknown LMP PDU */
2875 	case 0x1a: /* Unsupported remote feature */
2876 	case 0x20: /* Unsupported LMP parameter value */
2877 	case 0x27: /* QoS is not supported */
2878 	case 0x29: /* Paring with unit key not supported */
2879 		return (EOPNOTSUPP);
2880 
2881 	case 0x12: /* Invalid HCI command parameter */
2882 	case 0x1e: /* Invalid LMP parameters */
2883 		return (EINVAL);
2884 
2885 	case 0x13: /* Other end terminated connection: User ended connection */
2886 	case 0x14: /* Other end terminated connection: Low resources */
2887 	case 0x15: /* Other end terminated connection: About to power off */
2888 		return (ECONNRESET);
2889 
2890 	case 0x16: /* Connection terminated by local host */
2891 		return (ECONNABORTED);
2892 
2893 #if 0 /* XXX not yet */
2894 	case 0x17: /* Repeated attempts */
2895 	case 0x1f: /* Unspecified error */
2896 	case 0x23: /* LMP error transaction collision */
2897 	case 0x28: /* Instant passed */
2898 #endif
2899 	}
2900 
2901 	return (ENOSYS);
2902 } /* ng_btsocket_l2cap_result2errno */
2903 
2904