6 * Copyright (c) 2001-2002 Maksim Yevmenkin <m_evmenkin@yahoo.com>
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
12 * 1. Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
30 * $Id: ng_btsocket_sco.c,v 1.2 2005/10/31 18:08:51 max Exp $
34 #include <sys/param.h>
35 #include <sys/systm.h>
36 #include <sys/bitstring.h>
37 #include <sys/domain.h>
38 #include <sys/endian.h>
39 #include <sys/errno.h>
40 #include <sys/filedesc.h>
41 #include <sys/ioccom.h>
42 #include <sys/kernel.h>
44 #include <sys/malloc.h>
46 #include <sys/mutex.h>
47 #include <sys/protosw.h>
48 #include <sys/queue.h>
49 #include <sys/socket.h>
50 #include <sys/socketvar.h>
51 #include <sys/sysctl.h>
52 #include <sys/taskqueue.h>
56 #include <netgraph/ng_message.h>
57 #include <netgraph/netgraph.h>
58 #include <netgraph/bluetooth/include/ng_bluetooth.h>
59 #include <netgraph/bluetooth/include/ng_hci.h>
60 #include <netgraph/bluetooth/include/ng_l2cap.h>
61 #include <netgraph/bluetooth/include/ng_btsocket.h>
62 #include <netgraph/bluetooth/include/ng_btsocket_sco.h>
65 #ifdef NG_SEPARATE_MALLOC
66 static MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_SCO, "netgraph_btsocks_sco",
67 "Netgraph Bluetooth SCO sockets");
69 #define M_NETGRAPH_BTSOCKET_SCO M_NETGRAPH
70 #endif /* NG_SEPARATE_MALLOC */
72 /* Netgraph node methods */
73 static ng_constructor_t ng_btsocket_sco_node_constructor;
74 static ng_rcvmsg_t ng_btsocket_sco_node_rcvmsg;
75 static ng_shutdown_t ng_btsocket_sco_node_shutdown;
76 static ng_newhook_t ng_btsocket_sco_node_newhook;
77 static ng_connect_t ng_btsocket_sco_node_connect;
78 static ng_rcvdata_t ng_btsocket_sco_node_rcvdata;
79 static ng_disconnect_t ng_btsocket_sco_node_disconnect;
81 static void ng_btsocket_sco_input (void *, int);
82 static void ng_btsocket_sco_rtclean (void *, int);
84 /* Netgraph type descriptor */
85 static struct ng_type typestruct = {
86 .version = NG_ABI_VERSION,
87 .name = NG_BTSOCKET_SCO_NODE_TYPE,
88 .constructor = ng_btsocket_sco_node_constructor,
89 .rcvmsg = ng_btsocket_sco_node_rcvmsg,
90 .shutdown = ng_btsocket_sco_node_shutdown,
91 .newhook = ng_btsocket_sco_node_newhook,
92 .connect = ng_btsocket_sco_node_connect,
93 .rcvdata = ng_btsocket_sco_node_rcvdata,
94 .disconnect = ng_btsocket_sco_node_disconnect,
98 static u_int32_t ng_btsocket_sco_debug_level;
99 static node_p ng_btsocket_sco_node;
100 static struct ng_bt_itemq ng_btsocket_sco_queue;
101 static struct mtx ng_btsocket_sco_queue_mtx;
102 static struct task ng_btsocket_sco_queue_task;
103 static struct mtx ng_btsocket_sco_sockets_mtx;
104 static LIST_HEAD(, ng_btsocket_sco_pcb) ng_btsocket_sco_sockets;
105 static LIST_HEAD(, ng_btsocket_sco_rtentry) ng_btsocket_sco_rt;
106 static struct mtx ng_btsocket_sco_rt_mtx;
107 static struct task ng_btsocket_sco_rt_task;
108 static struct timeval ng_btsocket_sco_lasttime;
109 static int ng_btsocket_sco_curpps;
112 SYSCTL_DECL(_net_bluetooth_sco_sockets);
113 static SYSCTL_NODE(_net_bluetooth_sco_sockets, OID_AUTO, seq, CTLFLAG_RW,
114 0, "Bluetooth SEQPACKET SCO sockets family");
115 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, debug_level,
117 &ng_btsocket_sco_debug_level, NG_BTSOCKET_WARN_LEVEL,
118 "Bluetooth SEQPACKET SCO sockets debug level");
119 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_len,
121 &ng_btsocket_sco_queue.len, 0,
122 "Bluetooth SEQPACKET SCO sockets input queue length");
123 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_maxlen,
125 &ng_btsocket_sco_queue.maxlen, 0,
126 "Bluetooth SEQPACKET SCO sockets input queue max. length");
127 SYSCTL_UINT(_net_bluetooth_sco_sockets_seq, OID_AUTO, queue_drops,
129 &ng_btsocket_sco_queue.drops, 0,
130 "Bluetooth SEQPACKET SCO sockets input queue drops");
133 #define NG_BTSOCKET_SCO_INFO \
134 if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_INFO_LEVEL && \
135 ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
138 #define NG_BTSOCKET_SCO_WARN \
139 if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_WARN_LEVEL && \
140 ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
143 #define NG_BTSOCKET_SCO_ERR \
144 if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_ERR_LEVEL && \
145 ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
148 #define NG_BTSOCKET_SCO_ALERT \
149 if (ng_btsocket_sco_debug_level >= NG_BTSOCKET_ALERT_LEVEL && \
150 ppsratecheck(&ng_btsocket_sco_lasttime, &ng_btsocket_sco_curpps, 1)) \
154 * Netgraph message processing routines
157 static int ng_btsocket_sco_process_lp_con_cfm
158 (struct ng_mesg *, ng_btsocket_sco_rtentry_p);
159 static int ng_btsocket_sco_process_lp_con_ind
160 (struct ng_mesg *, ng_btsocket_sco_rtentry_p);
161 static int ng_btsocket_sco_process_lp_discon_ind
162 (struct ng_mesg *, ng_btsocket_sco_rtentry_p);
165 * Send LP messages to the lower layer
168 static int ng_btsocket_sco_send_lp_con_req
169 (ng_btsocket_sco_pcb_p);
170 static int ng_btsocket_sco_send_lp_con_rsp
171 (ng_btsocket_sco_rtentry_p, bdaddr_p, int);
172 static int ng_btsocket_sco_send_lp_discon_req
173 (ng_btsocket_sco_pcb_p);
175 static int ng_btsocket_sco_send2
176 (ng_btsocket_sco_pcb_p);
179 * Timeout processing routines
182 static void ng_btsocket_sco_timeout (ng_btsocket_sco_pcb_p);
183 static void ng_btsocket_sco_untimeout (ng_btsocket_sco_pcb_p);
184 static void ng_btsocket_sco_process_timeout (void *);
190 static ng_btsocket_sco_pcb_p ng_btsocket_sco_pcb_by_addr(bdaddr_p);
191 static ng_btsocket_sco_pcb_p ng_btsocket_sco_pcb_by_handle(bdaddr_p, int);
192 static ng_btsocket_sco_pcb_p ng_btsocket_sco_pcb_by_addrs(bdaddr_p, bdaddr_p);
194 #define ng_btsocket_sco_wakeup_input_task() \
195 taskqueue_enqueue(taskqueue_swi, &ng_btsocket_sco_queue_task)
197 #define ng_btsocket_sco_wakeup_route_task() \
198 taskqueue_enqueue(taskqueue_swi, &ng_btsocket_sco_rt_task)
200 /*****************************************************************************
201 *****************************************************************************
202 ** Netgraph node interface
203 *****************************************************************************
204 *****************************************************************************/
207 * Netgraph node constructor. Do not allow to create node of this type.
211 ng_btsocket_sco_node_constructor(node_p node)
214 } /* ng_btsocket_sco_node_constructor */
217 * Do local shutdown processing. Let old node go and create new fresh one.
221 ng_btsocket_sco_node_shutdown(node_p node)
227 /* Create new node */
228 error = ng_make_node_common(&typestruct, &ng_btsocket_sco_node);
230 NG_BTSOCKET_SCO_ALERT(
231 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
233 ng_btsocket_sco_node = NULL;
238 error = ng_name_node(ng_btsocket_sco_node,
239 NG_BTSOCKET_SCO_NODE_TYPE);
241 NG_BTSOCKET_SCO_ALERT(
242 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
244 NG_NODE_UNREF(ng_btsocket_sco_node);
245 ng_btsocket_sco_node = NULL;
251 } /* ng_btsocket_sco_node_shutdown */
254 * We allow any hook to be connected to the node.
258 ng_btsocket_sco_node_newhook(node_p node, hook_p hook, char const *name)
261 } /* ng_btsocket_sco_node_newhook */
264 * Just say "YEP, that's OK by me!"
268 ng_btsocket_sco_node_connect(hook_p hook)
270 NG_HOOK_SET_PRIVATE(hook, NULL);
271 NG_HOOK_REF(hook); /* Keep extra reference to the hook */
274 NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
275 NG_HOOK_FORCE_QUEUE(hook);
279 } /* ng_btsocket_sco_node_connect */
282 * Hook disconnection. Schedule route cleanup task
286 ng_btsocket_sco_node_disconnect(hook_p hook)
289 * If hook has private information than we must have this hook in
290 * the routing table and must schedule cleaning for the routing table.
291 * Otherwise hook was connected but we never got "hook_info" message,
292 * so we have never added this hook to the routing table and it save
296 if (NG_HOOK_PRIVATE(hook) != NULL)
297 return (ng_btsocket_sco_wakeup_route_task());
299 NG_HOOK_UNREF(hook); /* Remove extra reference */
302 } /* ng_btsocket_sco_node_disconnect */
305 * Process incoming messages
309 ng_btsocket_sco_node_rcvmsg(node_p node, item_p item, hook_p hook)
311 struct ng_mesg *msg = NGI_MSG(item); /* item still has message */
314 if (msg != NULL && msg->header.typecookie == NGM_HCI_COOKIE) {
315 mtx_lock(&ng_btsocket_sco_queue_mtx);
316 if (NG_BT_ITEMQ_FULL(&ng_btsocket_sco_queue)) {
318 "%s: Input queue is full (msg)\n", __func__);
320 NG_BT_ITEMQ_DROP(&ng_btsocket_sco_queue);
326 NGI_SET_HOOK(item, hook);
329 NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_sco_queue, item);
330 error = ng_btsocket_sco_wakeup_input_task();
332 mtx_unlock(&ng_btsocket_sco_queue_mtx);
339 } /* ng_btsocket_sco_node_rcvmsg */
342 * Receive data on a hook
346 ng_btsocket_sco_node_rcvdata(hook_p hook, item_p item)
350 mtx_lock(&ng_btsocket_sco_queue_mtx);
351 if (NG_BT_ITEMQ_FULL(&ng_btsocket_sco_queue)) {
353 "%s: Input queue is full (data)\n", __func__);
355 NG_BT_ITEMQ_DROP(&ng_btsocket_sco_queue);
360 NGI_SET_HOOK(item, hook);
362 NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_sco_queue, item);
363 error = ng_btsocket_sco_wakeup_input_task();
365 mtx_unlock(&ng_btsocket_sco_queue_mtx);
368 } /* ng_btsocket_sco_node_rcvdata */
371 * Process LP_ConnectCfm event from the lower layer protocol
375 ng_btsocket_sco_process_lp_con_cfm(struct ng_mesg *msg,
376 ng_btsocket_sco_rtentry_p rt)
378 ng_hci_lp_con_cfm_ep *ep = NULL;
379 ng_btsocket_sco_pcb_t *pcb = NULL;
382 if (msg->header.arglen != sizeof(*ep))
385 ep = (ng_hci_lp_con_cfm_ep *)(msg->data);
387 mtx_lock(&ng_btsocket_sco_sockets_mtx);
389 /* Look for the socket with the token */
390 pcb = ng_btsocket_sco_pcb_by_addrs(&rt->src, &ep->bdaddr);
392 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
398 NG_BTSOCKET_SCO_INFO(
399 "%s: Got LP_ConnectCfm response, src bdaddr=%x:%x:%x:%x:%x:%x, " \
400 "dst bdaddr=%x:%x:%x:%x:%x:%x, status=%d, handle=%d, state=%d\n",
402 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
403 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
404 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
405 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
406 ep->status, ep->con_handle, pcb->state);
408 if (pcb->state != NG_BTSOCKET_SCO_CONNECTING) {
409 mtx_unlock(&pcb->pcb_mtx);
410 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
415 ng_btsocket_sco_untimeout(pcb);
417 if (ep->status == 0) {
419 * Connection is open. Update connection handle and
423 pcb->con_handle = ep->con_handle;
424 pcb->state = NG_BTSOCKET_SCO_OPEN;
425 soisconnected(pcb->so);
428 * We have failed to open connection, so disconnect the socket
431 pcb->so->so_error = ECONNREFUSED; /* XXX convert status ??? */
432 pcb->state = NG_BTSOCKET_SCO_CLOSED;
433 soisdisconnected(pcb->so);
436 mtx_unlock(&pcb->pcb_mtx);
437 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
440 } /* ng_btsocket_sco_process_lp_con_cfm */
443 * Process LP_ConnectInd indicator. Find socket that listens on address.
444 * Find exact or closest match.
448 ng_btsocket_sco_process_lp_con_ind(struct ng_mesg *msg,
449 ng_btsocket_sco_rtentry_p rt)
451 ng_hci_lp_con_ind_ep *ep = NULL;
452 ng_btsocket_sco_pcb_t *pcb = NULL, *pcb1 = NULL;
454 u_int16_t status = 0;
456 if (msg->header.arglen != sizeof(*ep))
459 ep = (ng_hci_lp_con_ind_ep *)(msg->data);
461 NG_BTSOCKET_SCO_INFO(
462 "%s: Got LP_ConnectInd indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
463 "dst bdaddr=%x:%x:%x:%x:%x:%x\n",
465 rt->src.b[5], rt->src.b[4], rt->src.b[3],
466 rt->src.b[2], rt->src.b[1], rt->src.b[0],
467 ep->bdaddr.b[5], ep->bdaddr.b[4], ep->bdaddr.b[3],
468 ep->bdaddr.b[2], ep->bdaddr.b[1], ep->bdaddr.b[0]);
470 mtx_lock(&ng_btsocket_sco_sockets_mtx);
472 pcb = ng_btsocket_sco_pcb_by_addr(&rt->src);
474 struct socket *so1 = NULL;
479 * First check the pending connections queue and if we have
480 * space then create new socket and set proper source address.
483 if (pcb->so->so_qlen <= pcb->so->so_qlimit) {
484 CURVNET_SET(pcb->so->so_vnet);
485 so1 = sonewconn(pcb->so, 0);
490 status = 0x0d; /* Rejected due to limited resources */
495 * If we got here than we have created new socket. So complete
496 * connection. If we we listening on specific address then copy
497 * source address from listening socket, otherwise copy source
498 * address from hook's routing information.
501 pcb1 = so2sco_pcb(so1);
502 KASSERT((pcb1 != NULL),
503 ("%s: pcb1 == NULL\n", __func__));
505 mtx_lock(&pcb1->pcb_mtx);
507 if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)) != 0)
508 bcopy(&pcb->src, &pcb1->src, sizeof(pcb1->src));
510 bcopy(&rt->src, &pcb1->src, sizeof(pcb1->src));
512 pcb1->flags &= ~NG_BTSOCKET_SCO_CLIENT;
514 bcopy(&ep->bdaddr, &pcb1->dst, sizeof(pcb1->dst));
517 /* Nobody listens on requested BDADDR */
518 status = 0x1f; /* Unspecified Error */
521 error = ng_btsocket_sco_send_lp_con_rsp(rt, &ep->bdaddr, status);
524 pcb1->so->so_error = error;
525 pcb1->state = NG_BTSOCKET_SCO_CLOSED;
526 soisdisconnected(pcb1->so);
528 pcb1->state = NG_BTSOCKET_SCO_CONNECTING;
529 soisconnecting(pcb1->so);
531 ng_btsocket_sco_timeout(pcb1);
534 mtx_unlock(&pcb1->pcb_mtx);
538 mtx_unlock(&pcb->pcb_mtx);
540 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
543 } /* ng_btsocket_sco_process_lp_con_ind */
546 * Process LP_DisconnectInd indicator
550 ng_btsocket_sco_process_lp_discon_ind(struct ng_mesg *msg,
551 ng_btsocket_sco_rtentry_p rt)
553 ng_hci_lp_discon_ind_ep *ep = NULL;
554 ng_btsocket_sco_pcb_t *pcb = NULL;
557 if (msg->header.arglen != sizeof(*ep))
560 ep = (ng_hci_lp_discon_ind_ep *)(msg->data);
562 mtx_lock(&ng_btsocket_sco_sockets_mtx);
564 /* Look for the socket with given channel ID */
565 pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, ep->con_handle);
567 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
572 * Disconnect the socket. If there was any pending request we can
573 * not do anything here anyway.
578 NG_BTSOCKET_SCO_INFO(
579 "%s: Got LP_DisconnectInd indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
580 "dst bdaddr=%x:%x:%x:%x:%x:%x, handle=%d, state=%d\n",
582 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
583 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
584 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
585 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
586 pcb->con_handle, pcb->state);
588 if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
589 ng_btsocket_sco_untimeout(pcb);
591 pcb->state = NG_BTSOCKET_SCO_CLOSED;
592 soisdisconnected(pcb->so);
594 mtx_unlock(&pcb->pcb_mtx);
595 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
598 } /* ng_btsocket_sco_process_lp_discon_ind */
601 * Send LP_ConnectReq request
605 ng_btsocket_sco_send_lp_con_req(ng_btsocket_sco_pcb_p pcb)
607 struct ng_mesg *msg = NULL;
608 ng_hci_lp_con_req_ep *ep = NULL;
611 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
613 if (pcb->rt == NULL ||
614 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
617 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_REQ,
618 sizeof(*ep), M_NOWAIT);
622 ep = (ng_hci_lp_con_req_ep *)(msg->data);
623 ep->link_type = NG_HCI_LINK_SCO;
624 bcopy(&pcb->dst, &ep->bdaddr, sizeof(ep->bdaddr));
626 NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, pcb->rt->hook, 0);
629 } /* ng_btsocket_sco_send_lp_con_req */
632 * Send LP_ConnectRsp response
636 ng_btsocket_sco_send_lp_con_rsp(ng_btsocket_sco_rtentry_p rt, bdaddr_p dst, int status)
638 struct ng_mesg *msg = NULL;
639 ng_hci_lp_con_rsp_ep *ep = NULL;
642 if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
645 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_CON_RSP,
646 sizeof(*ep), M_NOWAIT);
650 ep = (ng_hci_lp_con_rsp_ep *)(msg->data);
652 ep->link_type = NG_HCI_LINK_SCO;
653 bcopy(dst, &ep->bdaddr, sizeof(ep->bdaddr));
655 NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, rt->hook, 0);
658 } /* ng_btsocket_sco_send_lp_con_rsp */
661 * Send LP_DisconReq request
665 ng_btsocket_sco_send_lp_discon_req(ng_btsocket_sco_pcb_p pcb)
667 struct ng_mesg *msg = NULL;
668 ng_hci_lp_discon_req_ep *ep = NULL;
671 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
673 if (pcb->rt == NULL ||
674 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
677 NG_MKMESSAGE(msg, NGM_HCI_COOKIE, NGM_HCI_LP_DISCON_REQ,
678 sizeof(*ep), M_NOWAIT);
682 ep = (ng_hci_lp_discon_req_ep *)(msg->data);
683 ep->con_handle = pcb->con_handle;
684 ep->reason = 0x13; /* User Ended Connection */
686 NG_SEND_MSG_HOOK(error, ng_btsocket_sco_node, msg, pcb->rt->hook, 0);
689 } /* ng_btsocket_sco_send_lp_discon_req */
691 /*****************************************************************************
692 *****************************************************************************
694 *****************************************************************************
695 *****************************************************************************/
698 * SCO sockets data input routine
702 ng_btsocket_sco_data_input(struct mbuf *m, hook_p hook)
704 ng_hci_scodata_pkt_t *hdr = NULL;
705 ng_btsocket_sco_pcb_t *pcb = NULL;
706 ng_btsocket_sco_rtentry_t *rt = NULL;
707 u_int16_t con_handle;
710 NG_BTSOCKET_SCO_ALERT(
711 "%s: Invalid source hook for SCO data packet\n", __func__);
715 rt = (ng_btsocket_sco_rtentry_t *) NG_HOOK_PRIVATE(hook);
717 NG_BTSOCKET_SCO_ALERT(
718 "%s: Could not find out source bdaddr for SCO data packet\n", __func__);
722 /* Make sure we can access header */
723 if (m->m_pkthdr.len < sizeof(*hdr)) {
725 "%s: SCO data packet too small, len=%d\n", __func__, m->m_pkthdr.len);
729 if (m->m_len < sizeof(*hdr)) {
730 m = m_pullup(m, sizeof(*hdr));
735 /* Strip SCO packet header and verify packet length */
736 hdr = mtod(m, ng_hci_scodata_pkt_t *);
737 m_adj(m, sizeof(*hdr));
739 if (hdr->length != m->m_pkthdr.len) {
741 "%s: Bad SCO data packet length, len=%d, length=%d\n",
742 __func__, m->m_pkthdr.len, hdr->length);
750 con_handle = NG_HCI_CON_HANDLE(le16toh(hdr->con_handle));
752 NG_BTSOCKET_SCO_INFO(
753 "%s: Received SCO data packet: src bdaddr=%x:%x:%x:%x:%x:%x, handle=%d, " \
754 "length=%d\n", __func__,
755 rt->src.b[5], rt->src.b[4], rt->src.b[3],
756 rt->src.b[2], rt->src.b[1], rt->src.b[0],
757 con_handle, hdr->length);
759 mtx_lock(&ng_btsocket_sco_sockets_mtx);
762 pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, con_handle);
764 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
770 if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
772 "%s: No connected socket found, src bdaddr=%x:%x:%x:%x:%x:%x, state=%d\n",
774 rt->src.b[5], rt->src.b[4], rt->src.b[3],
775 rt->src.b[2], rt->src.b[1], rt->src.b[0],
778 mtx_unlock(&pcb->pcb_mtx);
779 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
783 /* Check if we have enough space in socket receive queue */
784 if (m->m_pkthdr.len > sbspace(&pcb->so->so_rcv)) {
786 "%s: Not enough space in socket receive queue. Dropping SCO data packet, " \
787 "src bdaddr=%x:%x:%x:%x:%x:%x, len=%d, space=%ld\n",
789 rt->src.b[5], rt->src.b[4], rt->src.b[3],
790 rt->src.b[2], rt->src.b[1], rt->src.b[0],
792 sbspace(&pcb->so->so_rcv));
794 mtx_unlock(&pcb->pcb_mtx);
795 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
799 /* Append packet to the socket receive queue and wakeup */
800 sbappendrecord(&pcb->so->so_rcv, m);
805 mtx_unlock(&pcb->pcb_mtx);
806 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
808 NG_FREE_M(m); /* checks for m != NULL */
809 } /* ng_btsocket_sco_data_input */
812 * SCO sockets default message input routine
816 ng_btsocket_sco_default_msg_input(struct ng_mesg *msg, hook_p hook)
818 ng_btsocket_sco_rtentry_t *rt = NULL;
820 if (hook == NULL || NG_HOOK_NOT_VALID(hook))
823 rt = (ng_btsocket_sco_rtentry_t *) NG_HOOK_PRIVATE(hook);
825 switch (msg->header.cmd) {
826 case NGM_HCI_NODE_UP: {
827 ng_hci_node_up_ep *ep = NULL;
829 if (msg->header.arglen != sizeof(*ep))
832 ep = (ng_hci_node_up_ep *)(msg->data);
833 if (bcmp(&ep->bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
837 rt = malloc(sizeof(*rt),
838 M_NETGRAPH_BTSOCKET_SCO, M_NOWAIT|M_ZERO);
842 NG_HOOK_SET_PRIVATE(hook, rt);
844 mtx_lock(&ng_btsocket_sco_rt_mtx);
846 LIST_INSERT_HEAD(&ng_btsocket_sco_rt, rt, next);
848 mtx_lock(&ng_btsocket_sco_rt_mtx);
850 bcopy(&ep->bdaddr, &rt->src, sizeof(rt->src));
851 rt->pkt_size = (ep->pkt_size == 0)? 60 : ep->pkt_size;
852 rt->num_pkts = ep->num_pkts;
855 mtx_unlock(&ng_btsocket_sco_rt_mtx);
857 NG_BTSOCKET_SCO_INFO(
858 "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x, pkt_size=%d, " \
859 "num_pkts=%d\n", __func__, NG_HOOK_NAME(hook),
860 rt->src.b[5], rt->src.b[4], rt->src.b[3],
861 rt->src.b[2], rt->src.b[1], rt->src.b[0],
862 rt->pkt_size, rt->num_pkts);
865 case NGM_HCI_SYNC_CON_QUEUE: {
866 ng_hci_sync_con_queue_ep *ep = NULL;
867 ng_btsocket_sco_pcb_t *pcb = NULL;
869 if (rt == NULL || msg->header.arglen != sizeof(*ep))
872 ep = (ng_hci_sync_con_queue_ep *)(msg->data);
874 rt->pending -= ep->completed;
875 if (rt->pending < 0) {
876 NG_BTSOCKET_SCO_WARN(
877 "%s: Pending packet counter is out of sync! bdaddr=%x:%x:%x:%x:%x:%x, " \
878 "handle=%d, pending=%d, completed=%d\n",
880 rt->src.b[5], rt->src.b[4], rt->src.b[3],
881 rt->src.b[2], rt->src.b[1], rt->src.b[0],
882 ep->con_handle, rt->pending,
888 mtx_lock(&ng_btsocket_sco_sockets_mtx);
891 pcb = ng_btsocket_sco_pcb_by_handle(&rt->src, ep->con_handle);
893 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
900 if (pcb->state == NG_BTSOCKET_SCO_OPEN) {
902 ng_btsocket_sco_untimeout(pcb);
904 /* Drop completed packets from the send queue */
905 for (; ep->completed > 0; ep->completed --)
906 sbdroprecord(&pcb->so->so_snd);
908 /* Send more if we have any */
909 if (sbavail(&pcb->so->so_snd) > 0)
910 if (ng_btsocket_sco_send2(pcb) == 0)
911 ng_btsocket_sco_timeout(pcb);
913 /* Wake up writers */
917 mtx_unlock(&pcb->pcb_mtx);
918 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
922 NG_BTSOCKET_SCO_WARN(
923 "%s: Unknown message, cmd=%d\n", __func__, msg->header.cmd);
927 NG_FREE_MSG(msg); /* Checks for msg != NULL */
928 } /* ng_btsocket_sco_default_msg_input */
931 * SCO sockets LP message input routine
935 ng_btsocket_sco_lp_msg_input(struct ng_mesg *msg, hook_p hook)
937 ng_btsocket_sco_rtentry_p rt = NULL;
940 NG_BTSOCKET_SCO_ALERT(
941 "%s: Invalid source hook for LP message\n", __func__);
945 rt = (ng_btsocket_sco_rtentry_p) NG_HOOK_PRIVATE(hook);
947 NG_BTSOCKET_SCO_ALERT(
948 "%s: Could not find out source bdaddr for LP message\n", __func__);
952 switch (msg->header.cmd) {
953 case NGM_HCI_LP_CON_CFM: /* Connection Confirmation Event */
954 ng_btsocket_sco_process_lp_con_cfm(msg, rt);
957 case NGM_HCI_LP_CON_IND: /* Connection Indication Event */
958 ng_btsocket_sco_process_lp_con_ind(msg, rt);
961 case NGM_HCI_LP_DISCON_IND: /* Disconnection Indication Event */
962 ng_btsocket_sco_process_lp_discon_ind(msg, rt);
965 /* XXX FIXME add other LP messages */
968 NG_BTSOCKET_SCO_WARN(
969 "%s: Unknown LP message, cmd=%d\n", __func__, msg->header.cmd);
974 } /* ng_btsocket_sco_lp_msg_input */
977 * SCO sockets input routine
981 ng_btsocket_sco_input(void *context, int pending)
987 mtx_lock(&ng_btsocket_sco_queue_mtx);
988 NG_BT_ITEMQ_DEQUEUE(&ng_btsocket_sco_queue, item);
989 mtx_unlock(&ng_btsocket_sco_queue_mtx);
994 NGI_GET_HOOK(item, hook);
995 if (hook != NULL && NG_HOOK_NOT_VALID(hook))
998 switch(item->el_flags & NGQF_TYPE) {
1000 struct mbuf *m = NULL;
1003 ng_btsocket_sco_data_input(m, hook);
1007 struct ng_mesg *msg = NULL;
1009 NGI_GET_MSG(item, msg);
1011 switch (msg->header.cmd) {
1012 case NGM_HCI_LP_CON_CFM:
1013 case NGM_HCI_LP_CON_IND:
1014 case NGM_HCI_LP_DISCON_IND:
1015 /* XXX FIXME add other LP messages */
1016 ng_btsocket_sco_lp_msg_input(msg, hook);
1020 ng_btsocket_sco_default_msg_input(msg, hook);
1027 ("%s: invalid item type=%ld\n", __func__, (item->el_flags & NGQF_TYPE)));
1032 NG_HOOK_UNREF(hook);
1036 } /* ng_btsocket_sco_input */
1039 * Route cleanup task. Gets scheduled when hook is disconnected. Here we
1040 * will find all sockets that use "invalid" hook and disconnect them.
1044 ng_btsocket_sco_rtclean(void *context, int pending)
1046 ng_btsocket_sco_pcb_p pcb = NULL, pcb_next = NULL;
1047 ng_btsocket_sco_rtentry_p rt = NULL;
1050 * First disconnect all sockets that use "invalid" hook
1053 mtx_lock(&ng_btsocket_sco_sockets_mtx);
1055 for(pcb = LIST_FIRST(&ng_btsocket_sco_sockets); pcb != NULL; ) {
1056 mtx_lock(&pcb->pcb_mtx);
1057 pcb_next = LIST_NEXT(pcb, next);
1059 if (pcb->rt != NULL &&
1060 pcb->rt->hook != NULL && NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1061 if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1062 ng_btsocket_sco_untimeout(pcb);
1065 pcb->so->so_error = ENETDOWN;
1066 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1067 soisdisconnected(pcb->so);
1070 mtx_unlock(&pcb->pcb_mtx);
1074 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1077 * Now cleanup routing table
1080 mtx_lock(&ng_btsocket_sco_rt_mtx);
1082 for (rt = LIST_FIRST(&ng_btsocket_sco_rt); rt != NULL; ) {
1083 ng_btsocket_sco_rtentry_p rt_next = LIST_NEXT(rt, next);
1085 if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
1086 LIST_REMOVE(rt, next);
1088 NG_HOOK_SET_PRIVATE(rt->hook, NULL);
1089 NG_HOOK_UNREF(rt->hook); /* Remove extra reference */
1091 bzero(rt, sizeof(*rt));
1092 free(rt, M_NETGRAPH_BTSOCKET_SCO);
1098 mtx_unlock(&ng_btsocket_sco_rt_mtx);
1099 } /* ng_btsocket_sco_rtclean */
1102 * Initialize everything
1106 ng_btsocket_sco_init(void)
1110 /* Skip initialization of globals for non-default instances. */
1111 if (!IS_DEFAULT_VNET(curvnet))
1114 ng_btsocket_sco_node = NULL;
1115 ng_btsocket_sco_debug_level = NG_BTSOCKET_WARN_LEVEL;
1117 /* Register Netgraph node type */
1118 error = ng_newtype(&typestruct);
1120 NG_BTSOCKET_SCO_ALERT(
1121 "%s: Could not register Netgraph node type, error=%d\n", __func__, error);
1126 /* Create Netgrapg node */
1127 error = ng_make_node_common(&typestruct, &ng_btsocket_sco_node);
1129 NG_BTSOCKET_SCO_ALERT(
1130 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
1132 ng_btsocket_sco_node = NULL;
1137 error = ng_name_node(ng_btsocket_sco_node, NG_BTSOCKET_SCO_NODE_TYPE);
1139 NG_BTSOCKET_SCO_ALERT(
1140 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
1142 NG_NODE_UNREF(ng_btsocket_sco_node);
1143 ng_btsocket_sco_node = NULL;
1148 /* Create input queue */
1149 NG_BT_ITEMQ_INIT(&ng_btsocket_sco_queue, 300);
1150 mtx_init(&ng_btsocket_sco_queue_mtx,
1151 "btsocks_sco_queue_mtx", NULL, MTX_DEF);
1152 TASK_INIT(&ng_btsocket_sco_queue_task, 0,
1153 ng_btsocket_sco_input, NULL);
1155 /* Create list of sockets */
1156 LIST_INIT(&ng_btsocket_sco_sockets);
1157 mtx_init(&ng_btsocket_sco_sockets_mtx,
1158 "btsocks_sco_sockets_mtx", NULL, MTX_DEF);
1161 LIST_INIT(&ng_btsocket_sco_rt);
1162 mtx_init(&ng_btsocket_sco_rt_mtx,
1163 "btsocks_sco_rt_mtx", NULL, MTX_DEF);
1164 TASK_INIT(&ng_btsocket_sco_rt_task, 0,
1165 ng_btsocket_sco_rtclean, NULL);
1166 } /* ng_btsocket_sco_init */
1169 * Abort connection on socket
1173 ng_btsocket_sco_abort(struct socket *so)
1175 so->so_error = ECONNABORTED;
1177 (void) ng_btsocket_sco_disconnect(so);
1178 } /* ng_btsocket_sco_abort */
1181 ng_btsocket_sco_close(struct socket *so)
1183 (void) ng_btsocket_sco_disconnect(so);
1184 } /* ng_btsocket_sco_close */
1187 * Accept connection on socket. Nothing to do here, socket must be connected
1188 * and ready, so just return peer address and be done with it.
1192 ng_btsocket_sco_accept(struct socket *so, struct sockaddr **nam)
1194 if (ng_btsocket_sco_node == NULL)
1197 return (ng_btsocket_sco_peeraddr(so, nam));
1198 } /* ng_btsocket_sco_accept */
1201 * Create and attach new socket
1205 ng_btsocket_sco_attach(struct socket *so, int proto, struct thread *td)
1207 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1210 /* Check socket and protocol */
1211 if (ng_btsocket_sco_node == NULL)
1212 return (EPROTONOSUPPORT);
1213 if (so->so_type != SOCK_SEQPACKET)
1214 return (ESOCKTNOSUPPORT);
1216 #if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
1218 if (proto != BLUETOOTH_PROTO_SCO)
1219 return (EPROTONOSUPPORT);
1225 /* Reserve send and receive space if it is not reserved yet */
1226 if ((so->so_snd.sb_hiwat == 0) || (so->so_rcv.sb_hiwat == 0)) {
1227 error = soreserve(so, NG_BTSOCKET_SCO_SENDSPACE,
1228 NG_BTSOCKET_SCO_RECVSPACE);
1233 /* Allocate the PCB */
1234 pcb = malloc(sizeof(*pcb),
1235 M_NETGRAPH_BTSOCKET_SCO, M_NOWAIT | M_ZERO);
1239 /* Link the PCB and the socket */
1240 so->so_pcb = (caddr_t) pcb;
1242 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1244 callout_init(&pcb->timo, 1);
1247 * Mark PCB mutex as DUPOK to prevent "duplicated lock of
1248 * the same type" message. When accepting new SCO connection
1249 * ng_btsocket_sco_process_lp_con_ind() holds both PCB mutexes
1250 * for "old" (accepting) PCB and "new" (created) PCB.
1253 mtx_init(&pcb->pcb_mtx, "btsocks_sco_pcb_mtx", NULL,
1257 * Add the PCB to the list
1259 * XXX FIXME VERY IMPORTANT!
1261 * This is totally FUBAR. We could get here in two cases:
1263 * 1) When user calls socket()
1264 * 2) When we need to accept new incomming connection and call
1267 * In the first case we must aquire ng_btsocket_sco_sockets_mtx.
1268 * In the second case we hold ng_btsocket_sco_sockets_mtx already.
1269 * So we now need to distinguish between these cases. From reading
1270 * /sys/kern/uipc_socket2.c we can find out that sonewconn() calls
1271 * pru_attach with proto == 0 and td == NULL. For now use this fact
1272 * to figure out if we were called from socket() or from sonewconn().
1276 mtx_lock(&ng_btsocket_sco_sockets_mtx);
1278 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1280 LIST_INSERT_HEAD(&ng_btsocket_sco_sockets, pcb, next);
1283 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1286 } /* ng_btsocket_sco_attach */
1293 ng_btsocket_sco_bind(struct socket *so, struct sockaddr *nam,
1296 ng_btsocket_sco_pcb_t *pcb = NULL;
1297 struct sockaddr_sco *sa = (struct sockaddr_sco *) nam;
1299 if (ng_btsocket_sco_node == NULL)
1302 /* Verify address */
1305 if (sa->sco_family != AF_BLUETOOTH)
1306 return (EAFNOSUPPORT);
1307 if (sa->sco_len != sizeof(*sa))
1310 mtx_lock(&ng_btsocket_sco_sockets_mtx);
1313 * Check if other socket has this address already (look for exact
1314 * match in bdaddr) and assign socket address if it's available.
1317 if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(sa->sco_bdaddr)) != 0) {
1318 LIST_FOREACH(pcb, &ng_btsocket_sco_sockets, next) {
1319 mtx_lock(&pcb->pcb_mtx);
1321 if (bcmp(&pcb->src, &sa->sco_bdaddr, sizeof(bdaddr_t)) == 0) {
1322 mtx_unlock(&pcb->pcb_mtx);
1323 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1325 return (EADDRINUSE);
1328 mtx_unlock(&pcb->pcb_mtx);
1333 pcb = so2sco_pcb(so);
1335 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1339 mtx_lock(&pcb->pcb_mtx);
1340 bcopy(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src));
1341 mtx_unlock(&pcb->pcb_mtx);
1343 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1346 } /* ng_btsocket_sco_bind */
1353 ng_btsocket_sco_connect(struct socket *so, struct sockaddr *nam,
1356 ng_btsocket_sco_pcb_t *pcb = so2sco_pcb(so);
1357 struct sockaddr_sco *sa = (struct sockaddr_sco *) nam;
1358 ng_btsocket_sco_rtentry_t *rt = NULL;
1359 int have_src, error = 0;
1364 if (ng_btsocket_sco_node == NULL)
1367 /* Verify address */
1370 if (sa->sco_family != AF_BLUETOOTH)
1371 return (EAFNOSUPPORT);
1372 if (sa->sco_len != sizeof(*sa))
1374 if (bcmp(&sa->sco_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
1375 return (EDESTADDRREQ);
1378 * Routing. Socket should be bound to some source address. The source
1379 * address can be ANY. Destination address must be set and it must not
1380 * be ANY. If source address is ANY then find first rtentry that has
1384 mtx_lock(&ng_btsocket_sco_rt_mtx);
1385 mtx_lock(&pcb->pcb_mtx);
1387 if (pcb->state == NG_BTSOCKET_SCO_CONNECTING) {
1388 mtx_unlock(&pcb->pcb_mtx);
1389 mtx_unlock(&ng_btsocket_sco_rt_mtx);
1391 return (EINPROGRESS);
1394 if (bcmp(&sa->sco_bdaddr, &pcb->src, sizeof(pcb->src)) == 0) {
1395 mtx_unlock(&pcb->pcb_mtx);
1396 mtx_unlock(&ng_btsocket_sco_rt_mtx);
1401 /* Send destination address and PSM */
1402 bcopy(&sa->sco_bdaddr, &pcb->dst, sizeof(pcb->dst));
1405 have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
1407 LIST_FOREACH(rt, &ng_btsocket_sco_rt, next) {
1408 if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
1411 /* Match src and dst */
1413 if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
1416 if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
1425 bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
1427 error = EHOSTUNREACH;
1430 * Send LP_Connect request
1434 error = ng_btsocket_sco_send_lp_con_req(pcb);
1436 pcb->flags |= NG_BTSOCKET_SCO_CLIENT;
1437 pcb->state = NG_BTSOCKET_SCO_CONNECTING;
1438 soisconnecting(pcb->so);
1440 ng_btsocket_sco_timeout(pcb);
1444 mtx_unlock(&pcb->pcb_mtx);
1445 mtx_unlock(&ng_btsocket_sco_rt_mtx);
1448 } /* ng_btsocket_sco_connect */
1451 * Process ioctl's calls on socket
1455 ng_btsocket_sco_control(struct socket *so, u_long cmd, caddr_t data,
1456 struct ifnet *ifp, struct thread *td)
1459 } /* ng_btsocket_sco_control */
1462 * Process getsockopt/setsockopt system calls
1466 ng_btsocket_sco_ctloutput(struct socket *so, struct sockopt *sopt)
1468 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1471 if (ng_btsocket_sco_node == NULL)
1476 if (sopt->sopt_level != SOL_SCO)
1479 mtx_lock(&pcb->pcb_mtx);
1481 switch (sopt->sopt_dir) {
1483 if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
1488 switch (sopt->sopt_name) {
1490 tmp = pcb->rt->pkt_size;
1491 error = sooptcopyout(sopt, &tmp, sizeof(tmp));
1494 case SO_SCO_CONNINFO:
1495 tmp = pcb->con_handle;
1496 error = sooptcopyout(sopt, &tmp, sizeof(tmp));
1506 error = ENOPROTOOPT;
1514 mtx_unlock(&pcb->pcb_mtx);
1517 } /* ng_btsocket_sco_ctloutput */
1520 * Detach and destroy socket
1524 ng_btsocket_sco_detach(struct socket *so)
1526 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1528 KASSERT(pcb != NULL, ("ng_btsocket_sco_detach: pcb == NULL"));
1530 if (ng_btsocket_sco_node == NULL)
1533 mtx_lock(&ng_btsocket_sco_sockets_mtx);
1534 mtx_lock(&pcb->pcb_mtx);
1536 if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1537 ng_btsocket_sco_untimeout(pcb);
1539 if (pcb->state == NG_BTSOCKET_SCO_OPEN)
1540 ng_btsocket_sco_send_lp_discon_req(pcb);
1542 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1544 LIST_REMOVE(pcb, next);
1546 mtx_unlock(&pcb->pcb_mtx);
1547 mtx_unlock(&ng_btsocket_sco_sockets_mtx);
1549 mtx_destroy(&pcb->pcb_mtx);
1550 bzero(pcb, sizeof(*pcb));
1551 free(pcb, M_NETGRAPH_BTSOCKET_SCO);
1553 soisdisconnected(so);
1555 } /* ng_btsocket_sco_detach */
1562 ng_btsocket_sco_disconnect(struct socket *so)
1564 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1568 if (ng_btsocket_sco_node == NULL)
1571 mtx_lock(&pcb->pcb_mtx);
1573 if (pcb->state == NG_BTSOCKET_SCO_DISCONNECTING) {
1574 mtx_unlock(&pcb->pcb_mtx);
1576 return (EINPROGRESS);
1579 if (pcb->flags & NG_BTSOCKET_SCO_TIMO)
1580 ng_btsocket_sco_untimeout(pcb);
1582 if (pcb->state == NG_BTSOCKET_SCO_OPEN) {
1583 ng_btsocket_sco_send_lp_discon_req(pcb);
1585 pcb->state = NG_BTSOCKET_SCO_DISCONNECTING;
1586 soisdisconnecting(so);
1588 ng_btsocket_sco_timeout(pcb);
1590 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1591 soisdisconnected(so);
1594 mtx_unlock(&pcb->pcb_mtx);
1597 } /* ng_btsocket_sco_disconnect */
1604 ng_btsocket_sco_listen(struct socket *so, int backlog, struct thread *td)
1606 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1611 if (ng_btsocket_sco_node == NULL)
1615 mtx_lock(&pcb->pcb_mtx);
1617 error = solisten_proto_check(so);
1621 if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) {
1622 error = EDESTADDRREQ;
1626 solisten_proto(so, backlog);
1628 mtx_unlock(&pcb->pcb_mtx);
1632 } /* ng_btsocket_listen */
1639 ng_btsocket_sco_peeraddr(struct socket *so, struct sockaddr **nam)
1641 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1642 struct sockaddr_sco sa;
1646 if (ng_btsocket_sco_node == NULL)
1649 mtx_lock(&pcb->pcb_mtx);
1650 bcopy(&pcb->dst, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
1651 mtx_unlock(&pcb->pcb_mtx);
1653 sa.sco_len = sizeof(sa);
1654 sa.sco_family = AF_BLUETOOTH;
1656 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
1658 return ((*nam == NULL)? ENOMEM : 0);
1659 } /* ng_btsocket_sco_peeraddr */
1662 * Send data to socket
1666 ng_btsocket_sco_send(struct socket *so, int flags, struct mbuf *m,
1667 struct sockaddr *nam, struct mbuf *control, struct thread *td)
1669 ng_btsocket_sco_pcb_t *pcb = so2sco_pcb(so);
1672 if (ng_btsocket_sco_node == NULL) {
1677 /* Check socket and input */
1678 if (pcb == NULL || m == NULL || control != NULL) {
1683 mtx_lock(&pcb->pcb_mtx);
1685 /* Make sure socket is connected */
1686 if (pcb->state != NG_BTSOCKET_SCO_OPEN) {
1687 mtx_unlock(&pcb->pcb_mtx);
1693 if (pcb->rt == NULL ||
1694 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1695 mtx_unlock(&pcb->pcb_mtx);
1700 /* Check packet size */
1701 if (m->m_pkthdr.len > pcb->rt->pkt_size) {
1702 NG_BTSOCKET_SCO_ERR(
1703 "%s: Packet too big, len=%d, pkt_size=%d\n",
1704 __func__, m->m_pkthdr.len, pcb->rt->pkt_size);
1706 mtx_unlock(&pcb->pcb_mtx);
1712 * First put packet on socket send queue. Then check if we have
1713 * pending timeout. If we do not have timeout then we must send
1714 * packet and schedule timeout. Otherwise do nothing and wait for
1715 * NGM_HCI_SYNC_CON_QUEUE message.
1718 sbappendrecord(&pcb->so->so_snd, m);
1721 if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
1722 error = ng_btsocket_sco_send2(pcb);
1724 ng_btsocket_sco_timeout(pcb);
1726 sbdroprecord(&pcb->so->so_snd); /* XXX */
1729 mtx_unlock(&pcb->pcb_mtx);
1731 NG_FREE_M(m); /* checks for != NULL */
1735 } /* ng_btsocket_sco_send */
1738 * Send first packet in the socket queue to the SCO layer
1742 ng_btsocket_sco_send2(ng_btsocket_sco_pcb_p pcb)
1744 struct mbuf *m = NULL;
1745 ng_hci_scodata_pkt_t *hdr = NULL;
1748 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1750 while (pcb->rt->pending < pcb->rt->num_pkts &&
1751 sbavail(&pcb->so->so_snd) > 0) {
1752 /* Get a copy of the first packet on send queue */
1753 m = m_dup(pcb->so->so_snd.sb_mb, M_NOWAIT);
1759 /* Create SCO packet header */
1760 M_PREPEND(m, sizeof(*hdr), M_NOWAIT);
1762 if (m->m_len < sizeof(*hdr))
1763 m = m_pullup(m, sizeof(*hdr));
1770 /* Fill in the header */
1771 hdr = mtod(m, ng_hci_scodata_pkt_t *);
1772 hdr->type = NG_HCI_SCO_DATA_PKT;
1773 hdr->con_handle = htole16(NG_HCI_MK_CON_HANDLE(pcb->con_handle, 0, 0));
1774 hdr->length = m->m_pkthdr.len - sizeof(*hdr);
1777 NG_SEND_DATA_ONLY(error, pcb->rt->hook, m);
1781 pcb->rt->pending ++;
1784 return ((pcb->rt->pending > 0)? 0 : error);
1785 } /* ng_btsocket_sco_send2 */
1788 * Get socket address
1792 ng_btsocket_sco_sockaddr(struct socket *so, struct sockaddr **nam)
1794 ng_btsocket_sco_pcb_p pcb = so2sco_pcb(so);
1795 struct sockaddr_sco sa;
1799 if (ng_btsocket_sco_node == NULL)
1802 mtx_lock(&pcb->pcb_mtx);
1803 bcopy(&pcb->src, &sa.sco_bdaddr, sizeof(sa.sco_bdaddr));
1804 mtx_unlock(&pcb->pcb_mtx);
1806 sa.sco_len = sizeof(sa);
1807 sa.sco_family = AF_BLUETOOTH;
1809 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT);
1811 return ((*nam == NULL)? ENOMEM : 0);
1812 } /* ng_btsocket_sco_sockaddr */
1814 /*****************************************************************************
1815 *****************************************************************************
1817 *****************************************************************************
1818 *****************************************************************************/
1821 * Look for the socket that listens on given bdaddr.
1822 * Returns exact or close match (if any).
1823 * Caller must hold ng_btsocket_sco_sockets_mtx.
1824 * Returns with locked pcb.
1827 static ng_btsocket_sco_pcb_p
1828 ng_btsocket_sco_pcb_by_addr(bdaddr_p bdaddr)
1830 ng_btsocket_sco_pcb_p p = NULL, p1 = NULL;
1832 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1834 LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1835 mtx_lock(&p->pcb_mtx);
1837 if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN)) {
1838 mtx_unlock(&p->pcb_mtx);
1842 if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0)
1843 return (p); /* return with locked pcb */
1845 if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0)
1848 mtx_unlock(&p->pcb_mtx);
1852 mtx_lock(&p1->pcb_mtx);
1855 } /* ng_btsocket_sco_pcb_by_addr */
1858 * Look for the socket that assigned to given source address and handle.
1859 * Caller must hold ng_btsocket_sco_sockets_mtx.
1860 * Returns with locked pcb.
1863 static ng_btsocket_sco_pcb_p
1864 ng_btsocket_sco_pcb_by_handle(bdaddr_p src, int con_handle)
1866 ng_btsocket_sco_pcb_p p = NULL;
1868 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1870 LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1871 mtx_lock(&p->pcb_mtx);
1873 if (p->con_handle == con_handle &&
1874 bcmp(src, &p->src, sizeof(p->src)) == 0)
1875 return (p); /* return with locked pcb */
1877 mtx_unlock(&p->pcb_mtx);
1881 } /* ng_btsocket_sco_pcb_by_handle */
1884 * Look for the socket in CONNECTING state with given source and destination
1885 * addresses. Caller must hold ng_btsocket_sco_sockets_mtx.
1886 * Returns with locked pcb.
1889 static ng_btsocket_sco_pcb_p
1890 ng_btsocket_sco_pcb_by_addrs(bdaddr_p src, bdaddr_p dst)
1892 ng_btsocket_sco_pcb_p p = NULL;
1894 mtx_assert(&ng_btsocket_sco_sockets_mtx, MA_OWNED);
1896 LIST_FOREACH(p, &ng_btsocket_sco_sockets, next) {
1897 mtx_lock(&p->pcb_mtx);
1899 if (p->state == NG_BTSOCKET_SCO_CONNECTING &&
1900 bcmp(src, &p->src, sizeof(p->src)) == 0 &&
1901 bcmp(dst, &p->dst, sizeof(p->dst)) == 0)
1902 return (p); /* return with locked pcb */
1904 mtx_unlock(&p->pcb_mtx);
1908 } /* ng_btsocket_sco_pcb_by_addrs */
1911 * Set timeout on socket
1915 ng_btsocket_sco_timeout(ng_btsocket_sco_pcb_p pcb)
1917 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1919 if (!(pcb->flags & NG_BTSOCKET_SCO_TIMO)) {
1920 pcb->flags |= NG_BTSOCKET_SCO_TIMO;
1921 callout_reset(&pcb->timo, bluetooth_sco_rtx_timeout(),
1922 ng_btsocket_sco_process_timeout, pcb);
1925 ("%s: Duplicated socket timeout?!\n", __func__));
1926 } /* ng_btsocket_sco_timeout */
1929 * Unset timeout on socket
1933 ng_btsocket_sco_untimeout(ng_btsocket_sco_pcb_p pcb)
1935 mtx_assert(&pcb->pcb_mtx, MA_OWNED);
1937 if (pcb->flags & NG_BTSOCKET_SCO_TIMO) {
1938 callout_stop(&pcb->timo);
1939 pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
1942 ("%s: No socket timeout?!\n", __func__));
1943 } /* ng_btsocket_sco_untimeout */
1946 * Process timeout on socket
1950 ng_btsocket_sco_process_timeout(void *xpcb)
1952 ng_btsocket_sco_pcb_p pcb = (ng_btsocket_sco_pcb_p) xpcb;
1954 mtx_lock(&pcb->pcb_mtx);
1956 pcb->flags &= ~NG_BTSOCKET_SCO_TIMO;
1957 pcb->so->so_error = ETIMEDOUT;
1959 switch (pcb->state) {
1960 case NG_BTSOCKET_SCO_CONNECTING:
1961 /* Connect timeout - close the socket */
1962 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1963 soisdisconnected(pcb->so);
1966 case NG_BTSOCKET_SCO_OPEN:
1967 /* Send timeout - did not get NGM_HCI_SYNC_CON_QUEUE */
1968 sbdroprecord(&pcb->so->so_snd);
1970 /* XXX FIXME what to do with pcb->rt->pending??? */
1973 case NG_BTSOCKET_SCO_DISCONNECTING:
1974 /* Disconnect timeout - disconnect the socket anyway */
1975 pcb->state = NG_BTSOCKET_SCO_CLOSED;
1976 soisdisconnected(pcb->so);
1980 NG_BTSOCKET_SCO_ERR(
1981 "%s: Invalid socket state=%d\n", __func__, pcb->state);
1985 mtx_unlock(&pcb->pcb_mtx);
1986 } /* ng_btsocket_sco_process_timeout */