sys/netgraph/bluetooth/socket/ng_btsocket_sco.c

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