1 /* 2 * ng_l2cap_evnt.c 3 */ 4 5 /*- 6 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD 7 * 8 * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 9 * All rights reserved. 10 * 11 * Redistribution and use in source and binary forms, with or without 12 * modification, are permitted provided that the following conditions 13 * are met: 14 * 1. Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * 2. Redistributions in binary form must reproduce the above copyright 17 * notice, this list of conditions and the following disclaimer in the 18 * documentation and/or other materials provided with the distribution. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 24 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 26 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 27 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 29 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 30 * SUCH DAMAGE. 31 * 32 * $Id: ng_l2cap_evnt.c,v 1.5 2003/09/08 19:11:45 max Exp $ 33 * $FreeBSD$ 34 */ 35 36 #include <sys/param.h> 37 #include <sys/systm.h> 38 #include <sys/kernel.h> 39 #include <sys/endian.h> 40 #include <sys/malloc.h> 41 #include <sys/mbuf.h> 42 #include <sys/queue.h> 43 #include <netgraph/ng_message.h> 44 #include <netgraph/netgraph.h> 45 #include <netgraph/bluetooth/include/ng_bluetooth.h> 46 #include <netgraph/bluetooth/include/ng_hci.h> 47 #include <netgraph/bluetooth/include/ng_l2cap.h> 48 #include <netgraph/bluetooth/l2cap/ng_l2cap_var.h> 49 #include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h> 50 #include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h> 51 #include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h> 52 #include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h> 53 #include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h> 54 55 /****************************************************************************** 56 ****************************************************************************** 57 ** L2CAP events processing module 58 ****************************************************************************** 59 ******************************************************************************/ 60 61 static int ng_l2cap_process_signal_cmd (ng_l2cap_con_p); 62 static int ng_l2cap_process_lesignal_cmd (ng_l2cap_con_p); 63 static int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t); 64 static int ng_l2cap_process_cmd_urq (ng_l2cap_con_p, u_int8_t); 65 static int ng_l2cap_process_cmd_urs (ng_l2cap_con_p, u_int8_t); 66 static int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t); 67 static int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t); 68 static int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t); 69 static int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t); 70 static int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t); 71 static int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t); 72 static int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t); 73 static int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t); 74 static int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t); 75 static int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t); 76 static int send_l2cap_reject 77 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t); 78 static int send_l2cap_con_rej 79 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t); 80 static int send_l2cap_cfg_rsp 81 (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *); 82 static int send_l2cap_param_urs 83 (ng_l2cap_con_p , u_int8_t , u_int16_t); 84 85 static int get_next_l2cap_opt 86 (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p); 87 88 /* 89 * Receive L2CAP packet. First get L2CAP header and verify packet. Than 90 * get destination channel and process packet. 91 */ 92 93 int 94 ng_l2cap_receive(ng_l2cap_con_p con) 95 { 96 ng_l2cap_p l2cap = con->l2cap; 97 ng_l2cap_hdr_t *hdr = NULL; 98 int error = 0; 99 100 /* Check packet */ 101 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 102 NG_L2CAP_ERR( 103 "%s: %s - invalid L2CAP packet. Packet too small, len=%d\n", 104 __func__, NG_NODE_NAME(l2cap->node), 105 con->rx_pkt->m_pkthdr.len); 106 error = EMSGSIZE; 107 goto drop; 108 } 109 110 /* Get L2CAP header */ 111 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 112 if (con->rx_pkt == NULL) 113 return (ENOBUFS); 114 115 hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); 116 hdr->length = le16toh(hdr->length); 117 hdr->dcid = le16toh(hdr->dcid); 118 119 /* Check payload size */ 120 if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) { 121 NG_L2CAP_ERR( 122 "%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n", 123 __func__, NG_NODE_NAME(l2cap->node), hdr->length, 124 con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 125 error = EMSGSIZE; 126 goto drop; 127 } 128 129 /* Process packet */ 130 switch (hdr->dcid) { 131 case NG_L2CAP_SIGNAL_CID: /* L2CAP command */ 132 m_adj(con->rx_pkt, sizeof(*hdr)); 133 error = ng_l2cap_process_signal_cmd(con); 134 break; 135 case NG_L2CAP_LESIGNAL_CID: 136 m_adj(con->rx_pkt, sizeof(*hdr)); 137 error = ng_l2cap_process_lesignal_cmd(con); 138 break; 139 case NG_L2CAP_CLT_CID: /* Connectionless packet */ 140 error = ng_l2cap_l2ca_clt_receive(con); 141 break; 142 143 default: /* Data packet */ 144 error = ng_l2cap_l2ca_receive(con); 145 break; 146 } 147 148 return (error); 149 drop: 150 NG_FREE_M(con->rx_pkt); 151 152 return (error); 153 } /* ng_l2cap_receive */ 154 155 /* 156 * Process L2CAP signaling command. We already know that destination channel ID 157 * is 0x1 that means we have received signaling command from peer's L2CAP layer. 158 * So get command header, decode and process it. 159 * 160 * XXX do we need to check signaling MTU here? 161 */ 162 163 static int 164 ng_l2cap_process_signal_cmd(ng_l2cap_con_p con) 165 { 166 ng_l2cap_p l2cap = con->l2cap; 167 ng_l2cap_cmd_hdr_t *hdr = NULL; 168 struct mbuf *m = NULL; 169 170 while (con->rx_pkt != NULL) { 171 /* Verify packet length */ 172 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 173 NG_L2CAP_ERR( 174 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", 175 __func__, NG_NODE_NAME(l2cap->node), 176 con->rx_pkt->m_pkthdr.len); 177 NG_FREE_M(con->rx_pkt); 178 179 return (EMSGSIZE); 180 } 181 182 /* Get signaling command */ 183 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 184 if (con->rx_pkt == NULL) 185 return (ENOBUFS); 186 187 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 188 hdr->length = le16toh(hdr->length); 189 m_adj(con->rx_pkt, sizeof(*hdr)); 190 191 /* Verify command length */ 192 if (con->rx_pkt->m_pkthdr.len < hdr->length) { 193 NG_L2CAP_ERR( 194 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ 195 "Invalid command length=%d, m_pkthdr.len=%d\n", 196 __func__, NG_NODE_NAME(l2cap->node), 197 hdr->code, hdr->ident, hdr->length, 198 con->rx_pkt->m_pkthdr.len); 199 NG_FREE_M(con->rx_pkt); 200 201 return (EMSGSIZE); 202 } 203 204 /* Get the command, save the rest (if any) */ 205 if (con->rx_pkt->m_pkthdr.len > hdr->length) 206 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT); 207 else 208 m = NULL; 209 210 /* Process command */ 211 switch (hdr->code) { 212 case NG_L2CAP_CMD_REJ: 213 ng_l2cap_process_cmd_rej(con, hdr->ident); 214 break; 215 216 case NG_L2CAP_CON_REQ: 217 ng_l2cap_process_con_req(con, hdr->ident); 218 break; 219 220 case NG_L2CAP_CON_RSP: 221 ng_l2cap_process_con_rsp(con, hdr->ident); 222 break; 223 224 case NG_L2CAP_CFG_REQ: 225 ng_l2cap_process_cfg_req(con, hdr->ident); 226 break; 227 228 case NG_L2CAP_CFG_RSP: 229 ng_l2cap_process_cfg_rsp(con, hdr->ident); 230 break; 231 232 case NG_L2CAP_DISCON_REQ: 233 ng_l2cap_process_discon_req(con, hdr->ident); 234 break; 235 236 case NG_L2CAP_DISCON_RSP: 237 ng_l2cap_process_discon_rsp(con, hdr->ident); 238 break; 239 240 case NG_L2CAP_ECHO_REQ: 241 ng_l2cap_process_echo_req(con, hdr->ident); 242 break; 243 244 case NG_L2CAP_ECHO_RSP: 245 ng_l2cap_process_echo_rsp(con, hdr->ident); 246 break; 247 248 case NG_L2CAP_INFO_REQ: 249 ng_l2cap_process_info_req(con, hdr->ident); 250 break; 251 252 case NG_L2CAP_INFO_RSP: 253 ng_l2cap_process_info_rsp(con, hdr->ident); 254 break; 255 256 default: 257 NG_L2CAP_ERR( 258 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 259 __func__, NG_NODE_NAME(l2cap->node), 260 hdr->code, hdr->ident, hdr->length); 261 262 /* 263 * Send L2CAP_CommandRej. Do not really care 264 * about the result 265 */ 266 267 send_l2cap_reject(con, hdr->ident, 268 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 269 NG_FREE_M(con->rx_pkt); 270 break; 271 } 272 273 con->rx_pkt = m; 274 } 275 276 return (0); 277 } /* ng_l2cap_process_signal_cmd */ 278 static int 279 ng_l2cap_process_lesignal_cmd(ng_l2cap_con_p con) 280 { 281 ng_l2cap_p l2cap = con->l2cap; 282 ng_l2cap_cmd_hdr_t *hdr = NULL; 283 struct mbuf *m = NULL; 284 285 while (con->rx_pkt != NULL) { 286 /* Verify packet length */ 287 if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 288 NG_L2CAP_ERR( 289 "%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", 290 __func__, NG_NODE_NAME(l2cap->node), 291 con->rx_pkt->m_pkthdr.len); 292 NG_FREE_M(con->rx_pkt); 293 294 return (EMSGSIZE); 295 } 296 297 /* Get signaling command */ 298 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 299 if (con->rx_pkt == NULL) 300 return (ENOBUFS); 301 302 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 303 hdr->length = le16toh(hdr->length); 304 m_adj(con->rx_pkt, sizeof(*hdr)); 305 306 /* Verify command length */ 307 if (con->rx_pkt->m_pkthdr.len < hdr->length) { 308 NG_L2CAP_ERR( 309 "%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ 310 "Invalid command length=%d, m_pkthdr.len=%d\n", 311 __func__, NG_NODE_NAME(l2cap->node), 312 hdr->code, hdr->ident, hdr->length, 313 con->rx_pkt->m_pkthdr.len); 314 NG_FREE_M(con->rx_pkt); 315 316 return (EMSGSIZE); 317 } 318 319 /* Get the command, save the rest (if any) */ 320 if (con->rx_pkt->m_pkthdr.len > hdr->length) 321 m = m_split(con->rx_pkt, hdr->length, M_NOWAIT); 322 else 323 m = NULL; 324 325 /* Process command */ 326 switch (hdr->code) { 327 case NG_L2CAP_CMD_REJ: 328 ng_l2cap_process_cmd_rej(con, hdr->ident); 329 break; 330 case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST: 331 ng_l2cap_process_cmd_urq(con, hdr->ident); 332 break; 333 case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE: 334 ng_l2cap_process_cmd_urs(con, hdr->ident); 335 break; 336 337 338 default: 339 NG_L2CAP_ERR( 340 "%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 341 __func__, NG_NODE_NAME(l2cap->node), 342 hdr->code, hdr->ident, hdr->length); 343 344 /* 345 * Send L2CAP_CommandRej. Do not really care 346 * about the result 347 */ 348 349 send_l2cap_reject(con, hdr->ident, 350 NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 351 NG_FREE_M(con->rx_pkt); 352 break; 353 } 354 355 con->rx_pkt = m; 356 } 357 358 return (0); 359 } /* ng_l2cap_process_signal_cmd */ 360 /*Update Paramater Request*/ 361 static int ng_l2cap_process_cmd_urq(ng_l2cap_con_p con, uint8_t ident) 362 { 363 /* We do not implement parameter negotiation for now. */ 364 send_l2cap_param_urs(con, ident, NG_L2CAP_UPDATE_PARAM_ACCEPT); 365 NG_FREE_M(con->rx_pkt); 366 return 0; 367 } 368 369 static int ng_l2cap_process_cmd_urs(ng_l2cap_con_p con, uint8_t ident) 370 { 371 /* We only support master side yet .*/ 372 //send_l2cap_reject(con,ident ... ); 373 374 NG_FREE_M(con->rx_pkt); 375 return 0; 376 } 377 378 /* 379 * Process L2CAP_CommandRej command 380 */ 381 382 static int 383 ng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident) 384 { 385 ng_l2cap_p l2cap = con->l2cap; 386 ng_l2cap_cmd_rej_cp *cp = NULL; 387 ng_l2cap_cmd_p cmd = NULL; 388 389 /* Get command parameters */ 390 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 391 if (con->rx_pkt == NULL) 392 return (ENOBUFS); 393 394 cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *); 395 cp->reason = le16toh(cp->reason); 396 397 /* Check if we have pending command descriptor */ 398 cmd = ng_l2cap_cmd_by_ident(con, ident); 399 if (cmd != NULL) { 400 /* If command timeout already happened then ignore reject */ 401 if (ng_l2cap_command_untimeout(cmd) != 0) { 402 NG_FREE_M(con->rx_pkt); 403 return (ETIMEDOUT); 404 } 405 406 ng_l2cap_unlink_cmd(cmd); 407 408 switch (cmd->code) { 409 case NG_L2CAP_CON_REQ: 410 ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0); 411 ng_l2cap_free_chan(cmd->ch); 412 break; 413 414 case NG_L2CAP_CFG_REQ: 415 ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason); 416 break; 417 418 case NG_L2CAP_DISCON_REQ: 419 ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason); 420 ng_l2cap_free_chan(cmd->ch); /* XXX free channel */ 421 break; 422 423 case NG_L2CAP_ECHO_REQ: 424 ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 425 cp->reason, NULL); 426 break; 427 428 case NG_L2CAP_INFO_REQ: 429 ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 430 cp->reason, NULL); 431 break; 432 433 default: 434 NG_L2CAP_ALERT( 435 "%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", 436 __func__, NG_NODE_NAME(l2cap->node), cmd->code); 437 break; 438 } 439 440 ng_l2cap_free_cmd(cmd); 441 } else 442 NG_L2CAP_ERR( 443 "%s: %s - unexpected L2CAP_CommandRej command. " \ 444 "Requested ident does not exist, ident=%d\n", 445 __func__, NG_NODE_NAME(l2cap->node), ident); 446 447 NG_FREE_M(con->rx_pkt); 448 449 return (0); 450 } /* ng_l2cap_process_cmd_rej */ 451 452 /* 453 * Process L2CAP_ConnectReq command 454 */ 455 456 static int 457 ng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident) 458 { 459 ng_l2cap_p l2cap = con->l2cap; 460 struct mbuf *m = con->rx_pkt; 461 ng_l2cap_con_req_cp *cp = NULL; 462 ng_l2cap_chan_p ch = NULL; 463 int error = 0; 464 u_int16_t dcid, psm; 465 int idtype; 466 467 /* Get command parameters */ 468 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 469 if (m == NULL) 470 return (ENOBUFS); 471 472 cp = mtod(m, ng_l2cap_con_req_cp *); 473 psm = le16toh(cp->psm); 474 dcid = le16toh(cp->scid); 475 476 NG_FREE_M(m); 477 con->rx_pkt = NULL; 478 if(dcid == NG_L2CAP_ATT_CID) 479 idtype = NG_L2CAP_L2CA_IDTYPE_ATT; 480 else if(dcid == NG_L2CAP_SMP_CID) 481 idtype = NG_L2CAP_L2CA_IDTYPE_SMP; 482 else if( con->linktype != NG_HCI_LINK_ACL) 483 idtype = NG_L2CAP_L2CA_IDTYPE_LE; 484 else 485 idtype = NG_L2CAP_L2CA_IDTYPE_BREDR; 486 487 /* 488 * Create new channel and send L2CA_ConnectInd notification 489 * to the upper layer protocol. 490 */ 491 492 ch = ng_l2cap_new_chan(l2cap, con, psm, idtype); 493 494 if (ch == NULL) 495 return (send_l2cap_con_rej(con, ident, 0, dcid, 496 NG_L2CAP_NO_RESOURCES)); 497 498 /* Update channel IDs */ 499 ch->dcid = dcid; 500 501 /* Sent L2CA_ConnectInd notification to the upper layer */ 502 ch->ident = ident; 503 ch->state = NG_L2CAP_W4_L2CA_CON_RSP; 504 505 error = ng_l2cap_l2ca_con_ind(ch); 506 if (error != 0) { 507 send_l2cap_con_rej(con, ident, ch->scid, dcid, 508 (error == ENOMEM)? NG_L2CAP_NO_RESOURCES : 509 NG_L2CAP_PSM_NOT_SUPPORTED); 510 ng_l2cap_free_chan(ch); 511 } 512 513 return (error); 514 } /* ng_l2cap_process_con_req */ 515 516 /* 517 * Process L2CAP_ConnectRsp command 518 */ 519 520 static int 521 ng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident) 522 { 523 ng_l2cap_p l2cap = con->l2cap; 524 struct mbuf *m = con->rx_pkt; 525 ng_l2cap_con_rsp_cp *cp = NULL; 526 ng_l2cap_cmd_p cmd = NULL; 527 u_int16_t scid, dcid, result, status; 528 int error = 0; 529 530 /* Get command parameters */ 531 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 532 if (m == NULL) 533 return (ENOBUFS); 534 535 cp = mtod(m, ng_l2cap_con_rsp_cp *); 536 dcid = le16toh(cp->dcid); 537 scid = le16toh(cp->scid); 538 result = le16toh(cp->result); 539 status = le16toh(cp->status); 540 541 NG_FREE_M(m); 542 con->rx_pkt = NULL; 543 544 /* Check if we have pending command descriptor */ 545 cmd = ng_l2cap_cmd_by_ident(con, ident); 546 if (cmd == NULL) { 547 NG_L2CAP_ERR( 548 "%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", 549 __func__, NG_NODE_NAME(l2cap->node), ident, 550 con->con_handle); 551 552 return (ENOENT); 553 } 554 555 /* Verify channel state, if invalid - do nothing */ 556 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) { 557 NG_L2CAP_ERR( 558 "%s: %s - unexpected L2CAP_ConnectRsp. " \ 559 "Invalid channel state, cid=%d, state=%d\n", 560 __func__, NG_NODE_NAME(l2cap->node), scid, 561 cmd->ch->state); 562 goto reject; 563 } 564 565 /* Verify CIDs and send reject if does not match */ 566 if (cmd->ch->scid != scid) { 567 NG_L2CAP_ERR( 568 "%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", 569 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 570 scid); 571 goto reject; 572 } 573 574 /* 575 * Looks good. We got confirmation from our peer. Now process 576 * it. First disable RTX timer. Then check the result and send 577 * notification to the upper layer. If command timeout already 578 * happened then ignore response. 579 */ 580 581 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 582 return (error); 583 584 if (result == NG_L2CAP_PENDING) { 585 /* 586 * Our peer wants more time to complete connection. We shall 587 * start ERTX timer and wait. Keep command in the list. 588 */ 589 590 cmd->ch->dcid = dcid; 591 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout()); 592 593 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 594 result, status); 595 if (error != 0) 596 ng_l2cap_free_chan(cmd->ch); 597 } else { 598 ng_l2cap_unlink_cmd(cmd); 599 600 if (result == NG_L2CAP_SUCCESS) { 601 /* 602 * Channel is open. Complete command and move to CONFIG 603 * state. Since we have sent positive confirmation we 604 * expect to receive L2CA_Config request from the upper 605 * layer protocol. 606 */ 607 608 cmd->ch->dcid = dcid; 609 cmd->ch->state = ((cmd->ch->scid == NG_L2CAP_ATT_CID)|| 610 (cmd->ch->scid == NG_L2CAP_SMP_CID)) 611 ? 612 NG_L2CAP_OPEN : NG_L2CAP_CONFIG; 613 } else 614 /* There was an error, so close the channel */ 615 NG_L2CAP_INFO( 616 "%s: %s - failed to open L2CAP channel, result=%d, status=%d\n", 617 __func__, NG_NODE_NAME(l2cap->node), result, 618 status); 619 620 error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 621 result, status); 622 623 /* XXX do we have to remove the channel on error? */ 624 if (error != 0 || result != NG_L2CAP_SUCCESS) 625 ng_l2cap_free_chan(cmd->ch); 626 627 ng_l2cap_free_cmd(cmd); 628 } 629 630 return (error); 631 632 reject: 633 /* Send reject. Do not really care about the result */ 634 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 635 636 return (0); 637 } /* ng_l2cap_process_con_rsp */ 638 639 /* 640 * Process L2CAP_ConfigReq command 641 */ 642 643 static int 644 ng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident) 645 { 646 ng_l2cap_p l2cap = con->l2cap; 647 struct mbuf *m = con->rx_pkt; 648 ng_l2cap_cfg_req_cp *cp = NULL; 649 ng_l2cap_chan_p ch = NULL; 650 u_int16_t dcid, respond, result; 651 ng_l2cap_cfg_opt_t hdr; 652 ng_l2cap_cfg_opt_val_t val; 653 int off, error = 0; 654 655 /* Get command parameters */ 656 con->rx_pkt = NULL; 657 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 658 if (m == NULL) 659 return (ENOBUFS); 660 661 cp = mtod(m, ng_l2cap_cfg_req_cp *); 662 dcid = le16toh(cp->dcid); 663 respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 664 m_adj(m, sizeof(*cp)); 665 666 /* Check if we have this channel and it is in valid state */ 667 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR); 668 if (ch == NULL) { 669 NG_L2CAP_ERR( 670 "%s: %s - unexpected L2CAP_ConfigReq command. " \ 671 "Channel does not exist, cid=%d\n", 672 __func__, NG_NODE_NAME(l2cap->node), dcid); 673 goto reject; 674 } 675 676 /* Verify channel state */ 677 if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) { 678 NG_L2CAP_ERR( 679 "%s: %s - unexpected L2CAP_ConfigReq. " \ 680 "Invalid channel state, cid=%d, state=%d\n", 681 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 682 goto reject; 683 } 684 685 if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */ 686 ch->cfg_state = 0; 687 ch->state = NG_L2CAP_CONFIG; 688 } 689 690 for (result = 0, off = 0; ; ) { 691 error = get_next_l2cap_opt(m, &off, &hdr, &val); 692 if (error == 0) { /* We done with this packet */ 693 NG_FREE_M(m); 694 break; 695 } else if (error > 0) { /* Got option */ 696 switch (hdr.type) { 697 case NG_L2CAP_OPT_MTU: 698 ch->omtu = val.mtu; 699 break; 700 701 case NG_L2CAP_OPT_FLUSH_TIMO: 702 ch->flush_timo = val.flush_timo; 703 break; 704 705 case NG_L2CAP_OPT_QOS: 706 bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow)); 707 break; 708 709 default: /* Ignore unknown hint option */ 710 break; 711 } 712 } else { /* Oops, something is wrong */ 713 respond = 1; 714 715 if (error == -3) { 716 717 /* 718 * Adjust mbuf so we can get to the start 719 * of the first option we did not like. 720 */ 721 722 m_adj(m, off - sizeof(hdr)); 723 m->m_pkthdr.len = sizeof(hdr) + hdr.length; 724 725 result = NG_L2CAP_UNKNOWN_OPTION; 726 } else { 727 /* XXX FIXME Send other reject codes? */ 728 NG_FREE_M(m); 729 result = NG_L2CAP_REJECT; 730 } 731 732 break; 733 } 734 } 735 736 /* 737 * Now check and see if we have to respond. If everything was OK then 738 * respond contain "C flag" and (if set) we will respond with empty 739 * packet and will wait for more options. 740 * 741 * Other case is that we did not like peer's options and will respond 742 * with L2CAP_Config response command with Reject error code. 743 * 744 * When "respond == 0" than we have received all options and we will 745 * sent L2CA_ConfigInd event to the upper layer protocol. 746 */ 747 748 if (respond) { 749 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); 750 if (error != 0) { 751 ng_l2cap_l2ca_discon_ind(ch); 752 ng_l2cap_free_chan(ch); 753 } 754 } else { 755 /* Send L2CA_ConfigInd event to the upper layer protocol */ 756 ch->ident = ident; 757 error = ng_l2cap_l2ca_cfg_ind(ch); 758 if (error != 0) 759 ng_l2cap_free_chan(ch); 760 } 761 762 return (error); 763 764 reject: 765 /* Send reject. Do not really care about the result */ 766 NG_FREE_M(m); 767 768 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); 769 770 return (0); 771 } /* ng_l2cap_process_cfg_req */ 772 773 /* 774 * Process L2CAP_ConfigRsp command 775 */ 776 777 static int 778 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) 779 { 780 ng_l2cap_p l2cap = con->l2cap; 781 struct mbuf *m = con->rx_pkt; 782 ng_l2cap_cfg_rsp_cp *cp = NULL; 783 ng_l2cap_cmd_p cmd = NULL; 784 u_int16_t scid, cflag, result; 785 ng_l2cap_cfg_opt_t hdr; 786 ng_l2cap_cfg_opt_val_t val; 787 int off, error = 0; 788 789 /* Get command parameters */ 790 con->rx_pkt = NULL; 791 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 792 if (m == NULL) 793 return (ENOBUFS); 794 795 cp = mtod(m, ng_l2cap_cfg_rsp_cp *); 796 scid = le16toh(cp->scid); 797 cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 798 result = le16toh(cp->result); 799 m_adj(m, sizeof(*cp)); 800 801 /* Check if we have this command */ 802 cmd = ng_l2cap_cmd_by_ident(con, ident); 803 if (cmd == NULL) { 804 NG_L2CAP_ERR( 805 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 806 __func__, NG_NODE_NAME(l2cap->node), ident, 807 con->con_handle); 808 NG_FREE_M(m); 809 810 return (ENOENT); 811 } 812 813 /* Verify CIDs and send reject if does not match */ 814 if (cmd->ch->scid != scid) { 815 NG_L2CAP_ERR( 816 "%s: %s - unexpected L2CAP_ConfigRsp. " \ 817 "Channel ID does not match, scid=%d(%d)\n", 818 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 819 scid); 820 goto reject; 821 } 822 823 /* Verify channel state and reject if invalid */ 824 if (cmd->ch->state != NG_L2CAP_CONFIG) { 825 NG_L2CAP_ERR( 826 "%s: %s - unexpected L2CAP_ConfigRsp. " \ 827 "Invalid channel state, scid=%d, state=%d\n", 828 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 829 cmd->ch->state); 830 goto reject; 831 } 832 833 /* 834 * Looks like it is our response, so process it. First parse options, 835 * then verify C flag. If it is set then we shall expect more 836 * configuration options from the peer and we will wait. Otherwise we 837 * have received all options and we will send L2CA_ConfigRsp event to 838 * the upper layer protocol. If command timeout already happened then 839 * ignore response. 840 */ 841 842 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 843 NG_FREE_M(m); 844 return (error); 845 } 846 847 for (off = 0; ; ) { 848 error = get_next_l2cap_opt(m, &off, &hdr, &val); 849 if (error == 0) /* We done with this packet */ 850 break; 851 else if (error > 0) { /* Got option */ 852 switch (hdr.type) { 853 case NG_L2CAP_OPT_MTU: 854 cmd->ch->imtu = val.mtu; 855 break; 856 857 case NG_L2CAP_OPT_FLUSH_TIMO: 858 cmd->ch->flush_timo = val.flush_timo; 859 break; 860 861 case NG_L2CAP_OPT_QOS: 862 bcopy(&val.flow, &cmd->ch->oflow, 863 sizeof(cmd->ch->oflow)); 864 break; 865 866 default: /* Ignore unknown hint option */ 867 break; 868 } 869 } else { 870 /* 871 * XXX FIXME What to do here? 872 * 873 * This is really BAD :( options packet was broken, or 874 * peer sent us option that we did not understand. Let 875 * upper layer know and do not wait for more options. 876 */ 877 878 NG_L2CAP_ALERT( 879 "%s: %s - failed to parse configuration options, error=%d\n", 880 __func__, NG_NODE_NAME(l2cap->node), error); 881 882 result = NG_L2CAP_UNKNOWN; 883 cflag = 0; 884 885 break; 886 } 887 } 888 889 NG_FREE_M(m); 890 891 if (cflag) /* Restart timer and wait for more options */ 892 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); 893 else { 894 ng_l2cap_unlink_cmd(cmd); 895 896 /* Send L2CA_Config response to the upper layer protocol */ 897 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); 898 if (error != 0) { 899 /* 900 * XXX FIXME what to do here? we were not able to send 901 * response to the upper layer protocol, so for now 902 * just close the channel. Send L2CAP_Disconnect to 903 * remote peer? 904 */ 905 906 NG_L2CAP_ERR( 907 "%s: %s - failed to send L2CA_Config response, error=%d\n", 908 __func__, NG_NODE_NAME(l2cap->node), error); 909 910 ng_l2cap_free_chan(cmd->ch); 911 } 912 913 ng_l2cap_free_cmd(cmd); 914 } 915 916 return (error); 917 918 reject: 919 /* Send reject. Do not really care about the result */ 920 NG_FREE_M(m); 921 922 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); 923 924 return (0); 925 } /* ng_l2cap_process_cfg_rsp */ 926 927 /* 928 * Process L2CAP_DisconnectReq command 929 */ 930 931 static int 932 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) 933 { 934 ng_l2cap_p l2cap = con->l2cap; 935 ng_l2cap_discon_req_cp *cp = NULL; 936 ng_l2cap_chan_p ch = NULL; 937 ng_l2cap_cmd_p cmd = NULL; 938 u_int16_t scid, dcid; 939 940 /* Get command parameters */ 941 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 942 if (con->rx_pkt == NULL) 943 return (ENOBUFS); 944 945 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); 946 dcid = le16toh(cp->dcid); 947 scid = le16toh(cp->scid); 948 949 NG_FREE_M(con->rx_pkt); 950 951 /* Check if we have this channel and it is in valid state */ 952 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR); 953 if (ch == NULL) { 954 NG_L2CAP_ERR( 955 "%s: %s - unexpected L2CAP_DisconnectReq message. " \ 956 "Channel does not exist, cid=%d\n", 957 __func__, NG_NODE_NAME(l2cap->node), dcid); 958 goto reject; 959 } 960 961 /* XXX Verify channel state and reject if invalid -- is that true? */ 962 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG && 963 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 964 NG_L2CAP_ERR( 965 "%s: %s - unexpected L2CAP_DisconnectReq. " \ 966 "Invalid channel state, cid=%d, state=%d\n", 967 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 968 goto reject; 969 } 970 971 /* Match destination channel ID */ 972 if (ch->dcid != scid || ch->scid != dcid) { 973 NG_L2CAP_ERR( 974 "%s: %s - unexpected L2CAP_DisconnectReq. " \ 975 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 976 "request: scid=%d, dcid=%d\n", 977 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 978 scid, dcid); 979 goto reject; 980 } 981 982 /* 983 * Looks good, so notify upper layer protocol that channel is about 984 * to be disconnected and send L2CA_DisconnectInd message. Then respond 985 * with L2CAP_DisconnectRsp. 986 */ 987 988 if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 989 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 990 ng_l2cap_free_chan(ch); 991 } 992 993 /* Send L2CAP_DisconnectRsp */ 994 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 995 if (cmd == NULL) 996 return (ENOMEM); 997 998 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 999 if (cmd->aux == NULL) { 1000 ng_l2cap_free_cmd(cmd); 1001 1002 return (ENOBUFS); 1003 } 1004 1005 /* Link command to the queue */ 1006 ng_l2cap_link_cmd(con, cmd); 1007 ng_l2cap_lp_deliver(con); 1008 1009 return (0); 1010 1011 reject: 1012 /* Send reject. Do not really care about the result */ 1013 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 1014 1015 return (0); 1016 } /* ng_l2cap_process_discon_req */ 1017 1018 /* 1019 * Process L2CAP_DisconnectRsp command 1020 */ 1021 1022 static int 1023 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 1024 { 1025 ng_l2cap_p l2cap = con->l2cap; 1026 ng_l2cap_discon_rsp_cp *cp = NULL; 1027 ng_l2cap_cmd_p cmd = NULL; 1028 u_int16_t scid, dcid; 1029 int error = 0; 1030 1031 /* Get command parameters */ 1032 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1033 if (con->rx_pkt == NULL) 1034 return (ENOBUFS); 1035 1036 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 1037 dcid = le16toh(cp->dcid); 1038 scid = le16toh(cp->scid); 1039 1040 NG_FREE_M(con->rx_pkt); 1041 1042 /* Check if we have pending command descriptor */ 1043 cmd = ng_l2cap_cmd_by_ident(con, ident); 1044 if (cmd == NULL) { 1045 NG_L2CAP_ERR( 1046 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 1047 __func__, NG_NODE_NAME(l2cap->node), ident, 1048 con->con_handle); 1049 goto out; 1050 } 1051 1052 /* Verify channel state, do nothing if invalid */ 1053 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 1054 NG_L2CAP_ERR( 1055 "%s: %s - unexpected L2CAP_DisconnectRsp. " \ 1056 "Invalid channel state, cid=%d, state=%d\n", 1057 __func__, NG_NODE_NAME(l2cap->node), scid, 1058 cmd->ch->state); 1059 goto out; 1060 } 1061 1062 /* Verify CIDs and send reject if does not match */ 1063 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 1064 NG_L2CAP_ERR( 1065 "%s: %s - unexpected L2CAP_DisconnectRsp. " \ 1066 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 1067 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 1068 scid, cmd->ch->dcid, dcid); 1069 goto out; 1070 } 1071 1072 /* 1073 * Looks like we have successfully disconnected channel, so notify 1074 * upper layer. If command timeout already happened then ignore 1075 * response. 1076 */ 1077 1078 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 1079 goto out; 1080 1081 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 1082 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 1083 out: 1084 return (error); 1085 } /* ng_l2cap_process_discon_rsp */ 1086 1087 /* 1088 * Process L2CAP_EchoReq command 1089 */ 1090 1091 static int 1092 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 1093 { 1094 ng_l2cap_p l2cap = con->l2cap; 1095 ng_l2cap_cmd_hdr_t *hdr = NULL; 1096 ng_l2cap_cmd_p cmd = NULL; 1097 1098 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 1099 if (con->rx_pkt == NULL) { 1100 NG_L2CAP_ALERT( 1101 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n", 1102 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 1103 1104 return (ENOBUFS); 1105 } 1106 1107 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 1108 hdr->code = NG_L2CAP_ECHO_RSP; 1109 hdr->ident = ident; 1110 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 1111 1112 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 1113 if (cmd == NULL) { 1114 NG_FREE_M(con->rx_pkt); 1115 1116 return (ENOBUFS); 1117 } 1118 1119 /* Attach data and link command to the queue */ 1120 cmd->aux = con->rx_pkt; 1121 con->rx_pkt = NULL; 1122 ng_l2cap_link_cmd(con, cmd); 1123 ng_l2cap_lp_deliver(con); 1124 1125 return (0); 1126 } /* ng_l2cap_process_echo_req */ 1127 1128 /* 1129 * Process L2CAP_EchoRsp command 1130 */ 1131 1132 static int 1133 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1134 { 1135 ng_l2cap_p l2cap = con->l2cap; 1136 ng_l2cap_cmd_p cmd = NULL; 1137 int error = 0; 1138 1139 /* Check if we have this command */ 1140 cmd = ng_l2cap_cmd_by_ident(con, ident); 1141 if (cmd != NULL) { 1142 /* If command timeout already happened then ignore response */ 1143 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1144 NG_FREE_M(con->rx_pkt); 1145 return (error); 1146 } 1147 1148 ng_l2cap_unlink_cmd(cmd); 1149 1150 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1151 NG_L2CAP_SUCCESS, con->rx_pkt); 1152 1153 ng_l2cap_free_cmd(cmd); 1154 con->rx_pkt = NULL; 1155 } else { 1156 NG_L2CAP_ERR( 1157 "%s: %s - unexpected L2CAP_EchoRsp command. " \ 1158 "Requested ident does not exist, ident=%d\n", 1159 __func__, NG_NODE_NAME(l2cap->node), ident); 1160 NG_FREE_M(con->rx_pkt); 1161 } 1162 1163 return (error); 1164 } /* ng_l2cap_process_echo_rsp */ 1165 1166 /* 1167 * Process L2CAP_InfoReq command 1168 */ 1169 1170 static int 1171 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1172 { 1173 ng_l2cap_p l2cap = con->l2cap; 1174 ng_l2cap_cmd_p cmd = NULL; 1175 u_int16_t type; 1176 1177 /* Get command parameters */ 1178 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1179 if (con->rx_pkt == NULL) 1180 return (ENOBUFS); 1181 1182 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1183 NG_FREE_M(con->rx_pkt); 1184 1185 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1186 if (cmd == NULL) 1187 return (ENOMEM); 1188 1189 switch (type) { 1190 case NG_L2CAP_CONNLESS_MTU: 1191 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1192 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1193 break; 1194 1195 default: 1196 _ng_l2cap_info_rsp(cmd->aux, ident, type, 1197 NG_L2CAP_NOT_SUPPORTED, 0); 1198 break; 1199 } 1200 1201 if (cmd->aux == NULL) { 1202 ng_l2cap_free_cmd(cmd); 1203 1204 return (ENOBUFS); 1205 } 1206 1207 /* Link command to the queue */ 1208 ng_l2cap_link_cmd(con, cmd); 1209 ng_l2cap_lp_deliver(con); 1210 1211 return (0); 1212 } /* ng_l2cap_process_info_req */ 1213 1214 /* 1215 * Process L2CAP_InfoRsp command 1216 */ 1217 1218 static int 1219 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1220 { 1221 ng_l2cap_p l2cap = con->l2cap; 1222 ng_l2cap_info_rsp_cp *cp = NULL; 1223 ng_l2cap_cmd_p cmd = NULL; 1224 int error = 0; 1225 1226 /* Get command parameters */ 1227 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1228 if (con->rx_pkt == NULL) 1229 return (ENOBUFS); 1230 1231 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1232 cp->type = le16toh(cp->type); 1233 cp->result = le16toh(cp->result); 1234 m_adj(con->rx_pkt, sizeof(*cp)); 1235 1236 /* Check if we have pending command descriptor */ 1237 cmd = ng_l2cap_cmd_by_ident(con, ident); 1238 if (cmd == NULL) { 1239 NG_L2CAP_ERR( 1240 "%s: %s - unexpected L2CAP_InfoRsp command. " \ 1241 "Requested ident does not exist, ident=%d\n", 1242 __func__, NG_NODE_NAME(l2cap->node), ident); 1243 NG_FREE_M(con->rx_pkt); 1244 1245 return (ENOENT); 1246 } 1247 1248 /* If command timeout already happened then ignore response */ 1249 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1250 NG_FREE_M(con->rx_pkt); 1251 return (error); 1252 } 1253 1254 ng_l2cap_unlink_cmd(cmd); 1255 1256 if (cp->result == NG_L2CAP_SUCCESS) { 1257 switch (cp->type) { 1258 case NG_L2CAP_CONNLESS_MTU: 1259 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1260 *mtod(con->rx_pkt, u_int16_t *) = 1261 le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1262 else { 1263 cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1264 1265 NG_L2CAP_ERR( 1266 "%s: %s - invalid L2CAP_InfoRsp command. " \ 1267 "Bad connectionless MTU parameter, len=%d\n", 1268 __func__, NG_NODE_NAME(l2cap->node), 1269 con->rx_pkt->m_pkthdr.len); 1270 } 1271 break; 1272 1273 default: 1274 NG_L2CAP_WARN( 1275 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1276 __func__, NG_NODE_NAME(l2cap->node), cp->type); 1277 break; 1278 } 1279 } 1280 1281 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1282 cp->result, con->rx_pkt); 1283 1284 ng_l2cap_free_cmd(cmd); 1285 con->rx_pkt = NULL; 1286 1287 return (error); 1288 } /* ng_l2cap_process_info_rsp */ 1289 1290 /* 1291 * Send L2CAP reject 1292 */ 1293 1294 static int 1295 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1296 u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1297 { 1298 ng_l2cap_cmd_p cmd = NULL; 1299 1300 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1301 if (cmd == NULL) 1302 return (ENOMEM); 1303 1304 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1305 if (cmd->aux == NULL) { 1306 ng_l2cap_free_cmd(cmd); 1307 1308 return (ENOBUFS); 1309 } 1310 1311 /* Link command to the queue */ 1312 ng_l2cap_link_cmd(con, cmd); 1313 ng_l2cap_lp_deliver(con); 1314 1315 return (0); 1316 } /* send_l2cap_reject */ 1317 1318 /* 1319 * Send L2CAP connection reject 1320 */ 1321 1322 static int 1323 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1324 u_int16_t dcid, u_int16_t result) 1325 { 1326 ng_l2cap_cmd_p cmd = NULL; 1327 1328 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1329 if (cmd == NULL) 1330 return (ENOMEM); 1331 1332 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1333 if (cmd->aux == NULL) { 1334 ng_l2cap_free_cmd(cmd); 1335 1336 return (ENOBUFS); 1337 } 1338 1339 /* Link command to the queue */ 1340 ng_l2cap_link_cmd(con, cmd); 1341 ng_l2cap_lp_deliver(con); 1342 1343 return (0); 1344 } /* send_l2cap_con_rej */ 1345 1346 /* 1347 * Send L2CAP config response 1348 */ 1349 1350 static int 1351 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1352 u_int16_t result, struct mbuf *opt) 1353 { 1354 ng_l2cap_cmd_p cmd = NULL; 1355 1356 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1357 if (cmd == NULL) { 1358 NG_FREE_M(opt); 1359 1360 return (ENOMEM); 1361 } 1362 1363 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1364 if (cmd->aux == NULL) { 1365 ng_l2cap_free_cmd(cmd); 1366 1367 return (ENOBUFS); 1368 } 1369 1370 /* Link command to the queue */ 1371 ng_l2cap_link_cmd(con, cmd); 1372 ng_l2cap_lp_deliver(con); 1373 1374 return (0); 1375 } /* send_l2cap_cfg_rsp */ 1376 1377 static int 1378 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident, 1379 u_int16_t result) 1380 { 1381 ng_l2cap_cmd_p cmd = NULL; 1382 1383 cmd = ng_l2cap_new_cmd(con, NULL, ident, 1384 NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE, 1385 0); 1386 if (cmd == NULL) { 1387 1388 return (ENOMEM); 1389 } 1390 1391 _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result); 1392 if (cmd->aux == NULL) { 1393 ng_l2cap_free_cmd(cmd); 1394 1395 return (ENOBUFS); 1396 } 1397 1398 /* Link command to the queue */ 1399 ng_l2cap_link_cmd(con, cmd); 1400 ng_l2cap_lp_deliver(con); 1401 1402 return (0); 1403 } /* send_l2cap_cfg_rsp */ 1404 1405 /* 1406 * Get next L2CAP configuration option 1407 * 1408 * Return codes: 1409 * 0 no option 1410 * 1 we have got option 1411 * -1 header too short 1412 * -2 bad option value or length 1413 * -3 unknown option 1414 */ 1415 1416 static int 1417 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1418 ng_l2cap_cfg_opt_val_p val) 1419 { 1420 int hint, len = m->m_pkthdr.len - (*off); 1421 1422 if (len == 0) 1423 return (0); 1424 if (len < 0 || len < sizeof(*hdr)) 1425 return (-1); 1426 1427 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1428 *off += sizeof(*hdr); 1429 len -= sizeof(*hdr); 1430 1431 hint = NG_L2CAP_OPT_HINT(hdr->type); 1432 hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1433 1434 switch (hdr->type) { 1435 case NG_L2CAP_OPT_MTU: 1436 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1437 return (-2); 1438 1439 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1440 val->mtu = le16toh(val->mtu); 1441 *off += NG_L2CAP_OPT_MTU_SIZE; 1442 break; 1443 1444 case NG_L2CAP_OPT_FLUSH_TIMO: 1445 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1446 len < hdr->length) 1447 return (-2); 1448 1449 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1450 val->flush_timo = le16toh(val->flush_timo); 1451 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1452 break; 1453 1454 case NG_L2CAP_OPT_QOS: 1455 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1456 return (-2); 1457 1458 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1459 val->flow.token_rate = le32toh(val->flow.token_rate); 1460 val->flow.token_bucket_size = 1461 le32toh(val->flow.token_bucket_size); 1462 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1463 val->flow.latency = le32toh(val->flow.latency); 1464 val->flow.delay_variation = le32toh(val->flow.delay_variation); 1465 *off += NG_L2CAP_OPT_QOS_SIZE; 1466 break; 1467 1468 default: 1469 if (hint) 1470 *off += hdr->length; 1471 else 1472 return (-3); 1473 break; 1474 } 1475 1476 return (1); 1477 } /* get_next_l2cap_opt */ 1478 1479