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