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