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