sys/netgraph/bluetooth/socket/ng_btsocket_l2cap.c

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