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