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