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