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