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