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