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