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 ACCEPT_LOCK(); 1808 SOCK_LOCK(so); 1809 so->so_pcb = NULL; 1810 sotryfree(so); 1811 1812 goto next; 1813 } 1814 } 1815 1816 mtx_unlock(&pcb->pcb_mtx); 1817 next: 1818 pcb = pcb_next; 1819 } 1820 1821 /* 1822 * Now cleanup routing table 1823 */ 1824 1825 for (rt = LIST_FIRST(&ng_btsocket_l2cap_rt); rt != NULL; ) { 1826 ng_btsocket_l2cap_rtentry_p rt_next = LIST_NEXT(rt, next); 1827 1828 if (rt->hook != NULL && NG_HOOK_NOT_VALID(rt->hook)) { 1829 LIST_REMOVE(rt, next); 1830 1831 NG_HOOK_SET_PRIVATE(rt->hook, NULL); 1832 NG_HOOK_UNREF(rt->hook); /* Remove extra reference */ 1833 1834 bzero(rt, sizeof(*rt)); 1835 FREE(rt, M_NETGRAPH_BTSOCKET_L2CAP); 1836 } 1837 1838 rt = rt_next; 1839 } 1840 1841 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); 1842 mtx_unlock(&ng_btsocket_l2cap_rt_mtx); 1843 } /* ng_btsocket_l2cap_rtclean */ 1844 1845 /* 1846 * Initialize everything 1847 */ 1848 1849 void 1850 ng_btsocket_l2cap_init(void) 1851 { 1852 int error = 0; 1853 1854 ng_btsocket_l2cap_node = NULL; 1855 ng_btsocket_l2cap_debug_level = NG_BTSOCKET_WARN_LEVEL; 1856 1857 /* Register Netgraph node type */ 1858 error = ng_newtype(&typestruct); 1859 if (error != 0) { 1860 NG_BTSOCKET_L2CAP_ALERT( 1861 "%s: Could not register Netgraph node type, error=%d\n", __func__, error); 1862 1863 return; 1864 } 1865 1866 /* Create Netgrapg node */ 1867 error = ng_make_node_common(&typestruct, &ng_btsocket_l2cap_node); 1868 if (error != 0) { 1869 NG_BTSOCKET_L2CAP_ALERT( 1870 "%s: Could not create Netgraph node, error=%d\n", __func__, error); 1871 1872 ng_btsocket_l2cap_node = NULL; 1873 1874 return; 1875 } 1876 1877 error = ng_name_node(ng_btsocket_l2cap_node, 1878 NG_BTSOCKET_L2CAP_NODE_TYPE); 1879 if (error != 0) { 1880 NG_BTSOCKET_L2CAP_ALERT( 1881 "%s: Could not name Netgraph node, error=%d\n", __func__, error); 1882 1883 NG_NODE_UNREF(ng_btsocket_l2cap_node); 1884 ng_btsocket_l2cap_node = NULL; 1885 1886 return; 1887 } 1888 1889 /* Create input queue */ 1890 NG_BT_ITEMQ_INIT(&ng_btsocket_l2cap_queue, ifqmaxlen); 1891 mtx_init(&ng_btsocket_l2cap_queue_mtx, 1892 "btsocks_l2cap_queue_mtx", NULL, MTX_DEF); 1893 TASK_INIT(&ng_btsocket_l2cap_queue_task, 0, 1894 ng_btsocket_l2cap_input, NULL); 1895 1896 /* Create list of sockets */ 1897 LIST_INIT(&ng_btsocket_l2cap_sockets); 1898 mtx_init(&ng_btsocket_l2cap_sockets_mtx, 1899 "btsocks_l2cap_sockets_mtx", NULL, MTX_DEF); 1900 1901 /* Routing table */ 1902 LIST_INIT(&ng_btsocket_l2cap_rt); 1903 mtx_init(&ng_btsocket_l2cap_rt_mtx, 1904 "btsocks_l2cap_rt_mtx", NULL, MTX_DEF); 1905 TASK_INIT(&ng_btsocket_l2cap_rt_task, 0, 1906 ng_btsocket_l2cap_rtclean, NULL); 1907 } /* ng_btsocket_l2cap_init */ 1908 1909 /* 1910 * Abort connection on socket 1911 */ 1912 1913 int 1914 ng_btsocket_l2cap_abort(struct socket *so) 1915 { 1916 so->so_error = ECONNABORTED; 1917 1918 return (ng_btsocket_l2cap_detach(so)); 1919 } /* ng_btsocket_l2cap_abort */ 1920 1921 /* 1922 * Accept connection on socket. Nothing to do here, socket must be connected 1923 * and ready, so just return peer address and be done with it. 1924 */ 1925 1926 int 1927 ng_btsocket_l2cap_accept(struct socket *so, struct sockaddr **nam) 1928 { 1929 if (ng_btsocket_l2cap_node == NULL) 1930 return (EINVAL); 1931 1932 return (ng_btsocket_l2cap_peeraddr(so, nam)); 1933 } /* ng_btsocket_l2cap_accept */ 1934 1935 /* 1936 * Create and attach new socket 1937 */ 1938 1939 int 1940 ng_btsocket_l2cap_attach(struct socket *so, int proto, struct thread *td) 1941 { 1942 static u_int32_t token = 0; 1943 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so); 1944 int error; 1945 1946 /* Check socket and protocol */ 1947 if (ng_btsocket_l2cap_node == NULL) 1948 return (EPROTONOSUPPORT); 1949 if (so->so_type != SOCK_SEQPACKET) 1950 return (ESOCKTNOSUPPORT); 1951 1952 #if 0 /* XXX sonewconn() calls "pru_attach" with proto == 0 */ 1953 if (proto != 0) 1954 if (proto != BLUETOOTH_PROTO_L2CAP) 1955 return (EPROTONOSUPPORT); 1956 #endif /* XXX */ 1957 1958 if (pcb != NULL) 1959 return (EISCONN); 1960 1961 /* Reserve send and receive space if it is not reserved yet */ 1962 if ((so->so_snd.sb_hiwat == 0) || (so->so_rcv.sb_hiwat == 0)) { 1963 error = soreserve(so, NG_BTSOCKET_L2CAP_SENDSPACE, 1964 NG_BTSOCKET_L2CAP_RECVSPACE); 1965 if (error != 0) 1966 return (error); 1967 } 1968 1969 /* Allocate the PCB */ 1970 MALLOC(pcb, ng_btsocket_l2cap_pcb_p, sizeof(*pcb), 1971 M_NETGRAPH_BTSOCKET_L2CAP, M_NOWAIT | M_ZERO); 1972 if (pcb == NULL) 1973 return (ENOMEM); 1974 1975 /* Link the PCB and the socket */ 1976 so->so_pcb = (caddr_t) pcb; 1977 pcb->so = so; 1978 pcb->state = NG_BTSOCKET_L2CAP_CLOSED; 1979 1980 /* Initialize PCB */ 1981 pcb->imtu = pcb->omtu = NG_L2CAP_MTU_DEFAULT; 1982 1983 /* Default flow */ 1984 pcb->iflow.flags = 0x0; 1985 pcb->iflow.service_type = NG_HCI_SERVICE_TYPE_BEST_EFFORT; 1986 pcb->iflow.token_rate = 0xffffffff; /* maximum */ 1987 pcb->iflow.token_bucket_size = 0xffffffff; /* maximum */ 1988 pcb->iflow.peak_bandwidth = 0x00000000; /* maximum */ 1989 pcb->iflow.latency = 0xffffffff; /* don't care */ 1990 pcb->iflow.delay_variation = 0xffffffff; /* don't care */ 1991 1992 bcopy(&pcb->iflow, &pcb->oflow, sizeof(pcb->oflow)); 1993 1994 pcb->flush_timo = NG_L2CAP_FLUSH_TIMO_DEFAULT; 1995 pcb->link_timo = NG_L2CAP_LINK_TIMO_DEFAULT; 1996 1997 callout_handle_init(&pcb->timo); 1998 1999 /* 2000 * XXX Mark PCB mutex as DUPOK to prevent "duplicated lock of 2001 * the same type" message. When accepting new L2CAP connection 2002 * ng_btsocket_l2cap_process_l2ca_con_ind() holds both PCB mutexes 2003 * for "old" (accepting) PCB and "new" (created) PCB. 2004 */ 2005 2006 mtx_init(&pcb->pcb_mtx, "btsocks_l2cap_pcb_mtx", NULL, 2007 MTX_DEF|MTX_DUPOK); 2008 2009 /* 2010 * Add the PCB to the list 2011 * 2012 * XXX FIXME VERY IMPORTANT! 2013 * 2014 * This is totally FUBAR. We could get here in two cases: 2015 * 2016 * 1) When user calls socket() 2017 * 2) When we need to accept new incomming connection and call 2018 * sonewconn() 2019 * 2020 * In the first case we must aquire ng_btsocket_l2cap_sockets_mtx. 2021 * In the second case we hold ng_btsocket_l2cap_sockets_mtx already. 2022 * So we now need to distinguish between these cases. From reading 2023 * /sys/kern/uipc_socket2.c we can find out that sonewconn() calls 2024 * pru_attach with proto == 0 and td == NULL. For now use this fact 2025 * to figure out if we were called from socket() or from sonewconn(). 2026 */ 2027 2028 if (td != NULL) 2029 mtx_lock(&ng_btsocket_l2cap_sockets_mtx); 2030 else 2031 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED); 2032 2033 /* Set PCB token. Use ng_btsocket_l2cap_sockets_mtx for protection */ 2034 if (++ token == 0) 2035 token ++; 2036 2037 pcb->token = token; 2038 2039 LIST_INSERT_HEAD(&ng_btsocket_l2cap_sockets, pcb, next); 2040 2041 if (td != NULL) 2042 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); 2043 2044 return (0); 2045 } /* ng_btsocket_l2cap_attach */ 2046 2047 /* 2048 * Bind socket 2049 */ 2050 2051 int 2052 ng_btsocket_l2cap_bind(struct socket *so, struct sockaddr *nam, 2053 struct thread *td) 2054 { 2055 ng_btsocket_l2cap_pcb_t *pcb = NULL; 2056 struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *) nam; 2057 int psm, error = 0; 2058 2059 if (ng_btsocket_l2cap_node == NULL) 2060 return (EINVAL); 2061 2062 /* Verify address */ 2063 if (sa == NULL) 2064 return (EINVAL); 2065 if (sa->l2cap_family != AF_BLUETOOTH) 2066 return (EAFNOSUPPORT); 2067 if (sa->l2cap_len != sizeof(*sa)) 2068 return (EINVAL); 2069 2070 psm = le16toh(sa->l2cap_psm); 2071 2072 /* 2073 * Check if other socket has this address already (look for exact 2074 * match PSM and bdaddr) and assign socket address if it's available. 2075 * 2076 * Note: socket can be bound to ANY PSM (zero) thus allowing several 2077 * channels with the same PSM between the same pair of BD_ADDR'es. 2078 */ 2079 2080 mtx_lock(&ng_btsocket_l2cap_sockets_mtx); 2081 2082 LIST_FOREACH(pcb, &ng_btsocket_l2cap_sockets, next) 2083 if (psm != 0 && psm == pcb->psm && 2084 bcmp(&pcb->src, &sa->l2cap_bdaddr, sizeof(bdaddr_t)) == 0) 2085 break; 2086 2087 if (pcb == NULL) { 2088 /* Set socket address */ 2089 pcb = so2l2cap_pcb(so); 2090 if (pcb != NULL) { 2091 bcopy(&sa->l2cap_bdaddr, &pcb->src, sizeof(pcb->src)); 2092 pcb->psm = psm; 2093 } else 2094 error = EINVAL; 2095 } else 2096 error = EADDRINUSE; 2097 2098 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); 2099 2100 return (error); 2101 } /* ng_btsocket_l2cap_bind */ 2102 2103 /* 2104 * Connect socket 2105 */ 2106 2107 int 2108 ng_btsocket_l2cap_connect(struct socket *so, struct sockaddr *nam, 2109 struct thread *td) 2110 { 2111 ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so); 2112 struct sockaddr_l2cap *sa = (struct sockaddr_l2cap *) nam; 2113 ng_btsocket_l2cap_rtentry_t *rt = NULL; 2114 int have_src, error = 0; 2115 2116 /* Check socket */ 2117 if (pcb == NULL) 2118 return (EINVAL); 2119 if (ng_btsocket_l2cap_node == NULL) 2120 return (EINVAL); 2121 if (pcb->state == NG_BTSOCKET_L2CAP_CONNECTING) 2122 return (EINPROGRESS); 2123 2124 /* Verify address */ 2125 if (sa == NULL) 2126 return (EINVAL); 2127 if (sa->l2cap_family != AF_BLUETOOTH) 2128 return (EAFNOSUPPORT); 2129 if (sa->l2cap_len != sizeof(*sa)) 2130 return (EINVAL); 2131 if (sa->l2cap_psm == 0 || 2132 bcmp(&sa->l2cap_bdaddr, NG_HCI_BDADDR_ANY, sizeof(bdaddr_t)) == 0) 2133 return (EDESTADDRREQ); 2134 if (pcb->psm != 0 && pcb->psm != le16toh(sa->l2cap_psm)) 2135 return (EINVAL); 2136 2137 /* 2138 * Routing. Socket should be bound to some source address. The source 2139 * address can be ANY. Destination address must be set and it must not 2140 * be ANY. If source address is ANY then find first rtentry that has 2141 * src != dst. 2142 */ 2143 2144 mtx_lock(&ng_btsocket_l2cap_rt_mtx); 2145 mtx_lock(&ng_btsocket_l2cap_sockets_mtx); 2146 mtx_lock(&pcb->pcb_mtx); 2147 2148 /* Send destination address and PSM */ 2149 bcopy(&sa->l2cap_bdaddr, &pcb->dst, sizeof(pcb->dst)); 2150 pcb->psm = le16toh(sa->l2cap_psm); 2151 2152 pcb->rt = NULL; 2153 have_src = bcmp(&pcb->src, NG_HCI_BDADDR_ANY, sizeof(pcb->src)); 2154 2155 LIST_FOREACH(rt, &ng_btsocket_l2cap_rt, next) { 2156 if (rt->hook == NULL || NG_HOOK_NOT_VALID(rt->hook)) 2157 continue; 2158 2159 /* Match src and dst */ 2160 if (have_src) { 2161 if (bcmp(&pcb->src, &rt->src, sizeof(rt->src)) == 0) 2162 break; 2163 } else { 2164 if (bcmp(&pcb->dst, &rt->src, sizeof(rt->src)) != 0) 2165 break; 2166 } 2167 } 2168 2169 if (rt != NULL) { 2170 pcb->rt = rt; 2171 2172 if (!have_src) 2173 bcopy(&rt->src, &pcb->src, sizeof(pcb->src)); 2174 } else 2175 error = EHOSTUNREACH; 2176 2177 /* 2178 * Send L2CA_Connect request 2179 */ 2180 2181 if (error == 0) { 2182 error = ng_btsocket_l2cap_send_l2ca_con_req(pcb); 2183 if (error == 0) { 2184 pcb->flags |= NG_BTSOCKET_L2CAP_CLIENT; 2185 pcb->state = NG_BTSOCKET_L2CAP_CONNECTING; 2186 soisconnecting(pcb->so); 2187 2188 ng_btsocket_l2cap_timeout(pcb); 2189 } 2190 } 2191 2192 mtx_unlock(&pcb->pcb_mtx); 2193 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); 2194 mtx_unlock(&ng_btsocket_l2cap_rt_mtx); 2195 2196 return (error); 2197 } /* ng_btsocket_l2cap_connect */ 2198 2199 /* 2200 * Process ioctl's calls on socket 2201 */ 2202 2203 int 2204 ng_btsocket_l2cap_control(struct socket *so, u_long cmd, caddr_t data, 2205 struct ifnet *ifp, struct thread *td) 2206 { 2207 return (EINVAL); 2208 } /* ng_btsocket_l2cap_control */ 2209 2210 /* 2211 * Process getsockopt/setsockopt system calls 2212 */ 2213 2214 int 2215 ng_btsocket_l2cap_ctloutput(struct socket *so, struct sockopt *sopt) 2216 { 2217 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so); 2218 int error = 0; 2219 ng_l2cap_cfg_opt_val_t v; 2220 2221 if (pcb == NULL) 2222 return (EINVAL); 2223 if (ng_btsocket_l2cap_node == NULL) 2224 return (EINVAL); 2225 2226 if (sopt->sopt_level != SOL_L2CAP) 2227 return (0); 2228 2229 mtx_lock(&pcb->pcb_mtx); 2230 2231 switch (sopt->sopt_dir) { 2232 case SOPT_GET: 2233 switch (sopt->sopt_name) { 2234 case SO_L2CAP_IMTU: /* get incoming MTU */ 2235 error = sooptcopyout(sopt, &pcb->imtu, 2236 sizeof(pcb->imtu)); 2237 break; 2238 2239 case SO_L2CAP_OMTU: /* get outgoing (peer incoming) MTU */ 2240 error = sooptcopyout(sopt, &pcb->omtu, 2241 sizeof(pcb->omtu)); 2242 break; 2243 2244 case SO_L2CAP_IFLOW: /* get incoming flow spec. */ 2245 error = sooptcopyout(sopt, &pcb->iflow, 2246 sizeof(pcb->iflow)); 2247 break; 2248 2249 case SO_L2CAP_OFLOW: /* get outgoing flow spec. */ 2250 error = sooptcopyout(sopt, &pcb->oflow, 2251 sizeof(pcb->oflow)); 2252 break; 2253 2254 case SO_L2CAP_FLUSH: /* get flush timeout */ 2255 error = sooptcopyout(sopt, &pcb->flush_timo, 2256 sizeof(pcb->flush_timo)); 2257 break; 2258 2259 default: 2260 error = ENOPROTOOPT; 2261 break; 2262 } 2263 break; 2264 2265 case SOPT_SET: 2266 /* 2267 * XXX 2268 * We do not allow to change these parameters while socket is 2269 * connected or we are in the process of creating a connection. 2270 * May be this should indicate re-configuration of the open 2271 * channel? 2272 */ 2273 2274 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) 2275 return (EACCES); 2276 2277 switch (sopt->sopt_name) { 2278 case SO_L2CAP_IMTU: /* set incoming MTU */ 2279 error = sooptcopyin(sopt, &v, sizeof(v), sizeof(v.mtu)); 2280 if (error == 0) 2281 pcb->imtu = v.mtu; 2282 break; 2283 2284 case SO_L2CAP_OFLOW: /* set outgoing flow spec. */ 2285 error = sooptcopyin(sopt, &v, sizeof(v),sizeof(v.flow)); 2286 if (error == 0) 2287 bcopy(&v.flow, &pcb->oflow, sizeof(pcb->oflow)); 2288 break; 2289 2290 case SO_L2CAP_FLUSH: /* set flush timeout */ 2291 error = sooptcopyin(sopt, &v, sizeof(v), 2292 sizeof(v.flush_timo)); 2293 if (error == 0) 2294 pcb->flush_timo = v.flush_timo; 2295 break; 2296 2297 default: 2298 error = ENOPROTOOPT; 2299 break; 2300 } 2301 break; 2302 2303 default: 2304 error = EINVAL; 2305 break; 2306 } 2307 2308 mtx_unlock(&pcb->pcb_mtx); 2309 2310 return (error); 2311 } /* ng_btsocket_l2cap_ctloutput */ 2312 2313 /* 2314 * Detach and destroy socket 2315 */ 2316 2317 int 2318 ng_btsocket_l2cap_detach(struct socket *so) 2319 { 2320 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so); 2321 2322 if (pcb == NULL) 2323 return (EINVAL); 2324 if (ng_btsocket_l2cap_node == NULL) 2325 return (EINVAL); 2326 2327 mtx_lock(&ng_btsocket_l2cap_sockets_mtx); 2328 mtx_lock(&pcb->pcb_mtx); 2329 2330 /* XXX what to do with pending request? */ 2331 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO) 2332 ng_btsocket_l2cap_untimeout(pcb); 2333 2334 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED && 2335 pcb->state != NG_BTSOCKET_L2CAP_DISCONNECTING) 2336 /* Send disconnect request with "zero" token */ 2337 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb); 2338 2339 pcb->state = NG_BTSOCKET_L2CAP_CLOSED; 2340 2341 LIST_REMOVE(pcb, next); 2342 2343 mtx_unlock(&pcb->pcb_mtx); 2344 mtx_unlock(&ng_btsocket_l2cap_sockets_mtx); 2345 2346 mtx_destroy(&pcb->pcb_mtx); 2347 bzero(pcb, sizeof(*pcb)); 2348 FREE(pcb, M_NETGRAPH_BTSOCKET_L2CAP); 2349 2350 soisdisconnected(so); 2351 ACCEPT_LOCK(); 2352 SOCK_LOCK(so); 2353 so->so_pcb = NULL; 2354 sotryfree(so); 2355 2356 return (0); 2357 } /* ng_btsocket_l2cap_detach */ 2358 2359 /* 2360 * Disconnect socket 2361 */ 2362 2363 int 2364 ng_btsocket_l2cap_disconnect(struct socket *so) 2365 { 2366 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so); 2367 int error = 0; 2368 2369 if (pcb == NULL) 2370 return (EINVAL); 2371 if (ng_btsocket_l2cap_node == NULL) 2372 return (EINVAL); 2373 2374 mtx_lock(&pcb->pcb_mtx); 2375 2376 if (pcb->state == NG_BTSOCKET_L2CAP_DISCONNECTING) { 2377 mtx_unlock(&pcb->pcb_mtx); 2378 return (EINPROGRESS); 2379 } 2380 2381 if (pcb->state != NG_BTSOCKET_L2CAP_CLOSED) { 2382 /* XXX FIXME what to do with pending request? */ 2383 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO) 2384 ng_btsocket_l2cap_untimeout(pcb); 2385 2386 error = ng_btsocket_l2cap_send_l2ca_discon_req(pcb->token, pcb); 2387 if (error == 0) { 2388 pcb->state = NG_BTSOCKET_L2CAP_DISCONNECTING; 2389 soisdisconnecting(so); 2390 2391 ng_btsocket_l2cap_timeout(pcb); 2392 } 2393 2394 /* XXX FIXME what to do if error != 0 */ 2395 } 2396 2397 mtx_unlock(&pcb->pcb_mtx); 2398 2399 return (error); 2400 } /* ng_btsocket_l2cap_disconnect */ 2401 2402 /* 2403 * Listen on socket 2404 */ 2405 2406 int 2407 ng_btsocket_l2cap_listen(struct socket *so, struct thread *td) 2408 { 2409 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so); 2410 2411 if (pcb == NULL) 2412 return (EINVAL); 2413 if (ng_btsocket_l2cap_node == NULL) 2414 return (EINVAL); 2415 2416 return ((pcb->psm == 0)? EDESTADDRREQ : 0); 2417 } /* ng_btsocket_listen */ 2418 2419 /* 2420 * Get peer address 2421 */ 2422 2423 int 2424 ng_btsocket_l2cap_peeraddr(struct socket *so, struct sockaddr **nam) 2425 { 2426 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so); 2427 struct sockaddr_l2cap sa; 2428 2429 if (pcb == NULL) 2430 return (EINVAL); 2431 if (ng_btsocket_l2cap_node == NULL) 2432 return (EINVAL); 2433 2434 bcopy(&pcb->dst, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr)); 2435 sa.l2cap_psm = htole16(pcb->psm); 2436 sa.l2cap_len = sizeof(sa); 2437 sa.l2cap_family = AF_BLUETOOTH; 2438 2439 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); 2440 2441 return ((*nam == NULL)? ENOMEM : 0); 2442 } /* ng_btsocket_l2cap_peeraddr */ 2443 2444 /* 2445 * Send data to socket 2446 */ 2447 2448 int 2449 ng_btsocket_l2cap_send(struct socket *so, int flags, struct mbuf *m, 2450 struct sockaddr *nam, struct mbuf *control, struct thread *td) 2451 { 2452 ng_btsocket_l2cap_pcb_t *pcb = so2l2cap_pcb(so); 2453 int error = 0; 2454 2455 if (ng_btsocket_l2cap_node == NULL) { 2456 error = ENETDOWN; 2457 goto drop; 2458 } 2459 2460 /* Check socket and input */ 2461 if (pcb == NULL || m == NULL || control != NULL) { 2462 error = EINVAL; 2463 goto drop; 2464 } 2465 2466 mtx_lock(&pcb->pcb_mtx); 2467 2468 /* Make sure socket is connected */ 2469 if (pcb->state != NG_BTSOCKET_L2CAP_OPEN) { 2470 mtx_unlock(&pcb->pcb_mtx); 2471 error = ENOTCONN; 2472 goto drop; 2473 } 2474 2475 /* Check route */ 2476 if (pcb->rt == NULL || 2477 pcb->rt->hook == NULL || NG_HOOK_NOT_VALID(pcb->rt->hook)) { 2478 mtx_unlock(&pcb->pcb_mtx); 2479 error = ENETDOWN; 2480 goto drop; 2481 } 2482 2483 /* Check packet size agains outgoing (peer's incoming) MTU) */ 2484 if (m->m_pkthdr.len > pcb->omtu) { 2485 NG_BTSOCKET_L2CAP_ERR( 2486 "%s: Packet too big, len=%d, omtu=%d\n", __func__, m->m_pkthdr.len, pcb->omtu); 2487 2488 mtx_unlock(&pcb->pcb_mtx); 2489 error = EMSGSIZE; 2490 goto drop; 2491 } 2492 2493 /* 2494 * First put packet on socket send queue. Then check if we have 2495 * pending timeout. If we do not have timeout then we must send 2496 * packet and schedule timeout. Otherwise do nothing and wait for 2497 * L2CA_WRITE_RSP. 2498 */ 2499 2500 sbappendrecord(&pcb->so->so_snd, m); 2501 m = NULL; 2502 2503 if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) { 2504 error = ng_btsocket_l2cap_send2(pcb); 2505 if (error == 0) 2506 ng_btsocket_l2cap_timeout(pcb); 2507 else 2508 sbdroprecord(&pcb->so->so_snd); /* XXX */ 2509 } 2510 2511 mtx_unlock(&pcb->pcb_mtx); 2512 drop: 2513 NG_FREE_M(m); /* checks for != NULL */ 2514 NG_FREE_M(control); 2515 2516 return (error); 2517 } /* ng_btsocket_l2cap_send */ 2518 2519 /* 2520 * Send first packet in the socket queue to the L2CAP layer 2521 */ 2522 2523 static int 2524 ng_btsocket_l2cap_send2(ng_btsocket_l2cap_pcb_p pcb) 2525 { 2526 struct mbuf *m = NULL; 2527 ng_l2cap_l2ca_hdr_t *hdr = NULL; 2528 int error = 0; 2529 2530 mtx_assert(&pcb->pcb_mtx, MA_OWNED); 2531 2532 if (pcb->so->so_snd.sb_cc == 0) 2533 return (EINVAL); /* XXX */ 2534 2535 m = m_dup(pcb->so->so_snd.sb_mb, M_DONTWAIT); 2536 if (m == NULL) 2537 return (ENOBUFS); 2538 2539 /* Create L2CA packet header */ 2540 M_PREPEND(m, sizeof(*hdr), M_DONTWAIT); 2541 if (m != NULL) 2542 if (m->m_len < sizeof(*hdr)) 2543 m = m_pullup(m, sizeof(*hdr)); 2544 2545 if (m == NULL) { 2546 NG_BTSOCKET_L2CAP_ERR( 2547 "%s: Failed to create L2CA packet header\n", __func__); 2548 2549 return (ENOBUFS); 2550 } 2551 2552 hdr = mtod(m, ng_l2cap_l2ca_hdr_t *); 2553 hdr->token = pcb->token; 2554 hdr->length = m->m_pkthdr.len - sizeof(*hdr); 2555 hdr->lcid = pcb->cid; 2556 2557 NG_BTSOCKET_L2CAP_INFO( 2558 "%s: Sending packet: len=%d, length=%d, lcid=%d, token=%d, state=%d\n", 2559 __func__, m->m_pkthdr.len, hdr->length, hdr->lcid, 2560 hdr->token, pcb->state); 2561 2562 /* 2563 * If we got here than we have successfuly creates new L2CAP 2564 * data packet and now we can send it to the L2CAP layer 2565 */ 2566 2567 NG_SEND_DATA_ONLY(error, pcb->rt->hook, m); 2568 2569 return (error); 2570 } /* ng_btsocket_l2cap_send2 */ 2571 2572 /* 2573 * Get socket address 2574 */ 2575 2576 int 2577 ng_btsocket_l2cap_sockaddr(struct socket *so, struct sockaddr **nam) 2578 { 2579 ng_btsocket_l2cap_pcb_p pcb = so2l2cap_pcb(so); 2580 struct sockaddr_l2cap sa; 2581 2582 if (pcb == NULL) 2583 return (EINVAL); 2584 if (ng_btsocket_l2cap_node == NULL) 2585 return (EINVAL); 2586 2587 bcopy(&pcb->src, &sa.l2cap_bdaddr, sizeof(sa.l2cap_bdaddr)); 2588 sa.l2cap_psm = htole16(pcb->psm); 2589 sa.l2cap_len = sizeof(sa); 2590 sa.l2cap_family = AF_BLUETOOTH; 2591 2592 *nam = sodupsockaddr((struct sockaddr *) &sa, M_NOWAIT); 2593 2594 return ((*nam == NULL)? ENOMEM : 0); 2595 } /* ng_btsocket_l2cap_sockaddr */ 2596 2597 /***************************************************************************** 2598 ***************************************************************************** 2599 ** Misc. functions 2600 ***************************************************************************** 2601 *****************************************************************************/ 2602 2603 /* 2604 * Look for the socket that listens on given PSM and bdaddr. Returns exact or 2605 * close match (if any). Caller must hold ng_btsocket_l2cap_sockets_mtx. 2606 */ 2607 2608 static ng_btsocket_l2cap_pcb_p 2609 ng_btsocket_l2cap_pcb_by_addr(bdaddr_p bdaddr, int psm) 2610 { 2611 ng_btsocket_l2cap_pcb_p p = NULL, p1 = NULL; 2612 2613 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED); 2614 2615 LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) { 2616 if (p->so == NULL || !(p->so->so_options & SO_ACCEPTCONN) || 2617 p->psm != psm) 2618 continue; 2619 2620 if (bcmp(&p->src, bdaddr, sizeof(p->src)) == 0) 2621 break; 2622 2623 if (bcmp(&p->src, NG_HCI_BDADDR_ANY, sizeof(p->src)) == 0) 2624 p1 = p; 2625 } 2626 2627 return ((p != NULL)? p : p1); 2628 } /* ng_btsocket_l2cap_pcb_by_addr */ 2629 2630 /* 2631 * Look for the socket that has given token. 2632 * Caller must hold ng_btsocket_l2cap_sockets_mtx. 2633 */ 2634 2635 static ng_btsocket_l2cap_pcb_p 2636 ng_btsocket_l2cap_pcb_by_token(u_int32_t token) 2637 { 2638 ng_btsocket_l2cap_pcb_p p = NULL; 2639 2640 if (token == 0) 2641 return (NULL); 2642 2643 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED); 2644 2645 LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) 2646 if (p->token == token) 2647 break; 2648 2649 return (p); 2650 } /* ng_btsocket_l2cap_pcb_by_token */ 2651 2652 /* 2653 * Look for the socket that assigned to given source address and channel ID. 2654 * Caller must hold ng_btsocket_l2cap_sockets_mtx 2655 */ 2656 2657 static ng_btsocket_l2cap_pcb_p 2658 ng_btsocket_l2cap_pcb_by_cid(bdaddr_p src, int cid) 2659 { 2660 ng_btsocket_l2cap_pcb_p p = NULL; 2661 2662 mtx_assert(&ng_btsocket_l2cap_sockets_mtx, MA_OWNED); 2663 2664 LIST_FOREACH(p, &ng_btsocket_l2cap_sockets, next) 2665 if (p->cid == cid && bcmp(src, &p->src, sizeof(p->src)) == 0) 2666 break; 2667 2668 return (p); 2669 } /* ng_btsocket_l2cap_pcb_by_cid */ 2670 2671 /* 2672 * Set timeout on socket 2673 */ 2674 2675 static void 2676 ng_btsocket_l2cap_timeout(ng_btsocket_l2cap_pcb_p pcb) 2677 { 2678 mtx_assert(&pcb->pcb_mtx, MA_OWNED); 2679 2680 if (!(pcb->flags & NG_BTSOCKET_L2CAP_TIMO)) { 2681 pcb->flags |= NG_BTSOCKET_L2CAP_TIMO; 2682 pcb->timo = timeout(ng_btsocket_l2cap_process_timeout, pcb, 2683 bluetooth_l2cap_ertx_timeout()); 2684 } else 2685 KASSERT(0, 2686 ("%s: Duplicated socket timeout?!\n", __func__)); 2687 } /* ng_btsocket_l2cap_timeout */ 2688 2689 /* 2690 * Unset timeout on socket 2691 */ 2692 2693 static void 2694 ng_btsocket_l2cap_untimeout(ng_btsocket_l2cap_pcb_p pcb) 2695 { 2696 mtx_assert(&pcb->pcb_mtx, MA_OWNED); 2697 2698 if (pcb->flags & NG_BTSOCKET_L2CAP_TIMO) { 2699 untimeout(ng_btsocket_l2cap_process_timeout, pcb, pcb->timo); 2700 pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO; 2701 } else 2702 KASSERT(0, 2703 ("%s: No socket timeout?!\n", __func__)); 2704 } /* ng_btsocket_l2cap_untimeout */ 2705 2706 /* 2707 * Process timeout on socket 2708 */ 2709 2710 static void 2711 ng_btsocket_l2cap_process_timeout(void *xpcb) 2712 { 2713 ng_btsocket_l2cap_pcb_p pcb = (ng_btsocket_l2cap_pcb_p) xpcb; 2714 struct socket *so = NULL; 2715 2716 mtx_lock(&pcb->pcb_mtx); 2717 2718 pcb->flags &= ~NG_BTSOCKET_L2CAP_TIMO; 2719 pcb->so->so_error = ETIMEDOUT; 2720 2721 switch (pcb->state) { 2722 case NG_BTSOCKET_L2CAP_CONNECTING: 2723 case NG_BTSOCKET_L2CAP_CONFIGURING: 2724 /* Send disconnect request with "zero" token */ 2725 if (pcb->cid != 0) 2726 ng_btsocket_l2cap_send_l2ca_discon_req(0, pcb); 2727 2728 /* ... and close the socket */ 2729 pcb->state = NG_BTSOCKET_L2CAP_CLOSED; 2730 soisdisconnected(pcb->so); 2731 2732 if (pcb->so->so_state & SS_NOFDREF) 2733 so = pcb->so; 2734 break; 2735 2736 case NG_BTSOCKET_L2CAP_OPEN: 2737 /* Send timeout - drop packet and wakeup sender */ 2738 sbdroprecord(&pcb->so->so_snd); 2739 sowwakeup(pcb->so); 2740 break; 2741 2742 case NG_BTSOCKET_L2CAP_DISCONNECTING: 2743 /* Disconnect timeout - disconnect the socket anyway */ 2744 pcb->state = NG_BTSOCKET_L2CAP_CLOSED; 2745 soisdisconnected(pcb->so); 2746 2747 if (pcb->so->so_state & SS_NOFDREF) 2748 so = pcb->so; 2749 break; 2750 2751 default: 2752 NG_BTSOCKET_L2CAP_ERR( 2753 "%s: Invalid socket state=%d\n", __func__, pcb->state); 2754 break; 2755 } 2756 2757 mtx_unlock(&pcb->pcb_mtx); 2758 2759 if (so != NULL) 2760 ng_btsocket_l2cap_detach(so); 2761 } /* ng_btsocket_l2cap_process_timeout */ 2762 2763 /* 2764 * Translate HCI/L2CAP error code into "errno" code 2765 * XXX Note: Some L2CAP and HCI error codes have the same value, but 2766 * different meaning 2767 */ 2768 2769 static int 2770 ng_btsocket_l2cap_result2errno(int result) 2771 { 2772 switch (result) { 2773 case 0x00: /* No error */ 2774 return (0); 2775 2776 case 0x01: /* Unknown HCI command */ 2777 return (ENODEV); 2778 2779 case 0x02: /* No connection */ 2780 return (ENOTCONN); 2781 2782 case 0x03: /* Hardware failure */ 2783 return (EIO); 2784 2785 case 0x04: /* Page timeout */ 2786 return (EHOSTDOWN); 2787 2788 case 0x05: /* Authentication failure */ 2789 case 0x06: /* Key missing */ 2790 case 0x18: /* Pairing not allowed */ 2791 case 0x21: /* Role change not allowed */ 2792 case 0x24: /* LMP PSU not allowed */ 2793 case 0x25: /* Encryption mode not acceptable */ 2794 case 0x26: /* Unit key used */ 2795 return (EACCES); 2796 2797 case 0x07: /* Memory full */ 2798 return (ENOMEM); 2799 2800 case 0x08: /* Connection timeout */ 2801 case 0x10: /* Host timeout */ 2802 case 0x22: /* LMP response timeout */ 2803 case 0xee: /* HCI timeout */ 2804 case 0xeeee: /* L2CAP timeout */ 2805 return (ETIMEDOUT); 2806 2807 case 0x09: /* Max number of connections */ 2808 case 0x0a: /* Max number of SCO connections to a unit */ 2809 return (EMLINK); 2810 2811 case 0x0b: /* ACL connection already exists */ 2812 return (EEXIST); 2813 2814 case 0x0c: /* Command disallowed */ 2815 return (EBUSY); 2816 2817 case 0x0d: /* Host rejected due to limited resources */ 2818 case 0x0e: /* Host rejected due to securiity reasons */ 2819 case 0x0f: /* Host rejected due to remote unit is a personal unit */ 2820 case 0x1b: /* SCO offset rejected */ 2821 case 0x1c: /* SCO interval rejected */ 2822 case 0x1d: /* SCO air mode rejected */ 2823 return (ECONNREFUSED); 2824 2825 case 0x11: /* Unsupported feature or parameter value */ 2826 case 0x19: /* Unknown LMP PDU */ 2827 case 0x1a: /* Unsupported remote feature */ 2828 case 0x20: /* Unsupported LMP parameter value */ 2829 case 0x27: /* QoS is not supported */ 2830 case 0x29: /* Paring with unit key not supported */ 2831 return (EOPNOTSUPP); 2832 2833 case 0x12: /* Invalid HCI command parameter */ 2834 case 0x1e: /* Invalid LMP parameters */ 2835 return (EINVAL); 2836 2837 case 0x13: /* Other end terminated connection: User ended connection */ 2838 case 0x14: /* Other end terminated connection: Low resources */ 2839 case 0x15: /* Other end terminated connection: About to power off */ 2840 return (ECONNRESET); 2841 2842 case 0x16: /* Connection terminated by local host */ 2843 return (ECONNABORTED); 2844 2845 #if 0 /* XXX not yet */ 2846 case 0x17: /* Repeated attempts */ 2847 case 0x1f: /* Unspecified error */ 2848 case 0x23: /* LMP error transaction collision */ 2849 case 0x28: /* Instant passed */ 2850 #endif 2851 } 2852 2853 return (ENOSYS); 2854 } /* ng_btsocket_l2cap_result2errno */ 2855 2856