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