sys/netgraph7/bluetooth/socket/ng_btsocket_l2cap.c

   1 /*
   2  * ng_btsocket_l2cap.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_l2cap.c,v 1.16 2003/09/14 23:29:06 max Exp $
  31  * $FreeBSD: src/sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c,v 1.25 2007/10/31 16:17:20 emax Exp $
  32  */
  33
  34 #include <sys/param.h>
  35 #include <sys/systm.h>
  36 #include <bitstring.h>          /* XXX */
  37 #include <sys/domain.h>
  38 #include <sys/endian.h>
  39 #include <sys/errno.h>
  40 #include <sys/filedesc.h>
  41 #include <sys/kernel.h>
  42 #include <sys/lock.h>
  43 #include <sys/malloc.h>
  44 #include <sys/mbuf.h>
  45 #include <sys/protosw.h>
  46 #include <sys/queue.h>
  47 #include <sys/socket.h>
  48 #include <sys/socketvar.h>
  49 #include <sys/sysctl.h>
  50 #include <sys/taskqueue.h>
  51 #include <sys/msgport2.h>
  52 #include <sys/refcount.h>
  53 #include <netgraph7/ng_message.h>
  54 #include <netgraph7/netgraph.h>
  55 #include <netgraph7/netgraph2.h>
  56 #include <netgraph7/bluetooth/include/ng_bluetooth.h>
  57 #include <netgraph7/bluetooth/include/ng_hci.h>
  58 #include <netgraph7/bluetooth/include/ng_l2cap.h>
  59 #include <netgraph7/bluetooth/include/ng_btsocket.h>
  60 #include <netgraph7/bluetooth/include/ng_btsocket_l2cap.h>
  61
  62 /* MALLOC define */
  63 #ifdef NG_SEPARATE_MALLOC
  64 MALLOC_DEFINE(M_NETGRAPH_BTSOCKET_L2CAP, "netgraph_btsocks_l2cap",
  65                 "Netgraph Bluetooth L2CAP sockets");
  66 #else
  67 #define M_NETGRAPH_BTSOCKET_L2CAP M_NETGRAPH
  68 #endif /* NG_SEPARATE_MALLOC */
  69
  70 /* Netgraph node methods */
  71 static ng_constructor_t ng_btsocket_l2cap_node_constructor;
  72 static ng_rcvmsg_t      ng_btsocket_l2cap_node_rcvmsg;
  73 static ng_shutdown_t    ng_btsocket_l2cap_node_shutdown;
  74 static ng_newhook_t     ng_btsocket_l2cap_node_newhook;
  75 static ng_connect_t     ng_btsocket_l2cap_node_connect;
  76 static ng_rcvdata_t     ng_btsocket_l2cap_node_rcvdata;
  77 static ng_disconnect_t  ng_btsocket_l2cap_node_disconnect;
  78
  79 static void             ng_btsocket_l2cap_input   (void *, int);
  80 static void             ng_btsocket_l2cap_rtclean (void *, int);
  81
  82 /* Netgraph type descriptor */
  83 static struct ng_type   typestruct = {
  84         .version =      NG_ABI_VERSION,
  85         .name =         NG_BTSOCKET_L2CAP_NODE_TYPE,
  86         .constructor =  ng_btsocket_l2cap_node_constructor,
  87         .rcvmsg =       ng_btsocket_l2cap_node_rcvmsg,
  88         .shutdown =     ng_btsocket_l2cap_node_shutdown,
  89         .newhook =      ng_btsocket_l2cap_node_newhook,
  90         .connect =      ng_btsocket_l2cap_node_connect,
  91         .rcvdata =      ng_btsocket_l2cap_node_rcvdata,
  92         .disconnect =   ng_btsocket_l2cap_node_disconnect,
  93 };
  94
  95 /* Globals */
  96 extern int                                      ifqmaxlen;
  97 static u_int32_t                                ng_btsocket_l2cap_debug_level;
  98 static node_p                                   ng_btsocket_l2cap_node;
  99 static struct ng_bt_itemq                       ng_btsocket_l2cap_queue;
 100 static struct lock                              ng_btsocket_l2cap_queue_lock;
 101 static struct task                              ng_btsocket_l2cap_queue_task;
 102 static LIST_HEAD(, ng_btsocket_l2cap_pcb)       ng_btsocket_l2cap_sockets;
 103 static struct lock                              ng_btsocket_l2cap_sockets_lock;
 104 static LIST_HEAD(, ng_btsocket_l2cap_rtentry)   ng_btsocket_l2cap_rt;
 105 static struct lock                              ng_btsocket_l2cap_rt_lock;
 106 static struct task                              ng_btsocket_l2cap_rt_task;
 107
 108 /* Sysctl tree */
 109 SYSCTL_DECL(_net_bluetooth_l2cap_sockets);
 110 SYSCTL_NODE(_net_bluetooth_l2cap_sockets, OID_AUTO, seq, CTLFLAG_RW,
 111         0, "Bluetooth SEQPACKET L2CAP sockets family");
 112 SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, debug_level,
 113         CTLFLAG_RW,
 114         &ng_btsocket_l2cap_debug_level, NG_BTSOCKET_WARN_LEVEL,
 115         "Bluetooth SEQPACKET L2CAP sockets debug level");
 116 SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_len,
 117         CTLFLAG_RD,
 118         &ng_btsocket_l2cap_queue.len, 0,
 119         "Bluetooth SEQPACKET L2CAP sockets input queue length");
 120 SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_maxlen,
 121         CTLFLAG_RD,
 122         &ng_btsocket_l2cap_queue.maxlen, 0,
 123         "Bluetooth SEQPACKET L2CAP sockets input queue max. length");
 124 SYSCTL_INT(_net_bluetooth_l2cap_sockets_seq, OID_AUTO, queue_drops,
 125         CTLFLAG_RD,
 126         &ng_btsocket_l2cap_queue.drops, 0,
 127         "Bluetooth SEQPACKET L2CAP sockets input queue drops");
 128
 129 /* Debug */
 130 #define NG_BTSOCKET_L2CAP_INFO \
 131         if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_INFO_LEVEL) \
 132                 kprintf
 133
 134 #define NG_BTSOCKET_L2CAP_WARN \
 135         if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_WARN_LEVEL) \
 136                 kprintf
 137
 138 #define NG_BTSOCKET_L2CAP_ERR \
 139         if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_ERR_LEVEL) \
 140                 kprintf
 141
 142 #define NG_BTSOCKET_L2CAP_ALERT \
 143         if (ng_btsocket_l2cap_debug_level >= NG_BTSOCKET_ALERT_LEVEL) \
 144                 kprintf
 145
 146 /*
 147  * Netgraph message processing routines
 148  */
 149
 150 static int ng_btsocket_l2cap_process_l2ca_con_req_rsp
 151         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 152 static int ng_btsocket_l2cap_process_l2ca_con_rsp_rsp
 153         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 154 static int ng_btsocket_l2cap_process_l2ca_con_ind
 155         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 156
 157 static int ng_btsocket_l2cap_process_l2ca_cfg_req_rsp
 158         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 159 static int ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp
 160         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 161 static int ng_btsocket_l2cap_process_l2ca_cfg_ind
 162         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 163
 164 static int ng_btsocket_l2cap_process_l2ca_discon_rsp
 165         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 166 static int ng_btsocket_l2cap_process_l2ca_discon_ind
 167         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 168
 169 static int ng_btsocket_l2cap_process_l2ca_write_rsp
 170         (struct ng_mesg *, ng_btsocket_l2cap_rtentry_p);
 171
 172 /*
 173  * Send L2CA_xxx messages to the lower layer
 174  */
 175
 176 static int  ng_btsocket_l2cap_send_l2ca_con_req
 177         (ng_btsocket_l2cap_pcb_p);
 178 static int  ng_btsocket_l2cap_send_l2ca_con_rsp_req
 179         (u_int32_t, ng_btsocket_l2cap_rtentry_p, bdaddr_p, int, int, int);
 180 static int  ng_btsocket_l2cap_send_l2ca_cfg_req
 181         (ng_btsocket_l2cap_pcb_p);
 182 static int  ng_btsocket_l2cap_send_l2ca_cfg_rsp
 183         (ng_btsocket_l2cap_pcb_p);
 184 static int  ng_btsocket_l2cap_send_l2ca_discon_req
 185         (u_int32_t, ng_btsocket_l2cap_pcb_p);
 186
 187 static int ng_btsocket_l2cap_send2
 188         (ng_btsocket_l2cap_pcb_p);
 189
 190 /*
 191  * Timeout processing routines
 192  */
 193
 194 static void ng_btsocket_l2cap_timeout         (ng_btsocket_l2cap_pcb_p);
 195 static void ng_btsocket_l2cap_untimeout       (ng_btsocket_l2cap_pcb_p);
 196 static void ng_btsocket_l2cap_process_timeout (void *);
 197
 198 /*
 199  * Other stuff
 200  */
 201
 202 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_addr(bdaddr_p, int);
 203 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_token(u_int32_t);
 204 static ng_btsocket_l2cap_pcb_p     ng_btsocket_l2cap_pcb_by_cid (bdaddr_p, int);
 205 static int                         ng_btsocket_l2cap_result2errno(int);
 206
 207 #define ng_btsocket_l2cap_wakeup_input_task() \
 208         taskqueue_enqueue(taskqueue_swi, &ng_btsocket_l2cap_queue_task)
 209
 210 #define ng_btsocket_l2cap_wakeup_route_task() \
 211         taskqueue_enqueue(taskqueue_swi, &ng_btsocket_l2cap_rt_task)
 212
 213 /*****************************************************************************
 214  *****************************************************************************
 215  **                        Netgraph node interface
 216  *****************************************************************************
 217  *****************************************************************************/
 218
 219 /*
 220  * Netgraph node constructor. Do not allow to create node of this type.
 221  */
 222
 223 static int
 224 ng_btsocket_l2cap_node_constructor(node_p node)
 225 {
 226         return (EINVAL);
 227 } /* ng_btsocket_l2cap_node_constructor */
 228
 229 /*
 230  * Do local shutdown processing. Let old node go and create new fresh one.
 231  */
 232
 233 static int
 234 ng_btsocket_l2cap_node_shutdown(node_p node)
 235 {
 236         int     error = 0;
 237
 238         NG_NODE_UNREF(node);
 239
 240         /* Create new node */
 241         error = ng_make_node_common(&typestruct, &ng_btsocket_l2cap_node);
 242         if (error != 0) {
 243                 NG_BTSOCKET_L2CAP_ALERT(
 244 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
 245
 246                 ng_btsocket_l2cap_node = NULL;
 247
 248                 return (error);
 249         }
 250
 251         error = ng_name_node(ng_btsocket_l2cap_node,
 252                                 NG_BTSOCKET_L2CAP_NODE_TYPE);
 253         if (error != 0) {
 254                 NG_BTSOCKET_L2CAP_ALERT(
 255 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
 256
 257                 NG_NODE_UNREF(ng_btsocket_l2cap_node);
 258                 ng_btsocket_l2cap_node = NULL;
 259
 260                 return (error);
 261         }
 262
 263         return (0);
 264 } /* ng_btsocket_l2cap_node_shutdown */
 265
 266 /*
 267  * We allow any hook to be connected to the node.
 268  */
 269
 270 static int
 271 ng_btsocket_l2cap_node_newhook(node_p node, hook_p hook, char const *name)
 272 {
 273         return (0);
 274 } /* ng_btsocket_l2cap_node_newhook */
 275
 276 /*
 277  * Just say "YEP, that's OK by me!"
 278  */
 279
 280 static int
 281 ng_btsocket_l2cap_node_connect(hook_p hook)
 282 {
 283         NG_HOOK_SET_PRIVATE(hook, NULL);
 284         NG_HOOK_REF(hook); /* Keep extra reference to the hook */
 285
 286 #if 0
 287         NG_HOOK_FORCE_QUEUE(NG_HOOK_PEER(hook));
 288         NG_HOOK_FORCE_QUEUE(hook);
 289 #endif
 290
 291         return (0);
 292 } /* ng_btsocket_l2cap_node_connect */
 293
 294 /*
 295  * Hook disconnection. Schedule route cleanup task
 296  */
 297
 298 static int
 299 ng_btsocket_l2cap_node_disconnect(hook_p hook)
 300 {
 301         /*
 302          * If hook has private information than we must have this hook in
 303          * the routing table and must schedule cleaning for the routing table.
 304          * Otherwise hook was connected but we never got "hook_info" message,
 305          * so we have never added this hook to the routing table and it save
 306          * to just delete it.
 307          */
 308
 309         if (NG_HOOK_PRIVATE(hook) != NULL)
 310                 return (ng_btsocket_l2cap_wakeup_route_task());
 311
 312         NG_HOOK_UNREF(hook); /* Remove extra reference */
 313
 314         return (0);
 315 } /* ng_btsocket_l2cap_node_disconnect */
 316
 317 /*
 318  * Process incoming messages
 319  */
 320
 321 static int
 322 ng_btsocket_l2cap_node_rcvmsg(node_p node, item_p item, hook_p hook)
 323 {
 324         struct ng_mesg  *msg = NGI_MSG(item); /* item still has message */
 325         int              error = 0;
 326
 327         if (msg != NULL && msg->header.typecookie == NGM_L2CAP_COOKIE) {
 328                 lockmgr(&ng_btsocket_l2cap_queue_lock, LK_EXCLUSIVE);
 329                 if (NG_BT_ITEMQ_FULL(&ng_btsocket_l2cap_queue)) {
 330                         NG_BTSOCKET_L2CAP_ERR(
 331 "%s: Input queue is full (msg)\n", __func__);
 332
 333                         NG_BT_ITEMQ_DROP(&ng_btsocket_l2cap_queue);
 334                         NG_FREE_ITEM(item);
 335                         error = ENOBUFS;
 336                 } else {
 337                         if (hook != NULL) {
 338                                 NG_HOOK_REF(hook);
 339                                 NGI_SET_HOOK(item, hook);
 340                         }
 341
 342                         ng_ref_item(item);
 343                         NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
 344                         error = ng_btsocket_l2cap_wakeup_input_task();
 345                 }
 346                 lockmgr(&ng_btsocket_l2cap_queue_lock, LK_RELEASE);
 347         } else {
 348                 NG_FREE_ITEM(item);
 349                 error = EINVAL;
 350         }
 351
 352         return (error);
 353 } /* ng_btsocket_l2cap_node_rcvmsg */
 354
 355 /*
 356  * Receive data on a hook
 357  */
 358
 359 static int
 360 ng_btsocket_l2cap_node_rcvdata(hook_p hook, item_p item)
 361 {
 362         int     error = 0;
 363
 364         lockmgr(&ng_btsocket_l2cap_queue_lock, LK_EXCLUSIVE);
 365         if (NG_BT_ITEMQ_FULL(&ng_btsocket_l2cap_queue)) {
 366                 NG_BTSOCKET_L2CAP_ERR(
 367 "%s: Input queue is full (data)\n", __func__);
 368
 369                 NG_BT_ITEMQ_DROP(&ng_btsocket_l2cap_queue);
 370                 NG_FREE_ITEM(item);
 371                 error = ENOBUFS;
 372         } else {
 373                 NG_HOOK_REF(hook);
 374                 NGI_SET_HOOK(item, hook);
 375
 376                 ng_ref_item(item);
 377                 NG_BT_ITEMQ_ENQUEUE(&ng_btsocket_l2cap_queue, item);
 378                 error = ng_btsocket_l2cap_wakeup_input_task();
 379         }
 380         lockmgr(&ng_btsocket_l2cap_queue_lock, LK_RELEASE);
 381
 382         return (error);
 383 } /* ng_btsocket_l2cap_node_rcvdata */
 384
 385 /*
 386  * Process L2CA_Connect respose. Socket layer must have initiated connection,
 387  * so we have to have a socket associated with message token.
 388  */
 389
 390 static int
 391 ng_btsocket_l2cap_process_l2ca_con_req_rsp(struct ng_mesg *msg,
 392                 ng_btsocket_l2cap_rtentry_p rt)
 393 {
 394         ng_l2cap_l2ca_con_op    *op = NULL;
 395         ng_btsocket_l2cap_pcb_t *pcb = NULL;
 396         int                      error = 0;
 397
 398         if (msg->header.arglen != sizeof(*op))
 399                 return (EMSGSIZE);
 400
 401         op = (ng_l2cap_l2ca_con_op *)(msg->data);
 402
 403         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
 404
 405         /* Look for the socket with the token */
 406         pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
 407         if (pcb == NULL) {
 408                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 409                 return (ENOENT);
 410         }
 411
 412         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
 413
 414         NG_BTSOCKET_L2CAP_INFO(
 415 "%s: Got L2CA_Connect response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
 416 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, status=%d, " \
 417 "state=%d\n",   __func__, msg->header.token,
 418                 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
 419                 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
 420                 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
 421                 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
 422                 pcb->psm, op->lcid, op->result, op->status,
 423                 pcb->state);
 424
 425         if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
 426                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
 427                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 428
 429                 return (ENOENT);
 430         }
 431
 432         ng_btsocket_l2cap_untimeout(pcb);
 433
 434         if (op->result == NG_L2CAP_PENDING) {
 435                 ng_btsocket_l2cap_timeout(pcb);
 436                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
 437                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 438
 439                 return (0);
 440         }
 441
 442         if (op->result == NG_L2CAP_SUCCESS) {
 443                 /*
 444                  * Channel is now open, so update local channel ID and
 445                  * start configuration process. Source and destination
 446                  * addresses as well as route must be already set.
 447                  */
 448
 449                 pcb->cid = op->lcid;
 450
 451                 error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb);
 452                 if (error != 0) {
 453                         /* Send disconnect request with "zero" token */
 454                         ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
 455
 456                         /* ... and close the socket */
 457                         pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 458                         soisdisconnected(pcb->so);
 459                 } else {
 460                         pcb->cfg_state = NG_BTSOCKET_L2CAP_CFG_IN_SENT;
 461                         pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING;
 462
 463                         ng_btsocket_l2cap_timeout(pcb);
 464                 }
 465         } else {
 466                 /*
 467                  * We have failed to open connection, so convert result
 468                  * code to "errno" code and disconnect the socket. Channel
 469                  * already has been closed.
 470                  */
 471
 472                 pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
 473                 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 474                 soisdisconnected(pcb->so);
 475         }
 476
 477         lockmgr(&pcb->pcb_lock, LK_RELEASE);
 478         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 479
 480         return (error);
 481 } /* ng_btsocket_l2cap_process_l2ca_con_req_rsp */
 482
 483 /*
 484  * Process L2CA_ConnectRsp response
 485  */
 486
 487 static int
 488 ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(struct ng_mesg *msg,
 489                 ng_btsocket_l2cap_rtentry_p rt)
 490 {
 491         ng_l2cap_l2ca_con_rsp_op        *op = NULL;
 492         ng_btsocket_l2cap_pcb_t         *pcb = NULL;
 493
 494         if (msg->header.arglen != sizeof(*op))
 495                 return (EMSGSIZE);
 496
 497         op = (ng_l2cap_l2ca_con_rsp_op *)(msg->data);
 498
 499         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
 500
 501         /* Look for the socket with the token */
 502         pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
 503         if (pcb == NULL) {
 504                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 505                 return (ENOENT);
 506         }
 507
 508         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
 509
 510         NG_BTSOCKET_L2CAP_INFO(
 511 "%s: Got L2CA_ConnectRsp response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
 512 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d\n",
 513                 __func__, msg->header.token,
 514                 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
 515                 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
 516                 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
 517                 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
 518                 pcb->psm, pcb->cid, op->result, pcb->state);
 519
 520         if (pcb->state != NG_BTSOCKET_L2CAP_CONNECTING) {
 521                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
 522                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 523
 524                 return (ENOENT);
 525         }
 526
 527         ng_btsocket_l2cap_untimeout(pcb);
 528
 529         /* Check the result and disconnect the socket on failure */
 530         if (op->result != NG_L2CAP_SUCCESS) {
 531                 /* Close the socket - channel already closed */
 532                 pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
 533                 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 534                 soisdisconnected(pcb->so);
 535         } else {
 536                 /* Move to CONFIGURING state and wait for CONFIG_IND */
 537                 pcb->cfg_state = 0;
 538                 pcb->state = NG_BTSOCKET_L2CAP_CONFIGURING;
 539                 ng_btsocket_l2cap_timeout(pcb);
 540         }
 541
 542         lockmgr(&pcb->pcb_lock, LK_RELEASE);
 543         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 544
 545         return (0);
 546 } /* ng_btsocket_process_l2ca_con_rsp_rsp */
 547
 548 /*
 549  * Process L2CA_Connect indicator. Find socket that listens on address
 550  * and PSM. Find exact or closest match. Create new socket and initiate
 551  * connection.
 552  */
 553
 554 static int
 555 ng_btsocket_l2cap_process_l2ca_con_ind(struct ng_mesg *msg,
 556                 ng_btsocket_l2cap_rtentry_p rt)
 557 {
 558         ng_l2cap_l2ca_con_ind_ip        *ip = NULL;
 559         ng_btsocket_l2cap_pcb_t         *pcb = NULL, *pcb1 = NULL;
 560         int                              error = 0;
 561         u_int32_t                        token = 0;
 562         u_int16_t                        result = 0;
 563
 564         if (msg->header.arglen != sizeof(*ip))
 565                 return (EMSGSIZE);
 566
 567         ip = (ng_l2cap_l2ca_con_ind_ip *)(msg->data);
 568
 569         NG_BTSOCKET_L2CAP_INFO(
 570 "%s: Got L2CA_Connect indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
 571 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, ident=%d\n",
 572                 __func__,
 573                 rt->src.b[5], rt->src.b[4], rt->src.b[3],
 574                 rt->src.b[2], rt->src.b[1], rt->src.b[0],
 575                 ip->bdaddr.b[5], ip->bdaddr.b[4], ip->bdaddr.b[3],
 576                 ip->bdaddr.b[2], ip->bdaddr.b[1], ip->bdaddr.b[0],
 577                 ip->psm, ip->lcid, ip->ident);
 578
 579         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
 580
 581         pcb = ng_btsocket_l2cap_pcb_by_addr(&rt->src, ip->psm);
 582         if (pcb != NULL) {
 583                 struct socket   *so1 = NULL;
 584
 585                 lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
 586
 587                 /*
 588                  * First check the pending connections queue and if we have
 589                  * space then create new socket and set proper source address.
 590                  */
 591
 592                 if (pcb->so->so_qlen <= pcb->so->so_qlimit)
 593                         so1 = sonewconn(pcb->so, 0);
 594
 595                 if (so1 == NULL) {
 596                         result = NG_L2CAP_NO_RESOURCES;
 597                         goto respond;
 598                 }
 599
 600                 /*
 601                  * If we got here than we have created new socket. So complete
 602                  * connection. If we we listening on specific address then copy
 603                  * source address from listening socket, otherwise copy source
 604                  * address from hook's routing information.
 605                  */
 606
 607                 pcb1 = so2l2cap_pcb(so1);
 608                 KASSERT((pcb1 != NULL),
 609 ("%s: pcb1 == NULL\n", __func__));
 610
 611                 lockmgr(&pcb1->pcb_lock, LK_EXCLUSIVE);
 612
 613                 if (bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)) != 0)
 614                         bcopy(&pcb->src, &pcb1->src, sizeof(pcb1->src));
 615                 else
 616                         bcopy(&rt->src, &pcb1->src, sizeof(pcb1->src));
 617
 618                 pcb1->flags &= ~NG_BTSOCKET_L2CAP_CLIENT;
 619
 620                 bcopy(&ip->bdaddr, &pcb1->dst, sizeof(pcb1->dst));
 621                 pcb1->psm = ip->psm;
 622                 pcb1->cid = ip->lcid;
 623                 pcb1->rt = rt;
 624
 625                 /* Copy socket settings */
 626                 pcb1->imtu = pcb->imtu;
 627                 bcopy(&pcb->oflow, &pcb1->oflow, sizeof(pcb1->oflow));
 628                 pcb1->flush_timo = pcb->flush_timo;
 629
 630                 token = pcb1->token;
 631         } else
 632                 /* Nobody listens on requested BDADDR/PSM */
 633                 result = NG_L2CAP_PSM_NOT_SUPPORTED;
 634
 635 respond:
 636         error = ng_btsocket_l2cap_send_l2ca_con_rsp_req(token, rt,
 637                         &ip->bdaddr, ip->ident, ip->lcid, result);
 638         if (pcb1 != NULL) {
 639                 if (error != 0) {
 640                         pcb1->so->so_error = error;
 641                         pcb1->state = NG_BTSOCKET_L2CAP_CLOSED;
 642                         soisdisconnected(pcb1->so);
 643                 } else {
 644                         pcb1->state = NG_BTSOCKET_L2CAP_CONNECTING;
 645                         soisconnecting(pcb1->so);
 646
 647                         ng_btsocket_l2cap_timeout(pcb1);
 648                 }
 649
 650                 lockmgr(&pcb1->pcb_lock, LK_RELEASE);
 651         }
 652
 653         if (pcb != NULL)
 654                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
 655
 656         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 657
 658         return (error);
 659 } /* ng_btsocket_l2cap_process_l2ca_con_ind */
 660
 661 /*
 662  * Process L2CA_Config response
 663  */
 664
 665 static int
 666 ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(struct ng_mesg *msg,
 667                 ng_btsocket_l2cap_rtentry_p rt)
 668 {
 669         ng_l2cap_l2ca_cfg_op    *op = NULL;
 670         ng_btsocket_l2cap_pcb_p  pcb = NULL;
 671
 672         if (msg->header.arglen != sizeof(*op))
 673                 return (EMSGSIZE);
 674
 675         op = (ng_l2cap_l2ca_cfg_op *)(msg->data);
 676
 677         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
 678
 679         /*
 680          * Socket must have issued a Configure request, so we must have a
 681          * socket that wants to be configured. Use Netgraph message token
 682          * to find it
 683          */
 684
 685         pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
 686         if (pcb == NULL) {
 687                 /*
 688                  * XXX FIXME what to do here? We could not find a
 689                  * socket with requested token. We even can not send
 690                  * Disconnect, because we do not know channel ID
 691                  */
 692
 693                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 694                 return (ENOENT);
 695         }
 696
 697         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
 698
 699         NG_BTSOCKET_L2CAP_INFO(
 700 "%s: Got L2CA_Config response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
 701 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d, " \
 702 "cfg_state=%x\n",
 703                 __func__, msg->header.token,
 704                 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
 705                 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
 706                 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
 707                 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
 708                 pcb->psm, pcb->cid, op->result, pcb->state, pcb->cfg_state);
 709
 710         if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
 711                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
 712                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 713
 714                 return (ENOENT);
 715         }
 716
 717         if (op->result == NG_L2CAP_SUCCESS) {
 718                 /*
 719                  * XXX FIXME Actually set flush and link timeout.
 720                  * Set QoS here if required. Resolve conficts (flush_timo).
 721                  * Save incoming MTU (peer's outgoing MTU) and outgoing flow
 722                  * spec.
 723                  */
 724
 725                 pcb->imtu = op->imtu;
 726                 bcopy(&op->oflow, &pcb->oflow, sizeof(pcb->oflow));
 727                 pcb->flush_timo = op->flush_timo;
 728
 729                 /*
 730                  * We have configured incoming side, so record it and check
 731                  * if configuration is complete. If complete then mark socket
 732                  * as connected, otherwise wait for the peer.
 733                  */
 734
 735                 pcb->cfg_state &= ~NG_BTSOCKET_L2CAP_CFG_IN_SENT;
 736                 pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_IN;
 737
 738                 if (pcb->cfg_state == NG_BTSOCKET_L2CAP_CFG_BOTH) {
 739                         /* Configuration complete - mark socket as open */
 740                         ng_btsocket_l2cap_untimeout(pcb);
 741                         pcb->state = NG_BTSOCKET_L2CAP_OPEN;
 742                         soisconnected(pcb->so);
 743                 }
 744         } else {
 745                 /*
 746                  * Something went wrong. Could be unacceptable parameters,
 747                  * reject or unknown option. That's too bad, but we will
 748                  * not negotiate. Send Disconnect and close the channel.
 749                  */
 750
 751                 ng_btsocket_l2cap_untimeout(pcb);
 752
 753                 switch (op->result) {
 754                 case NG_L2CAP_UNACCEPTABLE_PARAMS:
 755                 case NG_L2CAP_UNKNOWN_OPTION:
 756                         pcb->so->so_error = EINVAL;
 757                         break;
 758
 759                 default:
 760                         pcb->so->so_error = ECONNRESET;
 761                         break;
 762                 }
 763
 764                 /* Send disconnect with "zero" token */
 765                 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
 766
 767                 /* ... and close the socket */
 768                 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 769                 soisdisconnected(pcb->so);
 770         }
 771
 772         lockmgr(&pcb->pcb_lock, LK_RELEASE);
 773         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 774
 775         return (0);
 776 } /* ng_btsocket_l2cap_process_l2ca_cfg_req_rsp */
 777
 778 /*
 779  * Process L2CA_ConfigRsp response
 780  */
 781
 782 static int
 783 ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(struct ng_mesg *msg,
 784                 ng_btsocket_l2cap_rtentry_p rt)
 785 {
 786         ng_l2cap_l2ca_cfg_rsp_op        *op = NULL;
 787         ng_btsocket_l2cap_pcb_t         *pcb = NULL;
 788         int                              error = 0;
 789
 790         if (msg->header.arglen != sizeof(*op))
 791                 return (EMSGSIZE);
 792
 793         op = (ng_l2cap_l2ca_cfg_rsp_op *)(msg->data);
 794
 795         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
 796
 797         /* Look for the socket with the token */
 798         pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
 799         if (pcb == NULL) {
 800                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 801                 return (ENOENT);
 802         }
 803
 804         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
 805
 806         NG_BTSOCKET_L2CAP_INFO(
 807 "%s: Got L2CA_ConfigRsp response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
 808 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d, " \
 809 "cfg_state=%x\n",
 810                 __func__, msg->header.token,
 811                 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
 812                 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
 813                 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
 814                 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
 815                 pcb->psm, pcb->cid, op->result, pcb->state, pcb->cfg_state);
 816
 817         if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
 818                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
 819                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 820
 821                 return (ENOENT);
 822         }
 823
 824         /* Check the result and disconnect socket of failure */
 825         if (op->result != NG_L2CAP_SUCCESS)
 826                 goto disconnect;
 827
 828         /*
 829          * Now we done with remote side configuration. Configure local
 830          * side if we have not done it yet.
 831          */
 832
 833         pcb->cfg_state &= ~NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
 834         pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT;
 835
 836         if (pcb->cfg_state == NG_BTSOCKET_L2CAP_CFG_BOTH) {
 837                 /* Configuration complete - mask socket as open */
 838                 ng_btsocket_l2cap_untimeout(pcb);
 839                 pcb->state = NG_BTSOCKET_L2CAP_OPEN;
 840                 soisconnected(pcb->so);
 841         } else {
 842                 if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_IN_SENT)) {
 843                         /* Send L2CA_Config request - incoming path */
 844                         error = ng_btsocket_l2cap_send_l2ca_cfg_req(pcb);
 845                         if (error != 0)
 846                                 goto disconnect;
 847
 848                         pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_IN_SENT;
 849                 }
 850         }
 851
 852         lockmgr(&pcb->pcb_lock, LK_RELEASE);
 853         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 854
 855         return (error);
 856
 857 disconnect:
 858         ng_btsocket_l2cap_untimeout(pcb);
 859
 860         /* Send disconnect with "zero" token */
 861         ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
 862
 863         /* ... and close the socket */
 864         pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 865         soisdisconnected(pcb->so);
 866
 867         lockmgr(&pcb->pcb_lock, LK_RELEASE);
 868         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 869
 870         return (error);
 871 } /* ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp */
 872
 873 /*
 874  * Process L2CA_Config indicator
 875  */
 876
 877 static int
 878 ng_btsocket_l2cap_process_l2ca_cfg_ind(struct ng_mesg *msg,
 879                 ng_btsocket_l2cap_rtentry_p rt)
 880 {
 881         ng_l2cap_l2ca_cfg_ind_ip        *ip = NULL;
 882         ng_btsocket_l2cap_pcb_t         *pcb = NULL;
 883         int                              error = 0;
 884
 885         if (msg->header.arglen != sizeof(*ip))
 886                 return (EMSGSIZE);
 887
 888         ip = (ng_l2cap_l2ca_cfg_ind_ip *)(msg->data);
 889
 890         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
 891
 892         /* Check for the open socket that has given channel ID */
 893         pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid);
 894         if (pcb == NULL) {
 895                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 896                 return (ENOENT);
 897         }
 898
 899         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
 900
 901         NG_BTSOCKET_L2CAP_INFO(
 902 "%s: Got L2CA_Config indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
 903 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, state=%d, cfg_state=%x\n",
 904                 __func__,
 905                 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
 906                 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
 907                 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
 908                 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
 909                 pcb->psm, pcb->cid, pcb->state, pcb->cfg_state);
 910
 911         /* XXX FIXME re-configuration on open socket */
 912         if (pcb->state != NG_BTSOCKET_L2CAP_CONFIGURING) {
 913                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
 914                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 915
 916                 return (ENOENT);
 917         }
 918
 919         /*
 920          * XXX FIXME Actually set flush and link timeout. Set QoS here if
 921          * required. Resolve conficts (flush_timo). Note outgoing MTU (peer's
 922          * incoming MTU) and incoming flow spec.
 923          */
 924
 925         pcb->omtu = ip->omtu;
 926         bcopy(&ip->iflow, &pcb->iflow, sizeof(pcb->iflow));
 927         pcb->flush_timo = ip->flush_timo;
 928
 929         /*
 930          * Send L2CA_Config response to our peer and check for the errors,
 931          * if any send disconnect to close the channel.
 932          */
 933
 934         if (!(pcb->cfg_state & NG_BTSOCKET_L2CAP_CFG_OUT_SENT)) {
 935                 error = ng_btsocket_l2cap_send_l2ca_cfg_rsp(pcb);
 936                 if (error != 0) {
 937                         ng_btsocket_l2cap_untimeout(pcb);
 938
 939                         pcb->so->so_error = error;
 940
 941                         /* Send disconnect with "zero" token */
 942                         ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
 943
 944                         /* ... and close the socket */
 945                         pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
 946                         soisdisconnected(pcb->so);
 947                 } else
 948                         pcb->cfg_state |= NG_BTSOCKET_L2CAP_CFG_OUT_SENT;
 949         }
 950
 951         lockmgr(&pcb->pcb_lock, LK_RELEASE);
 952         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 953
 954         return (error);
 955 } /* ng_btsocket_l2cap_process_l2cap_cfg_ind */
 956
 957 /*
 958  * Process L2CA_Disconnect response
 959  */
 960
 961 static int
 962 ng_btsocket_l2cap_process_l2ca_discon_rsp(struct ng_mesg *msg,
 963                 ng_btsocket_l2cap_rtentry_p rt)
 964 {
 965         ng_l2cap_l2ca_discon_op *op = NULL;
 966         ng_btsocket_l2cap_pcb_t *pcb = NULL;
 967
 968         /* Check message */
 969         if (msg->header.arglen != sizeof(*op))
 970                 return (EMSGSIZE);
 971
 972         op = (ng_l2cap_l2ca_discon_op *)(msg->data);
 973
 974         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
 975
 976         /*
 977          * Socket layer must have issued L2CA_Disconnect request, so there
 978          * must be a socket that wants to be disconnected. Use Netgraph
 979          * message token to find it.
 980          */
 981
 982         pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
 983         if (pcb == NULL) {
 984                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
 985                 return (0);
 986         }
 987
 988         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
 989
 990         /* XXX Close socket no matter what op->result says */
 991         if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
 992                 NG_BTSOCKET_L2CAP_INFO(
 993 "%s: Got L2CA_Disconnect response, token=%d, src bdaddr=%x:%x:%x:%x:%x:%x, " \
 994 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, state=%d\n",
 995                         __func__, msg->header.token,
 996                         pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
 997                         pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
 998                         pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
 999                         pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1000                         pcb->psm, pcb->cid, op->result, pcb->state);
1001
1002                 ng_btsocket_l2cap_untimeout(pcb);
1003
1004                 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1005                 soisdisconnected(pcb->so);
1006         }
1007
1008         lockmgr(&pcb->pcb_lock, LK_RELEASE);
1009         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1010
1011         return (0);
1012 } /* ng_btsocket_l2cap_process_l2ca_discon_rsp */
1013
1014 /*
1015  * Process L2CA_Disconnect indicator
1016  */
1017
1018 static int
1019 ng_btsocket_l2cap_process_l2ca_discon_ind(struct ng_mesg *msg,
1020                 ng_btsocket_l2cap_rtentry_p rt)
1021 {
1022         ng_l2cap_l2ca_discon_ind_ip     *ip = NULL;
1023         ng_btsocket_l2cap_pcb_t         *pcb = NULL;
1024
1025         /* Check message */
1026         if (msg->header.arglen != sizeof(*ip))
1027                 return (EMSGSIZE);
1028
1029         ip = (ng_l2cap_l2ca_discon_ind_ip *)(msg->data);
1030
1031         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
1032
1033         /* Look for the socket with given channel ID */
1034         pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, ip->lcid);
1035         if (pcb == NULL) {
1036                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1037                 return (0);
1038         }
1039
1040         /*
1041          * Channel has already been destroyed, so disconnect the socket
1042          * and be done with it. If there was any pending request we can
1043          * not do anything here anyway.
1044          */
1045
1046         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
1047
1048         NG_BTSOCKET_L2CAP_INFO(
1049 "%s: Got L2CA_Disconnect indicator, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1050 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, state=%d\n",
1051                 __func__,
1052                 pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1053                 pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1054                 pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1055                 pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1056                 pcb->psm, pcb->cid, pcb->state);
1057
1058         if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
1059                 ng_btsocket_l2cap_untimeout(pcb);
1060
1061         pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1062         soisdisconnected(pcb->so);
1063
1064         lockmgr(&pcb->pcb_lock, LK_RELEASE);
1065         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1066
1067         return (0);
1068 } /* ng_btsocket_l2cap_process_l2ca_discon_ind */
1069
1070 /*
1071  * Process L2CA_Write response
1072  */
1073
1074 static int
1075 ng_btsocket_l2cap_process_l2ca_write_rsp(struct ng_mesg *msg,
1076                 ng_btsocket_l2cap_rtentry_p rt)
1077 {
1078         ng_l2cap_l2ca_write_op  *op = NULL;
1079         ng_btsocket_l2cap_pcb_t *pcb = NULL;
1080
1081         /* Check message */
1082         if (msg->header.arglen != sizeof(*op))
1083                 return (EMSGSIZE);
1084
1085         op = (ng_l2cap_l2ca_write_op *)(msg->data);
1086
1087         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
1088
1089         /* Look for the socket with given token */
1090         pcb = ng_btsocket_l2cap_pcb_by_token(msg->header.token);
1091         if (pcb == NULL) {
1092                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1093                 return (ENOENT);
1094         }
1095
1096         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
1097
1098         NG_BTSOCKET_L2CAP_INFO(
1099 "%s: Got L2CA_Write response, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1100 "dst bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, lcid=%d, result=%d, length=%d, " \
1101 "state=%d\n",           __func__,
1102                         pcb->src.b[5], pcb->src.b[4], pcb->src.b[3],
1103                         pcb->src.b[2], pcb->src.b[1], pcb->src.b[0],
1104                         pcb->dst.b[5], pcb->dst.b[4], pcb->dst.b[3],
1105                         pcb->dst.b[2], pcb->dst.b[1], pcb->dst.b[0],
1106                         pcb->psm, pcb->cid, op->result, op->length,
1107                         pcb->state);
1108
1109         if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
1110                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
1111                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1112
1113                 return (ENOENT);
1114         }
1115
1116         ng_btsocket_l2cap_untimeout(pcb);
1117
1118         /*
1119          * Check if we have more data to send
1120          */
1121
1122         sbdroprecord(&pcb->so->so_snd.sb);
1123         if (pcb->so->so_snd.sb.sb_cc > 0) {
1124                 if (ng_btsocket_l2cap_send2(pcb) == 0)
1125                         ng_btsocket_l2cap_timeout(pcb);
1126                 else
1127                         sbdroprecord(&pcb->so->so_snd.sb); /* XXX */
1128         }
1129
1130         /*
1131          * Now set the result, drop packet from the socket send queue and
1132          * ask for more (wakeup sender)
1133          */
1134
1135         pcb->so->so_error = ng_btsocket_l2cap_result2errno(op->result);
1136         sowwakeup(pcb->so);
1137
1138         lockmgr(&pcb->pcb_lock, LK_RELEASE);
1139         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1140
1141         return (0);
1142 } /* ng_btsocket_l2cap_process_l2ca_write_rsp */
1143
1144 /*
1145  * Send L2CA_Connect request
1146  */
1147
1148 static int
1149 ng_btsocket_l2cap_send_l2ca_con_req(ng_btsocket_l2cap_pcb_p pcb)
1150 {
1151         struct ng_mesg          *msg = NULL;
1152         ng_l2cap_l2ca_con_ip    *ip = NULL;
1153         int                      error = 0;
1154
1155         KKASSERT(lockowned(&pcb->pcb_lock) != 0);
1156
1157         if (pcb->rt == NULL ||
1158             pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1159                 return (ENETDOWN);
1160
1161         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON,
1162                 sizeof(*ip), M_WAITOK | M_NULLOK);
1163         if (msg == NULL)
1164                 return (ENOMEM);
1165
1166         msg->header.token = pcb->token;
1167
1168         ip = (ng_l2cap_l2ca_con_ip *)(msg->data);
1169         bcopy(&pcb->dst, &ip->bdaddr, sizeof(ip->bdaddr));
1170         ip->psm = pcb->psm;
1171
1172         NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1173
1174         return (error);
1175 } /* ng_btsocket_l2cap_send_l2ca_con_req */
1176
1177 /*
1178  * Send L2CA_Connect response
1179  */
1180
1181 static int
1182 ng_btsocket_l2cap_send_l2ca_con_rsp_req(u_int32_t token,
1183                 ng_btsocket_l2cap_rtentry_p rt, bdaddr_p dst, int ident,
1184                 int lcid, int result)
1185 {
1186         struct ng_mesg                  *msg = NULL;
1187         ng_l2cap_l2ca_con_rsp_ip        *ip = NULL;
1188         int                              error = 0;
1189
1190         if (rt == NULL || rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
1191                 return (ENETDOWN);
1192
1193         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CON_RSP,
1194                 sizeof(*ip), M_WAITOK | M_NULLOK);
1195         if (msg == NULL)
1196                 return (ENOMEM);
1197
1198         msg->header.token = token;
1199
1200         ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data);
1201         bcopy(dst, &ip->bdaddr, sizeof(ip->bdaddr));
1202         ip->ident = ident;
1203         ip->lcid = lcid;
1204         ip->result = result;
1205         ip->status = 0;
1206
1207         NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, rt->hook, 0);
1208
1209         return (error);
1210 } /* ng_btsocket_l2cap_send_l2ca_con_rsp_req */
1211
1212 /*
1213  * Send L2CA_Config request
1214  */
1215
1216 static int
1217 ng_btsocket_l2cap_send_l2ca_cfg_req(ng_btsocket_l2cap_pcb_p pcb)
1218 {
1219         struct ng_mesg          *msg = NULL;
1220         ng_l2cap_l2ca_cfg_ip    *ip = NULL;
1221         int                      error = 0;
1222
1223         KKASSERT(lockowned(&pcb->pcb_lock) != 0);
1224
1225         if (pcb->rt == NULL ||
1226             pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1227                 return (ENETDOWN);
1228
1229         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG,
1230                 sizeof(*ip), M_WAITOK | M_NULLOK);
1231         if (msg == NULL)
1232                 return (ENOMEM);
1233
1234         msg->header.token = pcb->token;
1235
1236         ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data);
1237         ip->lcid = pcb->cid;
1238         ip->imtu = pcb->imtu;
1239         bcopy(&pcb->oflow, &ip->oflow, sizeof(ip->oflow));
1240         ip->flush_timo = pcb->flush_timo;
1241         ip->link_timo = pcb->link_timo;
1242
1243         NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1244
1245         return (error);
1246 } /* ng_btsocket_l2cap_send_l2ca_cfg_req */
1247
1248 /*
1249  * Send L2CA_Config response
1250  */
1251
1252 static int
1253 ng_btsocket_l2cap_send_l2ca_cfg_rsp(ng_btsocket_l2cap_pcb_p pcb)
1254 {
1255         struct ng_mesg                  *msg = NULL;
1256         ng_l2cap_l2ca_cfg_rsp_ip        *ip = NULL;
1257         int                              error = 0;
1258
1259         KKASSERT(lockowned(&pcb->pcb_lock) != 0);
1260
1261         if (pcb->rt == NULL ||
1262             pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1263                 return (ENETDOWN);
1264
1265         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_CFG_RSP,
1266                 sizeof(*ip), M_WAITOK | M_NULLOK);
1267         if (msg == NULL)
1268                 return (ENOMEM);
1269
1270         msg->header.token = pcb->token;
1271
1272         ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data);
1273         ip->lcid = pcb->cid;
1274         ip->omtu = pcb->omtu;
1275         bcopy(&pcb->iflow, &ip->iflow, sizeof(ip->iflow));
1276
1277         NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg, pcb->rt->hook, 0);
1278
1279         return (error);
1280 } /* ng_btsocket_l2cap_send_l2ca_cfg_rsp */
1281
1282 /*
1283  * Send L2CA_Disconnect request
1284  */
1285
1286 static int
1287 ng_btsocket_l2cap_send_l2ca_discon_req(u_int32_t token,
1288                 ng_btsocket_l2cap_pcb_p pcb)
1289 {
1290         struct ng_mesg          *msg = NULL;
1291         ng_l2cap_l2ca_discon_ip *ip = NULL;
1292         int                      error = 0;
1293
1294         KKASSERT(lockowned(&pcb->pcb_lock) != 0);
1295
1296         if (pcb->rt == NULL ||
1297             pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook))
1298                 return (ENETDOWN);
1299
1300         NG_MKMESSAGE(msg, NGM_L2CAP_COOKIE, NGM_L2CAP_L2CA_DISCON,
1301                 sizeof(*ip), M_WAITOK | M_NULLOK);
1302         if (msg == NULL)
1303                 return (ENOMEM);
1304
1305         msg->header.token = token;
1306
1307         ip = (ng_l2cap_l2ca_discon_ip *)(msg->data);
1308         ip->lcid = pcb->cid;
1309
1310         NG_SEND_MSG_HOOK(error, ng_btsocket_l2cap_node, msg,pcb->rt->hook, 0);
1311
1312         return (error);
1313 } /* ng_btsocket_l2cap_send_l2ca_discon_req */
1314
1315 /*****************************************************************************
1316  *****************************************************************************
1317  **                              Socket interface
1318  *****************************************************************************
1319  *****************************************************************************/
1320
1321 /*
1322  * L2CAP sockets data input routine
1323  */
1324
1325 static void
1326 ng_btsocket_l2cap_data_input(struct mbuf *m, hook_p hook)
1327 {
1328         ng_l2cap_hdr_t                  *hdr = NULL;
1329         ng_l2cap_clt_hdr_t              *clt_hdr = NULL;
1330         ng_btsocket_l2cap_pcb_t         *pcb = NULL;
1331         ng_btsocket_l2cap_rtentry_t     *rt = NULL;
1332
1333         if (hook == NULL) {
1334                 NG_BTSOCKET_L2CAP_ALERT(
1335 "%s: Invalid source hook for L2CAP data packet\n", __func__);
1336                 goto drop;
1337         }
1338
1339         rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook);
1340         if (rt == NULL) {
1341                 NG_BTSOCKET_L2CAP_ALERT(
1342 "%s: Could not find out source bdaddr for L2CAP data packet\n", __func__);
1343                 goto drop;
1344         }
1345
1346         /* Make sure we can access header */
1347         if (m->m_pkthdr.len < sizeof(*hdr)) {
1348                 NG_BTSOCKET_L2CAP_ERR(
1349 "%s: L2CAP data packet too small, len=%d\n", __func__, m->m_pkthdr.len);
1350                 goto drop;
1351         }
1352
1353         if (m->m_len < sizeof(*hdr)) {
1354                 m = m_pullup(m, sizeof(*hdr));
1355                 if (m == NULL)
1356                         goto drop;
1357         }
1358
1359         /* Strip L2CAP packet header and verify packet length */
1360         hdr = mtod(m, ng_l2cap_hdr_t *);
1361         m_adj(m, sizeof(*hdr));
1362
1363         if (hdr->length != m->m_pkthdr.len) {
1364                 NG_BTSOCKET_L2CAP_ERR(
1365 "%s: Bad L2CAP data packet length, len=%d, length=%d\n",
1366                         __func__, m->m_pkthdr.len, hdr->length);
1367                 goto drop;
1368         }
1369
1370         /*
1371          * Now process packet. Two cases:
1372          *
1373          * 1) Normal packet (cid != 2) then find connected socket and append
1374          *    mbuf to the socket queue. Wakeup socket.
1375          *
1376          * 2) Broadcast packet (cid == 2) then find all sockets that connected
1377          *    to the given PSM and have SO_BROADCAST bit set and append mbuf
1378          *    to the socket queue. Wakeup socket.
1379          */
1380
1381         NG_BTSOCKET_L2CAP_INFO(
1382 "%s: Received L2CAP data packet: src bdaddr=%x:%x:%x:%x:%x:%x, " \
1383 "dcid=%d, length=%d\n",
1384                 __func__,
1385                 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1386                 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1387                 hdr->dcid, hdr->length);
1388
1389         if (hdr->dcid >= NG_L2CAP_FIRST_CID) {
1390
1391                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
1392
1393                 /* Normal packet: find connected socket */
1394                 pcb = ng_btsocket_l2cap_pcb_by_cid(&rt->src, hdr->dcid);
1395                 if (pcb == NULL) {
1396                         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1397                         goto drop;
1398                 }
1399
1400                 lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
1401
1402                 if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
1403                         NG_BTSOCKET_L2CAP_ERR(
1404 "%s: No connected socket found, src bdaddr=%x:%x:%x:%x:%x:%x, dcid=%d, " \
1405 "state=%d\n",                   __func__,
1406                                 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1407                                 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1408                                 hdr->dcid, pcb->state);
1409
1410                         lockmgr(&pcb->pcb_lock, LK_RELEASE);
1411                         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1412                         goto drop;
1413                 }
1414
1415                 /* Check packet size against socket's incoming MTU */
1416                 if (hdr->length > pcb->imtu) {
1417                         NG_BTSOCKET_L2CAP_ERR(
1418 "%s: L2CAP data packet too big, src bdaddr=%x:%x:%x:%x:%x:%x, " \
1419 "dcid=%d, length=%d, imtu=%d\n",
1420                                 __func__,
1421                                 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1422                                 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1423                                 hdr->dcid, hdr->length, pcb->imtu);
1424
1425                         lockmgr(&pcb->pcb_lock, LK_RELEASE);
1426                         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1427                         goto drop;
1428                 }
1429
1430                 /* Check if we have enough space in socket receive queue */
1431                 if (m->m_pkthdr.len > ssb_space(&pcb->so->so_rcv)) {
1432
1433                         /*
1434                          * This is really bad. Receive queue on socket does
1435                          * not have enough space for the packet. We do not
1436                          * have any other choice but drop the packet. L2CAP
1437                          * does not provide any flow control.
1438                          */
1439
1440                         NG_BTSOCKET_L2CAP_ERR(
1441 "%s: Not enough space in socket receive queue. Dropping L2CAP data packet, " \
1442 "src bdaddr=%x:%x:%x:%x:%x:%x, dcid=%d, len=%d, space=%ld\n",
1443                                 __func__,
1444                                 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1445                                 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1446                                 hdr->dcid, m->m_pkthdr.len,
1447                                 ssb_space(&pcb->so->so_rcv));
1448
1449                         lockmgr(&pcb->pcb_lock, LK_RELEASE);
1450                         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1451                         goto drop;
1452                 }
1453
1454                 /* Append packet to the socket receive queue and wakeup */
1455                 sbappendrecord(&pcb->so->so_rcv.sb, m);
1456                 m = NULL;
1457
1458                 sorwakeup(pcb->so);
1459
1460                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
1461                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1462         } else if (hdr->dcid == NG_L2CAP_CLT_CID) {
1463                 /* Broadcast packet: give packet to all sockets  */
1464
1465                 /* Check packet size against connectionless MTU */
1466                 if (hdr->length > NG_L2CAP_MTU_DEFAULT) {
1467                         NG_BTSOCKET_L2CAP_ERR(
1468 "%s: Connectionless L2CAP data packet too big, " \
1469 "src bdaddr=%x:%x:%x:%x:%x:%x, length=%d\n",
1470                                 __func__,
1471                                 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1472                                 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1473                                 hdr->length);
1474                         goto drop;
1475                 }
1476
1477                 /* Make sure we can access connectionless header */
1478                 if (m->m_pkthdr.len < sizeof(*clt_hdr)) {
1479                         NG_BTSOCKET_L2CAP_ERR(
1480 "%s: Can not get L2CAP connectionless packet header, " \
1481 "src bdaddr=%x:%x:%x:%x:%x:%x, length=%d\n",
1482                                 __func__,
1483                                 rt->src.b[5], rt->src.b[4], rt->src.b[3],
1484                                 rt->src.b[2], rt->src.b[1], rt->src.b[0],
1485                                 hdr->length);
1486                         goto drop;
1487                 }
1488
1489                 if (m->m_len < sizeof(*clt_hdr)) {
1490                         m = m_pullup(m, sizeof(*clt_hdr));
1491                         if (m == NULL)
1492                                 goto drop;
1493                 }
1494
1495                 /* Strip connectionless header and deliver packet */
1496                 clt_hdr = mtod(m, ng_l2cap_clt_hdr_t *);
1497                 m_adj(m, sizeof(*clt_hdr));
1498
1499                 NG_BTSOCKET_L2CAP_INFO(
1500 "%s: Got L2CAP connectionless data packet, " \
1501 "src bdaddr=%x:%x:%x:%x:%x:%x, psm=%d, length=%d\n",
1502                         __func__,
1503                         rt->src.b[5], rt->src.b[4], rt->src.b[3],
1504                         rt->src.b[2], rt->src.b[1], rt->src.b[0],
1505                         clt_hdr->psm, hdr->length);
1506
1507                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
1508
1509                 LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next) {
1510                         struct mbuf     *copy = NULL;
1511
1512                         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
1513
1514                         if (bcmp(&rt->src, &pcb->src, sizeof(pcb->src)) != 0 ||
1515                             pcb->psm != clt_hdr->psm ||
1516                             pcb->state != NG_BTSOCKET_L2CAP_OPEN ||
1517                             (pcb->so->so_options & SO_BROADCAST) == 0 ||
1518                             m->m_pkthdr.len > ssb_space(&pcb->so->so_rcv))
1519                                 goto next;
1520
1521                         /*
1522                          * Create a copy of the packet and append it to the
1523                          * socket's queue. If m_dup() failed - no big deal
1524                          * it is a broadcast traffic after all
1525                          */
1526
1527                         copy = m_dup(m, M_NOWAIT);
1528                         if (copy != NULL) {
1529                                 sbappendrecord(&pcb->so->so_rcv.sb, copy);
1530                                 sorwakeup(pcb->so);
1531                         }
1532 next:
1533                         lockmgr(&pcb->pcb_lock, LK_RELEASE);
1534                 }
1535
1536                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1537         }
1538 drop:
1539         NG_FREE_M(m); /* checks for m != NULL */
1540 } /* ng_btsocket_l2cap_data_input */
1541
1542 /*
1543  * L2CAP sockets default message input routine
1544  */
1545
1546 static void
1547 ng_btsocket_l2cap_default_msg_input(struct ng_mesg *msg, hook_p hook)
1548 {
1549         switch (msg->header.cmd) {
1550         case NGM_L2CAP_NODE_HOOK_INFO: {
1551                 ng_btsocket_l2cap_rtentry_t     *rt = NULL;
1552
1553                 if (hook == NULL || msg->header.arglen != sizeof(bdaddr_t))
1554                         break;
1555
1556                 if (bcmp(msg->data, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0)
1557                         break;
1558
1559                 lockmgr(&ng_btsocket_l2cap_rt_lock, LK_EXCLUSIVE);
1560
1561                 rt = (ng_btsocket_l2cap_rtentry_t *) NG_HOOK_PRIVATE(hook);
1562                 if (rt == NULL) {
1563                         rt = kmalloc(sizeof(*rt), M_NETGRAPH_BTSOCKET_L2CAP,
1564                                      M_WAITOK | M_NULLOK | M_ZERO);
1565                         if (rt == NULL) {
1566                                 lockmgr(&ng_btsocket_l2cap_rt_lock, LK_RELEASE);
1567                                 break;
1568                         }
1569
1570                         LIST_INSERT_HEAD(&ng_btsocket_l2cap_rt, rt, next);
1571
1572                         NG_HOOK_SET_PRIVATE(hook, rt);
1573                 }
1574
1575                 bcopy(msg->data, &rt->src, sizeof(rt->src));
1576                 rt->hook = hook;
1577
1578                 lockmgr(&ng_btsocket_l2cap_rt_lock, LK_RELEASE);
1579
1580                 NG_BTSOCKET_L2CAP_INFO(
1581 "%s: Updating hook \"%s\", src bdaddr=%x:%x:%x:%x:%x:%x\n",
1582                         __func__, NG_HOOK_NAME(hook),
1583                         rt->src.b[5], rt->src.b[4], rt->src.b[3],
1584                         rt->src.b[2], rt->src.b[1], rt->src.b[0]);
1585                 } break;
1586
1587         default:
1588                 NG_BTSOCKET_L2CAP_WARN(
1589 "%s: Unknown message, cmd=%d\n", __func__, msg->header.cmd);
1590                 break;
1591         }
1592
1593         NG_FREE_MSG(msg); /* Checks for msg != NULL */
1594 } /* ng_btsocket_l2cap_default_msg_input */
1595
1596 /*
1597  * L2CAP sockets L2CA message input routine
1598  */
1599
1600 static void
1601 ng_btsocket_l2cap_l2ca_msg_input(struct ng_mesg *msg, hook_p hook)
1602 {
1603         ng_btsocket_l2cap_rtentry_p     rt = NULL;
1604
1605         if (hook == NULL) {
1606                 NG_BTSOCKET_L2CAP_ALERT(
1607 "%s: Invalid source hook for L2CA message\n", __func__);
1608                 goto drop;
1609         }
1610
1611         rt = (ng_btsocket_l2cap_rtentry_p) NG_HOOK_PRIVATE(hook);
1612         if (rt == NULL) {
1613                 NG_BTSOCKET_L2CAP_ALERT(
1614 "%s: Could not find out source bdaddr for L2CA message\n", __func__);
1615                 goto drop;
1616         }
1617
1618         switch (msg->header.cmd) {
1619         case NGM_L2CAP_L2CA_CON: /* L2CA_Connect response */
1620                 ng_btsocket_l2cap_process_l2ca_con_req_rsp(msg, rt);
1621                 break;
1622
1623         case NGM_L2CAP_L2CA_CON_RSP: /* L2CA_ConnectRsp response */
1624                 ng_btsocket_l2cap_process_l2ca_con_rsp_rsp(msg, rt);
1625                 break;
1626
1627         case NGM_L2CAP_L2CA_CON_IND: /* L2CA_Connect indicator */
1628                 ng_btsocket_l2cap_process_l2ca_con_ind(msg, rt);
1629                 break;
1630
1631         case NGM_L2CAP_L2CA_CFG: /* L2CA_Config response */
1632                 ng_btsocket_l2cap_process_l2ca_cfg_req_rsp(msg, rt);
1633                 break;
1634
1635         case NGM_L2CAP_L2CA_CFG_RSP: /* L2CA_ConfigRsp response */
1636                 ng_btsocket_l2cap_process_l2ca_cfg_rsp_rsp(msg, rt);
1637                 break;
1638
1639         case NGM_L2CAP_L2CA_CFG_IND: /* L2CA_Config indicator */
1640                 ng_btsocket_l2cap_process_l2ca_cfg_ind(msg, rt);
1641                 break;
1642
1643         case NGM_L2CAP_L2CA_DISCON: /* L2CA_Disconnect response */
1644                 ng_btsocket_l2cap_process_l2ca_discon_rsp(msg, rt);
1645                 break;
1646
1647         case NGM_L2CAP_L2CA_DISCON_IND: /* L2CA_Disconnect indicator */
1648                 ng_btsocket_l2cap_process_l2ca_discon_ind(msg, rt);
1649                 break;
1650
1651         case NGM_L2CAP_L2CA_WRITE: /* L2CA_Write response */
1652                 ng_btsocket_l2cap_process_l2ca_write_rsp(msg, rt);
1653                 break;
1654
1655         /* XXX FIXME add other L2CA messages */
1656
1657         default:
1658                 NG_BTSOCKET_L2CAP_WARN(
1659 "%s: Unknown L2CA message, cmd=%d\n", __func__, msg->header.cmd);
1660                 break;
1661         }
1662 drop:
1663         NG_FREE_MSG(msg);
1664 } /* ng_btsocket_l2cap_l2ca_msg_input */
1665
1666 /*
1667  * L2CAP sockets input routine
1668  */
1669
1670 static void
1671 ng_btsocket_l2cap_input(void *context, int pending)
1672 {
1673         item_p  item = NULL;
1674         hook_p  hook = NULL;
1675
1676         for (;;) {
1677                 lockmgr(&ng_btsocket_l2cap_queue_lock, LK_EXCLUSIVE);
1678                 NG_BT_ITEMQ_DEQUEUE(&ng_btsocket_l2cap_queue, item);
1679                 lockmgr(&ng_btsocket_l2cap_queue_lock, LK_RELEASE);
1680
1681                 if (item == NULL)
1682                         break;
1683
1684                 NGI_GET_HOOK(item, hook);
1685                 if (hook != NULL && NG_HOOK_NOT_VALID(hook))
1686                         goto drop;
1687
1688                 switch(item->el_flags & NGQF_TYPE) {
1689                 case NGQF_DATA: {
1690                         struct mbuf     *m = NULL;
1691
1692                         NGI_GET_M(item, m);
1693                         ng_btsocket_l2cap_data_input(m, hook);
1694                         } break;
1695
1696                 case NGQF_MESG: {
1697                         struct ng_mesg  *msg = NULL;
1698
1699                         NGI_GET_MSG(item, msg);
1700
1701                         switch (msg->header.cmd) {
1702                         case NGM_L2CAP_L2CA_CON:
1703                         case NGM_L2CAP_L2CA_CON_RSP:
1704                         case NGM_L2CAP_L2CA_CON_IND:
1705                         case NGM_L2CAP_L2CA_CFG:
1706                         case NGM_L2CAP_L2CA_CFG_RSP:
1707                         case NGM_L2CAP_L2CA_CFG_IND:
1708                         case NGM_L2CAP_L2CA_DISCON:
1709                         case NGM_L2CAP_L2CA_DISCON_IND:
1710                         case NGM_L2CAP_L2CA_WRITE:
1711                         /* XXX FIXME add other L2CA messages */
1712                                 ng_btsocket_l2cap_l2ca_msg_input(msg, hook);
1713                                 break;
1714
1715                         default:
1716                                 ng_btsocket_l2cap_default_msg_input(msg, hook);
1717                                 break;
1718                         }
1719                         } break;
1720
1721                 default:
1722                         KASSERT(0,
1723 ("%s: invalid item type=%ld\n", __func__, (item->el_flags & NGQF_TYPE)));
1724                         break;
1725                 }
1726 drop:
1727                 if (hook != NULL)
1728                         NG_HOOK_UNREF(hook);
1729
1730                 NG_FREE_ITEM(item);
1731                 ng_unref_item(item, 0);
1732         }
1733 } /* ng_btsocket_l2cap_input */
1734
1735 /*
1736  * Route cleanup task. Gets scheduled when hook is disconnected. Here we
1737  * will find all sockets that use "invalid" hook and disconnect them.
1738  */
1739
1740 static void
1741 ng_btsocket_l2cap_rtclean(void *context, int pending)
1742 {
1743         ng_btsocket_l2cap_pcb_p         pcb = NULL, pcb_next = NULL;
1744         ng_btsocket_l2cap_rtentry_p     rt = NULL;
1745
1746         lockmgr(&ng_btsocket_l2cap_rt_lock, LK_EXCLUSIVE);
1747         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
1748
1749         /*
1750          * First disconnect all sockets that use "invalid" hook
1751          */
1752
1753         for (pcb = LIST_FIRST(&ng_btsocket_l2cap_sockets); pcb != NULL; ) {
1754                 lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
1755                 pcb_next = LIST_NEXT(pcb, next);
1756
1757                 if (pcb->rt != NULL &&
1758                     pcb->rt->hook != NULL && NG_HOOK_NOT_VALID(pcb->rt->hook)) {
1759                         if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
1760                                 ng_btsocket_l2cap_untimeout(pcb);
1761
1762                         pcb->so->so_error = ENETDOWN;
1763                         pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1764                         soisdisconnected(pcb->so);
1765
1766                         pcb->token = 0;
1767                         pcb->cid = 0;
1768                         pcb->rt = NULL;
1769                 }
1770
1771                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
1772                 pcb = pcb_next;
1773         }
1774
1775         /*
1776          * Now cleanup routing table
1777          */
1778
1779         for (rt = LIST_FIRST(&ng_btsocket_l2cap_rt); rt != NULL; ) {
1780                 ng_btsocket_l2cap_rtentry_p     rt_next = LIST_NEXT(rt, next);
1781
1782                 if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) {
1783                         LIST_REMOVE(rt, next);
1784
1785                         NG_HOOK_SET_PRIVATE(rt->hook, NULL);
1786                         NG_HOOK_UNREF(rt->hook); /* Remove extra reference */
1787
1788                         bzero(rt, sizeof(*rt));
1789                         kfree(rt, M_NETGRAPH_BTSOCKET_L2CAP);
1790                 }
1791
1792                 rt = rt_next;
1793         }
1794
1795         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
1796         lockmgr(&ng_btsocket_l2cap_rt_lock, LK_RELEASE);
1797 } /* ng_btsocket_l2cap_rtclean */
1798
1799 /*
1800  * Initialize everything
1801  */
1802
1803 void
1804 ng_btsocket_l2cap_init(void)
1805 {
1806         int     error = 0;
1807
1808         ng_btsocket_l2cap_node = NULL;
1809         ng_btsocket_l2cap_debug_level = NG_BTSOCKET_WARN_LEVEL;
1810
1811         /* Register Netgraph node type */
1812         error = ng_newtype(&typestruct);
1813         if (error != 0) {
1814                 NG_BTSOCKET_L2CAP_ALERT(
1815 "%s: Could not register Netgraph node type, error=%d\n", __func__, error);
1816
1817                 return;
1818         }
1819
1820         /* Create Netgrapg node */
1821         error = ng_make_node_common(&typestruct, &ng_btsocket_l2cap_node);
1822         if (error != 0) {
1823                 NG_BTSOCKET_L2CAP_ALERT(
1824 "%s: Could not create Netgraph node, error=%d\n", __func__, error);
1825
1826                 ng_btsocket_l2cap_node = NULL;
1827
1828                 return;
1829         }
1830
1831         error = ng_name_node(ng_btsocket_l2cap_node,
1832                                 NG_BTSOCKET_L2CAP_NODE_TYPE);
1833         if (error != 0) {
1834                 NG_BTSOCKET_L2CAP_ALERT(
1835 "%s: Could not name Netgraph node, error=%d\n", __func__, error);
1836
1837                 NG_NODE_UNREF(ng_btsocket_l2cap_node);
1838                 ng_btsocket_l2cap_node = NULL;
1839
1840                 return;
1841         }
1842
1843         /* Create input queue */
1844         NG_BT_ITEMQ_INIT(&ng_btsocket_l2cap_queue, ifqmaxlen);
1845         lockinit(&ng_btsocket_l2cap_queue_lock,
1846                 "btsocks_l2cap_queue_lock", 0, 0);
1847         TASK_INIT(&ng_btsocket_l2cap_queue_task, 0,
1848                 ng_btsocket_l2cap_input, NULL);
1849
1850         /* Create list of sockets */
1851         LIST_INIT(&ng_btsocket_l2cap_sockets);
1852         lockinit(&ng_btsocket_l2cap_sockets_lock,
1853                 "btsocks_l2cap_sockets_lock", 0, 0);
1854
1855         /* Routing table */
1856         LIST_INIT(&ng_btsocket_l2cap_rt);
1857         lockinit(&ng_btsocket_l2cap_rt_lock,
1858                 "btsocks_l2cap_rt_lock", 0, 0);
1859         TASK_INIT(&ng_btsocket_l2cap_rt_task, 0,
1860                 ng_btsocket_l2cap_rtclean, NULL);
1861 } /* ng_btsocket_l2cap_init */
1862
1863 /*
1864  * Abort connection on socket
1865  */
1866
1867 void
1868 ng_btsocket_l2cap_abort(netmsg_t msg)
1869 {
1870         struct socket           *so = msg->abort.base.nm_so;
1871
1872         so->so_error = ECONNABORTED;
1873
1874         ng_btsocket_l2cap_disconnect(msg);
1875 } /* ng_btsocket_l2cap_abort */
1876
1877 #if 0 /* XXX */
1878 void
1879 ng_btsocket_l2cap_close(struct socket *so)
1880 {
1881
1882         (void)ng_btsocket_l2cap_disconnect(so);
1883 } /* ng_btsocket_l2cap_close */
1884 #endif
1885
1886 /*
1887  * Accept connection on socket. Nothing to do here, socket must be connected
1888  * and ready, so just return peer address and be done with it.
1889  */
1890
1891 void
1892 ng_btsocket_l2cap_accept(netmsg_t msg)
1893 {
1894         int                     error = 0;
1895
1896         if (ng_btsocket_l2cap_node == NULL) {
1897                 error = EINVAL;
1898                 goto out;
1899         }
1900
1901         ng_btsocket_l2cap_peeraddr(msg);
1902         return;
1903
1904 out:
1905         lwkt_replymsg(&msg->accept.base.lmsg, error);
1906 } /* ng_btsocket_l2cap_accept */
1907
1908 /*
1909  * Create and attach new socket
1910  */
1911
1912 void
1913 ng_btsocket_l2cap_attach(netmsg_t msg)
1914 {
1915         struct socket           *so = msg->attach.base.nm_so;
1916         int                      proto = msg->attach.nm_proto;
1917         static u_int32_t         token = 0;
1918         ng_btsocket_l2cap_pcb_p  pcb = so2l2cap_pcb(so);
1919         int                      error = 0;
1920
1921         /* Check socket and protocol */
1922         if (ng_btsocket_l2cap_node == NULL) {
1923                 error = EPROTONOSUPPORT;
1924                 goto out;
1925         }
1926         if (so->so_type != SOCK_SEQPACKET) {
1927                 error = ESOCKTNOSUPPORT;
1928                 goto out;
1929         }
1930
1931 #if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */
1932         if (proto != 0)
1933                 if (proto != BLUETOOTH_PROTO_L2CAP) {
1934                         error = EPROTONOSUPPORT;
1935                         goto out;
1936                 }
1937 #endif /* XXX */
1938
1939         if (pcb != NULL) {
1940                 error = EISCONN;
1941                 goto out;
1942         }
1943
1944         /* Reserve send and receive space if it is not reserved yet */
1945         if ((so->so_snd.ssb_hiwat == 0) || (so->so_rcv.ssb_hiwat == 0)) {
1946                 error = soreserve(so, NG_BTSOCKET_L2CAP_SENDSPACE,
1947                                         NG_BTSOCKET_L2CAP_RECVSPACE, NULL);
1948                 if (error != 0)
1949                         goto out;
1950         }
1951
1952         /* Allocate the PCB */
1953         pcb = kmalloc(sizeof(*pcb), M_NETGRAPH_BTSOCKET_L2CAP,
1954                       M_WAITOK | M_NULLOK | M_ZERO);
1955         if (pcb == NULL) {
1956                 error = ENOMEM;
1957                 goto out;
1958         }
1959
1960         /* Link the PCB and the socket */
1961         so->so_pcb = (caddr_t) pcb;
1962         pcb->so = so;
1963         pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
1964
1965         /* Initialize PCB */
1966         pcb->imtu = pcb->omtu = NG_L2CAP_MTU_DEFAULT;
1967
1968         /* Default flow */
1969         pcb->iflow.flags = 0x0;
1970         pcb->iflow.service_type = NG_HCI_SERVICE_TYPE_BEST_EFFORT;
1971         pcb->iflow.token_rate = 0xffffffff; /* maximum */
1972         pcb->iflow.token_bucket_size = 0xffffffff; /* maximum */
1973         pcb->iflow.peak_bandwidth = 0x00000000; /* maximum */
1974         pcb->iflow.latency = 0xffffffff; /* don't care */
1975         pcb->iflow.delay_variation = 0xffffffff; /* don't care */
1976
1977         bcopy(&pcb->iflow, &pcb->oflow, sizeof(pcb->oflow));
1978
1979         pcb->flush_timo = NG_L2CAP_FLUSH_TIMO_DEFAULT;
1980         pcb->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT;
1981
1982         callout_init_mp(&pcb->timo);
1983
1984         /*
1985          * XXX Mark PCB mutex as DUPOK to prevent "duplicated lock of
1986          * the same type" message. When accepting new L2CAP connection
1987          * ng_btsocket_l2cap_process_l2ca_con_ind() holds both PCB mutexes
1988          * for "old" (accepting) PCB and "new" (created) PCB.
1989          */
1990
1991         lockinit(&pcb->pcb_lock, "btsocks_l2cap_pcb_lock", 0, LK_CANRECURSE);
1992
1993         /*
1994          * Add the PCB to the list
1995          *
1996          * XXX FIXME VERY IMPORTANT!
1997          *
1998          * This is totally FUBAR. We could get here in two cases:
1999          *
2000          * 1) When user calls socket()
2001          * 2) When we need to accept new incomming connection and call
2002          *    sonewconn()
2003          *
2004          * In the first case we must acquire ng_btsocket_l2cap_sockets_mtx.
2005          * In the second case we hold ng_btsocket_l2cap_sockets_mtx already.
2006          * So we now need to distinguish between these cases. From reading
2007          * /sys/kern/uipc_socket.c we can find out that sonewconn() calls
2008          * pru_attach with proto == 0 and td == NULL. For now use this fact
2009          * to figure out if we were called from socket() or from sonewconn().
2010          */
2011
2012         if (proto != 0)
2013                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
2014         else
2015                 KKASSERT(lockowned(&ng_btsocket_l2cap_sockets_lock) != 0);
2016
2017         /* Set PCB token. Use ng_btsocket_l2cap_sockets_mtx for protection */
2018         if (++ token == 0)
2019                 token ++;
2020
2021         pcb->token = token;
2022
2023         LIST_INSERT_HEAD(&ng_btsocket_l2cap_sockets, pcb, next);
2024
2025         if (proto != 0)
2026                 lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
2027
2028 out:
2029         lwkt_replymsg(&msg->attach.base.lmsg, error);
2030 } /* ng_btsocket_l2cap_attach */
2031
2032 /*
2033  * Bind socket
2034  */
2035
2036 void
2037 ng_btsocket_l2cap_bind(netmsg_t msg)
2038 {
2039         struct socket           *so = msg->bind.base.nm_so;
2040         struct sockaddr         *nam = msg->bind.nm_nam;
2041         ng_btsocket_l2cap_pcb_t *pcb = NULL;
2042         struct sockaddr_l2cap   *sa = (struct sockaddr_l2cap *) nam;
2043         int                      psm, error = 0;
2044
2045         if (ng_btsocket_l2cap_node == NULL) {
2046                 error = EINVAL;
2047                 goto out;
2048         }
2049
2050         /* Verify address */
2051         if (sa == NULL) {
2052                 error = EINVAL;
2053                 goto out;
2054         }
2055         if (sa->l2cap_family != AF_BLUETOOTH) {
2056                 error = EAFNOSUPPORT;
2057                 goto out;
2058         }
2059         if (sa->l2cap_len != sizeof(*sa)) {
2060                 error = EINVAL;
2061                 goto out;
2062         }
2063
2064         psm = le16toh(sa->l2cap_psm);
2065
2066         /*
2067          * Check if other socket has this address already (look for exact
2068          * match PSM and bdaddr) and assign socket address if it's available.
2069          *
2070          * Note: socket can be bound to ANY PSM (zero) thus allowing several
2071          * channels with the same PSM between the same pair of BD_ADDR'es.
2072          */
2073
2074         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
2075
2076         LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next)
2077                 if (psm != 0 && psm == pcb->psm &&
2078                     bcmp(&pcb->src, &sa->l2cap_bdaddr, sizeof(bdaddr_t)) == 0)
2079                         break;
2080
2081         if (pcb == NULL) {
2082                 /* Set socket address */
2083                 pcb = so2l2cap_pcb(so);
2084                 if (pcb != NULL) {
2085                         bcopy(&sa->l2cap_bdaddr, &pcb->src, sizeof(pcb->src));
2086                         pcb->psm = psm;
2087                 } else
2088                         error = EINVAL;
2089         } else
2090                 error = EADDRINUSE;
2091
2092         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
2093
2094 out:
2095         lwkt_replymsg(&msg->bind.base.lmsg, error);
2096 } /* ng_btsocket_l2cap_bind */
2097
2098 /*
2099  * Connect socket
2100  */
2101
2102 void
2103 ng_btsocket_l2cap_connect(netmsg_t msg)
2104 {
2105         struct socket                   *so = msg->connect.base.nm_so;
2106         struct sockaddr                 *nam = msg->connect.nm_nam;
2107         ng_btsocket_l2cap_pcb_t         *pcb = so2l2cap_pcb(so);
2108         struct sockaddr_l2cap           *sa = (struct sockaddr_l2cap *) nam;
2109         ng_btsocket_l2cap_rtentry_t     *rt = NULL;
2110         int                              have_src, error = 0;
2111
2112         /* Check socket */
2113         if (pcb == NULL) {
2114                 error = EINVAL;
2115                 goto out;
2116         }
2117         if (ng_btsocket_l2cap_node == NULL) {
2118                 error = EINVAL;
2119                 goto out;
2120         }
2121         if (pcb->state == NG_BTSOCKET_L2CAP_CONNECTING) {
2122                 error = EINPROGRESS;
2123                 goto out;
2124         }
2125
2126         /* Verify address */
2127         if (sa == NULL) {
2128                 error = EINVAL;
2129                 goto out;
2130         }
2131         if (sa->l2cap_family != AF_BLUETOOTH) {
2132                 error = EAFNOSUPPORT;
2133                 goto out;
2134         }
2135         if (sa->l2cap_len != sizeof(*sa)) {
2136                 error = EINVAL;
2137                 goto out;
2138         }
2139         if (sa->l2cap_psm == 0 ||
2140             bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) {
2141                 error = EDESTADDRREQ;
2142                 goto out;
2143         }
2144         if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm)) {
2145                 error = EINVAL;
2146                 goto out;
2147         }
2148
2149         /*
2150          * Routing. Socket should be bound to some source address. The source
2151          * address can be ANY. Destination address must be set and it must not
2152          * be ANY. If source address is ANY then find first rtentry that has
2153          * src != dst.
2154          */
2155
2156         lockmgr(&ng_btsocket_l2cap_rt_lock, LK_EXCLUSIVE);
2157         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
2158         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
2159
2160         /* Send destination address and PSM */
2161         bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst));
2162         pcb->psm = le16toh(sa->l2cap_psm);
2163
2164         pcb->rt = NULL;
2165         have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src));
2166
2167         LIST_FOREACH(rt, &ng_btsocket_l2cap_rt, next) {
2168                 if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook))
2169                         continue;
2170
2171                 /* Match src and dst */
2172                 if (have_src) {
2173                         if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0)
2174                                 break;
2175                 } else {
2176                         if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0)
2177                                 break;
2178                 }
2179         }
2180
2181         if (rt != NULL) {
2182                 pcb->rt = rt;
2183
2184                 if (!have_src)
2185                         bcopy(&rt->src, &pcb->src, sizeof(pcb->src));
2186         } else
2187                 error = EHOSTUNREACH;
2188
2189         /*
2190          * Send L2CA_Connect request
2191          */
2192
2193         if (error == 0) {
2194                 error = ng_btsocket_l2cap_send_l2ca_con_req(pcb);
2195                 if (error == 0) {
2196                         pcb->flags |= NG_BTSOCKET_L2CAP_CLIENT;
2197                         pcb->state = NG_BTSOCKET_L2CAP_CONNECTING;
2198                         soisconnecting(pcb->so);
2199
2200                         ng_btsocket_l2cap_timeout(pcb);
2201                 }
2202         }
2203
2204         lockmgr(&pcb->pcb_lock, LK_RELEASE);
2205         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
2206         lockmgr(&ng_btsocket_l2cap_rt_lock, LK_RELEASE);
2207
2208 out:
2209         lwkt_replymsg(&msg->connect.base.lmsg, error);
2210 } /* ng_btsocket_l2cap_connect */
2211
2212 /*
2213  * Process ioctl's calls on socket
2214  */
2215
2216 void
2217 ng_btsocket_l2cap_control(netmsg_t msg)
2218 {
2219         lwkt_replymsg(&msg->control.base.lmsg, EINVAL);
2220 } /* ng_btsocket_l2cap_control */
2221
2222 /*
2223  * Process getsockopt/setsockopt system calls
2224  */
2225
2226 void
2227 ng_btsocket_l2cap_ctloutput(netmsg_t msg)
2228 {
2229         struct socket           *so = msg->ctloutput.base.nm_so;
2230         struct sockopt          *sopt = msg->ctloutput.nm_sopt;
2231         ng_btsocket_l2cap_pcb_p  pcb = so2l2cap_pcb(so);
2232         int                      error = 0;
2233         ng_l2cap_cfg_opt_val_t   v;
2234
2235         if (pcb == NULL) {
2236                 error = EINVAL;
2237                 goto out;
2238         }
2239         if (ng_btsocket_l2cap_node == NULL) {
2240                 error = EINVAL;
2241                 goto out;
2242         }
2243
2244         if (sopt->sopt_level != SOL_L2CAP)
2245                 goto out;
2246
2247         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
2248
2249         switch (sopt->sopt_dir) {
2250         case SOPT_GET:
2251                 switch (sopt->sopt_name) {
2252                 case SO_L2CAP_IMTU: /* get incoming MTU */
2253                         error = sooptcopyout(sopt, &pcb->imtu,
2254                                                 sizeof(pcb->imtu));
2255                         break;
2256
2257                 case SO_L2CAP_OMTU: /* get outgoing (peer incoming) MTU */
2258                         error = sooptcopyout(sopt, &pcb->omtu,
2259                                                 sizeof(pcb->omtu));
2260                         break;
2261
2262                 case SO_L2CAP_IFLOW: /* get incoming flow spec. */
2263                         error = sooptcopyout(sopt, &pcb->iflow,
2264                                                 sizeof(pcb->iflow));
2265                         break;
2266
2267                 case SO_L2CAP_OFLOW: /* get outgoing flow spec. */
2268                         error = sooptcopyout(sopt, &pcb->oflow,
2269                                                 sizeof(pcb->oflow));
2270                         break;
2271
2272                 case SO_L2CAP_FLUSH: /* get flush timeout */
2273                         error = sooptcopyout(sopt, &pcb->flush_timo,
2274                                                 sizeof(pcb->flush_timo));
2275                         break;
2276
2277                 default:
2278                         error = ENOPROTOOPT;
2279                         break;
2280                 }
2281                 break;
2282
2283         case SOPT_SET:
2284                 /*
2285                  * XXX
2286                  * We do not allow to change these parameters while socket is
2287                  * connected or we are in the process of creating a connection.
2288                  * May be this should indicate re-configuration of the open
2289                  * channel?
2290                  */
2291
2292                 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
2293                         error = EACCES;
2294                         break;
2295                 }
2296
2297                 switch (sopt->sopt_name) {
2298                 case SO_L2CAP_IMTU: /* set incoming MTU */
2299                         error = sooptcopyin(sopt, &v, sizeof(v), sizeof(v.mtu));
2300                         if (error == 0)
2301                                 pcb->imtu = v.mtu;
2302                         break;
2303
2304                 case SO_L2CAP_OFLOW: /* set outgoing flow spec. */
2305                         error = sooptcopyin(sopt, &v, sizeof(v),sizeof(v.flow));
2306                         if (error == 0)
2307                                 bcopy(&v.flow, &pcb->oflow, sizeof(pcb->oflow));
2308                         break;
2309
2310                 case SO_L2CAP_FLUSH: /* set flush timeout */
2311                         error = sooptcopyin(sopt, &v, sizeof(v),
2312                                                 sizeof(v.flush_timo));
2313                         if (error == 0)
2314                                 pcb->flush_timo = v.flush_timo;
2315                         break;
2316
2317                 default:
2318                         error = ENOPROTOOPT;
2319                         break;
2320                 }
2321                 break;
2322
2323         default:
2324                 error = EINVAL;
2325                 break;
2326         }
2327
2328         lockmgr(&pcb->pcb_lock, LK_RELEASE);
2329
2330 out:
2331         lwkt_replymsg(&msg->ctloutput.base.lmsg, error);
2332 } /* ng_btsocket_l2cap_ctloutput */
2333
2334 /*
2335  * Detach and destroy socket
2336  */
2337
2338 void
2339 ng_btsocket_l2cap_detach(netmsg_t msg)
2340 {
2341         struct socket           *so = msg->detach.base.nm_so;
2342         ng_btsocket_l2cap_pcb_p  pcb = so2l2cap_pcb(so);
2343         int                      error = 0;
2344
2345         KASSERT(pcb != NULL, ("ng_btsocket_l2cap_detach: pcb == NULL"));
2346
2347         if (ng_btsocket_l2cap_node == NULL)
2348                 goto out;
2349
2350         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_EXCLUSIVE);
2351         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
2352
2353         /* XXX what to do with pending request? */
2354         if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
2355                 ng_btsocket_l2cap_untimeout(pcb);
2356
2357         if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED &&
2358             pcb->state != NG_BTSOCKET_L2CAP_DISCONNECTING)
2359                 /* Send disconnect request with "zero" token */
2360                 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
2361
2362         pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2363
2364         LIST_REMOVE(pcb, next);
2365
2366         lockmgr(&pcb->pcb_lock, LK_RELEASE);
2367         lockmgr(&ng_btsocket_l2cap_sockets_lock, LK_RELEASE);
2368
2369         lockuninit(&pcb->pcb_lock);
2370         bzero(pcb, sizeof(*pcb));
2371         kfree(pcb, M_NETGRAPH_BTSOCKET_L2CAP);
2372
2373         soisdisconnected(so);
2374         so->so_pcb = NULL;
2375
2376 out:
2377         lwkt_replymsg(&msg->detach.base.lmsg, error);
2378 } /* ng_btsocket_l2cap_detach */
2379
2380 /*
2381  * Disconnect socket
2382  */
2383
2384 void
2385 ng_btsocket_l2cap_disconnect(netmsg_t msg)
2386 {
2387         struct socket           *so = msg->disconnect.base.nm_so;
2388         ng_btsocket_l2cap_pcb_p  pcb = so2l2cap_pcb(so);
2389         int                      error = 0;
2390
2391         if (pcb == NULL) {
2392                 error = EINVAL;
2393                 goto out;
2394         }
2395         if (ng_btsocket_l2cap_node == NULL) {
2396                 error = EINVAL;
2397                 goto out;
2398         }
2399
2400         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
2401
2402         if (pcb->state == NG_BTSOCKET_L2CAP_DISCONNECTING) {
2403                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
2404                 error = EINPROGRESS;
2405                 goto out;
2406         }
2407
2408         if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) {
2409                 /* XXX FIXME what to do with pending request? */
2410                 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO)
2411                         ng_btsocket_l2cap_untimeout(pcb);
2412
2413                 error = ng_btsocket_l2cap_send_l2ca_discon_req(pcb->token, pcb);
2414                 if (error == 0) {
2415                         pcb->state = NG_BTSOCKET_L2CAP_DISCONNECTING;
2416                         soisdisconnecting(so);
2417
2418                         ng_btsocket_l2cap_timeout(pcb);
2419                 }
2420
2421                 /* XXX FIXME what to do if error != 0 */
2422         }
2423
2424         lockmgr(&pcb->pcb_lock, LK_RELEASE);
2425
2426 out:
2427         lwkt_replymsg(&msg->disconnect.base.lmsg, error);
2428 } /* ng_btsocket_l2cap_disconnect */
2429
2430 /*
2431  * Listen on socket
2432  */
2433
2434 void
2435 ng_btsocket_l2cap_listen(netmsg_t msg)
2436 {
2437         struct socket           *so = msg->listen.base.nm_so;
2438         struct thread           *td = msg->listen.nm_td;
2439         int                      backlog = msg->listen.nm_flags; /* XXX */
2440         ng_btsocket_l2cap_pcb_p  pcb = so2l2cap_pcb(so);
2441         int                      error = 0;
2442
2443         if (so->so_state &
2444             (SS_ISCONNECTED | SS_ISCONNECTING | SS_ISDISCONNECTING)) {
2445                 error = EINVAL;
2446                 goto out;
2447         }
2448         if (pcb == NULL) {
2449                 error = EINVAL;
2450                 goto out;
2451         }
2452         if (ng_btsocket_l2cap_node == NULL) {
2453                 error = EINVAL;
2454                 goto out;
2455         }
2456         if (pcb->psm == 0) {
2457                 error = EADDRNOTAVAIL;
2458                 goto out;
2459         }
2460         solisten(so, backlog, td);
2461
2462 out:
2463         lwkt_replymsg(&msg->listen.base.lmsg, error);
2464 } /* ng_btsocket_listen */
2465
2466 /*
2467  * Get peer address
2468  */
2469
2470 void
2471 ng_btsocket_l2cap_peeraddr(netmsg_t msg)
2472 {
2473         struct socket            *so = msg->peeraddr.base.nm_so;
2474         struct sockaddr         **nam = msg->peeraddr.nm_nam;
2475         ng_btsocket_l2cap_pcb_p   pcb = so2l2cap_pcb(so);
2476         struct sockaddr_l2cap     sa;
2477         int                       error = 0;
2478
2479         if (pcb == NULL) {
2480                 error = EINVAL;
2481                 goto out;
2482         }
2483         if (ng_btsocket_l2cap_node == NULL) {
2484                 error = EINVAL;
2485                 goto out;
2486         }
2487
2488         bcopy(&pcb->dst, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr));
2489         sa.l2cap_psm = htole16(pcb->psm);
2490         sa.l2cap_len = sizeof(sa);
2491         sa.l2cap_family = AF_BLUETOOTH;
2492
2493         *nam = dup_sockaddr((struct sockaddr *) &sa);
2494
2495         if (*nam == NULL)
2496                 error = ENOMEM;
2497
2498 out:
2499         lwkt_replymsg(&msg->peeraddr.base.lmsg, error);
2500 } /* ng_btsocket_l2cap_peeraddr */
2501
2502 /*
2503  * Send data to socket
2504  */
2505
2506 void
2507 ng_btsocket_l2cap_send(netmsg_t msg)
2508 {
2509         struct socket           *so = msg->send.base.nm_so;
2510         struct mbuf             *m = msg->send.nm_m;
2511         struct mbuf             *control = msg->send.nm_control;
2512         ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so);
2513         int                      error = 0;
2514
2515         if (ng_btsocket_l2cap_node == NULL) {
2516                 error = ENETDOWN;
2517                 goto drop;
2518         }
2519
2520         /* Check socket and input */
2521         if (pcb == NULL || m == NULL || control != NULL) {
2522                 error = EINVAL;
2523                 goto drop;
2524         }
2525
2526         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
2527
2528         /* Make sure socket is connected */
2529         if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) {
2530                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
2531                 error = ENOTCONN;
2532                 goto drop;
2533         }
2534
2535         /* Check route */
2536         if (pcb->rt == NULL ||
2537             pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) {
2538                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
2539                 error = ENETDOWN;
2540                 goto drop;
2541         }
2542
2543         /* Check packet size agains outgoing (peer's incoming) MTU) */
2544         if (m->m_pkthdr.len > pcb->omtu) {
2545                 NG_BTSOCKET_L2CAP_ERR(
2546 "%s: Packet too big, len=%d, omtu=%d\n", __func__, m->m_pkthdr.len, pcb->omtu);
2547
2548                 lockmgr(&pcb->pcb_lock, LK_RELEASE);
2549                 error = EMSGSIZE;
2550                 goto drop;
2551         }
2552
2553         /*
2554          * First put packet on socket send queue. Then check if we have
2555          * pending timeout. If we do not have timeout then we must send
2556          * packet and schedule timeout. Otherwise do nothing and wait for
2557          * L2CA_WRITE_RSP.
2558          */
2559
2560         sbappendrecord(&pcb->so->so_snd.sb, m);
2561         m = NULL;
2562
2563         if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) {
2564                 error = ng_btsocket_l2cap_send2(pcb);
2565                 if (error == 0)
2566                         ng_btsocket_l2cap_timeout(pcb);
2567                 else
2568                         sbdroprecord(&pcb->so->so_snd.sb); /* XXX */
2569         }
2570
2571         lockmgr(&pcb->pcb_lock, LK_RELEASE);
2572 drop:
2573         NG_FREE_M(m); /* checks for != NULL */
2574         NG_FREE_M(control);
2575
2576         lwkt_replymsg(&msg->send.base.lmsg, error);
2577 } /* ng_btsocket_l2cap_send */
2578
2579 /*
2580  * Send first packet in the socket queue to the L2CAP layer
2581  */
2582
2583 static int
2584 ng_btsocket_l2cap_send2(ng_btsocket_l2cap_pcb_p pcb)
2585 {
2586         struct  mbuf            *m = NULL;
2587         ng_l2cap_l2ca_hdr_t     *hdr = NULL;
2588         int                      error = 0;
2589
2590         KKASSERT(lockowned(&pcb->pcb_lock) != 0);
2591
2592         if (pcb->so->so_snd.sb.sb_cc == 0)
2593                 return (EINVAL); /* XXX */
2594
2595         m = m_dup(pcb->so->so_snd.sb.sb_mb, M_NOWAIT);
2596         if (m == NULL)
2597                 return (ENOBUFS);
2598
2599         /* Create L2CA packet header */
2600         M_PREPEND(m, sizeof(*hdr), M_NOWAIT);
2601         if (m != NULL)
2602                 if (m->m_len < sizeof(*hdr))
2603                         m = m_pullup(m, sizeof(*hdr));
2604
2605         if (m == NULL) {
2606                 NG_BTSOCKET_L2CAP_ERR(
2607 "%s: Failed to create L2CA packet header\n", __func__);
2608
2609                 return (ENOBUFS);
2610         }
2611
2612         hdr = mtod(m, ng_l2cap_l2ca_hdr_t *);
2613         hdr->token = pcb->token;
2614         hdr->length = m->m_pkthdr.len - sizeof(*hdr);
2615         hdr->lcid = pcb->cid;
2616
2617         NG_BTSOCKET_L2CAP_INFO(
2618 "%s: Sending packet: len=%d, length=%d, lcid=%d, token=%d, state=%d\n",
2619                 __func__, m->m_pkthdr.len, hdr->length, hdr->lcid,
2620                 hdr->token, pcb->state);
2621
2622         /*
2623          * If we got here than we have successfuly creates new L2CAP
2624          * data packet and now we can send it to the L2CAP layer
2625          */
2626
2627         NG_SEND_DATA_ONLY(error, pcb->rt->hook, m);
2628
2629         return (error);
2630 } /* ng_btsocket_l2cap_send2 */
2631
2632 /*
2633  * Get socket address
2634  */
2635
2636 void
2637 ng_btsocket_l2cap_sockaddr(netmsg_t msg)
2638 {
2639         struct socket            *so = msg->sockaddr.base.nm_so;
2640         struct sockaddr         **nam = msg->sockaddr.nm_nam;
2641         ng_btsocket_l2cap_pcb_p   pcb = so2l2cap_pcb(so);
2642         struct sockaddr_l2cap     sa;
2643         int                       error = 0;
2644
2645         if (pcb == NULL) {
2646                 error = EINVAL;
2647                 goto out;
2648         }
2649         if (ng_btsocket_l2cap_node == NULL) {
2650                 error = EINVAL;
2651                 goto out;
2652         }
2653
2654         bcopy(&pcb->src, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr));
2655         sa.l2cap_psm = htole16(pcb->psm);
2656         sa.l2cap_len = sizeof(sa);
2657         sa.l2cap_family = AF_BLUETOOTH;
2658
2659         *nam = dup_sockaddr((struct sockaddr *) &sa);
2660
2661         if (*nam == NULL)
2662                 error = ENOMEM;
2663
2664 out:
2665         lwkt_replymsg(&msg->sockaddr.base.lmsg, error);
2666 } /* ng_btsocket_l2cap_sockaddr */
2667
2668 /*****************************************************************************
2669  *****************************************************************************
2670  **                              Misc. functions
2671  *****************************************************************************
2672  *****************************************************************************/
2673
2674 /*
2675  * Look for the socket that listens on given PSM and bdaddr. Returns exact or
2676  * close match (if any). Caller must hold ng_btsocket_l2cap_sockets_mtx.
2677  */
2678
2679 static ng_btsocket_l2cap_pcb_p
2680 ng_btsocket_l2cap_pcb_by_addr(bdaddr_p bdaddr, int psm)
2681 {
2682         ng_btsocket_l2cap_pcb_p p = NULL, p1 = NULL;
2683
2684         KKASSERT(lockowned(&ng_btsocket_l2cap_sockets_lock) != 0);
2685
2686         LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) {
2687                 if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN) ||
2688                     p->psm != psm)
2689                         continue;
2690
2691                 if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0)
2692                         break;
2693
2694                 if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0)
2695                         p1 = p;
2696         }
2697
2698         return ((p != NULL)? p : p1);
2699 } /* ng_btsocket_l2cap_pcb_by_addr */
2700
2701 /*
2702  * Look for the socket that has given token.
2703  * Caller must hold ng_btsocket_l2cap_sockets_mtx.
2704  */
2705
2706 static ng_btsocket_l2cap_pcb_p
2707 ng_btsocket_l2cap_pcb_by_token(u_int32_t token)
2708 {
2709         ng_btsocket_l2cap_pcb_p p = NULL;
2710
2711         if (token == 0)
2712                 return (NULL);
2713
2714         KKASSERT(lockowned(&ng_btsocket_l2cap_sockets_lock) != 0);
2715
2716         LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next)
2717                 if (p->token == token)
2718                         break;
2719
2720         return (p);
2721 } /* ng_btsocket_l2cap_pcb_by_token */
2722
2723 /*
2724  * Look for the socket that assigned to given source address and channel ID.
2725  * Caller must hold ng_btsocket_l2cap_sockets_mtx
2726  */
2727
2728 static ng_btsocket_l2cap_pcb_p
2729 ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src, int cid)
2730 {
2731         ng_btsocket_l2cap_pcb_p p = NULL;
2732
2733         KKASSERT(lockowned(&ng_btsocket_l2cap_sockets_lock) != 0);
2734
2735         LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next)
2736                 if (p->cid == cid && bcmp(src, &p->src, sizeof(p->src)) == 0)
2737                         break;
2738
2739         return (p);
2740 } /* ng_btsocket_l2cap_pcb_by_cid */
2741
2742 /*
2743  * Set timeout on socket
2744  */
2745
2746 static void
2747 ng_btsocket_l2cap_timeout(ng_btsocket_l2cap_pcb_p pcb)
2748 {
2749         KKASSERT(lockowned(&pcb->pcb_lock) != 0);
2750
2751         if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) {
2752                 pcb->flags |= NG_BTSOCKET_L2CAP_TIMO;
2753                 callout_reset(&pcb->timo, bluetooth_l2cap_ertx_timeout(),
2754                     ng_btsocket_l2cap_process_timeout, pcb);
2755         } else
2756                 KASSERT(0,
2757 ("%s: Duplicated socket timeout?!\n", __func__));
2758 } /* ng_btsocket_l2cap_timeout */
2759
2760 /*
2761  * Unset timeout on socket
2762  */
2763
2764 static void
2765 ng_btsocket_l2cap_untimeout(ng_btsocket_l2cap_pcb_p pcb)
2766 {
2767         KKASSERT(lockowned(&pcb->pcb_lock) != 0);
2768
2769         if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO) {
2770                 callout_stop(&pcb->timo);
2771                 pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO;
2772         } else
2773                 KASSERT(0,
2774 ("%s: No socket timeout?!\n", __func__));
2775 } /* ng_btsocket_l2cap_untimeout */
2776
2777 /*
2778  * Process timeout on socket
2779  */
2780
2781 static void
2782 ng_btsocket_l2cap_process_timeout(void *xpcb)
2783 {
2784         ng_btsocket_l2cap_pcb_p pcb = (ng_btsocket_l2cap_pcb_p) xpcb;
2785
2786         lockmgr(&pcb->pcb_lock, LK_EXCLUSIVE);
2787
2788         pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO;
2789         pcb->so->so_error = ETIMEDOUT;
2790
2791         switch (pcb->state) {
2792         case NG_BTSOCKET_L2CAP_CONNECTING:
2793         case NG_BTSOCKET_L2CAP_CONFIGURING:
2794                 /* Send disconnect request with "zero" token */
2795                 if (pcb->cid != 0)
2796                         ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb);
2797
2798                 /* ... and close the socket */
2799                 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2800                 soisdisconnected(pcb->so);
2801                 break;
2802
2803         case NG_BTSOCKET_L2CAP_OPEN:
2804                 /* Send timeout - drop packet and wakeup sender */
2805                 sbdroprecord(&pcb->so->so_snd.sb);
2806                 sowwakeup(pcb->so);
2807                 break;
2808
2809         case NG_BTSOCKET_L2CAP_DISCONNECTING:
2810                 /* Disconnect timeout - disconnect the socket anyway */
2811                 pcb->state = NG_BTSOCKET_L2CAP_CLOSED;
2812                 soisdisconnected(pcb->so);
2813                 break;
2814
2815         default:
2816                 NG_BTSOCKET_L2CAP_ERR(
2817 "%s: Invalid socket state=%d\n", __func__, pcb->state);
2818                 break;
2819         }
2820
2821         lockmgr(&pcb->pcb_lock, LK_RELEASE);
2822 } /* ng_btsocket_l2cap_process_timeout */
2823
2824 /*
2825  * Translate HCI/L2CAP error code into "errno" code
2826  * XXX Note: Some L2CAP and HCI error codes have the same value, but
2827  *     different meaning
2828  */
2829
2830 static int
2831 ng_btsocket_l2cap_result2errno(int result)
2832 {
2833         switch (result) {
2834         case 0x00: /* No error */
2835                 return (0);
2836
2837         case 0x01: /* Unknown HCI command */
2838                 return (ENODEV);
2839
2840         case 0x02: /* No connection */
2841                 return (ENOTCONN);
2842
2843         case 0x03: /* Hardware failure */
2844                 return (EIO);
2845
2846         case 0x04: /* Page timeout */
2847                 return (EHOSTDOWN);
2848
2849         case 0x05: /* Authentication failure */
2850         case 0x06: /* Key missing */
2851         case 0x18: /* Pairing not allowed */
2852         case 0x21: /* Role change not allowed */
2853         case 0x24: /* LMP PSU not allowed */
2854         case 0x25: /* Encryption mode not acceptable */
2855         case 0x26: /* Unit key used */
2856                 return (EACCES);
2857
2858         case 0x07: /* Memory full */
2859                 return (ENOMEM);
2860
2861         case 0x08:   /* Connection timeout */
2862         case 0x10:   /* Host timeout */
2863         case 0x22:   /* LMP response timeout */
2864         case 0xee:   /* HCI timeout */
2865         case 0xeeee: /* L2CAP timeout */
2866                 return (ETIMEDOUT);
2867
2868         case 0x09: /* Max number of connections */
2869         case 0x0a: /* Max number of SCO connections to a unit */
2870                 return (EMLINK);
2871
2872         case 0x0b: /* ACL connection already exists */
2873                 return (EEXIST);
2874
2875         case 0x0c: /* Command disallowed */
2876                 return (EBUSY);
2877
2878         case 0x0d: /* Host rejected due to limited resources */
2879         case 0x0e: /* Host rejected due to securiity reasons */
2880         case 0x0f: /* Host rejected due to remote unit is a personal unit */
2881         case 0x1b: /* SCO offset rejected */
2882         case 0x1c: /* SCO interval rejected */
2883         case 0x1d: /* SCO air mode rejected */
2884                 return (ECONNREFUSED);
2885
2886         case 0x11: /* Unsupported feature or parameter value */
2887         case 0x19: /* Unknown LMP PDU */
2888         case 0x1a: /* Unsupported remote feature */
2889         case 0x20: /* Unsupported LMP parameter value */
2890         case 0x27: /* QoS is not supported */
2891         case 0x29: /* Paring with unit key not supported */
2892                 return (EOPNOTSUPP);
2893
2894         case 0x12: /* Invalid HCI command parameter */
2895         case 0x1e: /* Invalid LMP parameters */
2896                 return (EINVAL);
2897
2898         case 0x13: /* Other end terminated connection: User ended connection */
2899         case 0x14: /* Other end terminated connection: Low resources */
2900         case 0x15: /* Other end terminated connection: About to power off */
2901                 return (ECONNRESET);
2902
2903         case 0x16: /* Connection terminated by local host */
2904                 return (ECONNABORTED);
2905
2906 #if 0 /* XXX not yet */
2907         case 0x17: /* Repeated attempts */
2908         case 0x1f: /* Unspecified error */
2909         case 0x23: /* LMP error transaction collision */
2910         case 0x28: /* Instant passed */
2911 #endif
2912         }
2913
2914         return (ENOSYS);
2915 } /* ng_btsocket_l2cap_result2errno */
2916