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 * Adjust mbuf so we can get to the start 718 * of the first option we did not like. 719 */ 720 721 m_adj(m, off - sizeof(hdr)); 722 m->m_pkthdr.len = sizeof(hdr) + hdr.length; 723 724 result = NG_L2CAP_UNKNOWN_OPTION; 725 } else { 726 /* XXX FIXME Send other reject codes? */ 727 NG_FREE_M(m); 728 result = NG_L2CAP_REJECT; 729 } 730 731 break; 732 } 733 } 734 735 /* 736 * Now check and see if we have to respond. If everything was OK then 737 * respond contain "C flag" and (if set) we will respond with empty 738 * packet and will wait for more options. 739 * 740 * Other case is that we did not like peer's options and will respond 741 * with L2CAP_Config response command with Reject error code. 742 * 743 * When "respond == 0" than we have received all options and we will 744 * sent L2CA_ConfigInd event to the upper layer protocol. 745 */ 746 747 if (respond) { 748 error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); 749 if (error != 0) { 750 ng_l2cap_l2ca_discon_ind(ch); 751 ng_l2cap_free_chan(ch); 752 } 753 } else { 754 /* Send L2CA_ConfigInd event to the upper layer protocol */ 755 ch->ident = ident; 756 error = ng_l2cap_l2ca_cfg_ind(ch); 757 if (error != 0) 758 ng_l2cap_free_chan(ch); 759 } 760 761 return (error); 762 763 reject: 764 /* Send reject. Do not really care about the result */ 765 NG_FREE_M(m); 766 767 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); 768 769 return (0); 770 } /* ng_l2cap_process_cfg_req */ 771 772 /* 773 * Process L2CAP_ConfigRsp command 774 */ 775 776 static int 777 ng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) 778 { 779 ng_l2cap_p l2cap = con->l2cap; 780 struct mbuf *m = con->rx_pkt; 781 ng_l2cap_cfg_rsp_cp *cp = NULL; 782 ng_l2cap_cmd_p cmd = NULL; 783 u_int16_t scid, cflag, result; 784 ng_l2cap_cfg_opt_t hdr; 785 ng_l2cap_cfg_opt_val_t val; 786 int off, error = 0; 787 788 /* Get command parameters */ 789 con->rx_pkt = NULL; 790 NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 791 if (m == NULL) 792 return (ENOBUFS); 793 794 cp = mtod(m, ng_l2cap_cfg_rsp_cp *); 795 scid = le16toh(cp->scid); 796 cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 797 result = le16toh(cp->result); 798 m_adj(m, sizeof(*cp)); 799 800 /* Check if we have this command */ 801 cmd = ng_l2cap_cmd_by_ident(con, ident); 802 if (cmd == NULL) { 803 NG_L2CAP_ERR( 804 "%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 805 __func__, NG_NODE_NAME(l2cap->node), ident, 806 con->con_handle); 807 NG_FREE_M(m); 808 809 return (ENOENT); 810 } 811 812 /* Verify CIDs and send reject if does not match */ 813 if (cmd->ch->scid != scid) { 814 NG_L2CAP_ERR( 815 "%s: %s - unexpected L2CAP_ConfigRsp. " \ 816 "Channel ID does not match, scid=%d(%d)\n", 817 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 818 scid); 819 goto reject; 820 } 821 822 /* Verify channel state and reject if invalid */ 823 if (cmd->ch->state != NG_L2CAP_CONFIG) { 824 NG_L2CAP_ERR( 825 "%s: %s - unexpected L2CAP_ConfigRsp. " \ 826 "Invalid channel state, scid=%d, state=%d\n", 827 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 828 cmd->ch->state); 829 goto reject; 830 } 831 832 /* 833 * Looks like it is our response, so process it. First parse options, 834 * then verify C flag. If it is set then we shall expect more 835 * configuration options from the peer and we will wait. Otherwise we 836 * have received all options and we will send L2CA_ConfigRsp event to 837 * the upper layer protocol. If command timeout already happened then 838 * ignore response. 839 */ 840 841 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 842 NG_FREE_M(m); 843 return (error); 844 } 845 846 for (off = 0; ; ) { 847 error = get_next_l2cap_opt(m, &off, &hdr, &val); 848 if (error == 0) /* We done with this packet */ 849 break; 850 else if (error > 0) { /* Got option */ 851 switch (hdr.type) { 852 case NG_L2CAP_OPT_MTU: 853 cmd->ch->imtu = val.mtu; 854 break; 855 856 case NG_L2CAP_OPT_FLUSH_TIMO: 857 cmd->ch->flush_timo = val.flush_timo; 858 break; 859 860 case NG_L2CAP_OPT_QOS: 861 bcopy(&val.flow, &cmd->ch->oflow, 862 sizeof(cmd->ch->oflow)); 863 break; 864 865 default: /* Ignore unknown hint option */ 866 break; 867 } 868 } else { 869 /* 870 * XXX FIXME What to do here? 871 * 872 * This is really BAD :( options packet was broken, or 873 * peer sent us option that we did not understand. Let 874 * upper layer know and do not wait for more options. 875 */ 876 877 NG_L2CAP_ALERT( 878 "%s: %s - failed to parse configuration options, error=%d\n", 879 __func__, NG_NODE_NAME(l2cap->node), error); 880 881 result = NG_L2CAP_UNKNOWN; 882 cflag = 0; 883 884 break; 885 } 886 } 887 888 NG_FREE_M(m); 889 890 if (cflag) /* Restart timer and wait for more options */ 891 ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); 892 else { 893 ng_l2cap_unlink_cmd(cmd); 894 895 /* Send L2CA_Config response to the upper layer protocol */ 896 error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); 897 if (error != 0) { 898 /* 899 * XXX FIXME what to do here? we were not able to send 900 * response to the upper layer protocol, so for now 901 * just close the channel. Send L2CAP_Disconnect to 902 * remote peer? 903 */ 904 905 NG_L2CAP_ERR( 906 "%s: %s - failed to send L2CA_Config response, error=%d\n", 907 __func__, NG_NODE_NAME(l2cap->node), error); 908 909 ng_l2cap_free_chan(cmd->ch); 910 } 911 912 ng_l2cap_free_cmd(cmd); 913 } 914 915 return (error); 916 917 reject: 918 /* Send reject. Do not really care about the result */ 919 NG_FREE_M(m); 920 921 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); 922 923 return (0); 924 } /* ng_l2cap_process_cfg_rsp */ 925 926 /* 927 * Process L2CAP_DisconnectReq command 928 */ 929 930 static int 931 ng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) 932 { 933 ng_l2cap_p l2cap = con->l2cap; 934 ng_l2cap_discon_req_cp *cp = NULL; 935 ng_l2cap_chan_p ch = NULL; 936 ng_l2cap_cmd_p cmd = NULL; 937 u_int16_t scid, dcid; 938 939 /* Get command parameters */ 940 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 941 if (con->rx_pkt == NULL) 942 return (ENOBUFS); 943 944 cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); 945 dcid = le16toh(cp->dcid); 946 scid = le16toh(cp->scid); 947 948 NG_FREE_M(con->rx_pkt); 949 950 /* Check if we have this channel and it is in valid state */ 951 ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR); 952 if (ch == NULL) { 953 NG_L2CAP_ERR( 954 "%s: %s - unexpected L2CAP_DisconnectReq message. " \ 955 "Channel does not exist, cid=%d\n", 956 __func__, NG_NODE_NAME(l2cap->node), dcid); 957 goto reject; 958 } 959 960 /* XXX Verify channel state and reject if invalid -- is that true? */ 961 if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG && 962 ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 963 NG_L2CAP_ERR( 964 "%s: %s - unexpected L2CAP_DisconnectReq. " \ 965 "Invalid channel state, cid=%d, state=%d\n", 966 __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 967 goto reject; 968 } 969 970 /* Match destination channel ID */ 971 if (ch->dcid != scid || ch->scid != dcid) { 972 NG_L2CAP_ERR( 973 "%s: %s - unexpected L2CAP_DisconnectReq. " \ 974 "Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 975 "request: scid=%d, dcid=%d\n", 976 __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 977 scid, dcid); 978 goto reject; 979 } 980 981 /* 982 * Looks good, so notify upper layer protocol that channel is about 983 * to be disconnected and send L2CA_DisconnectInd message. Then respond 984 * with L2CAP_DisconnectRsp. 985 */ 986 987 if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 988 ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 989 ng_l2cap_free_chan(ch); 990 } 991 992 /* Send L2CAP_DisconnectRsp */ 993 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 994 if (cmd == NULL) 995 return (ENOMEM); 996 997 _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 998 if (cmd->aux == NULL) { 999 ng_l2cap_free_cmd(cmd); 1000 1001 return (ENOBUFS); 1002 } 1003 1004 /* Link command to the queue */ 1005 ng_l2cap_link_cmd(con, cmd); 1006 ng_l2cap_lp_deliver(con); 1007 1008 return (0); 1009 1010 reject: 1011 /* Send reject. Do not really care about the result */ 1012 send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 1013 1014 return (0); 1015 } /* ng_l2cap_process_discon_req */ 1016 1017 /* 1018 * Process L2CAP_DisconnectRsp command 1019 */ 1020 1021 static int 1022 ng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 1023 { 1024 ng_l2cap_p l2cap = con->l2cap; 1025 ng_l2cap_discon_rsp_cp *cp = NULL; 1026 ng_l2cap_cmd_p cmd = NULL; 1027 u_int16_t scid, dcid; 1028 int error = 0; 1029 1030 /* Get command parameters */ 1031 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1032 if (con->rx_pkt == NULL) 1033 return (ENOBUFS); 1034 1035 cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 1036 dcid = le16toh(cp->dcid); 1037 scid = le16toh(cp->scid); 1038 1039 NG_FREE_M(con->rx_pkt); 1040 1041 /* Check if we have pending command descriptor */ 1042 cmd = ng_l2cap_cmd_by_ident(con, ident); 1043 if (cmd == NULL) { 1044 NG_L2CAP_ERR( 1045 "%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 1046 __func__, NG_NODE_NAME(l2cap->node), ident, 1047 con->con_handle); 1048 goto out; 1049 } 1050 1051 /* Verify channel state, do nothing if invalid */ 1052 if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 1053 NG_L2CAP_ERR( 1054 "%s: %s - unexpected L2CAP_DisconnectRsp. " \ 1055 "Invalid channel state, cid=%d, state=%d\n", 1056 __func__, NG_NODE_NAME(l2cap->node), scid, 1057 cmd->ch->state); 1058 goto out; 1059 } 1060 1061 /* Verify CIDs and send reject if does not match */ 1062 if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 1063 NG_L2CAP_ERR( 1064 "%s: %s - unexpected L2CAP_DisconnectRsp. " \ 1065 "Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 1066 __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 1067 scid, cmd->ch->dcid, dcid); 1068 goto out; 1069 } 1070 1071 /* 1072 * Looks like we have successfully disconnected channel, so notify 1073 * upper layer. If command timeout already happened then ignore 1074 * response. 1075 */ 1076 1077 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 1078 goto out; 1079 1080 error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 1081 ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 1082 out: 1083 return (error); 1084 } /* ng_l2cap_process_discon_rsp */ 1085 1086 /* 1087 * Process L2CAP_EchoReq command 1088 */ 1089 1090 static int 1091 ng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 1092 { 1093 ng_l2cap_p l2cap = con->l2cap; 1094 ng_l2cap_cmd_hdr_t *hdr = NULL; 1095 ng_l2cap_cmd_p cmd = NULL; 1096 1097 con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 1098 if (con->rx_pkt == NULL) { 1099 NG_L2CAP_ALERT( 1100 "%s: %s - ng_l2cap_prepend() failed, size=%zd\n", 1101 __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 1102 1103 return (ENOBUFS); 1104 } 1105 1106 hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 1107 hdr->code = NG_L2CAP_ECHO_RSP; 1108 hdr->ident = ident; 1109 hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 1110 1111 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 1112 if (cmd == NULL) { 1113 NG_FREE_M(con->rx_pkt); 1114 1115 return (ENOBUFS); 1116 } 1117 1118 /* Attach data and link command to the queue */ 1119 cmd->aux = con->rx_pkt; 1120 con->rx_pkt = NULL; 1121 ng_l2cap_link_cmd(con, cmd); 1122 ng_l2cap_lp_deliver(con); 1123 1124 return (0); 1125 } /* ng_l2cap_process_echo_req */ 1126 1127 /* 1128 * Process L2CAP_EchoRsp command 1129 */ 1130 1131 static int 1132 ng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1133 { 1134 ng_l2cap_p l2cap = con->l2cap; 1135 ng_l2cap_cmd_p cmd = NULL; 1136 int error = 0; 1137 1138 /* Check if we have this command */ 1139 cmd = ng_l2cap_cmd_by_ident(con, ident); 1140 if (cmd != NULL) { 1141 /* If command timeout already happened then ignore response */ 1142 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1143 NG_FREE_M(con->rx_pkt); 1144 return (error); 1145 } 1146 1147 ng_l2cap_unlink_cmd(cmd); 1148 1149 error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1150 NG_L2CAP_SUCCESS, con->rx_pkt); 1151 1152 ng_l2cap_free_cmd(cmd); 1153 con->rx_pkt = NULL; 1154 } else { 1155 NG_L2CAP_ERR( 1156 "%s: %s - unexpected L2CAP_EchoRsp command. " \ 1157 "Requested ident does not exist, ident=%d\n", 1158 __func__, NG_NODE_NAME(l2cap->node), ident); 1159 NG_FREE_M(con->rx_pkt); 1160 } 1161 1162 return (error); 1163 } /* ng_l2cap_process_echo_rsp */ 1164 1165 /* 1166 * Process L2CAP_InfoReq command 1167 */ 1168 1169 static int 1170 ng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1171 { 1172 ng_l2cap_p l2cap = con->l2cap; 1173 ng_l2cap_cmd_p cmd = NULL; 1174 u_int16_t type; 1175 1176 /* Get command parameters */ 1177 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1178 if (con->rx_pkt == NULL) 1179 return (ENOBUFS); 1180 1181 type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1182 NG_FREE_M(con->rx_pkt); 1183 1184 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1185 if (cmd == NULL) 1186 return (ENOMEM); 1187 1188 switch (type) { 1189 case NG_L2CAP_CONNLESS_MTU: 1190 _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1191 NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1192 break; 1193 1194 default: 1195 _ng_l2cap_info_rsp(cmd->aux, ident, type, 1196 NG_L2CAP_NOT_SUPPORTED, 0); 1197 break; 1198 } 1199 1200 if (cmd->aux == NULL) { 1201 ng_l2cap_free_cmd(cmd); 1202 1203 return (ENOBUFS); 1204 } 1205 1206 /* Link command to the queue */ 1207 ng_l2cap_link_cmd(con, cmd); 1208 ng_l2cap_lp_deliver(con); 1209 1210 return (0); 1211 } /* ng_l2cap_process_info_req */ 1212 1213 /* 1214 * Process L2CAP_InfoRsp command 1215 */ 1216 1217 static int 1218 ng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1219 { 1220 ng_l2cap_p l2cap = con->l2cap; 1221 ng_l2cap_info_rsp_cp *cp = NULL; 1222 ng_l2cap_cmd_p cmd = NULL; 1223 int error = 0; 1224 1225 /* Get command parameters */ 1226 NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1227 if (con->rx_pkt == NULL) 1228 return (ENOBUFS); 1229 1230 cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1231 cp->type = le16toh(cp->type); 1232 cp->result = le16toh(cp->result); 1233 m_adj(con->rx_pkt, sizeof(*cp)); 1234 1235 /* Check if we have pending command descriptor */ 1236 cmd = ng_l2cap_cmd_by_ident(con, ident); 1237 if (cmd == NULL) { 1238 NG_L2CAP_ERR( 1239 "%s: %s - unexpected L2CAP_InfoRsp command. " \ 1240 "Requested ident does not exist, ident=%d\n", 1241 __func__, NG_NODE_NAME(l2cap->node), ident); 1242 NG_FREE_M(con->rx_pkt); 1243 1244 return (ENOENT); 1245 } 1246 1247 /* If command timeout already happened then ignore response */ 1248 if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1249 NG_FREE_M(con->rx_pkt); 1250 return (error); 1251 } 1252 1253 ng_l2cap_unlink_cmd(cmd); 1254 1255 if (cp->result == NG_L2CAP_SUCCESS) { 1256 switch (cp->type) { 1257 case NG_L2CAP_CONNLESS_MTU: 1258 if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1259 *mtod(con->rx_pkt, u_int16_t *) = 1260 le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1261 else { 1262 cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1263 1264 NG_L2CAP_ERR( 1265 "%s: %s - invalid L2CAP_InfoRsp command. " \ 1266 "Bad connectionless MTU parameter, len=%d\n", 1267 __func__, NG_NODE_NAME(l2cap->node), 1268 con->rx_pkt->m_pkthdr.len); 1269 } 1270 break; 1271 1272 default: 1273 NG_L2CAP_WARN( 1274 "%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1275 __func__, NG_NODE_NAME(l2cap->node), cp->type); 1276 break; 1277 } 1278 } 1279 1280 error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1281 cp->result, con->rx_pkt); 1282 1283 ng_l2cap_free_cmd(cmd); 1284 con->rx_pkt = NULL; 1285 1286 return (error); 1287 } /* ng_l2cap_process_info_rsp */ 1288 1289 /* 1290 * Send L2CAP reject 1291 */ 1292 1293 static int 1294 send_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1295 u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1296 { 1297 ng_l2cap_cmd_p cmd = NULL; 1298 1299 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1300 if (cmd == NULL) 1301 return (ENOMEM); 1302 1303 _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1304 if (cmd->aux == NULL) { 1305 ng_l2cap_free_cmd(cmd); 1306 1307 return (ENOBUFS); 1308 } 1309 1310 /* Link command to the queue */ 1311 ng_l2cap_link_cmd(con, cmd); 1312 ng_l2cap_lp_deliver(con); 1313 1314 return (0); 1315 } /* send_l2cap_reject */ 1316 1317 /* 1318 * Send L2CAP connection reject 1319 */ 1320 1321 static int 1322 send_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1323 u_int16_t dcid, u_int16_t result) 1324 { 1325 ng_l2cap_cmd_p cmd = NULL; 1326 1327 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1328 if (cmd == NULL) 1329 return (ENOMEM); 1330 1331 _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1332 if (cmd->aux == NULL) { 1333 ng_l2cap_free_cmd(cmd); 1334 1335 return (ENOBUFS); 1336 } 1337 1338 /* Link command to the queue */ 1339 ng_l2cap_link_cmd(con, cmd); 1340 ng_l2cap_lp_deliver(con); 1341 1342 return (0); 1343 } /* send_l2cap_con_rej */ 1344 1345 /* 1346 * Send L2CAP config response 1347 */ 1348 1349 static int 1350 send_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1351 u_int16_t result, struct mbuf *opt) 1352 { 1353 ng_l2cap_cmd_p cmd = NULL; 1354 1355 cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1356 if (cmd == NULL) { 1357 NG_FREE_M(opt); 1358 1359 return (ENOMEM); 1360 } 1361 1362 _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1363 if (cmd->aux == NULL) { 1364 ng_l2cap_free_cmd(cmd); 1365 1366 return (ENOBUFS); 1367 } 1368 1369 /* Link command to the queue */ 1370 ng_l2cap_link_cmd(con, cmd); 1371 ng_l2cap_lp_deliver(con); 1372 1373 return (0); 1374 } /* send_l2cap_cfg_rsp */ 1375 1376 static int 1377 send_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident, 1378 u_int16_t result) 1379 { 1380 ng_l2cap_cmd_p cmd = NULL; 1381 1382 cmd = ng_l2cap_new_cmd(con, NULL, ident, 1383 NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE, 1384 0); 1385 if (cmd == NULL) { 1386 return (ENOMEM); 1387 } 1388 1389 _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result); 1390 if (cmd->aux == NULL) { 1391 ng_l2cap_free_cmd(cmd); 1392 1393 return (ENOBUFS); 1394 } 1395 1396 /* Link command to the queue */ 1397 ng_l2cap_link_cmd(con, cmd); 1398 ng_l2cap_lp_deliver(con); 1399 1400 return (0); 1401 } /* send_l2cap_cfg_rsp */ 1402 1403 /* 1404 * Get next L2CAP configuration option 1405 * 1406 * Return codes: 1407 * 0 no option 1408 * 1 we have got option 1409 * -1 header too short 1410 * -2 bad option value or length 1411 * -3 unknown option 1412 */ 1413 1414 static int 1415 get_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1416 ng_l2cap_cfg_opt_val_p val) 1417 { 1418 int hint, len = m->m_pkthdr.len - (*off); 1419 1420 if (len == 0) 1421 return (0); 1422 if (len < 0 || len < sizeof(*hdr)) 1423 return (-1); 1424 1425 m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1426 *off += sizeof(*hdr); 1427 len -= sizeof(*hdr); 1428 1429 hint = NG_L2CAP_OPT_HINT(hdr->type); 1430 hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1431 1432 switch (hdr->type) { 1433 case NG_L2CAP_OPT_MTU: 1434 if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1435 return (-2); 1436 1437 m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1438 val->mtu = le16toh(val->mtu); 1439 *off += NG_L2CAP_OPT_MTU_SIZE; 1440 break; 1441 1442 case NG_L2CAP_OPT_FLUSH_TIMO: 1443 if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1444 len < hdr->length) 1445 return (-2); 1446 1447 m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1448 val->flush_timo = le16toh(val->flush_timo); 1449 *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1450 break; 1451 1452 case NG_L2CAP_OPT_QOS: 1453 if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1454 return (-2); 1455 1456 m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1457 val->flow.token_rate = le32toh(val->flow.token_rate); 1458 val->flow.token_bucket_size = 1459 le32toh(val->flow.token_bucket_size); 1460 val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1461 val->flow.latency = le32toh(val->flow.latency); 1462 val->flow.delay_variation = le32toh(val->flow.delay_variation); 1463 *off += NG_L2CAP_OPT_QOS_SIZE; 1464 break; 1465 1466 default: 1467 if (hint) 1468 *off += hdr->length; 1469 else 1470 return (-3); 1471 break; 1472 } 1473 1474 return (1); 1475 } /* get_next_l2cap_opt */ 1476