xref: /freebsd/sys/netgraph/bluetooth/socket/ng_btsocket_sco.c (revision 77a1348b3c1cfe8547be49a121b56299a1e18b69)
1 /*
2  * ng_btsocket_sco.c
3  */
4 
5 /*-
6  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
7  *
8  * Copyright (c) 2001-2002 Maksim Yevmenkin <m_evmenkin@yahoo.com>
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  * 1. Redistributions of source code must retain the above copyright
15  *    notice, this list of conditions and the following disclaimer.
16  * 2. Redistributions in binary form must reproduce the above copyright
17  *    notice, this list of conditions and the following disclaimer in the
18  *    documentation and/or other materials provided with the distribution.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
21  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
26  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
29  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
30  * SUCH DAMAGE.
31  *
32  * $Id: ng_btsocket_sco.c,v 1.2 2005/10/31 18:08:51 max Exp $
33  * $FreeBSD$
34  */
35 
36 #include <sys/param.h>
37 #include <sys/systm.h>
38 #include <sys/bitstring.h>
39 #include <sys/domain.h>
40 #include <sys/endian.h>
41 #include <sys/errno.h>
42 #include <sys/filedesc.h>
43 #include <sys/ioccom.h>
44 #include <sys/kernel.h>
45 #include <sys/lock.h>
46 #include <sys/malloc.h>
47 #include <sys/mbuf.h>
48 #include <sys/mutex.h>
49 #include <sys/protosw.h>
50 #include <sys/queue.h>
51 #include <sys/socket.h>
52 #include <sys/socketvar.h>
53 #include <sys/sysctl.h>
54 #include <sys/taskqueue.h>
55 
56 #include <net/vnet.h>
57 
58 #include <netgraph/ng_message.h>
59 #include <netgraph/netgraph.h>
60 #include <netgraph/bluetooth/include/ng_bluetooth.h>
61 #include <netgraph/bluetooth/include/ng_hci.h>
62 #include <netgraph/bluetooth/include/ng_l2cap.h>
63 #include <netgraph/bluetooth/include/ng_btsocket.h>
64 #include <netgraph/bluetooth/include/ng_btsocket_sco.h>
65 
66 /* MALLOC define */
67 #ifdef NG_SEPARATE_MALLOC
68 static MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_SCO, "netgraph_btsocks_sco",
69 		"Netgraph Bluetooth SCO sockets");
70 #else
71 #define M_NETGRAPH_BTSOCKET_SCO M_NETGRAPH
72 #endif /* NG_SEPARATE_MALLOC */
73 
74 /* Netgraph node methods */
75 static ng_constructor_t	ng_btsocket_sco_node_constructor;
76 static ng_rcvmsg_t	ng_btsocket_sco_node_rcvmsg;
77 static ng_shutdown_t	ng_btsocket_sco_node_shutdown;
78 static ng_newhook_t	ng_btsocket_sco_node_newhook;
79 static ng_connect_t	ng_btsocket_sco_node_connect;
80 static ng_rcvdata_t	ng_btsocket_sco_node_rcvdata;
81 static ng_disconnect_t	ng_btsocket_sco_node_disconnect;
82 
83 static void		ng_btsocket_sco_input   (void *, int);
84 static void		ng_btsocket_sco_rtclean (void *, int);
85 
86 /* Netgraph type descriptor */
87 static struct ng_type	typestruct = {
88 	.version =	NG_ABI_VERSION,
89 	.name =		NG_BTSOCKET_SCO_NODE_TYPE,
90 	.constructor =	ng_btsocket_sco_node_constructor,
91 	.rcvmsg =	ng_btsocket_sco_node_rcvmsg,
92 	.shutdown =	ng_btsocket_sco_node_shutdown,
93 	.newhook =	ng_btsocket_sco_node_newhook,
94 	.connect =	ng_btsocket_sco_node_connect,
95 	.rcvdata =	ng_btsocket_sco_node_rcvdata,
96 	.disconnect =	ng_btsocket_sco_node_disconnect,
97 };
98 
99 /* Globals */
100 static u_int32_t				ng_btsocket_sco_debug_level;
101 static node_p					ng_btsocket_sco_node;
102 static struct ng_bt_itemq			ng_btsocket_sco_queue;
103 static struct mtx				ng_btsocket_sco_queue_mtx;
104 static struct task				ng_btsocket_sco_queue_task;
105 static struct mtx				ng_btsocket_sco_sockets_mtx;
106 static LIST_HEAD(, ng_btsocket_sco_pcb)		ng_btsocket_sco_sockets;
107 static LIST_HEAD(, ng_btsocket_sco_rtentry)	ng_btsocket_sco_rt;
108 static struct mtx				ng_btsocket_sco_rt_mtx;
109 static struct task				ng_btsocket_sco_rt_task;
110 static struct timeval				ng_btsocket_sco_lasttime;
111 static int					ng_btsocket_sco_curpps;
112 
113 /* Sysctl tree */
114 SYSCTL_DECL(_net_bluetooth_sco_sockets);
115 static SYSCTL_NODE(_net_bluetooth_sco_sockets, OID_AUTO, seq,
116     CTLFLAG_RW | CTLFLAG_MPSAFE, 0,
117     "Bluetooth SEQPACKET SCO sockets family");
118 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, debug_level,
119 	CTLFLAG_RW,
120 	&ng_btsocket_sco_debug_level, NG_BTSOCKET_WARN_LEVEL,
121 	"Bluetooth SEQPACKET SCO sockets debug level");
122 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_len,
123 	CTLFLAG_RD,
124 	&ng_btsocket_sco_queue.len, 0,
125 	"Bluetooth SEQPACKET SCO sockets input queue length");
126 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_maxlen,
127 	CTLFLAG_RD,
128 	&ng_btsocket_sco_queue.maxlen, 0,
129 	"Bluetooth SEQPACKET SCO sockets input queue max. length");
130 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_drops,
131 	CTLFLAG_RD,
132 	&ng_btsocket_sco_queue.drops, 0,
133 	"Bluetooth SEQPACKET SCO sockets input queue drops");
134 
135 /* Debug */
136 #define NG_BTSOCKET_SCO_INFO \
137 	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_INFO_LEVEL && \
138 	    ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
139 		printf
140 
141 #define NG_BTSOCKET_SCO_WARN \
142 	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_WARN_LEVEL && \
143 	    ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
144 		printf
145 
146 #define NG_BTSOCKET_SCO_ERR \
147 	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_ERR_LEVEL && \
148 	    ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
149 		printf
150 
151 #define NG_BTSOCKET_SCO_ALERT \
152 	if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_ALERT_LEVEL && \
153 	    ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
154 		printf
155 
156 /*
157  * Netgraph message processing routines
158  */
159 
160 static int ng_btsocket_sco_process_lp_con_cfm
161 	(struct ng_mesg *, ng_btsocket_sco_rtentry_p);
162 static int ng_btsocket_sco_process_lp_con_ind
163 	(struct ng_mesg *, ng_btsocket_sco_rtentry_p);
164 static int ng_btsocket_sco_process_lp_discon_ind
165 	(struct ng_mesg *, ng_btsocket_sco_rtentry_p);
166 
167 /*
168  * Send LP messages to the lower layer
169  */
170 
171 static int  ng_btsocket_sco_send_lp_con_req
172 	(ng_btsocket_sco_pcb_p);
173 static int  ng_btsocket_sco_send_lp_con_rsp
174 	(ng_btsocket_sco_rtentry_p, bdaddr_p, int);
175 static int  ng_btsocket_sco_send_lp_discon_req
176 	(ng_btsocket_sco_pcb_p);
177 
178 static int ng_btsocket_sco_send2
179 	(ng_btsocket_sco_pcb_p);
180 
181 /*
182  * Timeout processing routines
183  */
184 
185 static void ng_btsocket_sco_timeout         (ng_btsocket_sco_pcb_p);
186 static void ng_btsocket_sco_untimeout       (ng_btsocket_sco_pcb_p);
187 static void ng_btsocket_sco_process_timeout (void *);
188 
189 /*
190  * Other stuff
191  */
192 
193 static ng_btsocket_sco_pcb_p	ng_btsocket_sco_pcb_by_addr(bdaddr_p);
194 static ng_btsocket_sco_pcb_p	ng_btsocket_sco_pcb_by_handle(bdaddr_p, int);
195 static ng_btsocket_sco_pcb_p	ng_btsocket_sco_pcb_by_addrs(bdaddr_p, bdaddr_p);
196 
197 #define ng_btsocket_sco_wakeup_input_task() \
198 	taskqueue_enqueue(taskqueue_swi, &ng_btsocket_sco_queue_task)
199 
200 #define ng_btsocket_sco_wakeup_route_task() \
201 	taskqueue_enqueue(taskqueue_swi, &ng_btsocket_sco_rt_task)
202 
203 /*****************************************************************************
204  *****************************************************************************
205  **                        Netgraph node interface
206  *****************************************************************************
207  *****************************************************************************/
208 
209 /*
210  * Netgraph node constructor. Do not allow to create node of this type.
211  */
212 
213 static int
214 ng_btsocket_sco_node_constructor(node_p node)
215 {
216 	return (EINVAL);
217 } /* ng_btsocket_sco_node_constructor */
218 
219 /*
220  * Do local shutdown processing. Let old node go and create new fresh one.
221  */
222 
223 static int
224 ng_btsocket_sco_node_shutdown(node_p node)
225 {
226 	int	error = 0;
227 
228 	NG_NODE_UNREF(node);
229 
230 	/* Create new node */
231 	error = ng_make_node_common(&typestruct, &ng_btsocket_sco_node);
232 	if (error != 0) {
233 		NG_BTSOCKET_SCO_ALERT(
234 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
235 
236 		ng_btsocket_sco_node = NULL;
237 
238 		return (error);
239 	}
240 
241 	error = ng_name_node(ng_btsocket_sco_node,
242 				NG_BTSOCKET_SCO_NODE_TYPE);
243 	if (error != 0) {
244 		NG_BTSOCKET_SCO_ALERT(
245 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
246 
247 		NG_NODE_UNREF(ng_btsocket_sco_node);
248 		ng_btsocket_sco_node = NULL;
249 
250 		return (error);
251 	}
252 
253 	return (0);
254 } /* ng_btsocket_sco_node_shutdown */
255 
256 /*
257  * We allow any hook to be connected to the node.
258  */
259 
260 static int
261 ng_btsocket_sco_node_newhook(node_p node, hook_p hook, char const *name)
262 {
263 	return (0);
264 } /* ng_btsocket_sco_node_newhook */
265 
266 /*
267  * Just say "YEP, that's OK by me!"
268  */
269 
270 static int
271 ng_btsocket_sco_node_connect(hook_p hook)
272 {
273 	NG_HOOK_SET_PRIVATE(hook, NULL);
274 	NG_HOOK_REF(hook); /* Keep extra reference to the hook */
275 
276 #if 0
277 	NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
278 	NG_HOOK_FORCE_QUEUE(hook);
279 #endif
280 
281 	return (0);
282 } /* ng_btsocket_sco_node_connect */
283 
284 /*
285  * Hook disconnection. Schedule route cleanup task
286  */
287 
288 static int
289 ng_btsocket_sco_node_disconnect(hook_p hook)
290 {
291 	/*
292 	 * If hook has private information than we must have this hook in
293 	 * the routing table and must schedule cleaning for the routing table.
294 	 * Otherwise hook was connected but we never got "hook_info" message,
295 	 * so we have never added this hook to the routing table and it save
296 	 * to just delete it.
297 	 */
298 
299 	if (NG_HOOK_PRIVATE(hook) != NULL)
300 		return (ng_btsocket_sco_wakeup_route_task());
301 
302 	NG_HOOK_UNREF(hook); /* Remove extra reference */
303 
304 	return (0);
305 } /* ng_btsocket_sco_node_disconnect */
306 
307 /*
308  * Process incoming messages
309  */
310 
311 static int
312 ng_btsocket_sco_node_rcvmsg(node_p node, item_p item, hook_p hook)
313 {
314 	struct ng_mesg	*msg = NGI_MSG(item); /* item still has message */
315 	int		 error = 0;
316 
317 	if (msg != NULL && msg->header.typecookie == NGM_HCI_COOKIE) {
318 		mtx_lock(&ng_btsocket_sco_queue_mtx);
319 		if (NG_BT_ITEMQ_FULL(&ng_btsocket_sco_queue)) {
320 			NG_BTSOCKET_SCO_ERR(
321 "%s: Input queue is full (msg)\n", __func__);
322 
323 			NG_BT_ITEMQ_DROP(&ng_btsocket_sco_queue);
324 			NG_FREE_ITEM(item);
325 			error = ENOBUFS;
326 		} else {
327 			if (hook != NULL) {
328 				NG_HOOK_REF(hook);
329 				NGI_SET_HOOK(item, hook);
330 			}
331 
332 			NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_sco_queue, item);
333 			error = ng_btsocket_sco_wakeup_input_task();
334 		}
335 		mtx_unlock(&ng_btsocket_sco_queue_mtx);
336 	} else {
337 		NG_FREE_ITEM(item);
338 		error = EINVAL;
339 	}
340 
341 	return (error);
342 } /* ng_btsocket_sco_node_rcvmsg */
343 
344 /*
345  * Receive data on a hook
346  */
347 
348 static int
349 ng_btsocket_sco_node_rcvdata(hook_p hook, item_p item)
350 {
351 	int	error = 0;
352 
353 	mtx_lock(&ng_btsocket_sco_queue_mtx);
354 	if (NG_BT_ITEMQ_FULL(&ng_btsocket_sco_queue)) {
355 		NG_BTSOCKET_SCO_ERR(
356 "%s: Input queue is full (data)\n", __func__);
357 
358 		NG_BT_ITEMQ_DROP(&ng_btsocket_sco_queue);
359 		NG_FREE_ITEM(item);
360 		error = ENOBUFS;
361 	} else {
362 		NG_HOOK_REF(hook);
363 		NGI_SET_HOOK(item, hook);
364 
365 		NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_sco_queue, item);
366 		error = ng_btsocket_sco_wakeup_input_task();
367 	}
368 	mtx_unlock(&ng_btsocket_sco_queue_mtx);
369 
370 	return (error);
371 } /* ng_btsocket_sco_node_rcvdata */
372 
373 /*
374  * Process LP_ConnectCfm event from the lower layer protocol
375  */
376 
377 static int
378 ng_btsocket_sco_process_lp_con_cfm(struct ng_mesg *msg,
379 		ng_btsocket_sco_rtentry_p rt)
380 {
381 	ng_hci_lp_con_cfm_ep	*ep = NULL;
382 	ng_btsocket_sco_pcb_t	*pcb = NULL;
383 	int			 error = 0;
384 
385 	if (msg->header.arglen != sizeof(*ep))
386 		return (EMSGSIZE);
387 
388 	ep = (ng_hci_lp_con_cfm_ep *)(msg->data);
389 
390 	mtx_lock(&ng_btsocket_sco_sockets_mtx);
391 
392 	/* Look for the socket with the token */
393 	pcb = ng_btsocket_sco_pcb_by_addrs(&rt->src, &ep->bdaddr);
394 	if (pcb == NULL) {
395 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
396 		return (ENOENT);
397 	}
398 
399 	/* pcb is locked */
400 
401 	NG_BTSOCKET_SCO_INFO(
402 "%s: Got LP_ConnectCfm response, src bdaddr=%x:%x:%x:%x:%x:%x, " \
403 "dst bdaddr=%x:%x:%x:%x:%x:%x, status=%d, handle=%d, state=%d\n",
404 		__func__,
405 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
406 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
407 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
408 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
409 		ep->status, ep->con_handle, pcb->state);
410 
411 	if (pcb->state != NG_BTSOCKET_SCO_CONNECTING) {
412 		mtx_unlock(&pcb->pcb_mtx);
413 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
414 
415 		return (ENOENT);
416 	}
417 
418 	ng_btsocket_sco_untimeout(pcb);
419 
420 	if (ep->status == 0) {
421 		/*
422 		 * Connection is open. Update connection handle and
423 		 * socket state
424 		 */
425 
426 		pcb->con_handle = ep->con_handle;
427 		pcb->state = NG_BTSOCKET_SCO_OPEN;
428 		soisconnected(pcb->so);
429 	} else {
430 		/*
431 		 * We have failed to open connection, so disconnect the socket
432 		 */
433 
434 		pcb->so->so_error = ECONNREFUSED; /* XXX convert status ??? */
435 		pcb->state = NG_BTSOCKET_SCO_CLOSED;
436 		soisdisconnected(pcb->so);
437 	}
438 
439 	mtx_unlock(&pcb->pcb_mtx);
440 	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
441 
442 	return (error);
443 } /* ng_btsocket_sco_process_lp_con_cfm */
444 
445 /*
446  * Process LP_ConnectInd indicator. Find socket that listens on address.
447  * Find exact or closest match.
448  */
449 
450 static int
451 ng_btsocket_sco_process_lp_con_ind(struct ng_mesg *msg,
452 		ng_btsocket_sco_rtentry_p rt)
453 {
454 	ng_hci_lp_con_ind_ep	*ep = NULL;
455 	ng_btsocket_sco_pcb_t	*pcb = NULL, *pcb1 = NULL;
456 	int			 error = 0;
457 	u_int16_t		 status = 0;
458 
459 	if (msg->header.arglen != sizeof(*ep))
460 		return (EMSGSIZE);
461 
462 	ep = (ng_hci_lp_con_ind_ep *)(msg->data);
463 
464 	NG_BTSOCKET_SCO_INFO(
465 "%s: Got LP_ConnectInd indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
466 "dst bdaddr=%x:%x:%x:%x:%x:%x\n",
467 		__func__,
468 		rt->src.b[5], rt->src.b[4], rt->src.b[3],
469 		rt->src.b[2], rt->src.b[1], rt->src.b[0],
470 		ep->bdaddr.b[5], ep->bdaddr.b[4], ep->bdaddr.b[3],
471 		ep->bdaddr.b[2], ep->bdaddr.b[1], ep->bdaddr.b[0]);
472 
473 	mtx_lock(&ng_btsocket_sco_sockets_mtx);
474 
475 	pcb = ng_btsocket_sco_pcb_by_addr(&rt->src);
476 	if (pcb != NULL) {
477 		struct socket *so1;
478 
479 		/* pcb is locked */
480 
481 		CURVNET_SET(pcb->so->so_vnet);
482 		so1 = sonewconn(pcb->so, 0);
483 		CURVNET_RESTORE();
484 
485 		if (so1 == NULL) {
486 			status = 0x0d; /* Rejected due to limited resources */
487 			goto respond;
488 		}
489 
490 		/*
491 		 * If we got here than we have created new socket. So complete
492 		 * connection. If we we listening on specific address then copy
493 		 * source address from listening socket, otherwise copy source
494 		 * address from hook's routing information.
495 		 */
496 
497 		pcb1 = so2sco_pcb(so1);
498 		KASSERT((pcb1 != NULL),
499 ("%s: pcb1 == NULL\n", __func__));
500 
501  		mtx_lock(&pcb1->pcb_mtx);
502 
503 		if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)) != 0)
504 			bcopy(&pcb->src, &pcb1->src, sizeof(pcb1->src));
505 		else
506 			bcopy(&rt->src, &pcb1->src, sizeof(pcb1->src));
507 
508 		pcb1->flags &= ~NG_BTSOCKET_SCO_CLIENT;
509 
510 		bcopy(&ep->bdaddr, &pcb1->dst, sizeof(pcb1->dst));
511 		pcb1->rt = rt;
512 	} else
513 		/* Nobody listens on requested BDADDR */
514 		status = 0x1f; /* Unspecified Error */
515 
516 respond:
517 	error = ng_btsocket_sco_send_lp_con_rsp(rt, &ep->bdaddr, status);
518 	if (pcb1 != NULL) {
519 		if (error != 0) {
520 			pcb1->so->so_error = error;
521 			pcb1->state = NG_BTSOCKET_SCO_CLOSED;
522 			soisdisconnected(pcb1->so);
523 		} else {
524 			pcb1->state = NG_BTSOCKET_SCO_CONNECTING;
525 			soisconnecting(pcb1->so);
526 
527 			ng_btsocket_sco_timeout(pcb1);
528 		}
529 
530 		mtx_unlock(&pcb1->pcb_mtx);
531 	}
532 
533 	if (pcb != NULL)
534 		mtx_unlock(&pcb->pcb_mtx);
535 
536 	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
537 
538 	return (error);
539 } /* ng_btsocket_sco_process_lp_con_ind */
540 
541 /*
542  * Process LP_DisconnectInd indicator
543  */
544 
545 static int
546 ng_btsocket_sco_process_lp_discon_ind(struct ng_mesg *msg,
547 		ng_btsocket_sco_rtentry_p rt)
548 {
549 	ng_hci_lp_discon_ind_ep	*ep = NULL;
550 	ng_btsocket_sco_pcb_t	*pcb = NULL;
551 
552 	/* Check message */
553 	if (msg->header.arglen != sizeof(*ep))
554 		return (EMSGSIZE);
555 
556 	ep = (ng_hci_lp_discon_ind_ep *)(msg->data);
557 
558 	mtx_lock(&ng_btsocket_sco_sockets_mtx);
559 
560 	/* Look for the socket with given channel ID */
561 	pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, ep->con_handle);
562 	if (pcb == NULL) {
563 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
564 		return (0);
565 	}
566 
567 	/*
568 	 * Disconnect the socket. If there was any pending request we can
569 	 * not do anything here anyway.
570 	 */
571 
572 	/* pcb is locked */
573 
574        	NG_BTSOCKET_SCO_INFO(
575 "%s: Got LP_DisconnectInd indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
576 "dst bdaddr=%x:%x:%x:%x:%x:%x, handle=%d, state=%d\n",
577 		__func__,
578 		pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
579 		pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
580 		pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
581 		pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
582 		pcb->con_handle, pcb->state);
583 
584 	if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
585 		ng_btsocket_sco_untimeout(pcb);
586 
587 	pcb->state = NG_BTSOCKET_SCO_CLOSED;
588 	soisdisconnected(pcb->so);
589 
590 	mtx_unlock(&pcb->pcb_mtx);
591 	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
592 
593 	return (0);
594 } /* ng_btsocket_sco_process_lp_discon_ind */
595 
596 /*
597  * Send LP_ConnectReq request
598  */
599 
600 static int
601 ng_btsocket_sco_send_lp_con_req(ng_btsocket_sco_pcb_p pcb)
602 {
603 	struct ng_mesg		*msg = NULL;
604 	ng_hci_lp_con_req_ep	*ep = NULL;
605 	int			 error = 0;
606 
607 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
608 
609 	if (pcb->rt == NULL ||
610 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
611 		return (ENETDOWN);
612 
613 	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ,
614 		sizeof(*ep), M_NOWAIT);
615 	if (msg == NULL)
616 		return (ENOMEM);
617 
618 	ep = (ng_hci_lp_con_req_ep *)(msg->data);
619 	ep->link_type = NG_HCI_LINK_SCO;
620 	bcopy(&pcb->dst, &ep->bdaddr, sizeof(ep->bdaddr));
621 
622 	NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, pcb->rt->hook, 0);
623 
624 	return (error);
625 } /* ng_btsocket_sco_send_lp_con_req */
626 
627 /*
628  * Send LP_ConnectRsp response
629  */
630 
631 static int
632 ng_btsocket_sco_send_lp_con_rsp(ng_btsocket_sco_rtentry_p rt, bdaddr_p dst, int status)
633 {
634 	struct ng_mesg		*msg = NULL;
635 	ng_hci_lp_con_rsp_ep	*ep = NULL;
636 	int			 error = 0;
637 
638 	if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
639 		return (ENETDOWN);
640 
641 	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP,
642 		sizeof(*ep), M_NOWAIT);
643 	if (msg == NULL)
644 		return (ENOMEM);
645 
646 	ep = (ng_hci_lp_con_rsp_ep *)(msg->data);
647 	ep->status = status;
648 	ep->link_type = NG_HCI_LINK_SCO;
649 	bcopy(dst, &ep->bdaddr, sizeof(ep->bdaddr));
650 
651 	NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, rt->hook, 0);
652 
653 	return (error);
654 } /* ng_btsocket_sco_send_lp_con_rsp */
655 
656 /*
657  * Send LP_DisconReq request
658  */
659 
660 static int
661 ng_btsocket_sco_send_lp_discon_req(ng_btsocket_sco_pcb_p pcb)
662 {
663 	struct ng_mesg		*msg = NULL;
664 	ng_hci_lp_discon_req_ep	*ep = NULL;
665 	int			 error = 0;
666 
667 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
668 
669 	if (pcb->rt == NULL ||
670 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
671 		return (ENETDOWN);
672 
673 	NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ,
674 		sizeof(*ep), M_NOWAIT);
675 	if (msg == NULL)
676 		return (ENOMEM);
677 
678 	ep = (ng_hci_lp_discon_req_ep *)(msg->data);
679 	ep->con_handle = pcb->con_handle;
680 	ep->reason = 0x13; /* User Ended Connection */
681 
682 	NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, pcb->rt->hook, 0);
683 
684 	return (error);
685 } /* ng_btsocket_sco_send_lp_discon_req */
686 
687 /*****************************************************************************
688  *****************************************************************************
689  **                              Socket interface
690  *****************************************************************************
691  *****************************************************************************/
692 
693 /*
694  * SCO sockets data input routine
695  */
696 
697 static void
698 ng_btsocket_sco_data_input(struct mbuf *m, hook_p hook)
699 {
700 	ng_hci_scodata_pkt_t		*hdr = NULL;
701 	ng_btsocket_sco_pcb_t		*pcb = NULL;
702 	ng_btsocket_sco_rtentry_t	*rt = NULL;
703 	u_int16_t			 con_handle;
704 
705 	if (hook == NULL) {
706 		NG_BTSOCKET_SCO_ALERT(
707 "%s: Invalid source hook for SCO data packet\n", __func__);
708 		goto drop;
709 	}
710 
711 	rt = (ng_btsocket_sco_rtentry_t *) NG_HOOK_PRIVATE(hook);
712 	if (rt == NULL) {
713 		NG_BTSOCKET_SCO_ALERT(
714 "%s: Could not find out source bdaddr for SCO data packet\n", __func__);
715 		goto drop;
716 	}
717 
718 	/* Make sure we can access header */
719 	if (m->m_pkthdr.len < sizeof(*hdr)) {
720 		NG_BTSOCKET_SCO_ERR(
721 "%s: SCO data packet too small, len=%d\n", __func__, m->m_pkthdr.len);
722 		goto drop;
723 	}
724 
725 	if (m->m_len < sizeof(*hdr)) {
726 		m = m_pullup(m, sizeof(*hdr));
727 		if (m == NULL)
728 			goto drop;
729 	}
730 
731 	/* Strip SCO packet header and verify packet length */
732 	hdr = mtod(m, ng_hci_scodata_pkt_t *);
733 	m_adj(m, sizeof(*hdr));
734 
735 	if (hdr->length != m->m_pkthdr.len) {
736 		NG_BTSOCKET_SCO_ERR(
737 "%s: Bad SCO data packet length, len=%d, length=%d\n",
738 			__func__, m->m_pkthdr.len, hdr->length);
739 		goto drop;
740 	}
741 
742 	/*
743 	 * Now process packet
744 	 */
745 
746 	con_handle = NG_HCI_CON_HANDLE(le16toh(hdr->con_handle));
747 
748 	NG_BTSOCKET_SCO_INFO(
749 "%s: Received SCO data packet: src bdaddr=%x:%x:%x:%x:%x:%x, handle=%d, " \
750 "length=%d\n",	__func__,
751 		rt->src.b[5], rt->src.b[4], rt->src.b[3],
752 		rt->src.b[2], rt->src.b[1], rt->src.b[0],
753 		con_handle, hdr->length);
754 
755 	mtx_lock(&ng_btsocket_sco_sockets_mtx);
756 
757 	/* Find socket */
758 	pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, con_handle);
759 	if (pcb == NULL) {
760 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
761 		goto drop;
762 	}
763 
764 	/* pcb is locked */
765 
766 	if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
767 		NG_BTSOCKET_SCO_ERR(
768 "%s: No connected socket found, src bdaddr=%x:%x:%x:%x:%x:%x, state=%d\n",
769 			__func__,
770 			rt->src.b[5], rt->src.b[4], rt->src.b[3],
771 			rt->src.b[2], rt->src.b[1], rt->src.b[0],
772 			pcb->state);
773 
774 		mtx_unlock(&pcb->pcb_mtx);
775 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
776 		goto drop;
777 	}
778 
779 	/* Check if we have enough space in socket receive queue */
780 	if (m->m_pkthdr.len > sbspace(&pcb->so->so_rcv)) {
781 		NG_BTSOCKET_SCO_ERR(
782 "%s: Not enough space in socket receive queue. Dropping SCO data packet, " \
783 "src bdaddr=%x:%x:%x:%x:%x:%x, len=%d, space=%ld\n",
784 			__func__,
785 			rt->src.b[5], rt->src.b[4], rt->src.b[3],
786 			rt->src.b[2], rt->src.b[1], rt->src.b[0],
787 			m->m_pkthdr.len,
788 			sbspace(&pcb->so->so_rcv));
789 
790 		mtx_unlock(&pcb->pcb_mtx);
791 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
792 		goto drop;
793 	}
794 
795 	/* Append packet to the socket receive queue and wakeup */
796 	sbappendrecord(&pcb->so->so_rcv, m);
797 	m = NULL;
798 
799 	sorwakeup(pcb->so);
800 
801 	mtx_unlock(&pcb->pcb_mtx);
802 	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
803 drop:
804 	NG_FREE_M(m); /* checks for m != NULL */
805 } /* ng_btsocket_sco_data_input */
806 
807 /*
808  * SCO sockets default message input routine
809  */
810 
811 static void
812 ng_btsocket_sco_default_msg_input(struct ng_mesg *msg, hook_p hook)
813 {
814 	ng_btsocket_sco_rtentry_t	*rt = NULL;
815 
816 	if (hook == NULL || NG_HOOK_NOT_VALID(hook))
817 		return;
818 
819 	rt = (ng_btsocket_sco_rtentry_t *) NG_HOOK_PRIVATE(hook);
820 
821 	switch (msg->header.cmd) {
822 	case NGM_HCI_NODE_UP: {
823 		ng_hci_node_up_ep	*ep = NULL;
824 
825 		if (msg->header.arglen != sizeof(*ep))
826 			break;
827 
828 		ep = (ng_hci_node_up_ep *)(msg->data);
829 		if (bcmp(&ep->bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
830 			break;
831 
832 		if (rt == NULL) {
833 			rt = malloc(sizeof(*rt),
834 				M_NETGRAPH_BTSOCKET_SCO, M_NOWAIT|M_ZERO);
835 			if (rt == NULL)
836 				break;
837 
838 			NG_HOOK_SET_PRIVATE(hook, rt);
839 
840 			mtx_lock(&ng_btsocket_sco_rt_mtx);
841 
842 			LIST_INSERT_HEAD(&ng_btsocket_sco_rt, rt, next);
843 		} else
844 			mtx_lock(&ng_btsocket_sco_rt_mtx);
845 
846 		bcopy(&ep->bdaddr, &rt->src, sizeof(rt->src));
847 		rt->pkt_size = (ep->pkt_size == 0)? 60 : ep->pkt_size;
848 		rt->num_pkts = ep->num_pkts;
849 		rt->hook = hook;
850 
851 		mtx_unlock(&ng_btsocket_sco_rt_mtx);
852 
853 		NG_BTSOCKET_SCO_INFO(
854 "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x, pkt_size=%d, " \
855 "num_pkts=%d\n",	__func__, NG_HOOK_NAME(hook),
856 			rt->src.b[5], rt->src.b[4], rt->src.b[3],
857 			rt->src.b[2], rt->src.b[1], rt->src.b[0],
858 			rt->pkt_size, rt->num_pkts);
859 		} break;
860 
861 	case NGM_HCI_SYNC_CON_QUEUE: {
862 		ng_hci_sync_con_queue_ep	*ep = NULL;
863 		ng_btsocket_sco_pcb_t		*pcb = NULL;
864 
865 		if (rt == NULL || msg->header.arglen != sizeof(*ep))
866 			break;
867 
868 		ep = (ng_hci_sync_con_queue_ep *)(msg->data);
869 
870 		rt->pending -= ep->completed;
871 		if (rt->pending < 0) {
872 			NG_BTSOCKET_SCO_WARN(
873 "%s: Pending packet counter is out of sync! bdaddr=%x:%x:%x:%x:%x:%x, " \
874 "handle=%d, pending=%d, completed=%d\n",
875 				__func__,
876 				rt->src.b[5], rt->src.b[4], rt->src.b[3],
877 				rt->src.b[2], rt->src.b[1], rt->src.b[0],
878 				ep->con_handle, rt->pending,
879 				ep->completed);
880 
881 			rt->pending = 0;
882 		}
883 
884 		mtx_lock(&ng_btsocket_sco_sockets_mtx);
885 
886 		/* Find socket */
887 		pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, ep->con_handle);
888 		if (pcb == NULL) {
889 			mtx_unlock(&ng_btsocket_sco_sockets_mtx);
890 			break;
891 		}
892 
893 		/* pcb is locked */
894 
895 		/* Check state */
896 		if (pcb->state == NG_BTSOCKET_SCO_OPEN) {
897 			/* Remove timeout */
898 			ng_btsocket_sco_untimeout(pcb);
899 
900 			/* Drop completed packets from the send queue */
901 			for (; ep->completed > 0; ep->completed --)
902 				sbdroprecord(&pcb->so->so_snd);
903 
904 			/* Send more if we have any */
905 			if (sbavail(&pcb->so->so_snd) > 0)
906 				if (ng_btsocket_sco_send2(pcb) == 0)
907 					ng_btsocket_sco_timeout(pcb);
908 
909 			/* Wake up writers */
910 			sowwakeup(pcb->so);
911 		}
912 
913 		mtx_unlock(&pcb->pcb_mtx);
914 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
915 	} break;
916 
917 	default:
918 		NG_BTSOCKET_SCO_WARN(
919 "%s: Unknown message, cmd=%d\n", __func__, msg->header.cmd);
920 		break;
921 	}
922 
923 	NG_FREE_MSG(msg); /* Checks for msg != NULL */
924 } /* ng_btsocket_sco_default_msg_input */
925 
926 /*
927  * SCO sockets LP message input routine
928  */
929 
930 static void
931 ng_btsocket_sco_lp_msg_input(struct ng_mesg *msg, hook_p hook)
932 {
933 	ng_btsocket_sco_rtentry_p	 rt = NULL;
934 
935 	if (hook == NULL) {
936 		NG_BTSOCKET_SCO_ALERT(
937 "%s: Invalid source hook for LP message\n", __func__);
938 		goto drop;
939 	}
940 
941 	rt = (ng_btsocket_sco_rtentry_p) NG_HOOK_PRIVATE(hook);
942 	if (rt == NULL) {
943 		NG_BTSOCKET_SCO_ALERT(
944 "%s: Could not find out source bdaddr for LP message\n", __func__);
945 		goto drop;
946 	}
947 
948 	switch (msg->header.cmd) {
949 	case NGM_HCI_LP_CON_CFM: /* Connection Confirmation Event */
950 		ng_btsocket_sco_process_lp_con_cfm(msg, rt);
951 		break;
952 
953 	case NGM_HCI_LP_CON_IND: /* Connection Indication Event */
954 		ng_btsocket_sco_process_lp_con_ind(msg, rt);
955 		break;
956 
957 	case NGM_HCI_LP_DISCON_IND: /* Disconnection Indication Event */
958 		ng_btsocket_sco_process_lp_discon_ind(msg, rt);
959 		break;
960 
961 	/* XXX FIXME add other LP messages */
962 
963 	default:
964 		NG_BTSOCKET_SCO_WARN(
965 "%s: Unknown LP message, cmd=%d\n", __func__, msg->header.cmd);
966 		break;
967 	}
968 drop:
969 	NG_FREE_MSG(msg);
970 } /* ng_btsocket_sco_lp_msg_input */
971 
972 /*
973  * SCO sockets input routine
974  */
975 
976 static void
977 ng_btsocket_sco_input(void *context, int pending)
978 {
979 	item_p	item = NULL;
980 	hook_p	hook = NULL;
981 
982 	for (;;) {
983 		mtx_lock(&ng_btsocket_sco_queue_mtx);
984 		NG_BT_ITEMQ_DEQUEUE(&ng_btsocket_sco_queue, item);
985 		mtx_unlock(&ng_btsocket_sco_queue_mtx);
986 
987 		if (item == NULL)
988 			break;
989 
990 		NGI_GET_HOOK(item, hook);
991 		if (hook != NULL && NG_HOOK_NOT_VALID(hook))
992 			goto drop;
993 
994 		switch(item->el_flags & NGQF_TYPE) {
995 		case NGQF_DATA: {
996 			struct mbuf     *m = NULL;
997 
998 			NGI_GET_M(item, m);
999 			ng_btsocket_sco_data_input(m, hook);
1000 			} break;
1001 
1002 		case NGQF_MESG: {
1003 			struct ng_mesg  *msg = NULL;
1004 
1005 			NGI_GET_MSG(item, msg);
1006 
1007 			switch (msg->header.cmd) {
1008 			case NGM_HCI_LP_CON_CFM:
1009 			case NGM_HCI_LP_CON_IND:
1010 			case NGM_HCI_LP_DISCON_IND:
1011 			/* XXX FIXME add other LP messages */
1012 				ng_btsocket_sco_lp_msg_input(msg, hook);
1013 				break;
1014 
1015 			default:
1016 				ng_btsocket_sco_default_msg_input(msg, hook);
1017 				break;
1018 			}
1019 			} break;
1020 
1021 		default:
1022 			KASSERT(0,
1023 ("%s: invalid item type=%ld\n", __func__, (item->el_flags & NGQF_TYPE)));
1024 			break;
1025 		}
1026 drop:
1027 		if (hook != NULL)
1028 			NG_HOOK_UNREF(hook);
1029 
1030 		NG_FREE_ITEM(item);
1031 	}
1032 } /* ng_btsocket_sco_input */
1033 
1034 /*
1035  * Route cleanup task. Gets scheduled when hook is disconnected. Here we
1036  * will find all sockets that use "invalid" hook and disconnect them.
1037  */
1038 
1039 static void
1040 ng_btsocket_sco_rtclean(void *context, int pending)
1041 {
1042 	ng_btsocket_sco_pcb_p		pcb = NULL, pcb_next = NULL;
1043 	ng_btsocket_sco_rtentry_p	rt = NULL;
1044 
1045 	/*
1046 	 * First disconnect all sockets that use "invalid" hook
1047 	 */
1048 
1049 	mtx_lock(&ng_btsocket_sco_sockets_mtx);
1050 
1051 	for(pcb = LIST_FIRST(&ng_btsocket_sco_sockets); pcb != NULL; ) {
1052 		mtx_lock(&pcb->pcb_mtx);
1053 		pcb_next = LIST_NEXT(pcb, next);
1054 
1055 		if (pcb->rt != NULL &&
1056 		    pcb->rt->hook != NULL && NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1057 			if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1058 				ng_btsocket_sco_untimeout(pcb);
1059 
1060 			pcb->rt = NULL;
1061 			pcb->so->so_error = ENETDOWN;
1062 			pcb->state = NG_BTSOCKET_SCO_CLOSED;
1063 			soisdisconnected(pcb->so);
1064 		}
1065 
1066 		mtx_unlock(&pcb->pcb_mtx);
1067 		pcb = pcb_next;
1068 	}
1069 
1070 	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1071 
1072 	/*
1073 	 * Now cleanup routing table
1074 	 */
1075 
1076 	mtx_lock(&ng_btsocket_sco_rt_mtx);
1077 
1078 	for (rt = LIST_FIRST(&ng_btsocket_sco_rt); rt != NULL; ) {
1079 		ng_btsocket_sco_rtentry_p	rt_next = LIST_NEXT(rt, next);
1080 
1081 		if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
1082 			LIST_REMOVE(rt, next);
1083 
1084 			NG_HOOK_SET_PRIVATE(rt->hook, NULL);
1085 			NG_HOOK_UNREF(rt->hook); /* Remove extra reference */
1086 
1087 			bzero(rt, sizeof(*rt));
1088 			free(rt, M_NETGRAPH_BTSOCKET_SCO);
1089 		}
1090 
1091 		rt = rt_next;
1092 	}
1093 
1094 	mtx_unlock(&ng_btsocket_sco_rt_mtx);
1095 } /* ng_btsocket_sco_rtclean */
1096 
1097 /*
1098  * Initialize everything
1099  */
1100 
1101 void
1102 ng_btsocket_sco_init(void)
1103 {
1104 	int	error = 0;
1105 
1106 	/* Skip initialization of globals for non-default instances. */
1107 	if (!IS_DEFAULT_VNET(curvnet))
1108 		return;
1109 
1110 	ng_btsocket_sco_node = NULL;
1111 	ng_btsocket_sco_debug_level = NG_BTSOCKET_WARN_LEVEL;
1112 
1113 	/* Register Netgraph node type */
1114 	error = ng_newtype(&typestruct);
1115 	if (error != 0) {
1116 		NG_BTSOCKET_SCO_ALERT(
1117 "%s: Could not register Netgraph node type, error=%d\n", __func__, error);
1118 
1119                 return;
1120 	}
1121 
1122 	/* Create Netgrapg node */
1123 	error = ng_make_node_common(&typestruct, &ng_btsocket_sco_node);
1124 	if (error != 0) {
1125 		NG_BTSOCKET_SCO_ALERT(
1126 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
1127 
1128 		ng_btsocket_sco_node = NULL;
1129 
1130 		return;
1131 	}
1132 
1133 	error = ng_name_node(ng_btsocket_sco_node, NG_BTSOCKET_SCO_NODE_TYPE);
1134 	if (error != 0) {
1135 		NG_BTSOCKET_SCO_ALERT(
1136 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
1137 
1138 		NG_NODE_UNREF(ng_btsocket_sco_node);
1139 		ng_btsocket_sco_node = NULL;
1140 
1141 		return;
1142 	}
1143 
1144 	/* Create input queue */
1145 	NG_BT_ITEMQ_INIT(&ng_btsocket_sco_queue, 300);
1146 	mtx_init(&ng_btsocket_sco_queue_mtx,
1147 		"btsocks_sco_queue_mtx", NULL, MTX_DEF);
1148 	TASK_INIT(&ng_btsocket_sco_queue_task, 0,
1149 		ng_btsocket_sco_input, NULL);
1150 
1151 	/* Create list of sockets */
1152 	LIST_INIT(&ng_btsocket_sco_sockets);
1153 	mtx_init(&ng_btsocket_sco_sockets_mtx,
1154 		"btsocks_sco_sockets_mtx", NULL, MTX_DEF);
1155 
1156 	/* Routing table */
1157 	LIST_INIT(&ng_btsocket_sco_rt);
1158 	mtx_init(&ng_btsocket_sco_rt_mtx,
1159 		"btsocks_sco_rt_mtx", NULL, MTX_DEF);
1160 	TASK_INIT(&ng_btsocket_sco_rt_task, 0,
1161 		ng_btsocket_sco_rtclean, NULL);
1162 } /* ng_btsocket_sco_init */
1163 
1164 /*
1165  * Abort connection on socket
1166  */
1167 
1168 void
1169 ng_btsocket_sco_abort(struct socket *so)
1170 {
1171 	so->so_error = ECONNABORTED;
1172 
1173 	(void) ng_btsocket_sco_disconnect(so);
1174 } /* ng_btsocket_sco_abort */
1175 
1176 void
1177 ng_btsocket_sco_close(struct socket *so)
1178 {
1179 	(void) ng_btsocket_sco_disconnect(so);
1180 } /* ng_btsocket_sco_close */
1181 
1182 /*
1183  * Accept connection on socket. Nothing to do here, socket must be connected
1184  * and ready, so just return peer address and be done with it.
1185  */
1186 
1187 int
1188 ng_btsocket_sco_accept(struct socket *so, struct sockaddr **nam)
1189 {
1190 	if (ng_btsocket_sco_node == NULL)
1191 		return (EINVAL);
1192 
1193 	return (ng_btsocket_sco_peeraddr(so, nam));
1194 } /* ng_btsocket_sco_accept */
1195 
1196 /*
1197  * Create and attach new socket
1198  */
1199 
1200 int
1201 ng_btsocket_sco_attach(struct socket *so, int proto, struct thread *td)
1202 {
1203 	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
1204 	int			error;
1205 
1206 	/* Check socket and protocol */
1207 	if (ng_btsocket_sco_node == NULL)
1208 		return (EPROTONOSUPPORT);
1209 	if (so->so_type != SOCK_SEQPACKET)
1210 		return (ESOCKTNOSUPPORT);
1211 
1212 #if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
1213 	if (proto != 0)
1214 		if (proto != BLUETOOTH_PROTO_SCO)
1215 			return (EPROTONOSUPPORT);
1216 #endif /* XXX */
1217 
1218 	if (pcb != NULL)
1219 		return (EISCONN);
1220 
1221 	/* Reserve send and receive space if it is not reserved yet */
1222 	if ((so->so_snd.sb_hiwat == 0) || (so->so_rcv.sb_hiwat == 0)) {
1223 		error = soreserve(so, NG_BTSOCKET_SCO_SENDSPACE,
1224 					NG_BTSOCKET_SCO_RECVSPACE);
1225 		if (error != 0)
1226 			return (error);
1227 	}
1228 
1229 	/* Allocate the PCB */
1230         pcb = malloc(sizeof(*pcb),
1231 		M_NETGRAPH_BTSOCKET_SCO, M_NOWAIT | M_ZERO);
1232         if (pcb == NULL)
1233                 return (ENOMEM);
1234 
1235 	/* Link the PCB and the socket */
1236 	so->so_pcb = (caddr_t) pcb;
1237 	pcb->so = so;
1238 	pcb->state = NG_BTSOCKET_SCO_CLOSED;
1239 
1240 	callout_init(&pcb->timo, 1);
1241 
1242 	/*
1243 	 * Mark PCB mutex as DUPOK to prevent "duplicated lock of
1244 	 * the same type" message. When accepting new SCO connection
1245 	 * ng_btsocket_sco_process_lp_con_ind() holds both PCB mutexes
1246 	 * for "old" (accepting) PCB and "new" (created) PCB.
1247 	 */
1248 
1249 	mtx_init(&pcb->pcb_mtx, "btsocks_sco_pcb_mtx", NULL,
1250 		MTX_DEF|MTX_DUPOK);
1251 
1252 	/*
1253 	 * Add the PCB to the list
1254 	 *
1255 	 * XXX FIXME VERY IMPORTANT!
1256 	 *
1257 	 * This is totally FUBAR. We could get here in two cases:
1258 	 *
1259 	 * 1) When user calls socket()
1260 	 * 2) When we need to accept new incoming connection and call
1261 	 *    sonewconn()
1262 	 *
1263 	 * In the first case we must acquire ng_btsocket_sco_sockets_mtx.
1264 	 * In the second case we hold ng_btsocket_sco_sockets_mtx already.
1265 	 * So we now need to distinguish between these cases. From reading
1266 	 * /sys/kern/uipc_socket2.c we can find out that sonewconn() calls
1267 	 * pru_attach with proto == 0 and td == NULL. For now use this fact
1268 	 * to figure out if we were called from socket() or from sonewconn().
1269 	 */
1270 
1271 	if (td != NULL)
1272 		mtx_lock(&ng_btsocket_sco_sockets_mtx);
1273 	else
1274 		mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1275 
1276 	LIST_INSERT_HEAD(&ng_btsocket_sco_sockets, pcb, next);
1277 
1278 	if (td != NULL)
1279 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1280 
1281         return (0);
1282 } /* ng_btsocket_sco_attach */
1283 
1284 /*
1285  * Bind socket
1286  */
1287 
1288 int
1289 ng_btsocket_sco_bind(struct socket *so, struct sockaddr *nam,
1290 		struct thread *td)
1291 {
1292 	ng_btsocket_sco_pcb_t	*pcb = NULL;
1293 	struct sockaddr_sco	*sa = (struct sockaddr_sco *) nam;
1294 
1295 	if (ng_btsocket_sco_node == NULL)
1296 		return (EINVAL);
1297 
1298 	/* Verify address */
1299 	if (sa == NULL)
1300 		return (EINVAL);
1301 	if (sa->sco_family != AF_BLUETOOTH)
1302 		return (EAFNOSUPPORT);
1303 	if (sa->sco_len != sizeof(*sa))
1304 		return (EINVAL);
1305 
1306 	mtx_lock(&ng_btsocket_sco_sockets_mtx);
1307 
1308 	/*
1309 	 * Check if other socket has this address already (look for exact
1310 	 * match in bdaddr) and assign socket address if it's available.
1311 	 */
1312 
1313 	if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(sa->sco_bdaddr)) != 0) {
1314  		LIST_FOREACH(pcb, &ng_btsocket_sco_sockets, next) {
1315 			mtx_lock(&pcb->pcb_mtx);
1316 
1317 			if (bcmp(&pcb->src, &sa->sco_bdaddr, sizeof(bdaddr_t)) == 0) {
1318 				mtx_unlock(&pcb->pcb_mtx);
1319 				mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1320 
1321 				return (EADDRINUSE);
1322 			}
1323 
1324 			mtx_unlock(&pcb->pcb_mtx);
1325 		}
1326 
1327 	}
1328 
1329 	pcb = so2sco_pcb(so);
1330 	if (pcb == NULL) {
1331 		mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1332 		return (EINVAL);
1333 	}
1334 
1335 	mtx_lock(&pcb->pcb_mtx);
1336 	bcopy(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src));
1337 	mtx_unlock(&pcb->pcb_mtx);
1338 
1339 	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1340 
1341 	return (0);
1342 } /* ng_btsocket_sco_bind */
1343 
1344 /*
1345  * Connect socket
1346  */
1347 
1348 int
1349 ng_btsocket_sco_connect(struct socket *so, struct sockaddr *nam,
1350 		struct thread *td)
1351 {
1352 	ng_btsocket_sco_pcb_t		*pcb = so2sco_pcb(so);
1353 	struct sockaddr_sco		*sa = (struct sockaddr_sco *) nam;
1354 	ng_btsocket_sco_rtentry_t	*rt = NULL;
1355 	int				 have_src, error = 0;
1356 
1357 	/* Check socket */
1358 	if (pcb == NULL)
1359 		return (EINVAL);
1360 	if (ng_btsocket_sco_node == NULL)
1361 		return (EINVAL);
1362 
1363 	/* Verify address */
1364 	if (sa == NULL)
1365 		return (EINVAL);
1366 	if (sa->sco_family != AF_BLUETOOTH)
1367 		return (EAFNOSUPPORT);
1368 	if (sa->sco_len != sizeof(*sa))
1369 		return (EINVAL);
1370 	if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
1371 		return (EDESTADDRREQ);
1372 
1373 	/*
1374 	 * Routing. Socket should be bound to some source address. The source
1375 	 * address can be ANY. Destination address must be set and it must not
1376 	 * be ANY. If source address is ANY then find first rtentry that has
1377 	 * src != dst.
1378 	 */
1379 
1380 	mtx_lock(&ng_btsocket_sco_rt_mtx);
1381 	mtx_lock(&pcb->pcb_mtx);
1382 
1383 	if (pcb->state == NG_BTSOCKET_SCO_CONNECTING) {
1384 		mtx_unlock(&pcb->pcb_mtx);
1385 		mtx_unlock(&ng_btsocket_sco_rt_mtx);
1386 
1387 		return (EINPROGRESS);
1388 	}
1389 
1390 	if (bcmp(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src)) == 0) {
1391 		mtx_unlock(&pcb->pcb_mtx);
1392 		mtx_unlock(&ng_btsocket_sco_rt_mtx);
1393 
1394 		return (EINVAL);
1395 	}
1396 
1397 	/* Send destination address and PSM */
1398 	bcopy(&sa->sco_bdaddr, &pcb->dst, sizeof(pcb->dst));
1399 
1400 	pcb->rt = NULL;
1401 	have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
1402 
1403 	LIST_FOREACH(rt, &ng_btsocket_sco_rt, next) {
1404 		if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
1405 			continue;
1406 
1407 		/* Match src and dst */
1408 		if (have_src) {
1409 			if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
1410 				break;
1411 		} else {
1412 			if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
1413 				break;
1414 		}
1415 	}
1416 
1417 	if (rt != NULL) {
1418 		pcb->rt = rt;
1419 
1420 		if (!have_src)
1421 			bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
1422 	} else
1423 		error = EHOSTUNREACH;
1424 
1425 	/*
1426 	 * Send LP_Connect request
1427 	 */
1428 
1429 	if (error == 0) {
1430 		error = ng_btsocket_sco_send_lp_con_req(pcb);
1431 		if (error == 0) {
1432 			pcb->flags |= NG_BTSOCKET_SCO_CLIENT;
1433 			pcb->state = NG_BTSOCKET_SCO_CONNECTING;
1434 			soisconnecting(pcb->so);
1435 
1436 			ng_btsocket_sco_timeout(pcb);
1437 		}
1438 	}
1439 
1440 	mtx_unlock(&pcb->pcb_mtx);
1441 	mtx_unlock(&ng_btsocket_sco_rt_mtx);
1442 
1443 	return (error);
1444 } /* ng_btsocket_sco_connect */
1445 
1446 /*
1447  * Process ioctl's calls on socket
1448  */
1449 
1450 int
1451 ng_btsocket_sco_control(struct socket *so, u_long cmd, caddr_t data,
1452 		struct ifnet *ifp, struct thread *td)
1453 {
1454 	return (EINVAL);
1455 } /* ng_btsocket_sco_control */
1456 
1457 /*
1458  * Process getsockopt/setsockopt system calls
1459  */
1460 
1461 int
1462 ng_btsocket_sco_ctloutput(struct socket *so, struct sockopt *sopt)
1463 {
1464 	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
1465         int			error, tmp;
1466 
1467 	if (ng_btsocket_sco_node == NULL)
1468 		return (EINVAL);
1469 	if (pcb == NULL)
1470 		return (EINVAL);
1471 
1472 	if (sopt->sopt_level != SOL_SCO)
1473 		return (0);
1474 
1475 	mtx_lock(&pcb->pcb_mtx);
1476 
1477 	switch (sopt->sopt_dir) {
1478 	case SOPT_GET:
1479 		if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
1480 			error = ENOTCONN;
1481 			break;
1482 		}
1483 
1484 		switch (sopt->sopt_name) {
1485 		case SO_SCO_MTU:
1486 			tmp = pcb->rt->pkt_size;
1487 			error = sooptcopyout(sopt, &tmp, sizeof(tmp));
1488 			break;
1489 
1490 		case SO_SCO_CONNINFO:
1491 			tmp = pcb->con_handle;
1492 			error = sooptcopyout(sopt, &tmp, sizeof(tmp));
1493 			break;
1494 
1495 		default:
1496 			error = EINVAL;
1497 			break;
1498 		}
1499 		break;
1500 
1501 	case SOPT_SET:
1502 		error = ENOPROTOOPT;
1503 		break;
1504 
1505 	default:
1506 		error = EINVAL;
1507 		break;
1508 	}
1509 
1510 	mtx_unlock(&pcb->pcb_mtx);
1511 
1512 	return (error);
1513 } /* ng_btsocket_sco_ctloutput */
1514 
1515 /*
1516  * Detach and destroy socket
1517  */
1518 
1519 void
1520 ng_btsocket_sco_detach(struct socket *so)
1521 {
1522 	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
1523 
1524 	KASSERT(pcb != NULL, ("ng_btsocket_sco_detach: pcb == NULL"));
1525 
1526 	if (ng_btsocket_sco_node == NULL)
1527 		return;
1528 
1529 	mtx_lock(&ng_btsocket_sco_sockets_mtx);
1530 	mtx_lock(&pcb->pcb_mtx);
1531 
1532 	if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1533 		ng_btsocket_sco_untimeout(pcb);
1534 
1535 	if (pcb->state == NG_BTSOCKET_SCO_OPEN)
1536 		ng_btsocket_sco_send_lp_discon_req(pcb);
1537 
1538 	pcb->state = NG_BTSOCKET_SCO_CLOSED;
1539 
1540 	LIST_REMOVE(pcb, next);
1541 
1542 	mtx_unlock(&pcb->pcb_mtx);
1543 	mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1544 
1545 	mtx_destroy(&pcb->pcb_mtx);
1546 	bzero(pcb, sizeof(*pcb));
1547 	free(pcb, M_NETGRAPH_BTSOCKET_SCO);
1548 
1549 	soisdisconnected(so);
1550 	so->so_pcb = NULL;
1551 } /* ng_btsocket_sco_detach */
1552 
1553 /*
1554  * Disconnect socket
1555  */
1556 
1557 int
1558 ng_btsocket_sco_disconnect(struct socket *so)
1559 {
1560 	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
1561 
1562 	if (pcb == NULL)
1563 		return (EINVAL);
1564 	if (ng_btsocket_sco_node == NULL)
1565 		return (EINVAL);
1566 
1567 	mtx_lock(&pcb->pcb_mtx);
1568 
1569 	if (pcb->state == NG_BTSOCKET_SCO_DISCONNECTING) {
1570 		mtx_unlock(&pcb->pcb_mtx);
1571 
1572 		return (EINPROGRESS);
1573 	}
1574 
1575 	if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1576 		ng_btsocket_sco_untimeout(pcb);
1577 
1578 	if (pcb->state == NG_BTSOCKET_SCO_OPEN) {
1579 		ng_btsocket_sco_send_lp_discon_req(pcb);
1580 
1581 		pcb->state = NG_BTSOCKET_SCO_DISCONNECTING;
1582 		soisdisconnecting(so);
1583 
1584 		ng_btsocket_sco_timeout(pcb);
1585 	} else {
1586 		pcb->state = NG_BTSOCKET_SCO_CLOSED;
1587 		soisdisconnected(so);
1588 	}
1589 
1590 	mtx_unlock(&pcb->pcb_mtx);
1591 
1592 	return (0);
1593 } /* ng_btsocket_sco_disconnect */
1594 
1595 /*
1596  * Listen on socket
1597  */
1598 
1599 int
1600 ng_btsocket_sco_listen(struct socket *so, int backlog, struct thread *td)
1601 {
1602 	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
1603 	int			error;
1604 
1605 	if (pcb == NULL)
1606 		return (EINVAL);
1607 	if (ng_btsocket_sco_node == NULL)
1608 		return (EINVAL);
1609 
1610 	SOCK_LOCK(so);
1611 	mtx_lock(&pcb->pcb_mtx);
1612 
1613 	error = solisten_proto_check(so);
1614 	if (error != 0)
1615 		goto out;
1616 #if 0
1617 	if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) {
1618 		error = EDESTADDRREQ;
1619 		goto out;
1620 	}
1621 #endif
1622 	solisten_proto(so, backlog);
1623 out:
1624 	mtx_unlock(&pcb->pcb_mtx);
1625 	SOCK_UNLOCK(so);
1626 
1627 	return (error);
1628 } /* ng_btsocket_listen */
1629 
1630 /*
1631  * Get peer address
1632  */
1633 
1634 int
1635 ng_btsocket_sco_peeraddr(struct socket *so, struct sockaddr **nam)
1636 {
1637 	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
1638 	struct sockaddr_sco	sa;
1639 
1640 	if (pcb == NULL)
1641 		return (EINVAL);
1642 	if (ng_btsocket_sco_node == NULL)
1643 		return (EINVAL);
1644 
1645 	mtx_lock(&pcb->pcb_mtx);
1646 	bcopy(&pcb->dst, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
1647 	mtx_unlock(&pcb->pcb_mtx);
1648 
1649 	sa.sco_len = sizeof(sa);
1650 	sa.sco_family = AF_BLUETOOTH;
1651 
1652 	*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
1653 
1654 	return ((*nam == NULL)? ENOMEM : 0);
1655 } /* ng_btsocket_sco_peeraddr */
1656 
1657 /*
1658  * Send data to socket
1659  */
1660 
1661 int
1662 ng_btsocket_sco_send(struct socket *so, int flags, struct mbuf *m,
1663 		struct sockaddr *nam, struct mbuf *control, struct thread *td)
1664 {
1665 	ng_btsocket_sco_pcb_t	*pcb = so2sco_pcb(so);
1666 	int			 error = 0;
1667 
1668 	if (ng_btsocket_sco_node == NULL) {
1669 		error = ENETDOWN;
1670 		goto drop;
1671 	}
1672 
1673 	/* Check socket and input */
1674 	if (pcb == NULL || m == NULL || control != NULL) {
1675 		error = EINVAL;
1676 		goto drop;
1677 	}
1678 
1679 	mtx_lock(&pcb->pcb_mtx);
1680 
1681 	/* Make sure socket is connected */
1682 	if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
1683 		mtx_unlock(&pcb->pcb_mtx);
1684 		error = ENOTCONN;
1685 		goto drop;
1686 	}
1687 
1688 	/* Check route */
1689 	if (pcb->rt == NULL ||
1690 	    pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1691 		mtx_unlock(&pcb->pcb_mtx);
1692 		error = ENETDOWN;
1693 		goto drop;
1694 	}
1695 
1696 	/* Check packet size */
1697 	if (m->m_pkthdr.len > pcb->rt->pkt_size) {
1698 		NG_BTSOCKET_SCO_ERR(
1699 "%s: Packet too big, len=%d, pkt_size=%d\n",
1700 			__func__, m->m_pkthdr.len, pcb->rt->pkt_size);
1701 
1702 		mtx_unlock(&pcb->pcb_mtx);
1703 		error = EMSGSIZE;
1704 		goto drop;
1705 	}
1706 
1707 	/*
1708 	 * First put packet on socket send queue. Then check if we have
1709 	 * pending timeout. If we do not have timeout then we must send
1710 	 * packet and schedule timeout. Otherwise do nothing and wait for
1711 	 * NGM_HCI_SYNC_CON_QUEUE message.
1712 	 */
1713 
1714 	sbappendrecord(&pcb->so->so_snd, m);
1715 	m = NULL;
1716 
1717 	if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
1718 		error = ng_btsocket_sco_send2(pcb);
1719 		if (error == 0)
1720 			ng_btsocket_sco_timeout(pcb);
1721 		else
1722 			sbdroprecord(&pcb->so->so_snd); /* XXX */
1723 	}
1724 
1725 	mtx_unlock(&pcb->pcb_mtx);
1726 drop:
1727 	NG_FREE_M(m); /* checks for != NULL */
1728 	NG_FREE_M(control);
1729 
1730 	return (error);
1731 } /* ng_btsocket_sco_send */
1732 
1733 /*
1734  * Send first packet in the socket queue to the SCO layer
1735  */
1736 
1737 static int
1738 ng_btsocket_sco_send2(ng_btsocket_sco_pcb_p pcb)
1739 {
1740 	struct  mbuf		*m = NULL;
1741 	ng_hci_scodata_pkt_t	*hdr = NULL;
1742 	int			 error = 0;
1743 
1744 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1745 
1746 	while (pcb->rt->pending < pcb->rt->num_pkts &&
1747 	       sbavail(&pcb->so->so_snd) > 0) {
1748 		/* Get a copy of the first packet on send queue */
1749 		m = m_dup(pcb->so->so_snd.sb_mb, M_NOWAIT);
1750 		if (m == NULL) {
1751 			error = ENOBUFS;
1752 			break;
1753 		}
1754 
1755 		/* Create SCO packet header */
1756 		M_PREPEND(m, sizeof(*hdr), M_NOWAIT);
1757 		if (m != NULL)
1758 			if (m->m_len < sizeof(*hdr))
1759 				m = m_pullup(m, sizeof(*hdr));
1760 
1761 		if (m == NULL) {
1762 			error = ENOBUFS;
1763 			break;
1764 		}
1765 
1766 		/* Fill in the header */
1767 		hdr = mtod(m, ng_hci_scodata_pkt_t *);
1768 		hdr->type = NG_HCI_SCO_DATA_PKT;
1769 		hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(pcb->con_handle, 0, 0));
1770 		hdr->length = m->m_pkthdr.len - sizeof(*hdr);
1771 
1772 		/* Send packet */
1773 		NG_SEND_DATA_ONLY(error, pcb->rt->hook, m);
1774 		if (error != 0)
1775 			break;
1776 
1777 		pcb->rt->pending ++;
1778 	}
1779 
1780 	return ((pcb->rt->pending > 0)? 0 : error);
1781 } /* ng_btsocket_sco_send2 */
1782 
1783 /*
1784  * Get socket address
1785  */
1786 
1787 int
1788 ng_btsocket_sco_sockaddr(struct socket *so, struct sockaddr **nam)
1789 {
1790 	ng_btsocket_sco_pcb_p	pcb = so2sco_pcb(so);
1791 	struct sockaddr_sco	sa;
1792 
1793 	if (pcb == NULL)
1794 		return (EINVAL);
1795 	if (ng_btsocket_sco_node == NULL)
1796 		return (EINVAL);
1797 
1798 	mtx_lock(&pcb->pcb_mtx);
1799 	bcopy(&pcb->src, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
1800 	mtx_unlock(&pcb->pcb_mtx);
1801 
1802 	sa.sco_len = sizeof(sa);
1803 	sa.sco_family = AF_BLUETOOTH;
1804 
1805 	*nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
1806 
1807 	return ((*nam == NULL)? ENOMEM : 0);
1808 } /* ng_btsocket_sco_sockaddr */
1809 
1810 /*****************************************************************************
1811  *****************************************************************************
1812  **                              Misc. functions
1813  *****************************************************************************
1814  *****************************************************************************/
1815 
1816 /*
1817  * Look for the socket that listens on given bdaddr.
1818  * Returns exact or close match (if any).
1819  * Caller must hold ng_btsocket_sco_sockets_mtx.
1820  * Returns with locked pcb.
1821  */
1822 
1823 static ng_btsocket_sco_pcb_p
1824 ng_btsocket_sco_pcb_by_addr(bdaddr_p bdaddr)
1825 {
1826 	ng_btsocket_sco_pcb_p	p = NULL, p1 = NULL;
1827 
1828 	mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1829 
1830 	LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1831 		mtx_lock(&p->pcb_mtx);
1832 
1833 		if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN)) {
1834 			mtx_unlock(&p->pcb_mtx);
1835 			continue;
1836 		}
1837 
1838 		if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0)
1839 			return (p); /* return with locked pcb */
1840 
1841 		if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0)
1842 			p1 = p;
1843 
1844 		mtx_unlock(&p->pcb_mtx);
1845 	}
1846 
1847 	if (p1 != NULL)
1848 		mtx_lock(&p1->pcb_mtx);
1849 
1850 	return (p1);
1851 } /* ng_btsocket_sco_pcb_by_addr */
1852 
1853 /*
1854  * Look for the socket that assigned to given source address and handle.
1855  * Caller must hold ng_btsocket_sco_sockets_mtx.
1856  * Returns with locked pcb.
1857  */
1858 
1859 static ng_btsocket_sco_pcb_p
1860 ng_btsocket_sco_pcb_by_handle(bdaddr_p src, int con_handle)
1861 {
1862 	ng_btsocket_sco_pcb_p	p = NULL;
1863 
1864 	mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1865 
1866 	LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1867 		mtx_lock(&p->pcb_mtx);
1868 
1869 		if (p->con_handle == con_handle &&
1870 		    bcmp(src, &p->src, sizeof(p->src)) == 0)
1871 			return (p); /* return with locked pcb */
1872 
1873 		mtx_unlock(&p->pcb_mtx);
1874 	}
1875 
1876 	return (NULL);
1877 } /* ng_btsocket_sco_pcb_by_handle */
1878 
1879 /*
1880  * Look for the socket in CONNECTING state with given source and destination
1881  * addresses. Caller must hold ng_btsocket_sco_sockets_mtx.
1882  * Returns with locked pcb.
1883  */
1884 
1885 static ng_btsocket_sco_pcb_p
1886 ng_btsocket_sco_pcb_by_addrs(bdaddr_p src, bdaddr_p dst)
1887 {
1888 	ng_btsocket_sco_pcb_p	p = NULL;
1889 
1890 	mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1891 
1892 	LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1893 		mtx_lock(&p->pcb_mtx);
1894 
1895 		if (p->state == NG_BTSOCKET_SCO_CONNECTING &&
1896 		    bcmp(src, &p->src, sizeof(p->src)) == 0 &&
1897 		    bcmp(dst, &p->dst, sizeof(p->dst)) == 0)
1898 			return (p); /* return with locked pcb */
1899 
1900 		mtx_unlock(&p->pcb_mtx);
1901 	}
1902 
1903 	return (NULL);
1904 } /* ng_btsocket_sco_pcb_by_addrs */
1905 
1906 /*
1907  * Set timeout on socket
1908  */
1909 
1910 static void
1911 ng_btsocket_sco_timeout(ng_btsocket_sco_pcb_p pcb)
1912 {
1913 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1914 
1915 	if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
1916 		pcb->flags |= NG_BTSOCKET_SCO_TIMO;
1917 		callout_reset(&pcb->timo, bluetooth_sco_rtx_timeout(),
1918 					ng_btsocket_sco_process_timeout, pcb);
1919 	} else
1920 		KASSERT(0,
1921 ("%s: Duplicated socket timeout?!\n", __func__));
1922 } /* ng_btsocket_sco_timeout */
1923 
1924 /*
1925  * Unset timeout on socket
1926  */
1927 
1928 static void
1929 ng_btsocket_sco_untimeout(ng_btsocket_sco_pcb_p pcb)
1930 {
1931 	mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1932 
1933 	if (pcb->flags & NG_BTSOCKET_SCO_TIMO) {
1934 		callout_stop(&pcb->timo);
1935 		pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
1936 	} else
1937 		KASSERT(0,
1938 ("%s: No socket timeout?!\n", __func__));
1939 } /* ng_btsocket_sco_untimeout */
1940 
1941 /*
1942  * Process timeout on socket
1943  */
1944 
1945 static void
1946 ng_btsocket_sco_process_timeout(void *xpcb)
1947 {
1948 	ng_btsocket_sco_pcb_p	 pcb = (ng_btsocket_sco_pcb_p) xpcb;
1949 
1950 	mtx_lock(&pcb->pcb_mtx);
1951 
1952 	pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
1953 	pcb->so->so_error = ETIMEDOUT;
1954 
1955 	switch (pcb->state) {
1956 	case NG_BTSOCKET_SCO_CONNECTING:
1957 		/* Connect timeout - close the socket */
1958 		pcb->state = NG_BTSOCKET_SCO_CLOSED;
1959 		soisdisconnected(pcb->so);
1960 		break;
1961 
1962 	case NG_BTSOCKET_SCO_OPEN:
1963 		/* Send timeout - did not get NGM_HCI_SYNC_CON_QUEUE */
1964 		sbdroprecord(&pcb->so->so_snd);
1965 		sowwakeup(pcb->so);
1966 		/* XXX FIXME what to do with pcb->rt->pending??? */
1967 		break;
1968 
1969 	case NG_BTSOCKET_SCO_DISCONNECTING:
1970 		/* Disconnect timeout - disconnect the socket anyway */
1971 		pcb->state = NG_BTSOCKET_SCO_CLOSED;
1972 		soisdisconnected(pcb->so);
1973 		break;
1974 
1975 	default:
1976 		NG_BTSOCKET_SCO_ERR(
1977 "%s: Invalid socket state=%d\n", __func__, pcb->state);
1978 		break;
1979 	}
1980 
1981 	mtx_unlock(&pcb->pcb_mtx);
1982 } /* ng_btsocket_sco_process_timeout */
1983 
1984