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