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: releng/10.3/sys/netgraph/bluetooth/l2cap/ng_l2cap_evnt.c 243882 2012-12-05 08:04:20Z glebius $ 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); 60107120Sjulianstatic int ng_l2cap_process_cmd_rej (ng_l2cap_con_p, u_int8_t); 61107120Sjulianstatic int ng_l2cap_process_con_req (ng_l2cap_con_p, u_int8_t); 62107120Sjulianstatic int ng_l2cap_process_con_rsp (ng_l2cap_con_p, u_int8_t); 63107120Sjulianstatic int ng_l2cap_process_cfg_req (ng_l2cap_con_p, u_int8_t); 64107120Sjulianstatic int ng_l2cap_process_cfg_rsp (ng_l2cap_con_p, u_int8_t); 65107120Sjulianstatic int ng_l2cap_process_discon_req (ng_l2cap_con_p, u_int8_t); 66107120Sjulianstatic int ng_l2cap_process_discon_rsp (ng_l2cap_con_p, u_int8_t); 67107120Sjulianstatic int ng_l2cap_process_echo_req (ng_l2cap_con_p, u_int8_t); 68107120Sjulianstatic int ng_l2cap_process_echo_rsp (ng_l2cap_con_p, u_int8_t); 69107120Sjulianstatic int ng_l2cap_process_info_req (ng_l2cap_con_p, u_int8_t); 70107120Sjulianstatic int ng_l2cap_process_info_rsp (ng_l2cap_con_p, u_int8_t); 71107120Sjulianstatic int send_l2cap_reject 72107120Sjulian (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t, u_int16_t); 73107120Sjulianstatic int send_l2cap_con_rej 74107120Sjulian (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, u_int16_t); 75107120Sjulianstatic int send_l2cap_cfg_rsp 76107120Sjulian (ng_l2cap_con_p, u_int8_t, u_int16_t, u_int16_t, struct mbuf *); 77107120Sjulianstatic int get_next_l2cap_opt 78107120Sjulian (struct mbuf *, int *, ng_l2cap_cfg_opt_p, ng_l2cap_cfg_opt_val_p); 79107120Sjulian 80107120Sjulian/* 81107120Sjulian * Receive L2CAP packet. First get L2CAP header and verify packet. Than 82107120Sjulian * get destination channel and process packet. 83107120Sjulian */ 84107120Sjulian 85107120Sjulianint 86107120Sjulianng_l2cap_receive(ng_l2cap_con_p con) 87107120Sjulian{ 88107120Sjulian ng_l2cap_p l2cap = con->l2cap; 89107120Sjulian ng_l2cap_hdr_t *hdr = NULL; 90107120Sjulian int error = 0; 91107120Sjulian 92107120Sjulian /* Check packet */ 93107120Sjulian if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 94107120Sjulian NG_L2CAP_ERR( 95107120Sjulian"%s: %s - invalid L2CAP packet. Packet too small, len=%d\n", 96107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 97107120Sjulian con->rx_pkt->m_pkthdr.len); 98107120Sjulian error = EMSGSIZE; 99107120Sjulian goto drop; 100107120Sjulian } 101107120Sjulian 102107120Sjulian /* Get L2CAP header */ 103107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 104107120Sjulian if (con->rx_pkt == NULL) 105107120Sjulian return (ENOBUFS); 106107120Sjulian 107107120Sjulian hdr = mtod(con->rx_pkt, ng_l2cap_hdr_t *); 108107120Sjulian hdr->length = le16toh(hdr->length); 109107120Sjulian hdr->dcid = le16toh(hdr->dcid); 110107120Sjulian 111107120Sjulian /* Check payload size */ 112107120Sjulian if (hdr->length != con->rx_pkt->m_pkthdr.len - sizeof(*hdr)) { 113107120Sjulian NG_L2CAP_ERR( 114128076Semax"%s: %s - invalid L2CAP packet. Payload length mismatch, length=%d, len=%zd\n", 115107120Sjulian __func__, NG_NODE_NAME(l2cap->node), hdr->length, 116107120Sjulian con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 117107120Sjulian error = EMSGSIZE; 118107120Sjulian goto drop; 119107120Sjulian } 120107120Sjulian 121107120Sjulian /* Process packet */ 122107120Sjulian switch (hdr->dcid) { 123107120Sjulian case NG_L2CAP_SIGNAL_CID: /* L2CAP command */ 124107120Sjulian m_adj(con->rx_pkt, sizeof(*hdr)); 125107120Sjulian error = ng_l2cap_process_signal_cmd(con); 126107120Sjulian break; 127107120Sjulian 128107120Sjulian case NG_L2CAP_CLT_CID: /* Connectionless packet */ 129107120Sjulian error = ng_l2cap_l2ca_clt_receive(con); 130107120Sjulian break; 131107120Sjulian 132107120Sjulian default: /* Data packet */ 133107120Sjulian error = ng_l2cap_l2ca_receive(con); 134107120Sjulian break; 135107120Sjulian } 136107120Sjulian 137107120Sjulian return (error); 138107120Sjuliandrop: 139107120Sjulian NG_FREE_M(con->rx_pkt); 140107120Sjulian 141107120Sjulian return (error); 142107120Sjulian} /* ng_l2cap_receive */ 143107120Sjulian 144107120Sjulian/* 145107120Sjulian * Process L2CAP signaling command. We already know that destination channel ID 146107120Sjulian * is 0x1 that means we have received signaling command from peer's L2CAP layer. 147107120Sjulian * So get command header, decode and process it. 148107120Sjulian * 149107120Sjulian * XXX do we need to check signaling MTU here? 150107120Sjulian */ 151107120Sjulian 152107120Sjulianstatic int 153107120Sjulianng_l2cap_process_signal_cmd(ng_l2cap_con_p con) 154107120Sjulian{ 155107120Sjulian ng_l2cap_p l2cap = con->l2cap; 156107120Sjulian ng_l2cap_cmd_hdr_t *hdr = NULL; 157107120Sjulian struct mbuf *m = NULL; 158107120Sjulian 159107120Sjulian while (con->rx_pkt != NULL) { 160107120Sjulian /* Verify packet length */ 161107120Sjulian if (con->rx_pkt->m_pkthdr.len < sizeof(*hdr)) { 162107120Sjulian NG_L2CAP_ERR( 163107120Sjulian"%s: %s - invalid L2CAP signaling command. Packet too small, len=%d\n", 164107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 165107120Sjulian con->rx_pkt->m_pkthdr.len); 166107120Sjulian NG_FREE_M(con->rx_pkt); 167107120Sjulian 168107120Sjulian return (EMSGSIZE); 169107120Sjulian } 170107120Sjulian 171107120Sjulian /* Get signaling command */ 172107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr)); 173107120Sjulian if (con->rx_pkt == NULL) 174107120Sjulian return (ENOBUFS); 175107120Sjulian 176107120Sjulian hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 177107120Sjulian hdr->length = le16toh(hdr->length); 178107120Sjulian m_adj(con->rx_pkt, sizeof(*hdr)); 179107120Sjulian 180107120Sjulian /* Verify command length */ 181107120Sjulian if (con->rx_pkt->m_pkthdr.len < hdr->length) { 182107120Sjulian NG_L2CAP_ERR( 183107120Sjulian"%s: %s - invalid L2CAP signaling command, code=%#x, ident=%d. " \ 184107120Sjulian"Invalid command length=%d, m_pkthdr.len=%d\n", 185107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 186107120Sjulian hdr->code, hdr->ident, hdr->length, 187107120Sjulian con->rx_pkt->m_pkthdr.len); 188107120Sjulian NG_FREE_M(con->rx_pkt); 189107120Sjulian 190107120Sjulian return (EMSGSIZE); 191107120Sjulian } 192107120Sjulian 193107120Sjulian /* Get the command, save the rest (if any) */ 194107120Sjulian if (con->rx_pkt->m_pkthdr.len > hdr->length) 195243882Sglebius m = m_split(con->rx_pkt, hdr->length, M_NOWAIT); 196107120Sjulian else 197107120Sjulian m = NULL; 198107120Sjulian 199107120Sjulian /* Process command */ 200107120Sjulian switch (hdr->code) { 201107120Sjulian case NG_L2CAP_CMD_REJ: 202107120Sjulian ng_l2cap_process_cmd_rej(con, hdr->ident); 203107120Sjulian break; 204107120Sjulian 205107120Sjulian case NG_L2CAP_CON_REQ: 206107120Sjulian ng_l2cap_process_con_req(con, hdr->ident); 207107120Sjulian break; 208107120Sjulian 209107120Sjulian case NG_L2CAP_CON_RSP: 210107120Sjulian ng_l2cap_process_con_rsp(con, hdr->ident); 211107120Sjulian break; 212107120Sjulian 213107120Sjulian case NG_L2CAP_CFG_REQ: 214107120Sjulian ng_l2cap_process_cfg_req(con, hdr->ident); 215107120Sjulian break; 216107120Sjulian 217107120Sjulian case NG_L2CAP_CFG_RSP: 218107120Sjulian ng_l2cap_process_cfg_rsp(con, hdr->ident); 219107120Sjulian break; 220107120Sjulian 221107120Sjulian case NG_L2CAP_DISCON_REQ: 222107120Sjulian ng_l2cap_process_discon_req(con, hdr->ident); 223107120Sjulian break; 224107120Sjulian 225107120Sjulian case NG_L2CAP_DISCON_RSP: 226107120Sjulian ng_l2cap_process_discon_rsp(con, hdr->ident); 227107120Sjulian break; 228107120Sjulian 229107120Sjulian case NG_L2CAP_ECHO_REQ: 230107120Sjulian ng_l2cap_process_echo_req(con, hdr->ident); 231107120Sjulian break; 232107120Sjulian 233107120Sjulian case NG_L2CAP_ECHO_RSP: 234107120Sjulian ng_l2cap_process_echo_rsp(con, hdr->ident); 235107120Sjulian break; 236107120Sjulian 237107120Sjulian case NG_L2CAP_INFO_REQ: 238107120Sjulian ng_l2cap_process_info_req(con, hdr->ident); 239107120Sjulian break; 240107120Sjulian 241107120Sjulian case NG_L2CAP_INFO_RSP: 242107120Sjulian ng_l2cap_process_info_rsp(con, hdr->ident); 243107120Sjulian break; 244107120Sjulian 245107120Sjulian default: 246107120Sjulian NG_L2CAP_ERR( 247107120Sjulian"%s: %s - unknown L2CAP signaling command, code=%#x, ident=%d, length=%d\n", 248107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 249107120Sjulian hdr->code, hdr->ident, hdr->length); 250107120Sjulian 251107120Sjulian /* 252107120Sjulian * Send L2CAP_CommandRej. Do not really care 253107120Sjulian * about the result 254107120Sjulian */ 255107120Sjulian 256107120Sjulian send_l2cap_reject(con, hdr->ident, 257107120Sjulian NG_L2CAP_REJ_NOT_UNDERSTOOD, 0, 0, 0); 258107120Sjulian NG_FREE_M(con->rx_pkt); 259107120Sjulian break; 260107120Sjulian } 261107120Sjulian 262107120Sjulian con->rx_pkt = m; 263107120Sjulian } 264107120Sjulian 265107120Sjulian return (0); 266107120Sjulian} /* ng_l2cap_process_signal_cmd */ 267107120Sjulian 268107120Sjulian/* 269107120Sjulian * Process L2CAP_CommandRej command 270107120Sjulian */ 271107120Sjulian 272107120Sjulianstatic int 273107120Sjulianng_l2cap_process_cmd_rej(ng_l2cap_con_p con, u_int8_t ident) 274107120Sjulian{ 275107120Sjulian ng_l2cap_p l2cap = con->l2cap; 276107120Sjulian ng_l2cap_cmd_rej_cp *cp = NULL; 277107120Sjulian ng_l2cap_cmd_p cmd = NULL; 278107120Sjulian 279107120Sjulian /* Get command parameters */ 280107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 281107120Sjulian if (con->rx_pkt == NULL) 282107120Sjulian return (ENOBUFS); 283107120Sjulian 284107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_cmd_rej_cp *); 285107120Sjulian cp->reason = le16toh(cp->reason); 286107120Sjulian 287107120Sjulian /* Check if we have pending command descriptor */ 288107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 289107120Sjulian if (cmd != NULL) { 290121054Semax /* If command timeout already happened then ignore reject */ 291121054Semax if (ng_l2cap_command_untimeout(cmd) != 0) { 292121054Semax NG_FREE_M(con->rx_pkt); 293121054Semax return (ETIMEDOUT); 294121054Semax } 295107120Sjulian 296107120Sjulian ng_l2cap_unlink_cmd(cmd); 297107120Sjulian 298107120Sjulian switch (cmd->code) { 299107120Sjulian case NG_L2CAP_CON_REQ: 300107120Sjulian ng_l2cap_l2ca_con_rsp(cmd->ch,cmd->token,cp->reason,0); 301107120Sjulian ng_l2cap_free_chan(cmd->ch); 302107120Sjulian break; 303107120Sjulian 304107120Sjulian case NG_L2CAP_CFG_REQ: 305107120Sjulian ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, cp->reason); 306107120Sjulian break; 307107120Sjulian 308107120Sjulian case NG_L2CAP_DISCON_REQ: 309107120Sjulian ng_l2cap_l2ca_discon_rsp(cmd->ch,cmd->token,cp->reason); 310107120Sjulian ng_l2cap_free_chan(cmd->ch); /* XXX free channel */ 311107120Sjulian break; 312107120Sjulian 313107120Sjulian case NG_L2CAP_ECHO_REQ: 314107120Sjulian ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 315107120Sjulian cp->reason, NULL); 316107120Sjulian break; 317107120Sjulian 318107120Sjulian case NG_L2CAP_INFO_REQ: 319107120Sjulian ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 320107120Sjulian cp->reason, NULL); 321107120Sjulian break; 322107120Sjulian 323107120Sjulian default: 324107120Sjulian NG_L2CAP_ALERT( 325107120Sjulian"%s: %s - unexpected L2CAP_CommandRej. Unexpected L2CAP command opcode=%d\n", 326107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->code); 327107120Sjulian break; 328107120Sjulian } 329107120Sjulian 330107120Sjulian ng_l2cap_free_cmd(cmd); 331107120Sjulian } else 332107120Sjulian NG_L2CAP_ERR( 333107120Sjulian"%s: %s - unexpected L2CAP_CommandRej command. " \ 334107120Sjulian"Requested ident does not exist, ident=%d\n", 335107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident); 336107120Sjulian 337107120Sjulian NG_FREE_M(con->rx_pkt); 338107120Sjulian 339107120Sjulian return (0); 340107120Sjulian} /* ng_l2cap_process_cmd_rej */ 341107120Sjulian 342107120Sjulian/* 343107120Sjulian * Process L2CAP_ConnectReq command 344107120Sjulian */ 345107120Sjulian 346107120Sjulianstatic int 347107120Sjulianng_l2cap_process_con_req(ng_l2cap_con_p con, u_int8_t ident) 348107120Sjulian{ 349107120Sjulian ng_l2cap_p l2cap = con->l2cap; 350107120Sjulian struct mbuf *m = con->rx_pkt; 351107120Sjulian ng_l2cap_con_req_cp *cp = NULL; 352107120Sjulian ng_l2cap_chan_p ch = NULL; 353107120Sjulian int error = 0; 354107120Sjulian u_int16_t dcid, psm; 355107120Sjulian 356107120Sjulian /* Get command parameters */ 357107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 358107120Sjulian if (m == NULL) 359107120Sjulian return (ENOBUFS); 360107120Sjulian 361107120Sjulian cp = mtod(m, ng_l2cap_con_req_cp *); 362107120Sjulian psm = le16toh(cp->psm); 363107120Sjulian dcid = le16toh(cp->scid); 364107120Sjulian 365107120Sjulian NG_FREE_M(m); 366107120Sjulian con->rx_pkt = NULL; 367107120Sjulian 368107120Sjulian /* 369107120Sjulian * Create new channel and send L2CA_ConnectInd notification 370107120Sjulian * to the upper layer protocol. 371107120Sjulian */ 372107120Sjulian 373107120Sjulian ch = ng_l2cap_new_chan(l2cap, con, psm); 374107120Sjulian if (ch == NULL) 375107120Sjulian return (send_l2cap_con_rej(con, ident, 0, dcid, 376107120Sjulian NG_L2CAP_NO_RESOURCES)); 377107120Sjulian 378107120Sjulian /* Update channel IDs */ 379107120Sjulian ch->dcid = dcid; 380107120Sjulian 381107120Sjulian /* Sent L2CA_ConnectInd notification to the upper layer */ 382107120Sjulian ch->ident = ident; 383107120Sjulian ch->state = NG_L2CAP_W4_L2CA_CON_RSP; 384107120Sjulian 385107120Sjulian error = ng_l2cap_l2ca_con_ind(ch); 386107120Sjulian if (error != 0) { 387107120Sjulian send_l2cap_con_rej(con, ident, ch->scid, dcid, 388107120Sjulian (error == ENOMEM)? NG_L2CAP_NO_RESOURCES : 389107120Sjulian NG_L2CAP_PSM_NOT_SUPPORTED); 390107120Sjulian ng_l2cap_free_chan(ch); 391107120Sjulian } 392107120Sjulian 393107120Sjulian return (error); 394107120Sjulian} /* ng_l2cap_process_con_req */ 395107120Sjulian 396107120Sjulian/* 397107120Sjulian * Process L2CAP_ConnectRsp command 398107120Sjulian */ 399107120Sjulian 400107120Sjulianstatic int 401107120Sjulianng_l2cap_process_con_rsp(ng_l2cap_con_p con, u_int8_t ident) 402107120Sjulian{ 403107120Sjulian ng_l2cap_p l2cap = con->l2cap; 404107120Sjulian struct mbuf *m = con->rx_pkt; 405107120Sjulian ng_l2cap_con_rsp_cp *cp = NULL; 406107120Sjulian ng_l2cap_cmd_p cmd = NULL; 407107120Sjulian u_int16_t scid, dcid, result, status; 408107120Sjulian int error = 0; 409107120Sjulian 410107120Sjulian /* Get command parameters */ 411107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 412107120Sjulian if (m == NULL) 413107120Sjulian return (ENOBUFS); 414107120Sjulian 415107120Sjulian cp = mtod(m, ng_l2cap_con_rsp_cp *); 416107120Sjulian dcid = le16toh(cp->dcid); 417107120Sjulian scid = le16toh(cp->scid); 418107120Sjulian result = le16toh(cp->result); 419107120Sjulian status = le16toh(cp->status); 420107120Sjulian 421107120Sjulian NG_FREE_M(m); 422107120Sjulian con->rx_pkt = NULL; 423107120Sjulian 424107120Sjulian /* Check if we have pending command descriptor */ 425107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 426107120Sjulian if (cmd == NULL) { 427107120Sjulian NG_L2CAP_ERR( 428107120Sjulian"%s: %s - unexpected L2CAP_ConnectRsp command. ident=%d, con_handle=%d\n", 429107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident, 430107120Sjulian con->con_handle); 431107120Sjulian 432107120Sjulian return (ENOENT); 433107120Sjulian } 434107120Sjulian 435107120Sjulian /* Verify channel state, if invalid - do nothing */ 436107120Sjulian if (cmd->ch->state != NG_L2CAP_W4_L2CAP_CON_RSP) { 437107120Sjulian NG_L2CAP_ERR( 438107120Sjulian"%s: %s - unexpected L2CAP_ConnectRsp. " \ 439107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 440107120Sjulian __func__, NG_NODE_NAME(l2cap->node), scid, 441107120Sjulian cmd->ch->state); 442107120Sjulian goto reject; 443107120Sjulian } 444107120Sjulian 445107120Sjulian /* Verify CIDs and send reject if does not match */ 446107120Sjulian if (cmd->ch->scid != scid) { 447107120Sjulian NG_L2CAP_ERR( 448107120Sjulian"%s: %s - unexpected L2CAP_ConnectRsp. Channel IDs do not match, scid=%d(%d)\n", 449107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 450107120Sjulian scid); 451107120Sjulian goto reject; 452107120Sjulian } 453107120Sjulian 454107120Sjulian /* 455107120Sjulian * Looks good. We got confirmation from our peer. Now process 456107120Sjulian * it. First disable RTX timer. Then check the result and send 457121054Semax * notification to the upper layer. If command timeout already 458121054Semax * happened then ignore response. 459107120Sjulian */ 460107120Sjulian 461121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 462121054Semax return (error); 463107120Sjulian 464107120Sjulian if (result == NG_L2CAP_PENDING) { 465107120Sjulian /* 466107120Sjulian * Our peer wants more time to complete connection. We shall 467107120Sjulian * start ERTX timer and wait. Keep command in the list. 468107120Sjulian */ 469107120Sjulian 470107120Sjulian cmd->ch->dcid = dcid; 471107120Sjulian ng_l2cap_command_timeout(cmd, bluetooth_l2cap_ertx_timeout()); 472107120Sjulian 473107120Sjulian error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 474107120Sjulian result, status); 475107120Sjulian if (error != 0) 476107120Sjulian ng_l2cap_free_chan(cmd->ch); 477107120Sjulian } else { 478107120Sjulian ng_l2cap_unlink_cmd(cmd); 479107120Sjulian 480107120Sjulian if (result == NG_L2CAP_SUCCESS) { 481107120Sjulian /* 482107120Sjulian * Channel is open. Complete command and move to CONFIG 483107120Sjulian * state. Since we have sent positive confirmation we 484107120Sjulian * expect to receive L2CA_Config request from the upper 485107120Sjulian * layer protocol. 486107120Sjulian */ 487107120Sjulian 488107120Sjulian cmd->ch->dcid = dcid; 489107120Sjulian cmd->ch->state = NG_L2CAP_CONFIG; 490107120Sjulian } else 491107120Sjulian /* There was an error, so close the channel */ 492107120Sjulian NG_L2CAP_INFO( 493107120Sjulian"%s: %s - failed to open L2CAP channel, result=%d, status=%d\n", 494107120Sjulian __func__, NG_NODE_NAME(l2cap->node), result, 495107120Sjulian status); 496107120Sjulian 497107120Sjulian error = ng_l2cap_l2ca_con_rsp(cmd->ch, cmd->token, 498107120Sjulian result, status); 499107120Sjulian 500107120Sjulian /* XXX do we have to remove the channel on error? */ 501107120Sjulian if (error != 0 || result != NG_L2CAP_SUCCESS) 502107120Sjulian ng_l2cap_free_chan(cmd->ch); 503107120Sjulian 504107120Sjulian ng_l2cap_free_cmd(cmd); 505107120Sjulian } 506107120Sjulian 507107120Sjulian return (error); 508107120Sjulian 509107120Sjulianreject: 510107120Sjulian /* Send reject. Do not really care about the result */ 511107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 512107120Sjulian 513107120Sjulian return (0); 514107120Sjulian} /* ng_l2cap_process_con_rsp */ 515107120Sjulian 516107120Sjulian/* 517107120Sjulian * Process L2CAP_ConfigReq command 518107120Sjulian */ 519107120Sjulian 520107120Sjulianstatic int 521107120Sjulianng_l2cap_process_cfg_req(ng_l2cap_con_p con, u_int8_t ident) 522107120Sjulian{ 523107120Sjulian ng_l2cap_p l2cap = con->l2cap; 524107120Sjulian struct mbuf *m = con->rx_pkt; 525107120Sjulian ng_l2cap_cfg_req_cp *cp = NULL; 526107120Sjulian ng_l2cap_chan_p ch = NULL; 527107120Sjulian u_int16_t dcid, respond, result; 528107120Sjulian ng_l2cap_cfg_opt_t hdr; 529107120Sjulian ng_l2cap_cfg_opt_val_t val; 530107120Sjulian int off, error = 0; 531107120Sjulian 532107120Sjulian /* Get command parameters */ 533107120Sjulian con->rx_pkt = NULL; 534107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 535107120Sjulian if (m == NULL) 536107120Sjulian return (ENOBUFS); 537107120Sjulian 538107120Sjulian cp = mtod(m, ng_l2cap_cfg_req_cp *); 539107120Sjulian dcid = le16toh(cp->dcid); 540107120Sjulian respond = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 541107120Sjulian m_adj(m, sizeof(*cp)); 542107120Sjulian 543107120Sjulian /* Check if we have this channel and it is in valid state */ 544107120Sjulian ch = ng_l2cap_chan_by_scid(l2cap, dcid); 545107120Sjulian if (ch == NULL) { 546107120Sjulian NG_L2CAP_ERR( 547107120Sjulian"%s: %s - unexpected L2CAP_ConfigReq command. " \ 548107120Sjulian"Channel does not exist, cid=%d\n", 549107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid); 550107120Sjulian goto reject; 551107120Sjulian } 552107120Sjulian 553107120Sjulian /* Verify channel state */ 554107120Sjulian if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN) { 555107120Sjulian NG_L2CAP_ERR( 556107120Sjulian"%s: %s - unexpected L2CAP_ConfigReq. " \ 557107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 558107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 559107120Sjulian goto reject; 560107120Sjulian } 561107120Sjulian 562107120Sjulian if (ch->state == NG_L2CAP_OPEN) { /* Re-configuration */ 563107120Sjulian ch->cfg_state = 0; 564107120Sjulian ch->state = NG_L2CAP_CONFIG; 565107120Sjulian } 566107120Sjulian 567107120Sjulian for (result = 0, off = 0; ; ) { 568107120Sjulian error = get_next_l2cap_opt(m, &off, &hdr, &val); 569107120Sjulian if (error == 0) { /* We done with this packet */ 570107120Sjulian NG_FREE_M(m); 571107120Sjulian break; 572107120Sjulian } else if (error > 0) { /* Got option */ 573107120Sjulian switch (hdr.type) { 574107120Sjulian case NG_L2CAP_OPT_MTU: 575107120Sjulian ch->omtu = val.mtu; 576107120Sjulian break; 577107120Sjulian 578107120Sjulian case NG_L2CAP_OPT_FLUSH_TIMO: 579107120Sjulian ch->flush_timo = val.flush_timo; 580107120Sjulian break; 581107120Sjulian 582107120Sjulian case NG_L2CAP_OPT_QOS: 583107120Sjulian bcopy(&val.flow, &ch->iflow, sizeof(ch->iflow)); 584107120Sjulian break; 585107120Sjulian 586121054Semax default: /* Ignore unknown hint option */ 587107120Sjulian break; 588107120Sjulian } 589107120Sjulian } else { /* Oops, something is wrong */ 590107120Sjulian respond = 1; 591107120Sjulian 592107120Sjulian if (error == -3) { 593107120Sjulian 594107120Sjulian /* 595107120Sjulian * Adjust mbuf so we can get to the start 596107120Sjulian * of the first option we did not like. 597107120Sjulian */ 598107120Sjulian 599107120Sjulian m_adj(m, off - sizeof(hdr)); 600107120Sjulian m->m_pkthdr.len = sizeof(hdr) + hdr.length; 601107120Sjulian 602107120Sjulian result = NG_L2CAP_UNKNOWN_OPTION; 603107120Sjulian } else { 604107120Sjulian /* XXX FIXME Send other reject codes? */ 605107120Sjulian NG_FREE_M(m); 606107120Sjulian result = NG_L2CAP_REJECT; 607107120Sjulian } 608107120Sjulian 609107120Sjulian break; 610107120Sjulian } 611107120Sjulian } 612107120Sjulian 613107120Sjulian /* 614107120Sjulian * Now check and see if we have to respond. If everything was OK then 615107120Sjulian * respond contain "C flag" and (if set) we will respond with empty 616107120Sjulian * packet and will wait for more options. 617107120Sjulian * 618107120Sjulian * Other case is that we did not like peer's options and will respond 619107120Sjulian * with L2CAP_Config response command with Reject error code. 620107120Sjulian * 621107120Sjulian * When "respond == 0" than we have received all options and we will 622107120Sjulian * sent L2CA_ConfigInd event to the upper layer protocol. 623107120Sjulian */ 624107120Sjulian 625107120Sjulian if (respond) { 626107120Sjulian error = send_l2cap_cfg_rsp(con, ident, ch->dcid, result, m); 627107120Sjulian if (error != 0) { 628107120Sjulian ng_l2cap_l2ca_discon_ind(ch); 629107120Sjulian ng_l2cap_free_chan(ch); 630107120Sjulian } 631107120Sjulian } else { 632107120Sjulian /* Send L2CA_ConfigInd event to the upper layer protocol */ 633107120Sjulian ch->ident = ident; 634107120Sjulian error = ng_l2cap_l2ca_cfg_ind(ch); 635107120Sjulian if (error != 0) 636107120Sjulian ng_l2cap_free_chan(ch); 637107120Sjulian } 638107120Sjulian 639107120Sjulian return (error); 640107120Sjulian 641107120Sjulianreject: 642107120Sjulian /* Send reject. Do not really care about the result */ 643107120Sjulian NG_FREE_M(m); 644107120Sjulian 645107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, 0, dcid); 646107120Sjulian 647107120Sjulian return (0); 648107120Sjulian} /* ng_l2cap_process_cfg_req */ 649107120Sjulian 650107120Sjulian/* 651107120Sjulian * Process L2CAP_ConfigRsp command 652107120Sjulian */ 653107120Sjulian 654107120Sjulianstatic int 655107120Sjulianng_l2cap_process_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident) 656107120Sjulian{ 657107120Sjulian ng_l2cap_p l2cap = con->l2cap; 658107120Sjulian struct mbuf *m = con->rx_pkt; 659107120Sjulian ng_l2cap_cfg_rsp_cp *cp = NULL; 660107120Sjulian ng_l2cap_cmd_p cmd = NULL; 661107120Sjulian u_int16_t scid, cflag, result; 662107120Sjulian ng_l2cap_cfg_opt_t hdr; 663107120Sjulian ng_l2cap_cfg_opt_val_t val; 664107120Sjulian int off, error = 0; 665107120Sjulian 666107120Sjulian /* Get command parameters */ 667107120Sjulian con->rx_pkt = NULL; 668107120Sjulian NG_L2CAP_M_PULLUP(m, sizeof(*cp)); 669107120Sjulian if (m == NULL) 670107120Sjulian return (ENOBUFS); 671107120Sjulian 672107120Sjulian cp = mtod(m, ng_l2cap_cfg_rsp_cp *); 673107120Sjulian scid = le16toh(cp->scid); 674107120Sjulian cflag = NG_L2CAP_OPT_CFLAG(le16toh(cp->flags)); 675107120Sjulian result = le16toh(cp->result); 676107120Sjulian m_adj(m, sizeof(*cp)); 677107120Sjulian 678107120Sjulian /* Check if we have this command */ 679107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 680107120Sjulian if (cmd == NULL) { 681107120Sjulian NG_L2CAP_ERR( 682107120Sjulian"%s: %s - unexpected L2CAP_ConfigRsp command. ident=%d, con_handle=%d\n", 683107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident, 684107120Sjulian con->con_handle); 685107120Sjulian NG_FREE_M(m); 686107120Sjulian 687107120Sjulian return (ENOENT); 688107120Sjulian } 689107120Sjulian 690107120Sjulian /* Verify CIDs and send reject if does not match */ 691107120Sjulian if (cmd->ch->scid != scid) { 692107120Sjulian NG_L2CAP_ERR( 693107120Sjulian"%s: %s - unexpected L2CAP_ConfigRsp. " \ 694107120Sjulian"Channel ID does not match, scid=%d(%d)\n", 695107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 696107120Sjulian scid); 697107120Sjulian goto reject; 698107120Sjulian } 699107120Sjulian 700107120Sjulian /* Verify channel state and reject if invalid */ 701107120Sjulian if (cmd->ch->state != NG_L2CAP_CONFIG) { 702107120Sjulian NG_L2CAP_ERR( 703107120Sjulian"%s: %s - unexpected L2CAP_ConfigRsp. " \ 704107120Sjulian"Invalid channel state, scid=%d, state=%d\n", 705107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 706107120Sjulian cmd->ch->state); 707107120Sjulian goto reject; 708107120Sjulian } 709107120Sjulian 710107120Sjulian /* 711107120Sjulian * Looks like it is our response, so process it. First parse options, 712107120Sjulian * then verify C flag. If it is set then we shall expect more 713107120Sjulian * configuration options from the peer and we will wait. Otherwise we 714107120Sjulian * have received all options and we will send L2CA_ConfigRsp event to 715121054Semax * the upper layer protocol. If command timeout already happened then 716121054Semax * ignore response. 717107120Sjulian */ 718107120Sjulian 719121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 720121054Semax NG_FREE_M(m); 721121054Semax return (error); 722121054Semax } 723107120Sjulian 724107120Sjulian for (off = 0; ; ) { 725107120Sjulian error = get_next_l2cap_opt(m, &off, &hdr, &val); 726107120Sjulian if (error == 0) /* We done with this packet */ 727107120Sjulian break; 728107120Sjulian else if (error > 0) { /* Got option */ 729107120Sjulian switch (hdr.type) { 730107120Sjulian case NG_L2CAP_OPT_MTU: 731107120Sjulian cmd->ch->imtu = val.mtu; 732107120Sjulian break; 733107120Sjulian 734107120Sjulian case NG_L2CAP_OPT_FLUSH_TIMO: 735107120Sjulian cmd->ch->flush_timo = val.flush_timo; 736107120Sjulian break; 737107120Sjulian 738107120Sjulian case NG_L2CAP_OPT_QOS: 739107120Sjulian bcopy(&val.flow, &cmd->ch->oflow, 740107120Sjulian sizeof(cmd->ch->oflow)); 741107120Sjulian break; 742107120Sjulian 743121054Semax default: /* Ignore unknown hint option */ 744107120Sjulian break; 745107120Sjulian } 746107120Sjulian } else { 747107120Sjulian /* 748107120Sjulian * XXX FIXME What to do here? 749107120Sjulian * 750121054Semax * This is really BAD :( options packet was broken, or 751121054Semax * peer sent us option that we did not understand. Let 752121054Semax * upper layer know and do not wait for more options. 753107120Sjulian */ 754107120Sjulian 755107120Sjulian NG_L2CAP_ALERT( 756107120Sjulian"%s: %s - failed to parse configuration options, error=%d\n", 757107120Sjulian __func__, NG_NODE_NAME(l2cap->node), error); 758107120Sjulian 759107120Sjulian result = NG_L2CAP_UNKNOWN; 760107120Sjulian cflag = 0; 761107120Sjulian 762107120Sjulian break; 763107120Sjulian } 764107120Sjulian } 765107120Sjulian 766107120Sjulian NG_FREE_M(m); 767107120Sjulian 768107120Sjulian if (cflag) /* Restart timer and wait for more options */ 769107120Sjulian ng_l2cap_command_timeout(cmd, bluetooth_l2cap_rtx_timeout()); 770107120Sjulian else { 771107120Sjulian ng_l2cap_unlink_cmd(cmd); 772107120Sjulian 773107120Sjulian /* Send L2CA_Config response to the upper layer protocol */ 774107120Sjulian error = ng_l2cap_l2ca_cfg_rsp(cmd->ch, cmd->token, result); 775107120Sjulian if (error != 0) { 776107120Sjulian /* 777107120Sjulian * XXX FIXME what to do here? we were not able to send 778107120Sjulian * response to the upper layer protocol, so for now 779107120Sjulian * just close the channel. Send L2CAP_Disconnect to 780107120Sjulian * remote peer? 781107120Sjulian */ 782107120Sjulian 783107120Sjulian NG_L2CAP_ERR( 784107120Sjulian"%s: %s - failed to send L2CA_Config response, error=%d\n", 785107120Sjulian __func__, NG_NODE_NAME(l2cap->node), error); 786107120Sjulian 787107120Sjulian ng_l2cap_free_chan(cmd->ch); 788107120Sjulian } 789107120Sjulian 790107120Sjulian ng_l2cap_free_cmd(cmd); 791107120Sjulian } 792107120Sjulian 793107120Sjulian return (error); 794107120Sjulian 795107120Sjulianreject: 796107120Sjulian /* Send reject. Do not really care about the result */ 797107120Sjulian NG_FREE_M(m); 798107120Sjulian 799107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, 0); 800107120Sjulian 801107120Sjulian return (0); 802107120Sjulian} /* ng_l2cap_process_cfg_rsp */ 803107120Sjulian 804107120Sjulian/* 805107120Sjulian * Process L2CAP_DisconnectReq command 806107120Sjulian */ 807107120Sjulian 808107120Sjulianstatic int 809107120Sjulianng_l2cap_process_discon_req(ng_l2cap_con_p con, u_int8_t ident) 810107120Sjulian{ 811107120Sjulian ng_l2cap_p l2cap = con->l2cap; 812107120Sjulian ng_l2cap_discon_req_cp *cp = NULL; 813107120Sjulian ng_l2cap_chan_p ch = NULL; 814107120Sjulian ng_l2cap_cmd_p cmd = NULL; 815107120Sjulian u_int16_t scid, dcid; 816107120Sjulian 817107120Sjulian /* Get command parameters */ 818107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 819107120Sjulian if (con->rx_pkt == NULL) 820107120Sjulian return (ENOBUFS); 821107120Sjulian 822107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_discon_req_cp *); 823107120Sjulian dcid = le16toh(cp->dcid); 824107120Sjulian scid = le16toh(cp->scid); 825107120Sjulian 826107120Sjulian NG_FREE_M(con->rx_pkt); 827107120Sjulian 828107120Sjulian /* Check if we have this channel and it is in valid state */ 829107120Sjulian ch = ng_l2cap_chan_by_scid(l2cap, dcid); 830107120Sjulian if (ch == NULL) { 831107120Sjulian NG_L2CAP_ERR( 832107120Sjulian"%s: %s - unexpected L2CAP_DisconnectReq message. " \ 833107120Sjulian"Channel does not exist, cid=%d\n", 834107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid); 835107120Sjulian goto reject; 836107120Sjulian } 837107120Sjulian 838107120Sjulian /* XXX Verify channel state and reject if invalid -- is that true? */ 839114878Sjulian if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG && 840114878Sjulian ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 841107120Sjulian NG_L2CAP_ERR( 842107120Sjulian"%s: %s - unexpected L2CAP_DisconnectReq. " \ 843107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 844107120Sjulian __func__, NG_NODE_NAME(l2cap->node), dcid, ch->state); 845107120Sjulian goto reject; 846107120Sjulian } 847107120Sjulian 848107120Sjulian /* Match destination channel ID */ 849107120Sjulian if (ch->dcid != scid || ch->scid != dcid) { 850107120Sjulian NG_L2CAP_ERR( 851107120Sjulian"%s: %s - unexpected L2CAP_DisconnectReq. " \ 852107120Sjulian"Channel IDs does not match, channel: scid=%d, dcid=%d, " \ 853107120Sjulian"request: scid=%d, dcid=%d\n", 854107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ch->scid, ch->dcid, 855107120Sjulian scid, dcid); 856107120Sjulian goto reject; 857107120Sjulian } 858107120Sjulian 859107120Sjulian /* 860107120Sjulian * Looks good, so notify upper layer protocol that channel is about 861107120Sjulian * to be disconnected and send L2CA_DisconnectInd message. Then respond 862107120Sjulian * with L2CAP_DisconnectRsp. 863107120Sjulian */ 864107120Sjulian 865114878Sjulian if (ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 866114878Sjulian ng_l2cap_l2ca_discon_ind(ch); /* do not care about result */ 867114878Sjulian ng_l2cap_free_chan(ch); 868114878Sjulian } 869107120Sjulian 870107120Sjulian /* Send L2CAP_DisconnectRsp */ 871107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_DISCON_RSP, 0); 872107120Sjulian if (cmd == NULL) 873107120Sjulian return (ENOMEM); 874107120Sjulian 875107120Sjulian _ng_l2cap_discon_rsp(cmd->aux, ident, dcid, scid); 876107120Sjulian if (cmd->aux == NULL) { 877107120Sjulian ng_l2cap_free_cmd(cmd); 878107120Sjulian 879107120Sjulian return (ENOBUFS); 880107120Sjulian } 881107120Sjulian 882107120Sjulian /* Link command to the queue */ 883107120Sjulian ng_l2cap_link_cmd(con, cmd); 884107120Sjulian ng_l2cap_lp_deliver(con); 885107120Sjulian 886107120Sjulian return (0); 887107120Sjulian 888107120Sjulianreject: 889107120Sjulian /* Send reject. Do not really care about the result */ 890107120Sjulian send_l2cap_reject(con, ident, NG_L2CAP_REJ_INVALID_CID, 0, scid, dcid); 891107120Sjulian 892107120Sjulian return (0); 893107120Sjulian} /* ng_l2cap_process_discon_req */ 894107120Sjulian 895107120Sjulian/* 896107120Sjulian * Process L2CAP_DisconnectRsp command 897107120Sjulian */ 898107120Sjulian 899107120Sjulianstatic int 900107120Sjulianng_l2cap_process_discon_rsp(ng_l2cap_con_p con, u_int8_t ident) 901107120Sjulian{ 902107120Sjulian ng_l2cap_p l2cap = con->l2cap; 903107120Sjulian ng_l2cap_discon_rsp_cp *cp = NULL; 904107120Sjulian ng_l2cap_cmd_p cmd = NULL; 905107120Sjulian u_int16_t scid, dcid; 906107120Sjulian int error = 0; 907107120Sjulian 908107120Sjulian /* Get command parameters */ 909107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 910107120Sjulian if (con->rx_pkt == NULL) 911107120Sjulian return (ENOBUFS); 912107120Sjulian 913107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_discon_rsp_cp *); 914107120Sjulian dcid = le16toh(cp->dcid); 915107120Sjulian scid = le16toh(cp->scid); 916107120Sjulian 917107120Sjulian NG_FREE_M(con->rx_pkt); 918107120Sjulian 919107120Sjulian /* Check if we have pending command descriptor */ 920107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 921107120Sjulian if (cmd == NULL) { 922107120Sjulian NG_L2CAP_ERR( 923107120Sjulian"%s: %s - unexpected L2CAP_DisconnectRsp command. ident=%d, con_handle=%d\n", 924107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident, 925107120Sjulian con->con_handle); 926107120Sjulian goto out; 927107120Sjulian } 928107120Sjulian 929107120Sjulian /* Verify channel state, do nothing if invalid */ 930107120Sjulian if (cmd->ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) { 931107120Sjulian NG_L2CAP_ERR( 932107120Sjulian"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 933107120Sjulian"Invalid channel state, cid=%d, state=%d\n", 934107120Sjulian __func__, NG_NODE_NAME(l2cap->node), scid, 935107120Sjulian cmd->ch->state); 936107120Sjulian goto out; 937107120Sjulian } 938107120Sjulian 939107120Sjulian /* Verify CIDs and send reject if does not match */ 940107120Sjulian if (cmd->ch->scid != scid || cmd->ch->dcid != dcid) { 941107120Sjulian NG_L2CAP_ERR( 942107120Sjulian"%s: %s - unexpected L2CAP_DisconnectRsp. " \ 943107120Sjulian"Channel IDs do not match, scid=%d(%d), dcid=%d(%d)\n", 944107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cmd->ch->scid, 945107120Sjulian scid, cmd->ch->dcid, dcid); 946107120Sjulian goto out; 947107120Sjulian } 948107120Sjulian 949107120Sjulian /* 950121054Semax * Looks like we have successfuly disconnected channel, so notify 951121054Semax * upper layer. If command timeout already happened then ignore 952121054Semax * response. 953107120Sjulian */ 954107120Sjulian 955121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) 956121054Semax goto out; 957121054Semax 958107120Sjulian error = ng_l2cap_l2ca_discon_rsp(cmd->ch, cmd->token, NG_L2CAP_SUCCESS); 959107120Sjulian ng_l2cap_free_chan(cmd->ch); /* this will free commands too */ 960107120Sjulianout: 961107120Sjulian return (error); 962107120Sjulian} /* ng_l2cap_process_discon_rsp */ 963107120Sjulian 964107120Sjulian/* 965107120Sjulian * Process L2CAP_EchoReq command 966107120Sjulian */ 967107120Sjulian 968107120Sjulianstatic int 969107120Sjulianng_l2cap_process_echo_req(ng_l2cap_con_p con, u_int8_t ident) 970107120Sjulian{ 971107120Sjulian ng_l2cap_p l2cap = con->l2cap; 972107120Sjulian ng_l2cap_cmd_hdr_t *hdr = NULL; 973107120Sjulian ng_l2cap_cmd_p cmd = NULL; 974107120Sjulian 975107120Sjulian con->rx_pkt = ng_l2cap_prepend(con->rx_pkt, sizeof(*hdr)); 976107120Sjulian if (con->rx_pkt == NULL) { 977107120Sjulian NG_L2CAP_ALERT( 978128076Semax"%s: %s - ng_l2cap_prepend() failed, size=%zd\n", 979107120Sjulian __func__, NG_NODE_NAME(l2cap->node), sizeof(*hdr)); 980107120Sjulian 981107120Sjulian return (ENOBUFS); 982107120Sjulian } 983107120Sjulian 984107120Sjulian hdr = mtod(con->rx_pkt, ng_l2cap_cmd_hdr_t *); 985107120Sjulian hdr->code = NG_L2CAP_ECHO_RSP; 986107120Sjulian hdr->ident = ident; 987107120Sjulian hdr->length = htole16(con->rx_pkt->m_pkthdr.len - sizeof(*hdr)); 988107120Sjulian 989107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_ECHO_RSP, 0); 990107120Sjulian if (cmd == NULL) { 991107120Sjulian NG_FREE_M(con->rx_pkt); 992107120Sjulian 993107120Sjulian return (ENOBUFS); 994107120Sjulian } 995107120Sjulian 996107120Sjulian /* Attach data and link command to the queue */ 997107120Sjulian cmd->aux = con->rx_pkt; 998107120Sjulian con->rx_pkt = NULL; 999107120Sjulian ng_l2cap_link_cmd(con, cmd); 1000107120Sjulian ng_l2cap_lp_deliver(con); 1001107120Sjulian 1002107120Sjulian return (0); 1003107120Sjulian} /* ng_l2cap_process_echo_req */ 1004107120Sjulian 1005107120Sjulian/* 1006107120Sjulian * Process L2CAP_EchoRsp command 1007107120Sjulian */ 1008107120Sjulian 1009107120Sjulianstatic int 1010107120Sjulianng_l2cap_process_echo_rsp(ng_l2cap_con_p con, u_int8_t ident) 1011107120Sjulian{ 1012107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1013107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1014107120Sjulian int error = 0; 1015107120Sjulian 1016107120Sjulian /* Check if we have this command */ 1017107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 1018107120Sjulian if (cmd != NULL) { 1019121054Semax /* If command timeout already happened then ignore response */ 1020121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1021121054Semax NG_FREE_M(con->rx_pkt); 1022121054Semax return (error); 1023121054Semax } 1024107120Sjulian 1025107120Sjulian ng_l2cap_unlink_cmd(cmd); 1026107120Sjulian 1027107120Sjulian error = ng_l2cap_l2ca_ping_rsp(cmd->con, cmd->token, 1028107120Sjulian NG_L2CAP_SUCCESS, con->rx_pkt); 1029107120Sjulian 1030107120Sjulian ng_l2cap_free_cmd(cmd); 1031107120Sjulian con->rx_pkt = NULL; 1032107120Sjulian } else { 1033107120Sjulian NG_L2CAP_ERR( 1034107120Sjulian"%s: %s - unexpected L2CAP_EchoRsp command. " \ 1035107120Sjulian"Requested ident does not exist, ident=%d\n", 1036107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident); 1037107120Sjulian NG_FREE_M(con->rx_pkt); 1038107120Sjulian } 1039107120Sjulian 1040107120Sjulian return (error); 1041107120Sjulian} /* ng_l2cap_process_echo_rsp */ 1042107120Sjulian 1043107120Sjulian/* 1044107120Sjulian * Process L2CAP_InfoReq command 1045107120Sjulian */ 1046107120Sjulian 1047107120Sjulianstatic int 1048107120Sjulianng_l2cap_process_info_req(ng_l2cap_con_p con, u_int8_t ident) 1049107120Sjulian{ 1050107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1051107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1052107120Sjulian u_int16_t type; 1053107120Sjulian 1054107120Sjulian /* Get command parameters */ 1055107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(ng_l2cap_info_req_cp)); 1056107120Sjulian if (con->rx_pkt == NULL) 1057107120Sjulian return (ENOBUFS); 1058107120Sjulian 1059107120Sjulian type = le16toh(mtod(con->rx_pkt, ng_l2cap_info_req_cp *)->type); 1060107120Sjulian NG_FREE_M(con->rx_pkt); 1061107120Sjulian 1062107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_INFO_RSP, 0); 1063107120Sjulian if (cmd == NULL) 1064107120Sjulian return (ENOMEM); 1065107120Sjulian 1066107120Sjulian switch (type) { 1067107120Sjulian case NG_L2CAP_CONNLESS_MTU: 1068107120Sjulian _ng_l2cap_info_rsp(cmd->aux, ident, NG_L2CAP_CONNLESS_MTU, 1069107120Sjulian NG_L2CAP_SUCCESS, NG_L2CAP_MTU_DEFAULT); 1070107120Sjulian break; 1071107120Sjulian 1072107120Sjulian default: 1073107120Sjulian _ng_l2cap_info_rsp(cmd->aux, ident, type, 1074107120Sjulian NG_L2CAP_NOT_SUPPORTED, 0); 1075107120Sjulian break; 1076107120Sjulian } 1077107120Sjulian 1078107120Sjulian if (cmd->aux == NULL) { 1079107120Sjulian ng_l2cap_free_cmd(cmd); 1080107120Sjulian 1081107120Sjulian return (ENOBUFS); 1082107120Sjulian } 1083107120Sjulian 1084107120Sjulian /* Link command to the queue */ 1085107120Sjulian ng_l2cap_link_cmd(con, cmd); 1086107120Sjulian ng_l2cap_lp_deliver(con); 1087107120Sjulian 1088107120Sjulian return (0); 1089107120Sjulian} /* ng_l2cap_process_info_req */ 1090107120Sjulian 1091107120Sjulian/* 1092107120Sjulian * Process L2CAP_InfoRsp command 1093107120Sjulian */ 1094107120Sjulian 1095107120Sjulianstatic int 1096107120Sjulianng_l2cap_process_info_rsp(ng_l2cap_con_p con, u_int8_t ident) 1097107120Sjulian{ 1098107120Sjulian ng_l2cap_p l2cap = con->l2cap; 1099107120Sjulian ng_l2cap_info_rsp_cp *cp = NULL; 1100107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1101107120Sjulian int error = 0; 1102107120Sjulian 1103107120Sjulian /* Get command parameters */ 1104107120Sjulian NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*cp)); 1105107120Sjulian if (con->rx_pkt == NULL) 1106107120Sjulian return (ENOBUFS); 1107107120Sjulian 1108107120Sjulian cp = mtod(con->rx_pkt, ng_l2cap_info_rsp_cp *); 1109107120Sjulian cp->type = le16toh(cp->type); 1110107120Sjulian cp->result = le16toh(cp->result); 1111107120Sjulian m_adj(con->rx_pkt, sizeof(*cp)); 1112107120Sjulian 1113107120Sjulian /* Check if we have pending command descriptor */ 1114107120Sjulian cmd = ng_l2cap_cmd_by_ident(con, ident); 1115107120Sjulian if (cmd == NULL) { 1116107120Sjulian NG_L2CAP_ERR( 1117107120Sjulian"%s: %s - unexpected L2CAP_InfoRsp command. " \ 1118107120Sjulian"Requested ident does not exist, ident=%d\n", 1119107120Sjulian __func__, NG_NODE_NAME(l2cap->node), ident); 1120107120Sjulian NG_FREE_M(con->rx_pkt); 1121107120Sjulian 1122107120Sjulian return (ENOENT); 1123107120Sjulian } 1124107120Sjulian 1125121054Semax /* If command timeout already happened then ignore response */ 1126121054Semax if ((error = ng_l2cap_command_untimeout(cmd)) != 0) { 1127121054Semax NG_FREE_M(con->rx_pkt); 1128121054Semax return (error); 1129121054Semax } 1130107120Sjulian 1131107120Sjulian ng_l2cap_unlink_cmd(cmd); 1132107120Sjulian 1133107120Sjulian if (cp->result == NG_L2CAP_SUCCESS) { 1134107120Sjulian switch (cp->type) { 1135107120Sjulian case NG_L2CAP_CONNLESS_MTU: 1136107120Sjulian if (con->rx_pkt->m_pkthdr.len == sizeof(u_int16_t)) 1137107120Sjulian *mtod(con->rx_pkt, u_int16_t *) = 1138107120Sjulian le16toh(*mtod(con->rx_pkt,u_int16_t *)); 1139107120Sjulian else { 1140107120Sjulian cp->result = NG_L2CAP_UNKNOWN; /* XXX */ 1141107120Sjulian 1142107120Sjulian NG_L2CAP_ERR( 1143107120Sjulian"%s: %s - invalid L2CAP_InfoRsp command. " \ 1144107120Sjulian"Bad connectionless MTU parameter, len=%d\n", 1145107120Sjulian __func__, NG_NODE_NAME(l2cap->node), 1146107120Sjulian con->rx_pkt->m_pkthdr.len); 1147107120Sjulian } 1148107120Sjulian break; 1149107120Sjulian 1150107120Sjulian default: 1151107120Sjulian NG_L2CAP_WARN( 1152107120Sjulian"%s: %s - invalid L2CAP_InfoRsp command. Unknown info type=%d\n", 1153107120Sjulian __func__, NG_NODE_NAME(l2cap->node), cp->type); 1154107120Sjulian break; 1155107120Sjulian } 1156107120Sjulian } 1157107120Sjulian 1158107120Sjulian error = ng_l2cap_l2ca_get_info_rsp(cmd->con, cmd->token, 1159107120Sjulian cp->result, con->rx_pkt); 1160107120Sjulian 1161107120Sjulian ng_l2cap_free_cmd(cmd); 1162107120Sjulian con->rx_pkt = NULL; 1163107120Sjulian 1164107120Sjulian return (error); 1165107120Sjulian} /* ng_l2cap_process_info_rsp */ 1166107120Sjulian 1167107120Sjulian/* 1168107120Sjulian * Send L2CAP reject 1169107120Sjulian */ 1170107120Sjulian 1171107120Sjulianstatic int 1172107120Sjuliansend_l2cap_reject(ng_l2cap_con_p con, u_int8_t ident, u_int16_t reason, 1173107120Sjulian u_int16_t mtu, u_int16_t scid, u_int16_t dcid) 1174107120Sjulian{ 1175107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1176107120Sjulian 1177107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CMD_REJ, 0); 1178107120Sjulian if (cmd == NULL) 1179107120Sjulian return (ENOMEM); 1180107120Sjulian 1181107120Sjulian _ng_l2cap_cmd_rej(cmd->aux, cmd->ident, reason, mtu, scid, dcid); 1182107120Sjulian if (cmd->aux == NULL) { 1183107120Sjulian ng_l2cap_free_cmd(cmd); 1184107120Sjulian 1185107120Sjulian return (ENOBUFS); 1186107120Sjulian } 1187107120Sjulian 1188107120Sjulian /* Link command to the queue */ 1189107120Sjulian ng_l2cap_link_cmd(con, cmd); 1190107120Sjulian ng_l2cap_lp_deliver(con); 1191107120Sjulian 1192107120Sjulian return (0); 1193107120Sjulian} /* send_l2cap_reject */ 1194107120Sjulian 1195107120Sjulian/* 1196107120Sjulian * Send L2CAP connection reject 1197107120Sjulian */ 1198107120Sjulian 1199107120Sjulianstatic int 1200107120Sjuliansend_l2cap_con_rej(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1201107120Sjulian u_int16_t dcid, u_int16_t result) 1202107120Sjulian{ 1203107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1204107120Sjulian 1205107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CON_RSP, 0); 1206107120Sjulian if (cmd == NULL) 1207107120Sjulian return (ENOMEM); 1208107120Sjulian 1209107120Sjulian _ng_l2cap_con_rsp(cmd->aux, cmd->ident, scid, dcid, result, 0); 1210107120Sjulian if (cmd->aux == NULL) { 1211107120Sjulian ng_l2cap_free_cmd(cmd); 1212107120Sjulian 1213107120Sjulian return (ENOBUFS); 1214107120Sjulian } 1215107120Sjulian 1216107120Sjulian /* Link command to the queue */ 1217107120Sjulian ng_l2cap_link_cmd(con, cmd); 1218107120Sjulian ng_l2cap_lp_deliver(con); 1219107120Sjulian 1220107120Sjulian return (0); 1221107120Sjulian} /* send_l2cap_con_rej */ 1222107120Sjulian 1223107120Sjulian/* 1224107120Sjulian * Send L2CAP config response 1225107120Sjulian */ 1226107120Sjulian 1227107120Sjulianstatic int 1228107120Sjuliansend_l2cap_cfg_rsp(ng_l2cap_con_p con, u_int8_t ident, u_int16_t scid, 1229107120Sjulian u_int16_t result, struct mbuf *opt) 1230107120Sjulian{ 1231107120Sjulian ng_l2cap_cmd_p cmd = NULL; 1232107120Sjulian 1233107120Sjulian cmd = ng_l2cap_new_cmd(con, NULL, ident, NG_L2CAP_CFG_RSP, 0); 1234107120Sjulian if (cmd == NULL) { 1235107120Sjulian NG_FREE_M(opt); 1236107120Sjulian 1237107120Sjulian return (ENOMEM); 1238107120Sjulian } 1239107120Sjulian 1240107120Sjulian _ng_l2cap_cfg_rsp(cmd->aux, cmd->ident, scid, 0, result, opt); 1241107120Sjulian if (cmd->aux == NULL) { 1242107120Sjulian ng_l2cap_free_cmd(cmd); 1243107120Sjulian 1244107120Sjulian return (ENOBUFS); 1245107120Sjulian } 1246107120Sjulian 1247107120Sjulian /* Link command to the queue */ 1248107120Sjulian ng_l2cap_link_cmd(con, cmd); 1249107120Sjulian ng_l2cap_lp_deliver(con); 1250107120Sjulian 1251107120Sjulian return (0); 1252107120Sjulian} /* send_l2cap_cfg_rsp */ 1253107120Sjulian 1254107120Sjulian/* 1255107120Sjulian * Get next L2CAP configuration option 1256107120Sjulian * 1257107120Sjulian * Return codes: 1258107120Sjulian * 0 no option 1259107120Sjulian * 1 we have got option 1260107120Sjulian * -1 header too short 1261107120Sjulian * -2 bad option value or length 1262107120Sjulian * -3 unknown option 1263107120Sjulian */ 1264107120Sjulian 1265107120Sjulianstatic int 1266107120Sjulianget_next_l2cap_opt(struct mbuf *m, int *off, ng_l2cap_cfg_opt_p hdr, 1267107120Sjulian ng_l2cap_cfg_opt_val_p val) 1268107120Sjulian{ 1269107120Sjulian int hint, len = m->m_pkthdr.len - (*off); 1270107120Sjulian 1271107120Sjulian if (len == 0) 1272107120Sjulian return (0); 1273107120Sjulian if (len < 0 || len < sizeof(*hdr)) 1274107120Sjulian return (-1); 1275107120Sjulian 1276107120Sjulian m_copydata(m, *off, sizeof(*hdr), (caddr_t) hdr); 1277107120Sjulian *off += sizeof(*hdr); 1278107120Sjulian len -= sizeof(*hdr); 1279107120Sjulian 1280107120Sjulian hint = NG_L2CAP_OPT_HINT(hdr->type); 1281107120Sjulian hdr->type &= NG_L2CAP_OPT_HINT_MASK; 1282107120Sjulian 1283107120Sjulian switch (hdr->type) { 1284107120Sjulian case NG_L2CAP_OPT_MTU: 1285107120Sjulian if (hdr->length != NG_L2CAP_OPT_MTU_SIZE || len < hdr->length) 1286107120Sjulian return (-2); 1287107120Sjulian 1288107120Sjulian m_copydata(m, *off, NG_L2CAP_OPT_MTU_SIZE, (caddr_t) val); 1289107120Sjulian val->mtu = le16toh(val->mtu); 1290107120Sjulian *off += NG_L2CAP_OPT_MTU_SIZE; 1291107120Sjulian break; 1292107120Sjulian 1293107120Sjulian case NG_L2CAP_OPT_FLUSH_TIMO: 1294107120Sjulian if (hdr->length != NG_L2CAP_OPT_FLUSH_TIMO_SIZE || 1295107120Sjulian len < hdr->length) 1296107120Sjulian return (-2); 1297107120Sjulian 1298107120Sjulian m_copydata(m, *off, NG_L2CAP_OPT_FLUSH_TIMO_SIZE, (caddr_t)val); 1299107120Sjulian val->flush_timo = le16toh(val->flush_timo); 1300107120Sjulian *off += NG_L2CAP_OPT_FLUSH_TIMO_SIZE; 1301107120Sjulian break; 1302107120Sjulian 1303107120Sjulian case NG_L2CAP_OPT_QOS: 1304107120Sjulian if (hdr->length != NG_L2CAP_OPT_QOS_SIZE || len < hdr->length) 1305107120Sjulian return (-2); 1306107120Sjulian 1307107120Sjulian m_copydata(m, *off, NG_L2CAP_OPT_QOS_SIZE, (caddr_t) val); 1308107120Sjulian val->flow.token_rate = le32toh(val->flow.token_rate); 1309107120Sjulian val->flow.token_bucket_size = 1310107120Sjulian le32toh(val->flow.token_bucket_size); 1311107120Sjulian val->flow.peak_bandwidth = le32toh(val->flow.peak_bandwidth); 1312107120Sjulian val->flow.latency = le32toh(val->flow.latency); 1313107120Sjulian val->flow.delay_variation = le32toh(val->flow.delay_variation); 1314107120Sjulian *off += NG_L2CAP_OPT_QOS_SIZE; 1315107120Sjulian break; 1316107120Sjulian 1317107120Sjulian default: 1318107120Sjulian if (hint) 1319107120Sjulian *off += hdr->length; 1320107120Sjulian else 1321107120Sjulian return (-3); 1322107120Sjulian break; 1323107120Sjulian } 1324107120Sjulian 1325107120Sjulian return (1); 1326107120Sjulian} /* get_next_l2cap_opt */ 1327107120Sjulian 1328