1107120Sjulian/* 2107120Sjulian * ng_l2cap_evnt.c 3139823Simp */ 4139823Simp 5139823Simp/*- 6107120Sjulian * Copyright (c) Maksim Yevmenkin <m_evmenkin@yahoo.com> 7107120Sjulian * All rights reserved. 8107120Sjulian * 9107120Sjulian * Redistribution and use in source and binary forms, with or without 10107120Sjulian * modification, are permitted provided that the following conditions 11107120Sjulian * are met: 12107120Sjulian * 1. Redistributions of source code must retain the above copyright 13107120Sjulian * notice, this list of conditions and the following disclaimer. 14107120Sjulian * 2. Redistributions in binary form must reproduce the above copyright 15107120Sjulian * notice, this list of conditions and the following disclaimer in the 16107120Sjulian * documentation and/or other materials provided with the distribution. 17107120Sjulian * 18107120Sjulian * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 19107120Sjulian * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20107120Sjulian * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21107120Sjulian * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 22107120Sjulian * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23107120Sjulian * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 24107120Sjulian * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 25107120Sjulian * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26107120Sjulian * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 27107120Sjulian * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 28107120Sjulian * SUCH DAMAGE. 29107120Sjulian * 30121054Semax * $Id: ng_l2cap_evnt.c,v 1.5 2003/09/08 19:11:45 max Exp $ 31107120Sjulian * $FreeBSD$ 32107120Sjulian */ 33107120Sjulian 34107120Sjulian#include <sys/param.h> 35107120Sjulian#include <sys/systm.h> 36107120Sjulian#include <sys/kernel.h> 37107120Sjulian#include <sys/endian.h> 38107120Sjulian#include <sys/malloc.h> 39107120Sjulian#include <sys/mbuf.h> 40107120Sjulian#include <sys/queue.h> 41107120Sjulian#include <netgraph/ng_message.h> 42107120Sjulian#include <netgraph/netgraph.h> 43128688Semax#include <netgraph/bluetooth/include/ng_bluetooth.h> 44128688Semax#include <netgraph/bluetooth/include/ng_hci.h> 45128688Semax#include <netgraph/bluetooth/include/ng_l2cap.h> 46128688Semax#include <netgraph/bluetooth/l2cap/ng_l2cap_var.h> 47128688Semax#include <netgraph/bluetooth/l2cap/ng_l2cap_cmds.h> 48128688Semax#include <netgraph/bluetooth/l2cap/ng_l2cap_evnt.h> 49128688Semax#include <netgraph/bluetooth/l2cap/ng_l2cap_llpi.h> 50128688Semax#include <netgraph/bluetooth/l2cap/ng_l2cap_ulpi.h> 51128688Semax#include <netgraph/bluetooth/l2cap/ng_l2cap_misc.h> 52107120Sjulian 53107120Sjulian/****************************************************************************** 54107120Sjulian ****************************************************************************** 55107120Sjulian ** L2CAP events processing module 56107120Sjulian ****************************************************************************** 57107120Sjulian ******************************************************************************/ 58107120Sjulian 59107120Sjulianstatic int ng_l2cap_process_signal_cmd (ng_l2cap_con_p); 60281198Stakawatastatic int ng_l2cap_process_lesignal_cmd (ng_l2cap_con_p); 61107120Sjulianstatic int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t); 62281198Stakawatastatic int ng_l2cap_process_cmd_urq (ng_l2cap_con_p, u_int8_t); 63281198Stakawatastatic int ng_l2cap_process_cmd_urs (ng_l2cap_con_p, u_int8_t); 64107120Sjulianstatic int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t); 65107120Sjulianstatic int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t); 66107120Sjulianstatic int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t); 67107120Sjulianstatic int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t); 68107120Sjulianstatic int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t); 69107120Sjulianstatic int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t); 70107120Sjulianstatic int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t); 71107120Sjulianstatic int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t); 72107120Sjulianstatic int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t); 73107120Sjulianstatic int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t); 74107120Sjulianstatic int send_l2cap_reject 75107120Sjulian (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t); 76107120Sjulianstatic int send_l2cap_con_rej 77107120Sjulian (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t); 78107120Sjulianstatic int send_l2cap_cfg_rsp 79107120Sjulian (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *); 80281198Stakawatastatic int send_l2cap_param_urs 81281198Stakawata (ng_l2cap_con_p , u_int8_t , u_int16_t); 82281198Stakawata 83107120Sjulianstatic int get_next_l2cap_opt 84107120Sjulian (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p); 85107120Sjulian 86107120Sjulian/* 87107120Sjulian * Receive L2CAP packet. First get L2CAP header and verify packet. Than 88107120Sjulian * get destination channel and process packet. 89107120Sjulian */ 90107120Sjulian 91107120Sjulianint 92107120Sjulianng_l2cap_receive(ng_l2cap_con_p con) 93107120Sjulian{ 94107120Sjulian ng_l2cap_p l2cap = con->l2cap; 95107120Sjulian ng_l2cap_hdr_t *hdr = NULL; 96107120Sjulian int error = 0; 97107120Sjulian 98107120Sjulian /* Check packet */ 99107120Sjulian if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 100107120Sjulian NG_L2CAP_ERR( 101107120Sjulian"%s: %s - invalid L2CAP packet. Packet too small, len=%d\n", 102107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 103107120Sjulian con->rx_pkt->m_pkthdr.len); 104107120Sjulian error = EMSGSIZE; 105107120Sjulian goto drop; 106107120Sjulian } 107107120Sjulian 108107120Sjulian /* Get L2CAP header */ 109107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 110107120Sjulian if (con->rx_pkt == NULL) 111107120Sjulian return (ENOBUFS); 112107120Sjulian 113107120Sjulian hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); 114107120Sjulian hdr->length = le16toh(hdr->length); 115107120Sjulian hdr->dcid = le16toh(hdr->dcid); 116107120Sjulian 117107120Sjulian /* Check payload size */ 118107120Sjulian if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) { 119107120Sjulian NG_L2CAP_ERR( 120128076Semax"%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n", 121107120Sjulian __func__, NG_NODE_NAME(l2cap->node), hdr->length, 122107120Sjulian con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 123107120Sjulian error = EMSGSIZE; 124107120Sjulian goto drop; 125107120Sjulian } 126107120Sjulian 127107120Sjulian /* Process packet */ 128107120Sjulian switch (hdr->dcid) { 129107120Sjulian case NG_L2CAP_SIGNAL_CID: /* L2CAP command */ 130107120Sjulian m_adj(con->rx_pkt, sizeof(*hdr)); 131107120Sjulian error = ng_l2cap_process_signal_cmd(con); 132107120Sjulian break; 133281198Stakawata case NG_L2CAP_LESIGNAL_CID: 134281198Stakawata m_adj(con->rx_pkt, sizeof(*hdr)); 135281198Stakawata error = ng_l2cap_process_lesignal_cmd(con); 136281198Stakawata break; 137107120Sjulian case NG_L2CAP_CLT_CID: /* Connectionless packet */ 138107120Sjulian error = ng_l2cap_l2ca_clt_receive(con); 139107120Sjulian break; 140107120Sjulian 141107120Sjulian default: /* Data packet */ 142107120Sjulian error = ng_l2cap_l2ca_receive(con); 143107120Sjulian break; 144107120Sjulian } 145107120Sjulian 146107120Sjulian return (error); 147107120Sjuliandrop: 148107120Sjulian NG_FREE_M(con->rx_pkt); 149107120Sjulian 150107120Sjulian return (error); 151107120Sjulian} /* ng_l2cap_receive */ 152107120Sjulian 153107120Sjulian/* 154107120Sjulian * Process L2CAP signaling command. We already know that destination channel ID 155107120Sjulian * is 0x1 that means we have received signaling command from peer's L2CAP layer. 156107120Sjulian * So get command header, decode and process it. 157107120Sjulian * 158107120Sjulian * XXX do we need to check signaling MTU here? 159107120Sjulian */ 160107120Sjulian 161107120Sjulianstatic int 162107120Sjulianng_l2cap_process_signal_cmd(ng_l2cap_con_p con) 163107120Sjulian{ 164107120Sjulian ng_l2cap_p l2cap = con->l2cap; 165107120Sjulian ng_l2cap_cmd_hdr_t *hdr = NULL; 166107120Sjulian struct mbuf *m = NULL; 167107120Sjulian 168107120Sjulian while (con->rx_pkt != NULL) { 169107120Sjulian /* Verify packet length */ 170107120Sjulian if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 171107120Sjulian NG_L2CAP_ERR( 172107120Sjulian"%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", 173107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 174107120Sjulian con->rx_pkt->m_pkthdr.len); 175107120Sjulian NG_FREE_M(con->rx_pkt); 176107120Sjulian 177107120Sjulian return (EMSGSIZE); 178107120Sjulian } 179107120Sjulian 180107120Sjulian /* Get signaling command */ 181107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 182107120Sjulian if (con->rx_pkt == NULL) 183107120Sjulian return (ENOBUFS); 184107120Sjulian 185107120Sjulian hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 186107120Sjulian hdr->length = le16toh(hdr->length); 187107120Sjulian m_adj(con->rx_pkt, sizeof(*hdr)); 188107120Sjulian 189107120Sjulian /* Verify command length */ 190107120Sjulian if (con->rx_pkt->m_pkthdr.len < hdr->length) { 191107120Sjulian NG_L2CAP_ERR( 192107120Sjulian"%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ 193107120Sjulian"Invalid command length=%d, m_pkthdr.len=%d\n", 194107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 195107120Sjulian hdr->code, hdr->ident, hdr->length, 196107120Sjulian con->rx_pkt->m_pkthdr.len); 197107120Sjulian NG_FREE_M(con->rx_pkt); 198107120Sjulian 199107120Sjulian return (EMSGSIZE); 200107120Sjulian } 201107120Sjulian 202107120Sjulian /* Get the command, save the rest (if any) */ 203107120Sjulian if (con->rx_pkt->m_pkthdr.len > hdr->length) 204243882Sglebius m = m_split(con->rx_pkt, hdr->length, M_NOWAIT); 205107120Sjulian else 206107120Sjulian m = NULL; 207107120Sjulian 208107120Sjulian /* Process command */ 209107120Sjulian switch (hdr->code) { 210107120Sjulian case NG_L2CAP_CMD_REJ: 211107120Sjulian ng_l2cap_process_cmd_rej(con, hdr->ident); 212107120Sjulian break; 213107120Sjulian 214107120Sjulian case NG_L2CAP_CON_REQ: 215107120Sjulian ng_l2cap_process_con_req(con, hdr->ident); 216107120Sjulian break; 217107120Sjulian 218107120Sjulian case NG_L2CAP_CON_RSP: 219107120Sjulian ng_l2cap_process_con_rsp(con, hdr->ident); 220107120Sjulian break; 221107120Sjulian 222107120Sjulian case NG_L2CAP_CFG_REQ: 223107120Sjulian ng_l2cap_process_cfg_req(con, hdr->ident); 224107120Sjulian break; 225107120Sjulian 226107120Sjulian case NG_L2CAP_CFG_RSP: 227107120Sjulian ng_l2cap_process_cfg_rsp(con, hdr->ident); 228107120Sjulian break; 229107120Sjulian 230107120Sjulian case NG_L2CAP_DISCON_REQ: 231107120Sjulian ng_l2cap_process_discon_req(con, hdr->ident); 232107120Sjulian break; 233107120Sjulian 234107120Sjulian case NG_L2CAP_DISCON_RSP: 235107120Sjulian ng_l2cap_process_discon_rsp(con, hdr->ident); 236107120Sjulian break; 237107120Sjulian 238107120Sjulian case NG_L2CAP_ECHO_REQ: 239107120Sjulian ng_l2cap_process_echo_req(con, hdr->ident); 240107120Sjulian break; 241107120Sjulian 242107120Sjulian case NG_L2CAP_ECHO_RSP: 243107120Sjulian ng_l2cap_process_echo_rsp(con, hdr->ident); 244107120Sjulian break; 245107120Sjulian 246107120Sjulian case NG_L2CAP_INFO_REQ: 247107120Sjulian ng_l2cap_process_info_req(con, hdr->ident); 248107120Sjulian break; 249107120Sjulian 250107120Sjulian case NG_L2CAP_INFO_RSP: 251107120Sjulian ng_l2cap_process_info_rsp(con, hdr->ident); 252107120Sjulian break; 253107120Sjulian 254107120Sjulian default: 255107120Sjulian NG_L2CAP_ERR( 256107120Sjulian"%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 257107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 258107120Sjulian hdr->code, hdr->ident, hdr->length); 259107120Sjulian 260107120Sjulian /* 261107120Sjulian * Send L2CAP_CommandRej. Do not really care 262107120Sjulian * about the result 263107120Sjulian */ 264107120Sjulian 265107120Sjulian send_l2cap_reject(con, hdr->ident, 266107120Sjulian NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 267107120Sjulian NG_FREE_M(con->rx_pkt); 268107120Sjulian break; 269107120Sjulian } 270107120Sjulian 271107120Sjulian con->rx_pkt = m; 272107120Sjulian } 273107120Sjulian 274107120Sjulian return (0); 275107120Sjulian} /* ng_l2cap_process_signal_cmd */ 276281198Stakawatastatic int 277281198Stakawatang_l2cap_process_lesignal_cmd(ng_l2cap_con_p con) 278281198Stakawata{ 279281198Stakawata ng_l2cap_p l2cap = con->l2cap; 280281198Stakawata ng_l2cap_cmd_hdr_t *hdr = NULL; 281281198Stakawata struct mbuf *m = NULL; 282107120Sjulian 283281198Stakawata while (con->rx_pkt != NULL) { 284281198Stakawata /* Verify packet length */ 285281198Stakawata if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 286281198Stakawata NG_L2CAP_ERR( 287281198Stakawata"%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", 288281198Stakawata __func__, NG_NODE_NAME(l2cap->node), 289281198Stakawata con->rx_pkt->m_pkthdr.len); 290281198Stakawata NG_FREE_M(con->rx_pkt); 291281198Stakawata 292281198Stakawata return (EMSGSIZE); 293281198Stakawata } 294281198Stakawata 295281198Stakawata /* Get signaling command */ 296281198Stakawata NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 297281198Stakawata if (con->rx_pkt == NULL) 298281198Stakawata return (ENOBUFS); 299281198Stakawata 300281198Stakawata hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 301281198Stakawata hdr->length = le16toh(hdr->length); 302281198Stakawata m_adj(con->rx_pkt, sizeof(*hdr)); 303281198Stakawata 304281198Stakawata /* Verify command length */ 305281198Stakawata if (con->rx_pkt->m_pkthdr.len < hdr->length) { 306281198Stakawata NG_L2CAP_ERR( 307281198Stakawata"%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ 308281198Stakawata"Invalid command length=%d, m_pkthdr.len=%d\n", 309281198Stakawata __func__, NG_NODE_NAME(l2cap->node), 310281198Stakawata hdr->code, hdr->ident, hdr->length, 311281198Stakawata con->rx_pkt->m_pkthdr.len); 312281198Stakawata NG_FREE_M(con->rx_pkt); 313281198Stakawata 314281198Stakawata return (EMSGSIZE); 315281198Stakawata } 316281198Stakawata 317281198Stakawata /* Get the command, save the rest (if any) */ 318281198Stakawata if (con->rx_pkt->m_pkthdr.len > hdr->length) 319281198Stakawata m = m_split(con->rx_pkt, hdr->length, M_NOWAIT); 320281198Stakawata else 321281198Stakawata m = NULL; 322281198Stakawata 323281198Stakawata /* Process command */ 324281198Stakawata switch (hdr->code) { 325281198Stakawata case NG_L2CAP_CMD_REJ: 326281198Stakawata ng_l2cap_process_cmd_rej(con, hdr->ident); 327281198Stakawata break; 328281198Stakawata case NG_L2CAP_CMD_PARAM_UPDATE_REQUEST: 329281198Stakawata ng_l2cap_process_cmd_urq(con, hdr->ident); 330281198Stakawata break; 331281198Stakawata case NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE: 332281198Stakawata ng_l2cap_process_cmd_urs(con, hdr->ident); 333281198Stakawata break; 334281198Stakawata 335281198Stakawata 336281198Stakawata default: 337281198Stakawata NG_L2CAP_ERR( 338281198Stakawata"%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 339281198Stakawata __func__, NG_NODE_NAME(l2cap->node), 340281198Stakawata hdr->code, hdr->ident, hdr->length); 341281198Stakawata 342281198Stakawata /* 343281198Stakawata * Send L2CAP_CommandRej. Do not really care 344281198Stakawata * about the result 345281198Stakawata */ 346281198Stakawata 347281198Stakawata send_l2cap_reject(con, hdr->ident, 348281198Stakawata NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 349281198Stakawata NG_FREE_M(con->rx_pkt); 350281198Stakawata break; 351281198Stakawata } 352281198Stakawata 353281198Stakawata con->rx_pkt = m; 354281198Stakawata } 355281198Stakawata 356281198Stakawata return (0); 357281198Stakawata} /* ng_l2cap_process_signal_cmd */ 358281198Stakawata/*Update Paramater Request*/ 359281198Stakawatastatic int ng_l2cap_process_cmd_urq(ng_l2cap_con_p con, uint8_t ident) 360281198Stakawata{ 361298813Spfg /* We do not implement parameter negotiation for now. */ 362281198Stakawata send_l2cap_param_urs(con, ident, NG_L2CAP_UPDATE_PARAM_ACCEPT); 363281198Stakawata NG_FREE_M(con->rx_pkt); 364281198Stakawata return 0; 365281198Stakawata} 366281198Stakawata 367281198Stakawatastatic int ng_l2cap_process_cmd_urs(ng_l2cap_con_p con, uint8_t ident) 368281198Stakawata{ 369281198Stakawata /* We only support master side yet .*/ 370281198Stakawata //send_l2cap_reject(con,ident ... ); 371281198Stakawata 372281198Stakawata NG_FREE_M(con->rx_pkt); 373281198Stakawata return 0; 374281198Stakawata} 375281198Stakawata 376107120Sjulian/* 377107120Sjulian * Process L2CAP_CommandRej command 378107120Sjulian */ 379107120Sjulian 380107120Sjulianstatic int 381107120Sjulianng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident) 382107120Sjulian{ 383107120Sjulian ng_l2cap_p l2cap = con->l2cap; 384107120Sjulian ng_l2cap_cmd_rej_cp *cp = NULL; 385107120Sjulian ng_l2cap_cmd_p cmd = NULL; 386107120Sjulian 387107120Sjulian /* Get command parameters */ 388107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 389107120Sjulian if (con->rx_pkt == NULL) 390107120Sjulian return (ENOBUFS); 391107120Sjulian 392107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *); 393107120Sjulian cp->reason = le16toh(cp->reason); 394107120Sjulian 395107120Sjulian /* Check if we have pending command descriptor */ 396107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 397107120Sjulian if (cmd != NULL) { 398121054Semax /* If command timeout already happened then ignore reject */ 399121054Semax if (ng_l2cap_command_untimeout(cmd) != 0) { 400121054Semax NG_FREE_M(con->rx_pkt); 401121054Semax return (ETIMEDOUT); 402121054Semax } 403107120Sjulian 404107120Sjulian ng_l2cap_unlink_cmd(cmd); 405107120Sjulian 406107120Sjulian switch (cmd->code) { 407107120Sjulian case NG_L2CAP_CON_REQ: 408107120Sjulian ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0); 409107120Sjulian ng_l2cap_free_chan(cmd->ch); 410107120Sjulian break; 411107120Sjulian 412107120Sjulian case NG_L2CAP_CFG_REQ: 413107120Sjulian ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason); 414107120Sjulian break; 415107120Sjulian 416107120Sjulian case NG_L2CAP_DISCON_REQ: 417107120Sjulian ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason); 418107120Sjulian ng_l2cap_free_chan(cmd->ch); /* XXX free channel */ 419107120Sjulian break; 420107120Sjulian 421107120Sjulian case NG_L2CAP_ECHO_REQ: 422107120Sjulian ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 423107120Sjulian cp->reason, NULL); 424107120Sjulian break; 425107120Sjulian 426107120Sjulian case NG_L2CAP_INFO_REQ: 427107120Sjulian ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 428107120Sjulian cp->reason, NULL); 429107120Sjulian break; 430107120Sjulian 431107120Sjulian default: 432107120Sjulian NG_L2CAP_ALERT( 433107120Sjulian"%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", 434107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->code); 435107120Sjulian break; 436107120Sjulian } 437107120Sjulian 438107120Sjulian ng_l2cap_free_cmd(cmd); 439107120Sjulian } else 440107120Sjulian NG_L2CAP_ERR( 441107120Sjulian"%s: %s - unexpected L2CAP_CommandRej command. " \ 442107120Sjulian"Requested ident does not exist, ident=%d\n", 443107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident); 444107120Sjulian 445107120Sjulian NG_FREE_M(con->rx_pkt); 446107120Sjulian 447107120Sjulian return (0); 448107120Sjulian} /* ng_l2cap_process_cmd_rej */ 449107120Sjulian 450107120Sjulian/* 451107120Sjulian * Process L2CAP_ConnectReq command 452107120Sjulian */ 453107120Sjulian 454107120Sjulianstatic int 455107120Sjulianng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident) 456107120Sjulian{ 457107120Sjulian ng_l2cap_p l2cap = con->l2cap; 458107120Sjulian struct mbuf *m = con->rx_pkt; 459107120Sjulian ng_l2cap_con_req_cp *cp = NULL; 460107120Sjulian ng_l2cap_chan_p ch = NULL; 461107120Sjulian int error = 0; 462107120Sjulian u_int16_t dcid, psm; 463281198Stakawata int idtype; 464281198Stakawata 465107120Sjulian /* Get command parameters */ 466107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 467107120Sjulian if (m == NULL) 468107120Sjulian return (ENOBUFS); 469107120Sjulian 470107120Sjulian cp = mtod(m, ng_l2cap_con_req_cp *); 471107120Sjulian psm = le16toh(cp->psm); 472107120Sjulian dcid = le16toh(cp->scid); 473107120Sjulian 474107120Sjulian NG_FREE_M(m); 475107120Sjulian con->rx_pkt = NULL; 476281198Stakawata if(dcid == NG_L2CAP_ATT_CID) 477281198Stakawata idtype = NG_L2CAP_L2CA_IDTYPE_ATT; 478290038Stakawata else if(dcid == NG_L2CAP_SMP_CID) 479290038Stakawata idtype = NG_L2CAP_L2CA_IDTYPE_SMP; 480281198Stakawata else if( con->linktype != NG_HCI_LINK_ACL) 481281198Stakawata idtype = NG_L2CAP_L2CA_IDTYPE_LE; 482281198Stakawata else 483281198Stakawata idtype = NG_L2CAP_L2CA_IDTYPE_BREDR; 484107120Sjulian 485107120Sjulian /* 486107120Sjulian * Create new channel and send L2CA_ConnectInd notification 487107120Sjulian * to the upper layer protocol. 488107120Sjulian */ 489107120Sjulian 490281198Stakawata ch = ng_l2cap_new_chan(l2cap, con, psm, idtype); 491281198Stakawata 492107120Sjulian if (ch == NULL) 493107120Sjulian return (send_l2cap_con_rej(con, ident, 0, dcid, 494107120Sjulian NG_L2CAP_NO_RESOURCES)); 495107120Sjulian 496107120Sjulian /* Update channel IDs */ 497107120Sjulian ch->dcid = dcid; 498107120Sjulian 499107120Sjulian /* Sent L2CA_ConnectInd notification to the upper layer */ 500107120Sjulian ch->ident = ident; 501107120Sjulian ch->state = NG_L2CAP_W4_L2CA_CON_RSP; 502107120Sjulian 503107120Sjulian error = ng_l2cap_l2ca_con_ind(ch); 504107120Sjulian if (error != 0) { 505107120Sjulian send_l2cap_con_rej(con, ident, ch->scid, dcid, 506107120Sjulian (error == ENOMEM)? NG_L2CAP_NO_RESOURCES : 507107120Sjulian NG_L2CAP_PSM_NOT_SUPPORTED); 508107120Sjulian ng_l2cap_free_chan(ch); 509107120Sjulian } 510107120Sjulian 511107120Sjulian return (error); 512107120Sjulian} /* ng_l2cap_process_con_req */ 513107120Sjulian 514107120Sjulian/* 515107120Sjulian * Process L2CAP_ConnectRsp command 516107120Sjulian */ 517107120Sjulian 518107120Sjulianstatic int 519107120Sjulianng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident) 520107120Sjulian{ 521107120Sjulian ng_l2cap_p l2cap = con->l2cap; 522107120Sjulian struct mbuf *m = con->rx_pkt; 523107120Sjulian ng_l2cap_con_rsp_cp *cp = NULL; 524107120Sjulian ng_l2cap_cmd_p cmd = NULL; 525107120Sjulian u_int16_t scid, dcid, result, status; 526107120Sjulian int error = 0; 527107120Sjulian 528107120Sjulian /* Get command parameters */ 529107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 530107120Sjulian if (m == NULL) 531107120Sjulian return (ENOBUFS); 532107120Sjulian 533107120Sjulian cp = mtod(m, ng_l2cap_con_rsp_cp *); 534107120Sjulian dcid = le16toh(cp->dcid); 535107120Sjulian scid = le16toh(cp->scid); 536107120Sjulian result = le16toh(cp->result); 537107120Sjulian status = le16toh(cp->status); 538107120Sjulian 539107120Sjulian NG_FREE_M(m); 540107120Sjulian con->rx_pkt = NULL; 541107120Sjulian 542107120Sjulian /* Check if we have pending command descriptor */ 543107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 544107120Sjulian if (cmd == NULL) { 545107120Sjulian NG_L2CAP_ERR( 546107120Sjulian"%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", 547107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident, 548107120Sjulian con->con_handle); 549107120Sjulian 550107120Sjulian return (ENOENT); 551107120Sjulian } 552107120Sjulian 553107120Sjulian /* Verify channel state, if invalid - do nothing */ 554107120Sjulian if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) { 555107120Sjulian NG_L2CAP_ERR( 556107120Sjulian"%s: %s - unexpected L2CAP_ConnectRsp. " \ 557107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 558107120Sjulian __func__, NG_NODE_NAME(l2cap->node), scid, 559107120Sjulian cmd->ch->state); 560107120Sjulian goto reject; 561107120Sjulian } 562107120Sjulian 563107120Sjulian /* Verify CIDs and send reject if does not match */ 564107120Sjulian if (cmd->ch->scid != scid) { 565107120Sjulian NG_L2CAP_ERR( 566107120Sjulian"%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", 567107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 568107120Sjulian scid); 569107120Sjulian goto reject; 570107120Sjulian } 571107120Sjulian 572107120Sjulian /* 573107120Sjulian * Looks good. We got confirmation from our peer. Now process 574107120Sjulian * it. First disable RTX timer. Then check the result and send 575121054Semax * notification to the upper layer. If command timeout already 576121054Semax * happened then ignore response. 577107120Sjulian */ 578107120Sjulian 579121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 580121054Semax return (error); 581107120Sjulian 582107120Sjulian if (result == NG_L2CAP_PENDING) { 583107120Sjulian /* 584107120Sjulian * Our peer wants more time to complete connection. We shall 585107120Sjulian * start ERTX timer and wait. Keep command in the list. 586107120Sjulian */ 587107120Sjulian 588107120Sjulian cmd->ch->dcid = dcid; 589107120Sjulian ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout()); 590107120Sjulian 591107120Sjulian error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 592107120Sjulian result, status); 593107120Sjulian if (error != 0) 594107120Sjulian ng_l2cap_free_chan(cmd->ch); 595107120Sjulian } else { 596107120Sjulian ng_l2cap_unlink_cmd(cmd); 597107120Sjulian 598107120Sjulian if (result == NG_L2CAP_SUCCESS) { 599107120Sjulian /* 600107120Sjulian * Channel is open. Complete command and move to CONFIG 601107120Sjulian * state. Since we have sent positive confirmation we 602107120Sjulian * expect to receive L2CA_Config request from the upper 603107120Sjulian * layer protocol. 604107120Sjulian */ 605107120Sjulian 606107120Sjulian cmd->ch->dcid = dcid; 607290038Stakawata cmd->ch->state = ((cmd->ch->scid == NG_L2CAP_ATT_CID)|| 608290038Stakawata (cmd->ch->scid == NG_L2CAP_SMP_CID)) 609290038Stakawata ? 610281198Stakawata NG_L2CAP_OPEN : NG_L2CAP_CONFIG; 611107120Sjulian } else 612107120Sjulian /* There was an error, so close the channel */ 613107120Sjulian NG_L2CAP_INFO( 614107120Sjulian"%s: %s - failed to open L2CAP channel, result=%d, status=%d\n", 615107120Sjulian __func__, NG_NODE_NAME(l2cap->node), result, 616107120Sjulian status); 617107120Sjulian 618107120Sjulian error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 619107120Sjulian result, status); 620107120Sjulian 621107120Sjulian /* XXX do we have to remove the channel on error? */ 622107120Sjulian if (error != 0 || result != NG_L2CAP_SUCCESS) 623107120Sjulian ng_l2cap_free_chan(cmd->ch); 624107120Sjulian 625107120Sjulian ng_l2cap_free_cmd(cmd); 626107120Sjulian } 627107120Sjulian 628107120Sjulian return (error); 629107120Sjulian 630107120Sjulianreject: 631107120Sjulian /* Send reject. Do not really care about the result */ 632107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 633107120Sjulian 634107120Sjulian return (0); 635107120Sjulian} /* ng_l2cap_process_con_rsp */ 636107120Sjulian 637107120Sjulian/* 638107120Sjulian * Process L2CAP_ConfigReq command 639107120Sjulian */ 640107120Sjulian 641107120Sjulianstatic int 642107120Sjulianng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident) 643107120Sjulian{ 644107120Sjulian ng_l2cap_p l2cap = con->l2cap; 645107120Sjulian struct mbuf *m = con->rx_pkt; 646107120Sjulian ng_l2cap_cfg_req_cp *cp = NULL; 647107120Sjulian ng_l2cap_chan_p ch = NULL; 648107120Sjulian u_int16_t dcid, respond, result; 649107120Sjulian ng_l2cap_cfg_opt_t hdr; 650107120Sjulian ng_l2cap_cfg_opt_val_t val; 651107120Sjulian int off, error = 0; 652107120Sjulian 653107120Sjulian /* Get command parameters */ 654107120Sjulian con->rx_pkt = NULL; 655107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 656107120Sjulian if (m == NULL) 657107120Sjulian return (ENOBUFS); 658107120Sjulian 659107120Sjulian cp = mtod(m, ng_l2cap_cfg_req_cp *); 660107120Sjulian dcid = le16toh(cp->dcid); 661107120Sjulian respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 662107120Sjulian m_adj(m, sizeof(*cp)); 663107120Sjulian 664107120Sjulian /* Check if we have this channel and it is in valid state */ 665281198Stakawata ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR); 666107120Sjulian if (ch == NULL) { 667107120Sjulian NG_L2CAP_ERR( 668107120Sjulian"%s: %s - unexpected L2CAP_ConfigReq command. " \ 669107120Sjulian"Channel does not exist, cid=%d\n", 670107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid); 671107120Sjulian goto reject; 672107120Sjulian } 673107120Sjulian 674107120Sjulian /* Verify channel state */ 675107120Sjulian if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) { 676107120Sjulian NG_L2CAP_ERR( 677107120Sjulian"%s: %s - unexpected L2CAP_ConfigReq. " \ 678107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 679107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 680107120Sjulian goto reject; 681107120Sjulian } 682107120Sjulian 683107120Sjulian if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */ 684107120Sjulian ch->cfg_state = 0; 685107120Sjulian ch->state = NG_L2CAP_CONFIG; 686107120Sjulian } 687107120Sjulian 688107120Sjulian for (result = 0, off = 0; ; ) { 689107120Sjulian error = get_next_l2cap_opt(m, &off, &hdr, &val); 690107120Sjulian if (error == 0) { /* We done with this packet */ 691107120Sjulian NG_FREE_M(m); 692107120Sjulian break; 693107120Sjulian } else if (error > 0) { /* Got option */ 694107120Sjulian switch (hdr.type) { 695107120Sjulian case NG_L2CAP_OPT_MTU: 696107120Sjulian ch->omtu = val.mtu; 697107120Sjulian break; 698107120Sjulian 699107120Sjulian case NG_L2CAP_OPT_FLUSH_TIMO: 700107120Sjulian ch->flush_timo = val.flush_timo; 701107120Sjulian break; 702107120Sjulian 703107120Sjulian case NG_L2CAP_OPT_QOS: 704107120Sjulian bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow)); 705107120Sjulian break; 706107120Sjulian 707121054Semax default: /* Ignore unknown hint option */ 708107120Sjulian break; 709107120Sjulian } 710107120Sjulian } else { /* Oops, something is wrong */ 711107120Sjulian respond = 1; 712107120Sjulian 713107120Sjulian if (error == -3) { 714107120Sjulian 715107120Sjulian /* 716107120Sjulian * Adjust mbuf so we can get to the start 717107120Sjulian * of the first option we did not like. 718107120Sjulian */ 719107120Sjulian 720107120Sjulian m_adj(m, off - sizeof(hdr)); 721107120Sjulian m->m_pkthdr.len = sizeof(hdr) + hdr.length; 722107120Sjulian 723107120Sjulian result = NG_L2CAP_UNKNOWN_OPTION; 724107120Sjulian } else { 725107120Sjulian /* XXX FIXME Send other reject codes? */ 726107120Sjulian NG_FREE_M(m); 727107120Sjulian result = NG_L2CAP_REJECT; 728107120Sjulian } 729107120Sjulian 730107120Sjulian break; 731107120Sjulian } 732107120Sjulian } 733107120Sjulian 734107120Sjulian /* 735107120Sjulian * Now check and see if we have to respond. If everything was OK then 736107120Sjulian * respond contain "C flag" and (if set) we will respond with empty 737107120Sjulian * packet and will wait for more options. 738107120Sjulian * 739107120Sjulian * Other case is that we did not like peer's options and will respond 740107120Sjulian * with L2CAP_Config response command with Reject error code. 741107120Sjulian * 742107120Sjulian * When "respond == 0" than we have received all options and we will 743107120Sjulian * sent L2CA_ConfigInd event to the upper layer protocol. 744107120Sjulian */ 745107120Sjulian 746107120Sjulian if (respond) { 747107120Sjulian error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); 748107120Sjulian if (error != 0) { 749107120Sjulian ng_l2cap_l2ca_discon_ind(ch); 750107120Sjulian ng_l2cap_free_chan(ch); 751107120Sjulian } 752107120Sjulian } else { 753107120Sjulian /* Send L2CA_ConfigInd event to the upper layer protocol */ 754107120Sjulian ch->ident = ident; 755107120Sjulian error = ng_l2cap_l2ca_cfg_ind(ch); 756107120Sjulian if (error != 0) 757107120Sjulian ng_l2cap_free_chan(ch); 758107120Sjulian } 759107120Sjulian 760107120Sjulian return (error); 761107120Sjulian 762107120Sjulianreject: 763107120Sjulian /* Send reject. Do not really care about the result */ 764107120Sjulian NG_FREE_M(m); 765107120Sjulian 766107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); 767107120Sjulian 768107120Sjulian return (0); 769107120Sjulian} /* ng_l2cap_process_cfg_req */ 770107120Sjulian 771107120Sjulian/* 772107120Sjulian * Process L2CAP_ConfigRsp command 773107120Sjulian */ 774107120Sjulian 775107120Sjulianstatic int 776107120Sjulianng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) 777107120Sjulian{ 778107120Sjulian ng_l2cap_p l2cap = con->l2cap; 779107120Sjulian struct mbuf *m = con->rx_pkt; 780107120Sjulian ng_l2cap_cfg_rsp_cp *cp = NULL; 781107120Sjulian ng_l2cap_cmd_p cmd = NULL; 782107120Sjulian u_int16_t scid, cflag, result; 783107120Sjulian ng_l2cap_cfg_opt_t hdr; 784107120Sjulian ng_l2cap_cfg_opt_val_t val; 785107120Sjulian int off, error = 0; 786107120Sjulian 787107120Sjulian /* Get command parameters */ 788107120Sjulian con->rx_pkt = NULL; 789107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 790107120Sjulian if (m == NULL) 791107120Sjulian return (ENOBUFS); 792107120Sjulian 793107120Sjulian cp = mtod(m, ng_l2cap_cfg_rsp_cp *); 794107120Sjulian scid = le16toh(cp->scid); 795107120Sjulian cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 796107120Sjulian result = le16toh(cp->result); 797107120Sjulian m_adj(m, sizeof(*cp)); 798107120Sjulian 799107120Sjulian /* Check if we have this command */ 800107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 801107120Sjulian if (cmd == NULL) { 802107120Sjulian NG_L2CAP_ERR( 803107120Sjulian"%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 804107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident, 805107120Sjulian con->con_handle); 806107120Sjulian NG_FREE_M(m); 807107120Sjulian 808107120Sjulian return (ENOENT); 809107120Sjulian } 810107120Sjulian 811107120Sjulian /* Verify CIDs and send reject if does not match */ 812107120Sjulian if (cmd->ch->scid != scid) { 813107120Sjulian NG_L2CAP_ERR( 814107120Sjulian"%s: %s - unexpected L2CAP_ConfigRsp. " \ 815107120Sjulian"Channel ID does not match, scid=%d(%d)\n", 816107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 817107120Sjulian scid); 818107120Sjulian goto reject; 819107120Sjulian } 820107120Sjulian 821107120Sjulian /* Verify channel state and reject if invalid */ 822107120Sjulian if (cmd->ch->state != NG_L2CAP_CONFIG) { 823107120Sjulian NG_L2CAP_ERR( 824107120Sjulian"%s: %s - unexpected L2CAP_ConfigRsp. " \ 825107120Sjulian"Invalid channel state, scid=%d, state=%d\n", 826107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 827107120Sjulian cmd->ch->state); 828107120Sjulian goto reject; 829107120Sjulian } 830107120Sjulian 831107120Sjulian /* 832107120Sjulian * Looks like it is our response, so process it. First parse options, 833107120Sjulian * then verify C flag. If it is set then we shall expect more 834107120Sjulian * configuration options from the peer and we will wait. Otherwise we 835107120Sjulian * have received all options and we will send L2CA_ConfigRsp event to 836121054Semax * the upper layer protocol. If command timeout already happened then 837121054Semax * ignore response. 838107120Sjulian */ 839107120Sjulian 840121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 841121054Semax NG_FREE_M(m); 842121054Semax return (error); 843121054Semax } 844107120Sjulian 845107120Sjulian for (off = 0; ; ) { 846107120Sjulian error = get_next_l2cap_opt(m, &off, &hdr, &val); 847107120Sjulian if (error == 0) /* We done with this packet */ 848107120Sjulian break; 849107120Sjulian else if (error > 0) { /* Got option */ 850107120Sjulian switch (hdr.type) { 851107120Sjulian case NG_L2CAP_OPT_MTU: 852107120Sjulian cmd->ch->imtu = val.mtu; 853107120Sjulian break; 854107120Sjulian 855107120Sjulian case NG_L2CAP_OPT_FLUSH_TIMO: 856107120Sjulian cmd->ch->flush_timo = val.flush_timo; 857107120Sjulian break; 858107120Sjulian 859107120Sjulian case NG_L2CAP_OPT_QOS: 860107120Sjulian bcopy(&val.flow, &cmd->ch->oflow, 861107120Sjulian sizeof(cmd->ch->oflow)); 862107120Sjulian break; 863107120Sjulian 864121054Semax default: /* Ignore unknown hint option */ 865107120Sjulian break; 866107120Sjulian } 867107120Sjulian } else { 868107120Sjulian /* 869107120Sjulian * XXX FIXME What to do here? 870107120Sjulian * 871121054Semax * This is really BAD :( options packet was broken, or 872121054Semax * peer sent us option that we did not understand. Let 873121054Semax * upper layer know and do not wait for more options. 874107120Sjulian */ 875107120Sjulian 876107120Sjulian NG_L2CAP_ALERT( 877107120Sjulian"%s: %s - failed to parse configuration options, error=%d\n", 878107120Sjulian __func__, NG_NODE_NAME(l2cap->node), error); 879107120Sjulian 880107120Sjulian result = NG_L2CAP_UNKNOWN; 881107120Sjulian cflag = 0; 882107120Sjulian 883107120Sjulian break; 884107120Sjulian } 885107120Sjulian } 886107120Sjulian 887107120Sjulian NG_FREE_M(m); 888107120Sjulian 889107120Sjulian if (cflag) /* Restart timer and wait for more options */ 890107120Sjulian ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); 891107120Sjulian else { 892107120Sjulian ng_l2cap_unlink_cmd(cmd); 893107120Sjulian 894107120Sjulian /* Send L2CA_Config response to the upper layer protocol */ 895107120Sjulian error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); 896107120Sjulian if (error != 0) { 897107120Sjulian /* 898107120Sjulian * XXX FIXME what to do here? we were not able to send 899107120Sjulian * response to the upper layer protocol, so for now 900107120Sjulian * just close the channel. Send L2CAP_Disconnect to 901107120Sjulian * remote peer? 902107120Sjulian */ 903107120Sjulian 904107120Sjulian NG_L2CAP_ERR( 905107120Sjulian"%s: %s - failed to send L2CA_Config response, error=%d\n", 906107120Sjulian __func__, NG_NODE_NAME(l2cap->node), error); 907107120Sjulian 908107120Sjulian ng_l2cap_free_chan(cmd->ch); 909107120Sjulian } 910107120Sjulian 911107120Sjulian ng_l2cap_free_cmd(cmd); 912107120Sjulian } 913107120Sjulian 914107120Sjulian return (error); 915107120Sjulian 916107120Sjulianreject: 917107120Sjulian /* Send reject. Do not really care about the result */ 918107120Sjulian NG_FREE_M(m); 919107120Sjulian 920107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); 921107120Sjulian 922107120Sjulian return (0); 923107120Sjulian} /* ng_l2cap_process_cfg_rsp */ 924107120Sjulian 925107120Sjulian/* 926107120Sjulian * Process L2CAP_DisconnectReq command 927107120Sjulian */ 928107120Sjulian 929107120Sjulianstatic int 930107120Sjulianng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) 931107120Sjulian{ 932107120Sjulian ng_l2cap_p l2cap = con->l2cap; 933107120Sjulian ng_l2cap_discon_req_cp *cp = NULL; 934107120Sjulian ng_l2cap_chan_p ch = NULL; 935107120Sjulian ng_l2cap_cmd_p cmd = NULL; 936107120Sjulian u_int16_t scid, dcid; 937107120Sjulian 938107120Sjulian /* Get command parameters */ 939107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 940107120Sjulian if (con->rx_pkt == NULL) 941107120Sjulian return (ENOBUFS); 942107120Sjulian 943107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); 944107120Sjulian dcid = le16toh(cp->dcid); 945107120Sjulian scid = le16toh(cp->scid); 946107120Sjulian 947107120Sjulian NG_FREE_M(con->rx_pkt); 948107120Sjulian 949107120Sjulian /* Check if we have this channel and it is in valid state */ 950281198Stakawata ch = ng_l2cap_chan_by_scid(l2cap, dcid, NG_L2CAP_L2CA_IDTYPE_BREDR); 951107120Sjulian if (ch == NULL) { 952107120Sjulian NG_L2CAP_ERR( 953107120Sjulian"%s: %s - unexpected L2CAP_DisconnectReq message. " \ 954107120Sjulian"Channel does not exist, cid=%d\n", 955107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid); 956107120Sjulian goto reject; 957107120Sjulian } 958107120Sjulian 959107120Sjulian /* XXX Verify channel state and reject if invalid -- is that true? */ 960114878Sjulian if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG && 961114878Sjulian ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 962107120Sjulian NG_L2CAP_ERR( 963107120Sjulian"%s: %s - unexpected L2CAP_DisconnectReq. " \ 964107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 965107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 966107120Sjulian goto reject; 967107120Sjulian } 968107120Sjulian 969107120Sjulian /* Match destination channel ID */ 970107120Sjulian if (ch->dcid != scid || ch->scid != dcid) { 971107120Sjulian NG_L2CAP_ERR( 972107120Sjulian"%s: %s - unexpected L2CAP_DisconnectReq. " \ 973107120Sjulian"Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 974107120Sjulian"request: scid=%d, dcid=%d\n", 975107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 976107120Sjulian scid, dcid); 977107120Sjulian goto reject; 978107120Sjulian } 979107120Sjulian 980107120Sjulian /* 981107120Sjulian * Looks good, so notify upper layer protocol that channel is about 982107120Sjulian * to be disconnected and send L2CA_DisconnectInd message. Then respond 983107120Sjulian * with L2CAP_DisconnectRsp. 984107120Sjulian */ 985107120Sjulian 986114878Sjulian if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 987114878Sjulian ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 988114878Sjulian ng_l2cap_free_chan(ch); 989114878Sjulian } 990107120Sjulian 991107120Sjulian /* Send L2CAP_DisconnectRsp */ 992107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 993107120Sjulian if (cmd == NULL) 994107120Sjulian return (ENOMEM); 995107120Sjulian 996107120Sjulian _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 997107120Sjulian if (cmd->aux == NULL) { 998107120Sjulian ng_l2cap_free_cmd(cmd); 999107120Sjulian 1000107120Sjulian return (ENOBUFS); 1001107120Sjulian } 1002107120Sjulian 1003107120Sjulian /* Link command to the queue */ 1004107120Sjulian ng_l2cap_link_cmd(con, cmd); 1005107120Sjulian ng_l2cap_lp_deliver(con); 1006107120Sjulian 1007107120Sjulian return (0); 1008107120Sjulian 1009107120Sjulianreject: 1010107120Sjulian /* Send reject. Do not really care about the result */ 1011107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 1012107120Sjulian 1013107120Sjulian return (0); 1014107120Sjulian} /* ng_l2cap_process_discon_req */ 1015107120Sjulian 1016107120Sjulian/* 1017107120Sjulian * Process L2CAP_DisconnectRsp command 1018107120Sjulian */ 1019107120Sjulian 1020107120Sjulianstatic int 1021107120Sjulianng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 1022107120Sjulian{ 1023107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1024107120Sjulian ng_l2cap_discon_rsp_cp *cp = NULL; 1025107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1026107120Sjulian u_int16_t scid, dcid; 1027107120Sjulian int error = 0; 1028107120Sjulian 1029107120Sjulian /* Get command parameters */ 1030107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1031107120Sjulian if (con->rx_pkt == NULL) 1032107120Sjulian return (ENOBUFS); 1033107120Sjulian 1034107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 1035107120Sjulian dcid = le16toh(cp->dcid); 1036107120Sjulian scid = le16toh(cp->scid); 1037107120Sjulian 1038107120Sjulian NG_FREE_M(con->rx_pkt); 1039107120Sjulian 1040107120Sjulian /* Check if we have pending command descriptor */ 1041107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 1042107120Sjulian if (cmd == NULL) { 1043107120Sjulian NG_L2CAP_ERR( 1044107120Sjulian"%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 1045107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident, 1046107120Sjulian con->con_handle); 1047107120Sjulian goto out; 1048107120Sjulian } 1049107120Sjulian 1050107120Sjulian /* Verify channel state, do nothing if invalid */ 1051107120Sjulian if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 1052107120Sjulian NG_L2CAP_ERR( 1053107120Sjulian"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 1054107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 1055107120Sjulian __func__, NG_NODE_NAME(l2cap->node), scid, 1056107120Sjulian cmd->ch->state); 1057107120Sjulian goto out; 1058107120Sjulian } 1059107120Sjulian 1060107120Sjulian /* Verify CIDs and send reject if does not match */ 1061107120Sjulian if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 1062107120Sjulian NG_L2CAP_ERR( 1063107120Sjulian"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 1064107120Sjulian"Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 1065107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 1066107120Sjulian scid, cmd->ch->dcid, dcid); 1067107120Sjulian goto out; 1068107120Sjulian } 1069107120Sjulian 1070107120Sjulian /* 1071298813Spfg * Looks like we have successfully disconnected channel, so notify 1072121054Semax * upper layer. If command timeout already happened then ignore 1073121054Semax * response. 1074107120Sjulian */ 1075107120Sjulian 1076121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 1077121054Semax goto out; 1078121054Semax 1079107120Sjulian error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 1080107120Sjulian ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 1081107120Sjulianout: 1082107120Sjulian return (error); 1083107120Sjulian} /* ng_l2cap_process_discon_rsp */ 1084107120Sjulian 1085107120Sjulian/* 1086107120Sjulian * Process L2CAP_EchoReq command 1087107120Sjulian */ 1088107120Sjulian 1089107120Sjulianstatic int 1090107120Sjulianng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 1091107120Sjulian{ 1092107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1093107120Sjulian ng_l2cap_cmd_hdr_t *hdr = NULL; 1094107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1095107120Sjulian 1096107120Sjulian con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 1097107120Sjulian if (con->rx_pkt == NULL) { 1098107120Sjulian NG_L2CAP_ALERT( 1099128076Semax"%s: %s - ng_l2cap_prepend() failed, size=%zd\n", 1100107120Sjulian __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 1101107120Sjulian 1102107120Sjulian return (ENOBUFS); 1103107120Sjulian } 1104107120Sjulian 1105107120Sjulian hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 1106107120Sjulian hdr->code = NG_L2CAP_ECHO_RSP; 1107107120Sjulian hdr->ident = ident; 1108107120Sjulian hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 1109107120Sjulian 1110107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 1111107120Sjulian if (cmd == NULL) { 1112107120Sjulian NG_FREE_M(con->rx_pkt); 1113107120Sjulian 1114107120Sjulian return (ENOBUFS); 1115107120Sjulian } 1116107120Sjulian 1117107120Sjulian /* Attach data and link command to the queue */ 1118107120Sjulian cmd->aux = con->rx_pkt; 1119107120Sjulian con->rx_pkt = NULL; 1120107120Sjulian ng_l2cap_link_cmd(con, cmd); 1121107120Sjulian ng_l2cap_lp_deliver(con); 1122107120Sjulian 1123107120Sjulian return (0); 1124107120Sjulian} /* ng_l2cap_process_echo_req */ 1125107120Sjulian 1126107120Sjulian/* 1127107120Sjulian * Process L2CAP_EchoRsp command 1128107120Sjulian */ 1129107120Sjulian 1130107120Sjulianstatic int 1131107120Sjulianng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1132107120Sjulian{ 1133107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1134107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1135107120Sjulian int error = 0; 1136107120Sjulian 1137107120Sjulian /* Check if we have this command */ 1138107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 1139107120Sjulian if (cmd != NULL) { 1140121054Semax /* If command timeout already happened then ignore response */ 1141121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1142121054Semax NG_FREE_M(con->rx_pkt); 1143121054Semax return (error); 1144121054Semax } 1145107120Sjulian 1146107120Sjulian ng_l2cap_unlink_cmd(cmd); 1147107120Sjulian 1148107120Sjulian error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1149107120Sjulian NG_L2CAP_SUCCESS, con->rx_pkt); 1150107120Sjulian 1151107120Sjulian ng_l2cap_free_cmd(cmd); 1152107120Sjulian con->rx_pkt = NULL; 1153107120Sjulian } else { 1154107120Sjulian NG_L2CAP_ERR( 1155107120Sjulian"%s: %s - unexpected L2CAP_EchoRsp command. " \ 1156107120Sjulian"Requested ident does not exist, ident=%d\n", 1157107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident); 1158107120Sjulian NG_FREE_M(con->rx_pkt); 1159107120Sjulian } 1160107120Sjulian 1161107120Sjulian return (error); 1162107120Sjulian} /* ng_l2cap_process_echo_rsp */ 1163107120Sjulian 1164107120Sjulian/* 1165107120Sjulian * Process L2CAP_InfoReq command 1166107120Sjulian */ 1167107120Sjulian 1168107120Sjulianstatic int 1169107120Sjulianng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1170107120Sjulian{ 1171107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1172107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1173107120Sjulian u_int16_t type; 1174107120Sjulian 1175107120Sjulian /* Get command parameters */ 1176107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1177107120Sjulian if (con->rx_pkt == NULL) 1178107120Sjulian return (ENOBUFS); 1179107120Sjulian 1180107120Sjulian type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1181107120Sjulian NG_FREE_M(con->rx_pkt); 1182107120Sjulian 1183107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1184107120Sjulian if (cmd == NULL) 1185107120Sjulian return (ENOMEM); 1186107120Sjulian 1187107120Sjulian switch (type) { 1188107120Sjulian case NG_L2CAP_CONNLESS_MTU: 1189107120Sjulian _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1190107120Sjulian NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1191107120Sjulian break; 1192107120Sjulian 1193107120Sjulian default: 1194107120Sjulian _ng_l2cap_info_rsp(cmd->aux, ident, type, 1195107120Sjulian NG_L2CAP_NOT_SUPPORTED, 0); 1196107120Sjulian break; 1197107120Sjulian } 1198107120Sjulian 1199107120Sjulian if (cmd->aux == NULL) { 1200107120Sjulian ng_l2cap_free_cmd(cmd); 1201107120Sjulian 1202107120Sjulian return (ENOBUFS); 1203107120Sjulian } 1204107120Sjulian 1205107120Sjulian /* Link command to the queue */ 1206107120Sjulian ng_l2cap_link_cmd(con, cmd); 1207107120Sjulian ng_l2cap_lp_deliver(con); 1208107120Sjulian 1209107120Sjulian return (0); 1210107120Sjulian} /* ng_l2cap_process_info_req */ 1211107120Sjulian 1212107120Sjulian/* 1213107120Sjulian * Process L2CAP_InfoRsp command 1214107120Sjulian */ 1215107120Sjulian 1216107120Sjulianstatic int 1217107120Sjulianng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1218107120Sjulian{ 1219107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1220107120Sjulian ng_l2cap_info_rsp_cp *cp = NULL; 1221107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1222107120Sjulian int error = 0; 1223107120Sjulian 1224107120Sjulian /* Get command parameters */ 1225107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1226107120Sjulian if (con->rx_pkt == NULL) 1227107120Sjulian return (ENOBUFS); 1228107120Sjulian 1229107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1230107120Sjulian cp->type = le16toh(cp->type); 1231107120Sjulian cp->result = le16toh(cp->result); 1232107120Sjulian m_adj(con->rx_pkt, sizeof(*cp)); 1233107120Sjulian 1234107120Sjulian /* Check if we have pending command descriptor */ 1235107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 1236107120Sjulian if (cmd == NULL) { 1237107120Sjulian NG_L2CAP_ERR( 1238107120Sjulian"%s: %s - unexpected L2CAP_InfoRsp command. " \ 1239107120Sjulian"Requested ident does not exist, ident=%d\n", 1240107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident); 1241107120Sjulian NG_FREE_M(con->rx_pkt); 1242107120Sjulian 1243107120Sjulian return (ENOENT); 1244107120Sjulian } 1245107120Sjulian 1246121054Semax /* If command timeout already happened then ignore response */ 1247121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1248121054Semax NG_FREE_M(con->rx_pkt); 1249121054Semax return (error); 1250121054Semax } 1251107120Sjulian 1252107120Sjulian ng_l2cap_unlink_cmd(cmd); 1253107120Sjulian 1254107120Sjulian if (cp->result == NG_L2CAP_SUCCESS) { 1255107120Sjulian switch (cp->type) { 1256107120Sjulian case NG_L2CAP_CONNLESS_MTU: 1257107120Sjulian if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1258107120Sjulian *mtod(con->rx_pkt, u_int16_t *) = 1259107120Sjulian le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1260107120Sjulian else { 1261107120Sjulian cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1262107120Sjulian 1263107120Sjulian NG_L2CAP_ERR( 1264107120Sjulian"%s: %s - invalid L2CAP_InfoRsp command. " \ 1265107120Sjulian"Bad connectionless MTU parameter, len=%d\n", 1266107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 1267107120Sjulian con->rx_pkt->m_pkthdr.len); 1268107120Sjulian } 1269107120Sjulian break; 1270107120Sjulian 1271107120Sjulian default: 1272107120Sjulian NG_L2CAP_WARN( 1273107120Sjulian"%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1274107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cp->type); 1275107120Sjulian break; 1276107120Sjulian } 1277107120Sjulian } 1278107120Sjulian 1279107120Sjulian error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1280107120Sjulian cp->result, con->rx_pkt); 1281107120Sjulian 1282107120Sjulian ng_l2cap_free_cmd(cmd); 1283107120Sjulian con->rx_pkt = NULL; 1284107120Sjulian 1285107120Sjulian return (error); 1286107120Sjulian} /* ng_l2cap_process_info_rsp */ 1287107120Sjulian 1288107120Sjulian/* 1289107120Sjulian * Send L2CAP reject 1290107120Sjulian */ 1291107120Sjulian 1292107120Sjulianstatic int 1293107120Sjuliansend_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1294107120Sjulian u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1295107120Sjulian{ 1296107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1297107120Sjulian 1298107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1299107120Sjulian if (cmd == NULL) 1300107120Sjulian return (ENOMEM); 1301107120Sjulian 1302107120Sjulian _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1303107120Sjulian if (cmd->aux == NULL) { 1304107120Sjulian ng_l2cap_free_cmd(cmd); 1305107120Sjulian 1306107120Sjulian return (ENOBUFS); 1307107120Sjulian } 1308107120Sjulian 1309107120Sjulian /* Link command to the queue */ 1310107120Sjulian ng_l2cap_link_cmd(con, cmd); 1311107120Sjulian ng_l2cap_lp_deliver(con); 1312107120Sjulian 1313107120Sjulian return (0); 1314107120Sjulian} /* send_l2cap_reject */ 1315107120Sjulian 1316107120Sjulian/* 1317107120Sjulian * Send L2CAP connection reject 1318107120Sjulian */ 1319107120Sjulian 1320107120Sjulianstatic int 1321107120Sjuliansend_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1322107120Sjulian u_int16_t dcid, u_int16_t result) 1323107120Sjulian{ 1324107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1325107120Sjulian 1326107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1327107120Sjulian if (cmd == NULL) 1328107120Sjulian return (ENOMEM); 1329107120Sjulian 1330107120Sjulian _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1331107120Sjulian if (cmd->aux == NULL) { 1332107120Sjulian ng_l2cap_free_cmd(cmd); 1333107120Sjulian 1334107120Sjulian return (ENOBUFS); 1335107120Sjulian } 1336107120Sjulian 1337107120Sjulian /* Link command to the queue */ 1338107120Sjulian ng_l2cap_link_cmd(con, cmd); 1339107120Sjulian ng_l2cap_lp_deliver(con); 1340107120Sjulian 1341107120Sjulian return (0); 1342107120Sjulian} /* send_l2cap_con_rej */ 1343107120Sjulian 1344107120Sjulian/* 1345107120Sjulian * Send L2CAP config response 1346107120Sjulian */ 1347107120Sjulian 1348107120Sjulianstatic int 1349107120Sjuliansend_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1350107120Sjulian u_int16_t result, struct mbuf *opt) 1351107120Sjulian{ 1352107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1353107120Sjulian 1354107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1355107120Sjulian if (cmd == NULL) { 1356107120Sjulian NG_FREE_M(opt); 1357107120Sjulian 1358107120Sjulian return (ENOMEM); 1359107120Sjulian } 1360107120Sjulian 1361107120Sjulian _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1362107120Sjulian if (cmd->aux == NULL) { 1363107120Sjulian ng_l2cap_free_cmd(cmd); 1364107120Sjulian 1365107120Sjulian return (ENOBUFS); 1366107120Sjulian } 1367107120Sjulian 1368107120Sjulian /* Link command to the queue */ 1369107120Sjulian ng_l2cap_link_cmd(con, cmd); 1370107120Sjulian ng_l2cap_lp_deliver(con); 1371107120Sjulian 1372107120Sjulian return (0); 1373107120Sjulian} /* send_l2cap_cfg_rsp */ 1374107120Sjulian 1375281198Stakawatastatic int 1376281198Stakawatasend_l2cap_param_urs(ng_l2cap_con_p con, u_int8_t ident, 1377281198Stakawata u_int16_t result) 1378281198Stakawata{ 1379281198Stakawata ng_l2cap_cmd_p cmd = NULL; 1380281198Stakawata 1381281198Stakawata cmd = ng_l2cap_new_cmd(con, NULL, ident, 1382281198Stakawata NG_L2CAP_CMD_PARAM_UPDATE_RESPONSE, 1383281198Stakawata 0); 1384281198Stakawata if (cmd == NULL) { 1385281198Stakawata 1386281198Stakawata return (ENOMEM); 1387281198Stakawata } 1388281198Stakawata 1389281198Stakawata _ng_l2cap_cmd_urs(cmd->aux, cmd->ident, result); 1390281198Stakawata if (cmd->aux == NULL) { 1391281198Stakawata ng_l2cap_free_cmd(cmd); 1392281198Stakawata 1393281198Stakawata return (ENOBUFS); 1394281198Stakawata } 1395281198Stakawata 1396281198Stakawata /* Link command to the queue */ 1397281198Stakawata ng_l2cap_link_cmd(con, cmd); 1398281198Stakawata ng_l2cap_lp_deliver(con); 1399281198Stakawata 1400281198Stakawata return (0); 1401281198Stakawata} /* send_l2cap_cfg_rsp */ 1402281198Stakawata 1403107120Sjulian/* 1404107120Sjulian * Get next L2CAP configuration option 1405107120Sjulian * 1406107120Sjulian * Return codes: 1407107120Sjulian * 0 no option 1408107120Sjulian * 1 we have got option 1409107120Sjulian * -1 header too short 1410107120Sjulian * -2 bad option value or length 1411107120Sjulian * -3 unknown option 1412107120Sjulian */ 1413107120Sjulian 1414107120Sjulianstatic int 1415107120Sjulianget_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1416107120Sjulian ng_l2cap_cfg_opt_val_p val) 1417107120Sjulian{ 1418107120Sjulian int hint, len = m->m_pkthdr.len - (*off); 1419107120Sjulian 1420107120Sjulian if (len == 0) 1421107120Sjulian return (0); 1422107120Sjulian if (len < 0 || len < sizeof(*hdr)) 1423107120Sjulian return (-1); 1424107120Sjulian 1425107120Sjulian m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1426107120Sjulian *off += sizeof(*hdr); 1427107120Sjulian len -= sizeof(*hdr); 1428107120Sjulian 1429107120Sjulian hint = NG_L2CAP_OPT_HINT(hdr->type); 1430107120Sjulian hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1431107120Sjulian 1432107120Sjulian switch (hdr->type) { 1433107120Sjulian case NG_L2CAP_OPT_MTU: 1434107120Sjulian if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1435107120Sjulian return (-2); 1436107120Sjulian 1437107120Sjulian m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1438107120Sjulian val->mtu = le16toh(val->mtu); 1439107120Sjulian *off += NG_L2CAP_OPT_MTU_SIZE; 1440107120Sjulian break; 1441107120Sjulian 1442107120Sjulian case NG_L2CAP_OPT_FLUSH_TIMO: 1443107120Sjulian if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1444107120Sjulian len < hdr->length) 1445107120Sjulian return (-2); 1446107120Sjulian 1447107120Sjulian m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1448107120Sjulian val->flush_timo = le16toh(val->flush_timo); 1449107120Sjulian *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1450107120Sjulian break; 1451107120Sjulian 1452107120Sjulian case NG_L2CAP_OPT_QOS: 1453107120Sjulian if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1454107120Sjulian return (-2); 1455107120Sjulian 1456107120Sjulian m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1457107120Sjulian val->flow.token_rate = le32toh(val->flow.token_rate); 1458107120Sjulian val->flow.token_bucket_size = 1459107120Sjulian le32toh(val->flow.token_bucket_size); 1460107120Sjulian val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1461107120Sjulian val->flow.latency = le32toh(val->flow.latency); 1462107120Sjulian val->flow.delay_variation = le32toh(val->flow.delay_variation); 1463107120Sjulian *off += NG_L2CAP_OPT_QOS_SIZE; 1464107120Sjulian break; 1465107120Sjulian 1466107120Sjulian default: 1467107120Sjulian if (hint) 1468107120Sjulian *off += hdr->length; 1469107120Sjulian else 1470107120Sjulian return (-3); 1471107120Sjulian break; 1472107120Sjulian } 1473107120Sjulian 1474107120Sjulian return (1); 1475107120Sjulian} /* get_next_l2cap_opt */ 1476107120Sjulian 1477