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 * Accept connection on socket. Nothing to do here, socket must be connected
1181 * and ready, so just return peer address and be done with it.
1182 */
1183
1184 int
ng_btsocket_sco_accept(struct socket * so,struct sockaddr ** nam)1185 ng_btsocket_sco_accept(struct socket *so, struct sockaddr **nam)
1186 {
1187 if (ng_btsocket_sco_node == NULL)
1188 return (EINVAL);
1189
1190 return (ng_btsocket_sco_peeraddr(so, nam));
1191 } /* ng_btsocket_sco_accept */
1192
1193 /*
1194 * Create and attach new socket
1195 */
1196
1197 int
ng_btsocket_sco_attach(struct socket * so,int proto,struct thread * td)1198 ng_btsocket_sco_attach(struct socket *so, int proto, struct thread *td)
1199 {
1200 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1201 int error;
1202
1203 /* Check socket and protocol */
1204 if (ng_btsocket_sco_node == NULL)
1205 return (EPROTONOSUPPORT);
1206 if (so->so_type != SOCK_SEQPACKET)
1207 return (ESOCKTNOSUPPORT);
1208
1209 #if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
1210 if (proto != 0)
1211 if (proto != BLUETOOTH_PROTO_SCO)
1212 return (EPROTONOSUPPORT);
1213 #endif /* XXX */
1214
1215 if (pcb != NULL)
1216 return (EISCONN);
1217
1218 /* Reserve send and receive space if it is not reserved yet */
1219 if ((so->so_snd.sb_hiwat == 0) || (so->so_rcv.sb_hiwat == 0)) {
1220 error = soreserve(so, NG_BTSOCKET_SCO_SENDSPACE,
1221 NG_BTSOCKET_SCO_RECVSPACE);
1222 if (error != 0)
1223 return (error);
1224 }
1225
1226 /* Allocate the PCB */
1227 pcb = malloc(sizeof(*pcb),
1228 M_NETGRAPH_BTSOCKET_SCO, M_NOWAIT | M_ZERO);
1229 if (pcb == NULL)
1230 return (ENOMEM);
1231
1232 /* Link the PCB and the socket */
1233 so->so_pcb = (caddr_t) pcb;
1234 pcb->so = so;
1235 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1236
1237 callout_init(&pcb->timo, 1);
1238
1239 /*
1240 * Mark PCB mutex as DUPOK to prevent "duplicated lock of
1241 * the same type" message. When accepting new SCO connection
1242 * ng_btsocket_sco_process_lp_con_ind() holds both PCB mutexes
1243 * for "old" (accepting) PCB and "new" (created) PCB.
1244 */
1245
1246 mtx_init(&pcb->pcb_mtx, "btsocks_sco_pcb_mtx", NULL,
1247 MTX_DEF|MTX_DUPOK);
1248
1249 /*
1250 * Add the PCB to the list
1251 *
1252 * XXX FIXME VERY IMPORTANT!
1253 *
1254 * This is totally FUBAR. We could get here in two cases:
1255 *
1256 * 1) When user calls socket()
1257 * 2) When we need to accept new incoming connection and call
1258 * sonewconn()
1259 *
1260 * In the first case we must acquire ng_btsocket_sco_sockets_mtx.
1261 * In the second case we hold ng_btsocket_sco_sockets_mtx already.
1262 * So we now need to distinguish between these cases. From reading
1263 * /sys/kern/uipc_socket2.c we can find out that sonewconn() calls
1264 * pru_attach with proto == 0 and td == NULL. For now use this fact
1265 * to figure out if we were called from socket() or from sonewconn().
1266 */
1267
1268 if (td != NULL)
1269 mtx_lock(&ng_btsocket_sco_sockets_mtx);
1270 else
1271 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1272
1273 LIST_INSERT_HEAD(&ng_btsocket_sco_sockets, pcb, next);
1274
1275 if (td != NULL)
1276 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1277
1278 return (0);
1279 } /* ng_btsocket_sco_attach */
1280
1281 /*
1282 * Bind socket
1283 */
1284
1285 int
ng_btsocket_sco_bind(struct socket * so,struct sockaddr * nam,struct thread * td)1286 ng_btsocket_sco_bind(struct socket *so, struct sockaddr *nam,
1287 struct thread *td)
1288 {
1289 ng_btsocket_sco_pcb_t *pcb = NULL;
1290 struct sockaddr_sco *sa = (struct sockaddr_sco *) nam;
1291
1292 if (ng_btsocket_sco_node == NULL)
1293 return (EINVAL);
1294
1295 /* Verify address */
1296 if (sa == NULL)
1297 return (EINVAL);
1298 if (sa->sco_family != AF_BLUETOOTH)
1299 return (EAFNOSUPPORT);
1300 if (sa->sco_len != sizeof(*sa))
1301 return (EINVAL);
1302
1303 mtx_lock(&ng_btsocket_sco_sockets_mtx);
1304
1305 /*
1306 * Check if other socket has this address already (look for exact
1307 * match in bdaddr) and assign socket address if it's available.
1308 */
1309
1310 if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(sa->sco_bdaddr)) != 0) {
1311 LIST_FOREACH(pcb, &ng_btsocket_sco_sockets, next) {
1312 mtx_lock(&pcb->pcb_mtx);
1313
1314 if (bcmp(&pcb->src, &sa->sco_bdaddr, sizeof(bdaddr_t)) == 0) {
1315 mtx_unlock(&pcb->pcb_mtx);
1316 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1317
1318 return (EADDRINUSE);
1319 }
1320
1321 mtx_unlock(&pcb->pcb_mtx);
1322 }
1323 }
1324
1325 pcb = so2sco_pcb(so);
1326 if (pcb == NULL) {
1327 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1328 return (EINVAL);
1329 }
1330
1331 mtx_lock(&pcb->pcb_mtx);
1332 bcopy(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src));
1333 mtx_unlock(&pcb->pcb_mtx);
1334
1335 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1336
1337 return (0);
1338 } /* ng_btsocket_sco_bind */
1339
1340 /*
1341 * Connect socket
1342 */
1343
1344 int
ng_btsocket_sco_connect(struct socket * so,struct sockaddr * nam,struct thread * td)1345 ng_btsocket_sco_connect(struct socket *so, struct sockaddr *nam,
1346 struct thread *td)
1347 {
1348 ng_btsocket_sco_pcb_t *pcb = so2sco_pcb(so);
1349 struct sockaddr_sco *sa = (struct sockaddr_sco *) nam;
1350 ng_btsocket_sco_rtentry_t *rt = NULL;
1351 int have_src, error = 0;
1352
1353 /* Check socket */
1354 if (pcb == NULL)
1355 return (EINVAL);
1356 if (ng_btsocket_sco_node == NULL)
1357 return (EINVAL);
1358
1359 /* Verify address */
1360 if (sa == NULL)
1361 return (EINVAL);
1362 if (sa->sco_family != AF_BLUETOOTH)
1363 return (EAFNOSUPPORT);
1364 if (sa->sco_len != sizeof(*sa))
1365 return (EINVAL);
1366 if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
1367 return (EDESTADDRREQ);
1368
1369 /*
1370 * Routing. Socket should be bound to some source address. The source
1371 * address can be ANY. Destination address must be set and it must not
1372 * be ANY. If source address is ANY then find first rtentry that has
1373 * src != dst.
1374 */
1375
1376 mtx_lock(&ng_btsocket_sco_rt_mtx);
1377 mtx_lock(&pcb->pcb_mtx);
1378
1379 if (pcb->state == NG_BTSOCKET_SCO_CONNECTING) {
1380 mtx_unlock(&pcb->pcb_mtx);
1381 mtx_unlock(&ng_btsocket_sco_rt_mtx);
1382
1383 return (EINPROGRESS);
1384 }
1385
1386 if (bcmp(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src)) == 0) {
1387 mtx_unlock(&pcb->pcb_mtx);
1388 mtx_unlock(&ng_btsocket_sco_rt_mtx);
1389
1390 return (EINVAL);
1391 }
1392
1393 /* Send destination address and PSM */
1394 bcopy(&sa->sco_bdaddr, &pcb->dst, sizeof(pcb->dst));
1395
1396 pcb->rt = NULL;
1397 have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
1398
1399 LIST_FOREACH(rt, &ng_btsocket_sco_rt, next) {
1400 if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
1401 continue;
1402
1403 /* Match src and dst */
1404 if (have_src) {
1405 if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
1406 break;
1407 } else {
1408 if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
1409 break;
1410 }
1411 }
1412
1413 if (rt != NULL) {
1414 pcb->rt = rt;
1415
1416 if (!have_src)
1417 bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
1418 } else
1419 error = EHOSTUNREACH;
1420
1421 /*
1422 * Send LP_Connect request
1423 */
1424
1425 if (error == 0) {
1426 error = ng_btsocket_sco_send_lp_con_req(pcb);
1427 if (error == 0) {
1428 pcb->flags |= NG_BTSOCKET_SCO_CLIENT;
1429 pcb->state = NG_BTSOCKET_SCO_CONNECTING;
1430 soisconnecting(pcb->so);
1431
1432 ng_btsocket_sco_timeout(pcb);
1433 }
1434 }
1435
1436 mtx_unlock(&pcb->pcb_mtx);
1437 mtx_unlock(&ng_btsocket_sco_rt_mtx);
1438
1439 return (error);
1440 } /* ng_btsocket_sco_connect */
1441
1442 /*
1443 * Process ioctl's calls on socket
1444 */
1445
1446 int
ng_btsocket_sco_control(struct socket * so,u_long cmd,void * data,struct ifnet * ifp,struct thread * td)1447 ng_btsocket_sco_control(struct socket *so, u_long cmd, void *data,
1448 struct ifnet *ifp, struct thread *td)
1449 {
1450 return (EINVAL);
1451 } /* ng_btsocket_sco_control */
1452
1453 /*
1454 * Process getsockopt/setsockopt system calls
1455 */
1456
1457 int
ng_btsocket_sco_ctloutput(struct socket * so,struct sockopt * sopt)1458 ng_btsocket_sco_ctloutput(struct socket *so, struct sockopt *sopt)
1459 {
1460 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1461 int error, tmp;
1462
1463 if (ng_btsocket_sco_node == NULL)
1464 return (EINVAL);
1465 if (pcb == NULL)
1466 return (EINVAL);
1467
1468 if (sopt->sopt_level != SOL_SCO)
1469 return (0);
1470
1471 mtx_lock(&pcb->pcb_mtx);
1472
1473 switch (sopt->sopt_dir) {
1474 case SOPT_GET:
1475 if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
1476 error = ENOTCONN;
1477 break;
1478 }
1479
1480 switch (sopt->sopt_name) {
1481 case SO_SCO_MTU:
1482 tmp = pcb->rt->pkt_size;
1483 error = sooptcopyout(sopt, &tmp, sizeof(tmp));
1484 break;
1485
1486 case SO_SCO_CONNINFO:
1487 tmp = pcb->con_handle;
1488 error = sooptcopyout(sopt, &tmp, sizeof(tmp));
1489 break;
1490
1491 default:
1492 error = EINVAL;
1493 break;
1494 }
1495 break;
1496
1497 case SOPT_SET:
1498 error = ENOPROTOOPT;
1499 break;
1500
1501 default:
1502 error = EINVAL;
1503 break;
1504 }
1505
1506 mtx_unlock(&pcb->pcb_mtx);
1507
1508 return (error);
1509 } /* ng_btsocket_sco_ctloutput */
1510
1511 /*
1512 * Detach and destroy socket
1513 */
1514
1515 void
ng_btsocket_sco_detach(struct socket * so)1516 ng_btsocket_sco_detach(struct socket *so)
1517 {
1518 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1519
1520 KASSERT(pcb != NULL, ("ng_btsocket_sco_detach: pcb == NULL"));
1521
1522 if (ng_btsocket_sco_node == NULL)
1523 return;
1524
1525 mtx_lock(&ng_btsocket_sco_sockets_mtx);
1526 mtx_lock(&pcb->pcb_mtx);
1527
1528 if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1529 ng_btsocket_sco_untimeout(pcb);
1530
1531 if (pcb->state == NG_BTSOCKET_SCO_OPEN)
1532 ng_btsocket_sco_send_lp_discon_req(pcb);
1533
1534 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1535
1536 LIST_REMOVE(pcb, next);
1537
1538 mtx_unlock(&pcb->pcb_mtx);
1539 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1540
1541 mtx_destroy(&pcb->pcb_mtx);
1542 bzero(pcb, sizeof(*pcb));
1543 free(pcb, M_NETGRAPH_BTSOCKET_SCO);
1544
1545 soisdisconnected(so);
1546 so->so_pcb = NULL;
1547 } /* ng_btsocket_sco_detach */
1548
1549 /*
1550 * Disconnect socket
1551 */
1552
1553 int
ng_btsocket_sco_disconnect(struct socket * so)1554 ng_btsocket_sco_disconnect(struct socket *so)
1555 {
1556 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1557
1558 if (pcb == NULL)
1559 return (EINVAL);
1560 if (ng_btsocket_sco_node == NULL)
1561 return (EINVAL);
1562
1563 mtx_lock(&pcb->pcb_mtx);
1564
1565 if (pcb->state == NG_BTSOCKET_SCO_DISCONNECTING) {
1566 mtx_unlock(&pcb->pcb_mtx);
1567
1568 return (EINPROGRESS);
1569 }
1570
1571 if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1572 ng_btsocket_sco_untimeout(pcb);
1573
1574 if (pcb->state == NG_BTSOCKET_SCO_OPEN) {
1575 ng_btsocket_sco_send_lp_discon_req(pcb);
1576
1577 pcb->state = NG_BTSOCKET_SCO_DISCONNECTING;
1578 soisdisconnecting(so);
1579
1580 ng_btsocket_sco_timeout(pcb);
1581 } else {
1582 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1583 soisdisconnected(so);
1584 }
1585
1586 mtx_unlock(&pcb->pcb_mtx);
1587
1588 return (0);
1589 } /* ng_btsocket_sco_disconnect */
1590
1591 /*
1592 * Listen on socket
1593 */
1594
1595 int
ng_btsocket_sco_listen(struct socket * so,int backlog,struct thread * td)1596 ng_btsocket_sco_listen(struct socket *so, int backlog, struct thread *td)
1597 {
1598 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1599 int error;
1600
1601 if (pcb == NULL)
1602 return (EINVAL);
1603 if (ng_btsocket_sco_node == NULL)
1604 return (EINVAL);
1605
1606 SOCK_LOCK(so);
1607 mtx_lock(&pcb->pcb_mtx);
1608
1609 error = solisten_proto_check(so);
1610 if (error != 0)
1611 goto out;
1612 #if 0
1613 if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) {
1614 error = EDESTADDRREQ;
1615 goto out;
1616 }
1617 #endif
1618 solisten_proto(so, backlog);
1619 out:
1620 mtx_unlock(&pcb->pcb_mtx);
1621 SOCK_UNLOCK(so);
1622
1623 return (error);
1624 } /* ng_btsocket_listen */
1625
1626 /*
1627 * Get peer address
1628 */
1629
1630 int
ng_btsocket_sco_peeraddr(struct socket * so,struct sockaddr ** nam)1631 ng_btsocket_sco_peeraddr(struct socket *so, struct sockaddr **nam)
1632 {
1633 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1634 struct sockaddr_sco sa;
1635
1636 if (pcb == NULL)
1637 return (EINVAL);
1638 if (ng_btsocket_sco_node == NULL)
1639 return (EINVAL);
1640
1641 mtx_lock(&pcb->pcb_mtx);
1642 bcopy(&pcb->dst, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
1643 mtx_unlock(&pcb->pcb_mtx);
1644
1645 sa.sco_len = sizeof(sa);
1646 sa.sco_family = AF_BLUETOOTH;
1647
1648 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
1649
1650 return ((*nam == NULL)? ENOMEM : 0);
1651 } /* ng_btsocket_sco_peeraddr */
1652
1653 /*
1654 * Send data to socket
1655 */
1656
1657 int
ng_btsocket_sco_send(struct socket * so,int flags,struct mbuf * m,struct sockaddr * nam,struct mbuf * control,struct thread * td)1658 ng_btsocket_sco_send(struct socket *so, int flags, struct mbuf *m,
1659 struct sockaddr *nam, struct mbuf *control, struct thread *td)
1660 {
1661 ng_btsocket_sco_pcb_t *pcb = so2sco_pcb(so);
1662 int error = 0;
1663
1664 if (ng_btsocket_sco_node == NULL) {
1665 error = ENETDOWN;
1666 goto drop;
1667 }
1668
1669 /* Check socket and input */
1670 if (pcb == NULL || m == NULL || control != NULL) {
1671 error = EINVAL;
1672 goto drop;
1673 }
1674
1675 mtx_lock(&pcb->pcb_mtx);
1676
1677 /* Make sure socket is connected */
1678 if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
1679 mtx_unlock(&pcb->pcb_mtx);
1680 error = ENOTCONN;
1681 goto drop;
1682 }
1683
1684 /* Check route */
1685 if (pcb->rt == NULL ||
1686 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1687 mtx_unlock(&pcb->pcb_mtx);
1688 error = ENETDOWN;
1689 goto drop;
1690 }
1691
1692 /* Check packet size */
1693 if (m->m_pkthdr.len > pcb->rt->pkt_size) {
1694 NG_BTSOCKET_SCO_ERR(
1695 "%s: Packet too big, len=%d, pkt_size=%d\n",
1696 __func__, m->m_pkthdr.len, pcb->rt->pkt_size);
1697
1698 mtx_unlock(&pcb->pcb_mtx);
1699 error = EMSGSIZE;
1700 goto drop;
1701 }
1702
1703 /*
1704 * First put packet on socket send queue. Then check if we have
1705 * pending timeout. If we do not have timeout then we must send
1706 * packet and schedule timeout. Otherwise do nothing and wait for
1707 * NGM_HCI_SYNC_CON_QUEUE message.
1708 */
1709
1710 sbappendrecord(&pcb->so->so_snd, m);
1711 m = NULL;
1712
1713 if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
1714 error = ng_btsocket_sco_send2(pcb);
1715 if (error == 0)
1716 ng_btsocket_sco_timeout(pcb);
1717 else
1718 sbdroprecord(&pcb->so->so_snd); /* XXX */
1719 }
1720
1721 mtx_unlock(&pcb->pcb_mtx);
1722 drop:
1723 NG_FREE_M(m); /* checks for != NULL */
1724 NG_FREE_M(control);
1725
1726 return (error);
1727 } /* ng_btsocket_sco_send */
1728
1729 /*
1730 * Send first packet in the socket queue to the SCO layer
1731 */
1732
1733 static int
ng_btsocket_sco_send2(ng_btsocket_sco_pcb_p pcb)1734 ng_btsocket_sco_send2(ng_btsocket_sco_pcb_p pcb)
1735 {
1736 struct mbuf *m = NULL;
1737 ng_hci_scodata_pkt_t *hdr = NULL;
1738 int error = 0;
1739
1740 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1741
1742 while (pcb->rt->pending < pcb->rt->num_pkts &&
1743 sbavail(&pcb->so->so_snd) > 0) {
1744 /* Get a copy of the first packet on send queue */
1745 m = m_dup(pcb->so->so_snd.sb_mb, M_NOWAIT);
1746 if (m == NULL) {
1747 error = ENOBUFS;
1748 break;
1749 }
1750
1751 /* Create SCO packet header */
1752 M_PREPEND(m, sizeof(*hdr), M_NOWAIT);
1753 if (m != NULL)
1754 if (m->m_len < sizeof(*hdr))
1755 m = m_pullup(m, sizeof(*hdr));
1756
1757 if (m == NULL) {
1758 error = ENOBUFS;
1759 break;
1760 }
1761
1762 /* Fill in the header */
1763 hdr = mtod(m, ng_hci_scodata_pkt_t *);
1764 hdr->type = NG_HCI_SCO_DATA_PKT;
1765 hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(pcb->con_handle, 0, 0));
1766 hdr->length = m->m_pkthdr.len - sizeof(*hdr);
1767
1768 /* Send packet */
1769 NG_SEND_DATA_ONLY(error, pcb->rt->hook, m);
1770 if (error != 0)
1771 break;
1772
1773 pcb->rt->pending ++;
1774 }
1775
1776 return ((pcb->rt->pending > 0)? 0 : error);
1777 } /* ng_btsocket_sco_send2 */
1778
1779 /*
1780 * Get socket address
1781 */
1782
1783 int
ng_btsocket_sco_sockaddr(struct socket * so,struct sockaddr ** nam)1784 ng_btsocket_sco_sockaddr(struct socket *so, struct sockaddr **nam)
1785 {
1786 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1787 struct sockaddr_sco sa;
1788
1789 if (pcb == NULL)
1790 return (EINVAL);
1791 if (ng_btsocket_sco_node == NULL)
1792 return (EINVAL);
1793
1794 mtx_lock(&pcb->pcb_mtx);
1795 bcopy(&pcb->src, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
1796 mtx_unlock(&pcb->pcb_mtx);
1797
1798 sa.sco_len = sizeof(sa);
1799 sa.sco_family = AF_BLUETOOTH;
1800
1801 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
1802
1803 return ((*nam == NULL)? ENOMEM : 0);
1804 } /* ng_btsocket_sco_sockaddr */
1805
1806 /*****************************************************************************
1807 *****************************************************************************
1808 ** Misc. functions
1809 *****************************************************************************
1810 *****************************************************************************/
1811
1812 /*
1813 * Look for the socket that listens on given bdaddr.
1814 * Returns exact or close match (if any).
1815 * Caller must hold ng_btsocket_sco_sockets_mtx.
1816 * Returns with locked pcb.
1817 */
1818
1819 static ng_btsocket_sco_pcb_p
ng_btsocket_sco_pcb_by_addr(bdaddr_p bdaddr)1820 ng_btsocket_sco_pcb_by_addr(bdaddr_p bdaddr)
1821 {
1822 ng_btsocket_sco_pcb_p p = NULL, p1 = NULL;
1823
1824 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1825
1826 LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1827 mtx_lock(&p->pcb_mtx);
1828
1829 if (p->so == NULL || !SOLISTENING(p->so)) {
1830 mtx_unlock(&p->pcb_mtx);
1831 continue;
1832 }
1833
1834 if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0)
1835 return (p); /* return with locked pcb */
1836
1837 if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0)
1838 p1 = p;
1839
1840 mtx_unlock(&p->pcb_mtx);
1841 }
1842
1843 if (p1 != NULL)
1844 mtx_lock(&p1->pcb_mtx);
1845
1846 return (p1);
1847 } /* ng_btsocket_sco_pcb_by_addr */
1848
1849 /*
1850 * Look for the socket that assigned to given source address and handle.
1851 * Caller must hold ng_btsocket_sco_sockets_mtx.
1852 * Returns with locked pcb.
1853 */
1854
1855 static ng_btsocket_sco_pcb_p
ng_btsocket_sco_pcb_by_handle(bdaddr_p src,int con_handle)1856 ng_btsocket_sco_pcb_by_handle(bdaddr_p src, int con_handle)
1857 {
1858 ng_btsocket_sco_pcb_p p = NULL;
1859
1860 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1861
1862 LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1863 mtx_lock(&p->pcb_mtx);
1864
1865 if (p->con_handle == con_handle &&
1866 bcmp(src, &p->src, sizeof(p->src)) == 0)
1867 return (p); /* return with locked pcb */
1868
1869 mtx_unlock(&p->pcb_mtx);
1870 }
1871
1872 return (NULL);
1873 } /* ng_btsocket_sco_pcb_by_handle */
1874
1875 /*
1876 * Look for the socket in CONNECTING state with given source and destination
1877 * addresses. Caller must hold ng_btsocket_sco_sockets_mtx.
1878 * Returns with locked pcb.
1879 */
1880
1881 static ng_btsocket_sco_pcb_p
ng_btsocket_sco_pcb_by_addrs(bdaddr_p src,bdaddr_p dst)1882 ng_btsocket_sco_pcb_by_addrs(bdaddr_p src, bdaddr_p dst)
1883 {
1884 ng_btsocket_sco_pcb_p p = NULL;
1885
1886 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1887
1888 LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1889 mtx_lock(&p->pcb_mtx);
1890
1891 if (p->state == NG_BTSOCKET_SCO_CONNECTING &&
1892 bcmp(src, &p->src, sizeof(p->src)) == 0 &&
1893 bcmp(dst, &p->dst, sizeof(p->dst)) == 0)
1894 return (p); /* return with locked pcb */
1895
1896 mtx_unlock(&p->pcb_mtx);
1897 }
1898
1899 return (NULL);
1900 } /* ng_btsocket_sco_pcb_by_addrs */
1901
1902 /*
1903 * Set timeout on socket
1904 */
1905
1906 static void
ng_btsocket_sco_timeout(ng_btsocket_sco_pcb_p pcb)1907 ng_btsocket_sco_timeout(ng_btsocket_sco_pcb_p pcb)
1908 {
1909 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1910
1911 if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
1912 pcb->flags |= NG_BTSOCKET_SCO_TIMO;
1913 callout_reset(&pcb->timo, bluetooth_sco_rtx_timeout(),
1914 ng_btsocket_sco_process_timeout, pcb);
1915 } else
1916 KASSERT(0,
1917 ("%s: Duplicated socket timeout?!\n", __func__));
1918 } /* ng_btsocket_sco_timeout */
1919
1920 /*
1921 * Unset timeout on socket
1922 */
1923
1924 static void
ng_btsocket_sco_untimeout(ng_btsocket_sco_pcb_p pcb)1925 ng_btsocket_sco_untimeout(ng_btsocket_sco_pcb_p pcb)
1926 {
1927 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1928
1929 if (pcb->flags & NG_BTSOCKET_SCO_TIMO) {
1930 callout_stop(&pcb->timo);
1931 pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
1932 } else
1933 KASSERT(0,
1934 ("%s: No socket timeout?!\n", __func__));
1935 } /* ng_btsocket_sco_untimeout */
1936
1937 /*
1938 * Process timeout on socket
1939 */
1940
1941 static void
ng_btsocket_sco_process_timeout(void * xpcb)1942 ng_btsocket_sco_process_timeout(void *xpcb)
1943 {
1944 ng_btsocket_sco_pcb_p pcb = (ng_btsocket_sco_pcb_p) xpcb;
1945
1946 mtx_lock(&pcb->pcb_mtx);
1947
1948 pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
1949 pcb->so->so_error = ETIMEDOUT;
1950
1951 switch (pcb->state) {
1952 case NG_BTSOCKET_SCO_CONNECTING:
1953 /* Connect timeout - close the socket */
1954 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1955 soisdisconnected(pcb->so);
1956 break;
1957
1958 case NG_BTSOCKET_SCO_OPEN:
1959 /* Send timeout - did not get NGM_HCI_SYNC_CON_QUEUE */
1960 sbdroprecord(&pcb->so->so_snd);
1961 sowwakeup(pcb->so);
1962 /* XXX FIXME what to do with pcb->rt->pending??? */
1963 break;
1964
1965 case NG_BTSOCKET_SCO_DISCONNECTING:
1966 /* Disconnect timeout - disconnect the socket anyway */
1967 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1968 soisdisconnected(pcb->so);
1969 break;
1970
1971 default:
1972 NG_BTSOCKET_SCO_ERR(
1973 "%s: Invalid socket state=%d\n", __func__, pcb->state);
1974 break;
1975 }
1976
1977 mtx_unlock(&pcb->pcb_mtx);
1978 } /* ng_btsocket_sco_process_timeout */
1979