sys/netgraph/bluetooth/socket/ng_btsocket_sco.c

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