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