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