1/*
   2   BlueZ - Bluetooth protocol stack for Linux
   3   Copyright (C) 2000-2001 Qualcomm Incorporated
   4   Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
   5   Copyright (C) 2010 Google Inc.
   6   Copyright (C) 2011 ProFUSION Embedded Systems
   7   Copyright (c) 2012 Code Aurora Forum.  All rights reserved.
   8
   9   Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
  10
  11   This program is free software; you can redistribute it and/or modify
  12   it under the terms of the GNU General Public License version 2 as
  13   published by the Free Software Foundation;
  14
  15   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  16   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
  18   IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
  19   CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
  20   WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  21   ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  22   OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  23
  24   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
  25   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
  26   SOFTWARE IS DISCLAIMED.
  27*/
  28
  29/* Bluetooth L2CAP core. */
  30
  31#include <linux/module.h>
  32
  33#include <linux/debugfs.h>
  34#include <linux/crc16.h>
 
  35
  36#include <net/bluetooth/bluetooth.h>
  37#include <net/bluetooth/hci_core.h>
  38#include <net/bluetooth/l2cap.h>
  39
  40#include "smp.h"
  41#include "a2mp.h"
  42#include "amp.h"
  43
  44#define LE_FLOWCTL_MAX_CREDITS 65535
  45
  46bool disable_ertm;
  47
  48static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_UCD;
  49
  50static LIST_HEAD(chan_list);
  51static DEFINE_RWLOCK(chan_list_lock);
  52
  53static u16 le_max_credits = L2CAP_LE_MAX_CREDITS;
  54static u16 le_default_mps = L2CAP_LE_DEFAULT_MPS;
  55
  56static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
  57				       u8 code, u8 ident, u16 dlen, void *data);
  58static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
  59			   void *data);
  60static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
  61static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err);
  62
  63static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
  64		     struct sk_buff_head *skbs, u8 event);
  65
  66static inline u8 bdaddr_type(u8 link_type, u8 bdaddr_type)
  67{
  68	if (link_type == LE_LINK) {
  69		if (bdaddr_type == ADDR_LE_DEV_PUBLIC)
  70			return BDADDR_LE_PUBLIC;
  71		else
  72			return BDADDR_LE_RANDOM;
  73	}
  74
  75	return BDADDR_BREDR;
  76}
  77
  78static inline u8 bdaddr_src_type(struct hci_conn *hcon)
  79{
  80	return bdaddr_type(hcon->type, hcon->src_type);
  81}
  82
  83static inline u8 bdaddr_dst_type(struct hci_conn *hcon)
  84{
  85	return bdaddr_type(hcon->type, hcon->dst_type);
  86}
  87
  88/* ---- L2CAP channels ---- */
  89
  90static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
  91						   u16 cid)
  92{
  93	struct l2cap_chan *c;
  94
  95	list_for_each_entry(c, &conn->chan_l, list) {
  96		if (c->dcid == cid)
  97			return c;
  98	}
  99	return NULL;
 100}
 101
 102static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn,
 103						   u16 cid)
 104{
 105	struct l2cap_chan *c;
 106
 107	list_for_each_entry(c, &conn->chan_l, list) {
 108		if (c->scid == cid)
 109			return c;
 110	}
 111	return NULL;
 112}
 113
 114/* Find channel with given SCID.
 115 * Returns locked channel. */
 116static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn,
 117						 u16 cid)
 118{
 119	struct l2cap_chan *c;
 120
 121	mutex_lock(&conn->chan_lock);
 122	c = __l2cap_get_chan_by_scid(conn, cid);
 123	if (c)
 124		l2cap_chan_lock(c);
 125	mutex_unlock(&conn->chan_lock);
 126
 127	return c;
 128}
 129
 130/* Find channel with given DCID.
 131 * Returns locked channel.
 132 */
 133static struct l2cap_chan *l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
 134						 u16 cid)
 135{
 136	struct l2cap_chan *c;
 137
 138	mutex_lock(&conn->chan_lock);
 139	c = __l2cap_get_chan_by_dcid(conn, cid);
 140	if (c)
 141		l2cap_chan_lock(c);
 142	mutex_unlock(&conn->chan_lock);
 143
 144	return c;
 145}
 146
 147static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn,
 148						    u8 ident)
 149{
 150	struct l2cap_chan *c;
 151
 152	list_for_each_entry(c, &conn->chan_l, list) {
 153		if (c->ident == ident)
 154			return c;
 155	}
 156	return NULL;
 157}
 158
 159static struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn,
 160						  u8 ident)
 161{
 162	struct l2cap_chan *c;
 163
 164	mutex_lock(&conn->chan_lock);
 165	c = __l2cap_get_chan_by_ident(conn, ident);
 166	if (c)
 167		l2cap_chan_lock(c);
 168	mutex_unlock(&conn->chan_lock);
 169
 170	return c;
 171}
 172
 173static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
 174{
 175	struct l2cap_chan *c;
 176
 177	list_for_each_entry(c, &chan_list, global_l) {
 178		if (c->sport == psm && !bacmp(&c->src, src))
 179			return c;
 180	}
 181	return NULL;
 182}
 183
 184int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
 185{
 186	int err;
 187
 188	write_lock(&chan_list_lock);
 189
 190	if (psm && __l2cap_global_chan_by_addr(psm, src)) {
 191		err = -EADDRINUSE;
 192		goto done;
 193	}
 194
 195	if (psm) {
 196		chan->psm = psm;
 197		chan->sport = psm;
 198		err = 0;
 199	} else {
 200		u16 p, start, end, incr;
 201
 202		if (chan->src_type == BDADDR_BREDR) {
 203			start = L2CAP_PSM_DYN_START;
 204			end = L2CAP_PSM_AUTO_END;
 205			incr = 2;
 206		} else {
 207			start = L2CAP_PSM_LE_DYN_START;
 208			end = L2CAP_PSM_LE_DYN_END;
 209			incr = 1;
 210		}
 211
 212		err = -EINVAL;
 213		for (p = start; p <= end; p += incr)
 214			if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
 215				chan->psm   = cpu_to_le16(p);
 216				chan->sport = cpu_to_le16(p);
 217				err = 0;
 218				break;
 219			}
 220	}
 221
 222done:
 223	write_unlock(&chan_list_lock);
 224	return err;
 225}
 226EXPORT_SYMBOL_GPL(l2cap_add_psm);
 227
 228int l2cap_add_scid(struct l2cap_chan *chan,  __u16 scid)
 229{
 230	write_lock(&chan_list_lock);
 231
 232	/* Override the defaults (which are for conn-oriented) */
 233	chan->omtu = L2CAP_DEFAULT_MTU;
 234	chan->chan_type = L2CAP_CHAN_FIXED;
 235
 236	chan->scid = scid;
 237
 238	write_unlock(&chan_list_lock);
 239
 240	return 0;
 241}
 242
 243static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 244{
 245	u16 cid, dyn_end;
 246
 247	if (conn->hcon->type == LE_LINK)
 248		dyn_end = L2CAP_CID_LE_DYN_END;
 249	else
 250		dyn_end = L2CAP_CID_DYN_END;
 251
 252	for (cid = L2CAP_CID_DYN_START; cid <= dyn_end; cid++) {
 253		if (!__l2cap_get_chan_by_scid(conn, cid))
 254			return cid;
 255	}
 256
 257	return 0;
 258}
 259
 260static void l2cap_state_change(struct l2cap_chan *chan, int state)
 261{
 262	BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
 263	       state_to_string(state));
 264
 265	chan->state = state;
 266	chan->ops->state_change(chan, state, 0);
 267}
 268
 269static inline void l2cap_state_change_and_error(struct l2cap_chan *chan,
 270						int state, int err)
 271{
 272	chan->state = state;
 273	chan->ops->state_change(chan, chan->state, err);
 274}
 275
 276static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
 277{
 278	chan->ops->state_change(chan, chan->state, err);
 279}
 280
 281static void __set_retrans_timer(struct l2cap_chan *chan)
 282{
 283	if (!delayed_work_pending(&chan->monitor_timer) &&
 284	    chan->retrans_timeout) {
 285		l2cap_set_timer(chan, &chan->retrans_timer,
 286				msecs_to_jiffies(chan->retrans_timeout));
 287	}
 288}
 289
 290static void __set_monitor_timer(struct l2cap_chan *chan)
 291{
 292	__clear_retrans_timer(chan);
 293	if (chan->monitor_timeout) {
 294		l2cap_set_timer(chan, &chan->monitor_timer,
 295				msecs_to_jiffies(chan->monitor_timeout));
 296	}
 297}
 298
 299static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head,
 300					       u16 seq)
 301{
 302	struct sk_buff *skb;
 303
 304	skb_queue_walk(head, skb) {
 305		if (bt_cb(skb)->l2cap.txseq == seq)
 306			return skb;
 307	}
 308
 309	return NULL;
 310}
 311
 312/* ---- L2CAP sequence number lists ---- */
 313
 314/* For ERTM, ordered lists of sequence numbers must be tracked for
 315 * SREJ requests that are received and for frames that are to be
 316 * retransmitted. These seq_list functions implement a singly-linked
 317 * list in an array, where membership in the list can also be checked
 318 * in constant time. Items can also be added to the tail of the list
 319 * and removed from the head in constant time, without further memory
 320 * allocs or frees.
 321 */
 322
 323static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size)
 324{
 325	size_t alloc_size, i;
 326
 327	/* Allocated size is a power of 2 to map sequence numbers
 328	 * (which may be up to 14 bits) in to a smaller array that is
 329	 * sized for the negotiated ERTM transmit windows.
 330	 */
 331	alloc_size = roundup_pow_of_two(size);
 332
 333	seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL);
 334	if (!seq_list->list)
 335		return -ENOMEM;
 336
 337	seq_list->mask = alloc_size - 1;
 338	seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 339	seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 340	for (i = 0; i < alloc_size; i++)
 341		seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
 342
 343	return 0;
 344}
 345
 346static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list)
 347{
 348	kfree(seq_list->list);
 349}
 350
 351static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list,
 352					   u16 seq)
 353{
 354	/* Constant-time check for list membership */
 355	return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR;
 356}
 357
 358static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list)
 359{
 360	u16 seq = seq_list->head;
 361	u16 mask = seq_list->mask;
 362
 363	seq_list->head = seq_list->list[seq & mask];
 364	seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR;
 365
 366	if (seq_list->head == L2CAP_SEQ_LIST_TAIL) {
 367		seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 368		seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 369	}
 370
 371	return seq;
 372}
 373
 374static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list)
 375{
 376	u16 i;
 377
 378	if (seq_list->head == L2CAP_SEQ_LIST_CLEAR)
 379		return;
 380
 381	for (i = 0; i <= seq_list->mask; i++)
 382		seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR;
 383
 384	seq_list->head = L2CAP_SEQ_LIST_CLEAR;
 385	seq_list->tail = L2CAP_SEQ_LIST_CLEAR;
 386}
 387
 388static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
 389{
 390	u16 mask = seq_list->mask;
 391
 392	/* All appends happen in constant time */
 393
 394	if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR)
 395		return;
 396
 397	if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR)
 398		seq_list->head = seq;
 399	else
 400		seq_list->list[seq_list->tail & mask] = seq;
 401
 402	seq_list->tail = seq;
 403	seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL;
 404}
 405
 406static void l2cap_chan_timeout(struct work_struct *work)
 407{
 408	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
 409					       chan_timer.work);
 410	struct l2cap_conn *conn = chan->conn;
 411	int reason;
 412
 413	BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
 414
 415	mutex_lock(&conn->chan_lock);
 416	l2cap_chan_lock(chan);
 417
 418	if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
 419		reason = ECONNREFUSED;
 420	else if (chan->state == BT_CONNECT &&
 421		 chan->sec_level != BT_SECURITY_SDP)
 422		reason = ECONNREFUSED;
 423	else
 424		reason = ETIMEDOUT;
 425
 426	l2cap_chan_close(chan, reason);
 427
 428	l2cap_chan_unlock(chan);
 429
 430	chan->ops->close(chan);
 431	mutex_unlock(&conn->chan_lock);
 432
 433	l2cap_chan_put(chan);
 434}
 435
 436struct l2cap_chan *l2cap_chan_create(void)
 437{
 438	struct l2cap_chan *chan;
 439
 440	chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
 441	if (!chan)
 442		return NULL;
 443
 444	mutex_init(&chan->lock);
 445
 446	/* Set default lock nesting level */
 447	atomic_set(&chan->nesting, L2CAP_NESTING_NORMAL);
 448
 449	write_lock(&chan_list_lock);
 450	list_add(&chan->global_l, &chan_list);
 451	write_unlock(&chan_list_lock);
 452
 453	INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout);
 454
 455	chan->state = BT_OPEN;
 456
 457	kref_init(&chan->kref);
 458
 459	/* This flag is cleared in l2cap_chan_ready() */
 460	set_bit(CONF_NOT_COMPLETE, &chan->conf_state);
 461
 462	BT_DBG("chan %p", chan);
 463
 464	return chan;
 465}
 466EXPORT_SYMBOL_GPL(l2cap_chan_create);
 467
 468static void l2cap_chan_destroy(struct kref *kref)
 469{
 470	struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref);
 471
 472	BT_DBG("chan %p", chan);
 473
 474	write_lock(&chan_list_lock);
 475	list_del(&chan->global_l);
 476	write_unlock(&chan_list_lock);
 477
 478	kfree(chan);
 479}
 480
 481void l2cap_chan_hold(struct l2cap_chan *c)
 482{
 483	BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
 484
 485	kref_get(&c->kref);
 486}
 487
 488void l2cap_chan_put(struct l2cap_chan *c)
 489{
 490	BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount));
 491
 492	kref_put(&c->kref, l2cap_chan_destroy);
 493}
 494EXPORT_SYMBOL_GPL(l2cap_chan_put);
 495
 496void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 497{
 498	chan->fcs  = L2CAP_FCS_CRC16;
 499	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
 500	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
 501	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
 502	chan->remote_max_tx = chan->max_tx;
 503	chan->remote_tx_win = chan->tx_win;
 504	chan->ack_win = L2CAP_DEFAULT_TX_WINDOW;
 505	chan->sec_level = BT_SECURITY_LOW;
 506	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 507	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
 508	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
 509	chan->conf_state = 0;
 510
 511	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 512}
 513EXPORT_SYMBOL_GPL(l2cap_chan_set_defaults);
 514
 515static void l2cap_le_flowctl_init(struct l2cap_chan *chan)
 516{
 517	chan->sdu = NULL;
 518	chan->sdu_last_frag = NULL;
 519	chan->sdu_len = 0;
 520	chan->tx_credits = 0;
 521	chan->rx_credits = le_max_credits;
 522	chan->mps = min_t(u16, chan->imtu, le_default_mps);
 523
 524	skb_queue_head_init(&chan->tx_q);
 525}
 526
 527void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 528{
 529	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 530	       __le16_to_cpu(chan->psm), chan->dcid);
 531
 532	conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
 533
 534	chan->conn = conn;
 535
 536	switch (chan->chan_type) {
 537	case L2CAP_CHAN_CONN_ORIENTED:
 538		/* Alloc CID for connection-oriented socket */
 539		chan->scid = l2cap_alloc_cid(conn);
 540		if (conn->hcon->type == ACL_LINK)
 541			chan->omtu = L2CAP_DEFAULT_MTU;
 542		break;
 543
 544	case L2CAP_CHAN_CONN_LESS:
 545		/* Connectionless socket */
 546		chan->scid = L2CAP_CID_CONN_LESS;
 547		chan->dcid = L2CAP_CID_CONN_LESS;
 548		chan->omtu = L2CAP_DEFAULT_MTU;
 549		break;
 550
 551	case L2CAP_CHAN_FIXED:
 552		/* Caller will set CID and CID specific MTU values */
 553		break;
 554
 555	default:
 556		/* Raw socket can send/recv signalling messages only */
 557		chan->scid = L2CAP_CID_SIGNALING;
 558		chan->dcid = L2CAP_CID_SIGNALING;
 559		chan->omtu = L2CAP_DEFAULT_MTU;
 560	}
 561
 562	chan->local_id		= L2CAP_BESTEFFORT_ID;
 563	chan->local_stype	= L2CAP_SERV_BESTEFFORT;
 564	chan->local_msdu	= L2CAP_DEFAULT_MAX_SDU_SIZE;
 565	chan->local_sdu_itime	= L2CAP_DEFAULT_SDU_ITIME;
 566	chan->local_acc_lat	= L2CAP_DEFAULT_ACC_LAT;
 567	chan->local_flush_to	= L2CAP_EFS_DEFAULT_FLUSH_TO;
 568
 569	l2cap_chan_hold(chan);
 570
 571	/* Only keep a reference for fixed channels if they requested it */
 572	if (chan->chan_type != L2CAP_CHAN_FIXED ||
 573	    test_bit(FLAG_HOLD_HCI_CONN, &chan->flags))
 574		hci_conn_hold(conn->hcon);
 575
 576	list_add(&chan->list, &conn->chan_l);
 577}
 578
 579void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 580{
 581	mutex_lock(&conn->chan_lock);
 582	__l2cap_chan_add(conn, chan);
 583	mutex_unlock(&conn->chan_lock);
 584}
 585
 586void l2cap_chan_del(struct l2cap_chan *chan, int err)
 587{
 588	struct l2cap_conn *conn = chan->conn;
 589
 590	__clear_chan_timer(chan);
 591
 592	BT_DBG("chan %p, conn %p, err %d, state %s", chan, conn, err,
 593	       state_to_string(chan->state));
 594
 595	chan->ops->teardown(chan, err);
 596
 597	if (conn) {
 598		struct amp_mgr *mgr = conn->hcon->amp_mgr;
 599		/* Delete from channel list */
 600		list_del(&chan->list);
 601
 602		l2cap_chan_put(chan);
 603
 604		chan->conn = NULL;
 605
 606		/* Reference was only held for non-fixed channels or
 607		 * fixed channels that explicitly requested it using the
 608		 * FLAG_HOLD_HCI_CONN flag.
 609		 */
 610		if (chan->chan_type != L2CAP_CHAN_FIXED ||
 611		    test_bit(FLAG_HOLD_HCI_CONN, &chan->flags))
 612			hci_conn_drop(conn->hcon);
 613
 614		if (mgr && mgr->bredr_chan == chan)
 615			mgr->bredr_chan = NULL;
 616	}
 617
 618	if (chan->hs_hchan) {
 619		struct hci_chan *hs_hchan = chan->hs_hchan;
 620
 621		BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan);
 622		amp_disconnect_logical_link(hs_hchan);
 623	}
 624
 625	if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
 626		return;
 627
 628	switch(chan->mode) {
 629	case L2CAP_MODE_BASIC:
 630		break;
 631
 632	case L2CAP_MODE_LE_FLOWCTL:
 633		skb_queue_purge(&chan->tx_q);
 634		break;
 635
 636	case L2CAP_MODE_ERTM:
 637		__clear_retrans_timer(chan);
 638		__clear_monitor_timer(chan);
 639		__clear_ack_timer(chan);
 640
 641		skb_queue_purge(&chan->srej_q);
 642
 643		l2cap_seq_list_free(&chan->srej_list);
 644		l2cap_seq_list_free(&chan->retrans_list);
 645
 646		/* fall through */
 647
 648	case L2CAP_MODE_STREAMING:
 649		skb_queue_purge(&chan->tx_q);
 650		break;
 651	}
 652
 653	return;
 654}
 655EXPORT_SYMBOL_GPL(l2cap_chan_del);
 656
 657static void l2cap_conn_update_id_addr(struct work_struct *work)
 658{
 659	struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
 660					       id_addr_update_work);
 661	struct hci_conn *hcon = conn->hcon;
 662	struct l2cap_chan *chan;
 663
 664	mutex_lock(&conn->chan_lock);
 665
 666	list_for_each_entry(chan, &conn->chan_l, list) {
 667		l2cap_chan_lock(chan);
 668		bacpy(&chan->dst, &hcon->dst);
 669		chan->dst_type = bdaddr_dst_type(hcon);
 670		l2cap_chan_unlock(chan);
 671	}
 672
 673	mutex_unlock(&conn->chan_lock);
 674}
 675
 676static void l2cap_chan_le_connect_reject(struct l2cap_chan *chan)
 677{
 678	struct l2cap_conn *conn = chan->conn;
 679	struct l2cap_le_conn_rsp rsp;
 680	u16 result;
 681
 682	if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
 683		result = L2CAP_CR_AUTHORIZATION;
 684	else
 685		result = L2CAP_CR_BAD_PSM;
 686
 687	l2cap_state_change(chan, BT_DISCONN);
 688
 689	rsp.dcid    = cpu_to_le16(chan->scid);
 690	rsp.mtu     = cpu_to_le16(chan->imtu);
 691	rsp.mps     = cpu_to_le16(chan->mps);
 692	rsp.credits = cpu_to_le16(chan->rx_credits);
 693	rsp.result  = cpu_to_le16(result);
 694
 695	l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
 696		       &rsp);
 697}
 698
 699static void l2cap_chan_connect_reject(struct l2cap_chan *chan)
 700{
 701	struct l2cap_conn *conn = chan->conn;
 702	struct l2cap_conn_rsp rsp;
 703	u16 result;
 704
 705	if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
 706		result = L2CAP_CR_SEC_BLOCK;
 707	else
 708		result = L2CAP_CR_BAD_PSM;
 709
 710	l2cap_state_change(chan, BT_DISCONN);
 711
 712	rsp.scid   = cpu_to_le16(chan->dcid);
 713	rsp.dcid   = cpu_to_le16(chan->scid);
 714	rsp.result = cpu_to_le16(result);
 715	rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 716
 717	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 718}
 719
 720void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 721{
 722	struct l2cap_conn *conn = chan->conn;
 723
 724	BT_DBG("chan %p state %s", chan, state_to_string(chan->state));
 725
 726	switch (chan->state) {
 727	case BT_LISTEN:
 728		chan->ops->teardown(chan, 0);
 729		break;
 730
 731	case BT_CONNECTED:
 732	case BT_CONFIG:
 733		if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
 734			__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
 735			l2cap_send_disconn_req(chan, reason);
 736		} else
 737			l2cap_chan_del(chan, reason);
 738		break;
 739
 740	case BT_CONNECT2:
 741		if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
 742			if (conn->hcon->type == ACL_LINK)
 743				l2cap_chan_connect_reject(chan);
 744			else if (conn->hcon->type == LE_LINK)
 745				l2cap_chan_le_connect_reject(chan);
 746		}
 747
 748		l2cap_chan_del(chan, reason);
 749		break;
 750
 751	case BT_CONNECT:
 752	case BT_DISCONN:
 753		l2cap_chan_del(chan, reason);
 754		break;
 755
 756	default:
 757		chan->ops->teardown(chan, 0);
 758		break;
 759	}
 760}
 761EXPORT_SYMBOL(l2cap_chan_close);
 762
 763static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
 764{
 765	switch (chan->chan_type) {
 766	case L2CAP_CHAN_RAW:
 767		switch (chan->sec_level) {
 768		case BT_SECURITY_HIGH:
 769		case BT_SECURITY_FIPS:
 770			return HCI_AT_DEDICATED_BONDING_MITM;
 771		case BT_SECURITY_MEDIUM:
 772			return HCI_AT_DEDICATED_BONDING;
 773		default:
 774			return HCI_AT_NO_BONDING;
 775		}
 776		break;
 777	case L2CAP_CHAN_CONN_LESS:
 778		if (chan->psm == cpu_to_le16(L2CAP_PSM_3DSP)) {
 779			if (chan->sec_level == BT_SECURITY_LOW)
 780				chan->sec_level = BT_SECURITY_SDP;
 781		}
 782		if (chan->sec_level == BT_SECURITY_HIGH ||
 783		    chan->sec_level == BT_SECURITY_FIPS)
 784			return HCI_AT_NO_BONDING_MITM;
 785		else
 786			return HCI_AT_NO_BONDING;
 787		break;
 788	case L2CAP_CHAN_CONN_ORIENTED:
 789		if (chan->psm == cpu_to_le16(L2CAP_PSM_SDP)) {
 790			if (chan->sec_level == BT_SECURITY_LOW)
 791				chan->sec_level = BT_SECURITY_SDP;
 792
 793			if (chan->sec_level == BT_SECURITY_HIGH ||
 794			    chan->sec_level == BT_SECURITY_FIPS)
 795				return HCI_AT_NO_BONDING_MITM;
 796			else
 797				return HCI_AT_NO_BONDING;
 798		}
 799		/* fall through */
 800	default:
 801		switch (chan->sec_level) {
 802		case BT_SECURITY_HIGH:
 803		case BT_SECURITY_FIPS:
 804			return HCI_AT_GENERAL_BONDING_MITM;
 805		case BT_SECURITY_MEDIUM:
 806			return HCI_AT_GENERAL_BONDING;
 807		default:
 808			return HCI_AT_NO_BONDING;
 809		}
 810		break;
 811	}
 812}
 813
 814/* Service level security */
 815int l2cap_chan_check_security(struct l2cap_chan *chan, bool initiator)
 816{
 817	struct l2cap_conn *conn = chan->conn;
 818	__u8 auth_type;
 819
 820	if (conn->hcon->type == LE_LINK)
 821		return smp_conn_security(conn->hcon, chan->sec_level);
 822
 823	auth_type = l2cap_get_auth_type(chan);
 824
 825	return hci_conn_security(conn->hcon, chan->sec_level, auth_type,
 826				 initiator);
 827}
 828
 829static u8 l2cap_get_ident(struct l2cap_conn *conn)
 830{
 831	u8 id;
 832
 833	/* Get next available identificator.
 834	 *    1 - 128 are used by kernel.
 835	 *  129 - 199 are reserved.
 836	 *  200 - 254 are used by utilities like l2ping, etc.
 837	 */
 838
 839	mutex_lock(&conn->ident_lock);
 840
 841	if (++conn->tx_ident > 128)
 842		conn->tx_ident = 1;
 843
 844	id = conn->tx_ident;
 845
 846	mutex_unlock(&conn->ident_lock);
 847
 848	return id;
 849}
 850
 851static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
 852			   void *data)
 853{
 854	struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
 855	u8 flags;
 856
 857	BT_DBG("code 0x%2.2x", code);
 858
 859	if (!skb)
 860		return;
 861
 862	/* Use NO_FLUSH if supported or we have an LE link (which does
 863	 * not support auto-flushing packets) */
 864	if (lmp_no_flush_capable(conn->hcon->hdev) ||
 865	    conn->hcon->type == LE_LINK)
 866		flags = ACL_START_NO_FLUSH;
 867	else
 868		flags = ACL_START;
 869
 870	bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
 871	skb->priority = HCI_PRIO_MAX;
 872
 873	hci_send_acl(conn->hchan, skb, flags);
 874}
 875
 876static bool __chan_is_moving(struct l2cap_chan *chan)
 877{
 878	return chan->move_state != L2CAP_MOVE_STABLE &&
 879	       chan->move_state != L2CAP_MOVE_WAIT_PREPARE;
 880}
 881
 882static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
 883{
 884	struct hci_conn *hcon = chan->conn->hcon;
 885	u16 flags;
 886
 887	BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
 888	       skb->priority);
 889
 890	if (chan->hs_hcon && !__chan_is_moving(chan)) {
 891		if (chan->hs_hchan)
 892			hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE);
 893		else
 894			kfree_skb(skb);
 895
 896		return;
 897	}
 898
 899	/* Use NO_FLUSH for LE links (where this is the only option) or
 900	 * if the BR/EDR link supports it and flushing has not been
 901	 * explicitly requested (through FLAG_FLUSHABLE).
 902	 */
 903	if (hcon->type == LE_LINK ||
 904	    (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
 905	     lmp_no_flush_capable(hcon->hdev)))
 906		flags = ACL_START_NO_FLUSH;
 907	else
 908		flags = ACL_START;
 909
 910	bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 911	hci_send_acl(chan->conn->hchan, skb, flags);
 912}
 913
 914static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control)
 915{
 916	control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
 917	control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT;
 918
 919	if (enh & L2CAP_CTRL_FRAME_TYPE) {
 920		/* S-Frame */
 921		control->sframe = 1;
 922		control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT;
 923		control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
 924
 925		control->sar = 0;
 926		control->txseq = 0;
 927	} else {
 928		/* I-Frame */
 929		control->sframe = 0;
 930		control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
 931		control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
 932
 933		control->poll = 0;
 934		control->super = 0;
 935	}
 936}
 937
 938static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control)
 939{
 940	control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT;
 941	control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT;
 942
 943	if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) {
 944		/* S-Frame */
 945		control->sframe = 1;
 946		control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT;
 947		control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT;
 948
 949		control->sar = 0;
 950		control->txseq = 0;
 951	} else {
 952		/* I-Frame */
 953		control->sframe = 0;
 954		control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
 955		control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT;
 956
 957		control->poll = 0;
 958		control->super = 0;
 959	}
 960}
 961
 962static inline void __unpack_control(struct l2cap_chan *chan,
 963				    struct sk_buff *skb)
 964{
 965	if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
 966		__unpack_extended_control(get_unaligned_le32(skb->data),
 967					  &bt_cb(skb)->l2cap);
 968		skb_pull(skb, L2CAP_EXT_CTRL_SIZE);
 969	} else {
 970		__unpack_enhanced_control(get_unaligned_le16(skb->data),
 971					  &bt_cb(skb)->l2cap);
 972		skb_pull(skb, L2CAP_ENH_CTRL_SIZE);
 973	}
 974}
 975
 976static u32 __pack_extended_control(struct l2cap_ctrl *control)
 977{
 978	u32 packed;
 979
 980	packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT;
 981	packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT;
 982
 983	if (control->sframe) {
 984		packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT;
 985		packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT;
 986		packed |= L2CAP_EXT_CTRL_FRAME_TYPE;
 987	} else {
 988		packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT;
 989		packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT;
 990	}
 991
 992	return packed;
 993}
 994
 995static u16 __pack_enhanced_control(struct l2cap_ctrl *control)
 996{
 997	u16 packed;
 998
 999	packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT;
1000	packed |= control->final << L2CAP_CTRL_FINAL_SHIFT;
1001
1002	if (control->sframe) {
1003		packed |= control->poll << L2CAP_CTRL_POLL_SHIFT;
1004		packed |= control->super << L2CAP_CTRL_SUPER_SHIFT;
1005		packed |= L2CAP_CTRL_FRAME_TYPE;
1006	} else {
1007		packed |= control->sar << L2CAP_CTRL_SAR_SHIFT;
1008		packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT;
1009	}
1010
1011	return packed;
1012}
1013
1014static inline void __pack_control(struct l2cap_chan *chan,
1015				  struct l2cap_ctrl *control,
1016				  struct sk_buff *skb)
1017{
1018	if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
1019		put_unaligned_le32(__pack_extended_control(control),
1020				   skb->data + L2CAP_HDR_SIZE);
1021	} else {
1022		put_unaligned_le16(__pack_enhanced_control(control),
1023				   skb->data + L2CAP_HDR_SIZE);
1024	}
1025}
1026
1027static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan)
1028{
1029	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1030		return L2CAP_EXT_HDR_SIZE;
1031	else
1032		return L2CAP_ENH_HDR_SIZE;
1033}
1034
1035static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan,
1036					       u32 control)
1037{
1038	struct sk_buff *skb;
1039	struct l2cap_hdr *lh;
1040	int hlen = __ertm_hdr_size(chan);
1041
1042	if (chan->fcs == L2CAP_FCS_CRC16)
1043		hlen += L2CAP_FCS_SIZE;
1044
1045	skb = bt_skb_alloc(hlen, GFP_KERNEL);
1046
1047	if (!skb)
1048		return ERR_PTR(-ENOMEM);
1049
1050	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1051	lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
1052	lh->cid = cpu_to_le16(chan->dcid);
1053
1054	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1055		put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
1056	else
1057		put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
1058
1059	if (chan->fcs == L2CAP_FCS_CRC16) {
1060		u16 fcs = crc16(0, (u8 *)skb->data, skb->len);
1061		put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1062	}
1063
1064	skb->priority = HCI_PRIO_MAX;
1065	return skb;
1066}
1067
1068static void l2cap_send_sframe(struct l2cap_chan *chan,
1069			      struct l2cap_ctrl *control)
1070{
1071	struct sk_buff *skb;
1072	u32 control_field;
1073
1074	BT_DBG("chan %p, control %p", chan, control);
1075
1076	if (!control->sframe)
1077		return;
1078
1079	if (__chan_is_moving(chan))
1080		return;
1081
1082	if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) &&
1083	    !control->poll)
1084		control->final = 1;
1085
1086	if (control->super == L2CAP_SUPER_RR)
1087		clear_bit(CONN_RNR_SENT, &chan->conn_state);
1088	else if (control->super == L2CAP_SUPER_RNR)
1089		set_bit(CONN_RNR_SENT, &chan->conn_state);
1090
1091	if (control->super != L2CAP_SUPER_SREJ) {
1092		chan->last_acked_seq = control->reqseq;
1093		__clear_ack_timer(chan);
1094	}
1095
1096	BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq,
1097	       control->final, control->poll, control->super);
1098
1099	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1100		control_field = __pack_extended_control(control);
1101	else
1102		control_field = __pack_enhanced_control(control);
1103
1104	skb = l2cap_create_sframe_pdu(chan, control_field);
1105	if (!IS_ERR(skb))
1106		l2cap_do_send(chan, skb);
1107}
1108
1109static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll)
1110{
1111	struct l2cap_ctrl control;
1112
1113	BT_DBG("chan %p, poll %d", chan, poll);
1114
1115	memset(&control, 0, sizeof(control));
1116	control.sframe = 1;
1117	control.poll = poll;
1118
1119	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
1120		control.super = L2CAP_SUPER_RNR;
1121	else
1122		control.super = L2CAP_SUPER_RR;
1123
1124	control.reqseq = chan->buffer_seq;
1125	l2cap_send_sframe(chan, &control);
1126}
1127
1128static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
1129{
1130	if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
1131		return true;
1132
1133	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
1134}
1135
1136static bool __amp_capable(struct l2cap_chan *chan)
1137{
1138	struct l2cap_conn *conn = chan->conn;
1139	struct hci_dev *hdev;
1140	bool amp_available = false;
1141
1142	if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
1143		return false;
1144
1145	if (!(conn->remote_fixed_chan & L2CAP_FC_A2MP))
1146		return false;
1147
1148	read_lock(&hci_dev_list_lock);
1149	list_for_each_entry(hdev, &hci_dev_list, list) {
1150		if (hdev->amp_type != AMP_TYPE_BREDR &&
1151		    test_bit(HCI_UP, &hdev->flags)) {
1152			amp_available = true;
1153			break;
1154		}
1155	}
1156	read_unlock(&hci_dev_list_lock);
1157
1158	if (chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED)
1159		return amp_available;
1160
1161	return false;
1162}
1163
1164static bool l2cap_check_efs(struct l2cap_chan *chan)
1165{
1166	/* Check EFS parameters */
1167	return true;
1168}
1169
1170void l2cap_send_conn_req(struct l2cap_chan *chan)
1171{
1172	struct l2cap_conn *conn = chan->conn;
1173	struct l2cap_conn_req req;
1174
1175	req.scid = cpu_to_le16(chan->scid);
1176	req.psm  = chan->psm;
1177
1178	chan->ident = l2cap_get_ident(conn);
1179
1180	set_bit(CONF_CONNECT_PEND, &chan->conf_state);
1181
1182	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
1183}
1184
1185static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id)
1186{
1187	struct l2cap_create_chan_req req;
1188	req.scid = cpu_to_le16(chan->scid);
1189	req.psm  = chan->psm;
1190	req.amp_id = amp_id;
1191
1192	chan->ident = l2cap_get_ident(chan->conn);
1193
1194	l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
1195		       sizeof(req), &req);
1196}
1197
1198static void l2cap_move_setup(struct l2cap_chan *chan)
1199{
1200	struct sk_buff *skb;
1201
1202	BT_DBG("chan %p", chan);
1203
1204	if (chan->mode != L2CAP_MODE_ERTM)
1205		return;
1206
1207	__clear_retrans_timer(chan);
1208	__clear_monitor_timer(chan);
1209	__clear_ack_timer(chan);
1210
1211	chan->retry_count = 0;
1212	skb_queue_walk(&chan->tx_q, skb) {
1213		if (bt_cb(skb)->l2cap.retries)
1214			bt_cb(skb)->l2cap.retries = 1;
1215		else
1216			break;
1217	}
1218
1219	chan->expected_tx_seq = chan->buffer_seq;
1220
1221	clear_bit(CONN_REJ_ACT, &chan->conn_state);
1222	clear_bit(CONN_SREJ_ACT, &chan->conn_state);
1223	l2cap_seq_list_clear(&chan->retrans_list);
1224	l2cap_seq_list_clear(&chan->srej_list);
1225	skb_queue_purge(&chan->srej_q);
1226
1227	chan->tx_state = L2CAP_TX_STATE_XMIT;
1228	chan->rx_state = L2CAP_RX_STATE_MOVE;
1229
1230	set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
1231}
1232
1233static void l2cap_move_done(struct l2cap_chan *chan)
1234{
1235	u8 move_role = chan->move_role;
1236	BT_DBG("chan %p", chan);
1237
1238	chan->move_state = L2CAP_MOVE_STABLE;
1239	chan->move_role = L2CAP_MOVE_ROLE_NONE;
1240
1241	if (chan->mode != L2CAP_MODE_ERTM)
1242		return;
1243
1244	switch (move_role) {
1245	case L2CAP_MOVE_ROLE_INITIATOR:
1246		l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL);
1247		chan->rx_state = L2CAP_RX_STATE_WAIT_F;
1248		break;
1249	case L2CAP_MOVE_ROLE_RESPONDER:
1250		chan->rx_state = L2CAP_RX_STATE_WAIT_P;
1251		break;
1252	}
1253}
1254
1255static void l2cap_chan_ready(struct l2cap_chan *chan)
1256{
1257	/* The channel may have already been flagged as connected in
1258	 * case of receiving data before the L2CAP info req/rsp
1259	 * procedure is complete.
1260	 */
1261	if (chan->state == BT_CONNECTED)
1262		return;
1263
1264	/* This clears all conf flags, including CONF_NOT_COMPLETE */
1265	chan->conf_state = 0;
1266	__clear_chan_timer(chan);
1267
1268	if (chan->mode == L2CAP_MODE_LE_FLOWCTL && !chan->tx_credits)
1269		chan->ops->suspend(chan);
1270
1271	chan->state = BT_CONNECTED;
1272
1273	chan->ops->ready(chan);
1274}
1275
1276static void l2cap_le_connect(struct l2cap_chan *chan)
1277{
1278	struct l2cap_conn *conn = chan->conn;
1279	struct l2cap_le_conn_req req;
1280
1281	if (test_and_set_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags))
1282		return;
1283
1284	req.psm     = chan->psm;
1285	req.scid    = cpu_to_le16(chan->scid);
1286	req.mtu     = cpu_to_le16(chan->imtu);
1287	req.mps     = cpu_to_le16(chan->mps);
1288	req.credits = cpu_to_le16(chan->rx_credits);
1289
1290	chan->ident = l2cap_get_ident(conn);
1291
1292	l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_REQ,
1293		       sizeof(req), &req);
1294}
1295
1296static void l2cap_le_start(struct l2cap_chan *chan)
1297{
1298	struct l2cap_conn *conn = chan->conn;
1299
1300	if (!smp_conn_security(conn->hcon, chan->sec_level))
1301		return;
1302
1303	if (!chan->psm) {
1304		l2cap_chan_ready(chan);
1305		return;
1306	}
1307
1308	if (chan->state == BT_CONNECT)
1309		l2cap_le_connect(chan);
1310}
1311
1312static void l2cap_start_connection(struct l2cap_chan *chan)
1313{
1314	if (__amp_capable(chan)) {
1315		BT_DBG("chan %p AMP capable: discover AMPs", chan);
1316		a2mp_discover_amp(chan);
1317	} else if (chan->conn->hcon->type == LE_LINK) {
1318		l2cap_le_start(chan);
1319	} else {
1320		l2cap_send_conn_req(chan);
1321	}
1322}
1323
1324static void l2cap_request_info(struct l2cap_conn *conn)
1325{
1326	struct l2cap_info_req req;
1327
1328	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1329		return;
1330
1331	req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
1332
1333	conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
1334	conn->info_ident = l2cap_get_ident(conn);
1335
1336	schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
1337
1338	l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
1339		       sizeof(req), &req);
1340}
1341
1342static void l2cap_do_start(struct l2cap_chan *chan)
1343{
1344	struct l2cap_conn *conn = chan->conn;
1345
1346	if (conn->hcon->type == LE_LINK) {
1347		l2cap_le_start(chan);
1348		return;
1349	}
1350
1351	if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)) {
1352		l2cap_request_info(conn);
1353		return;
1354	}
1355
1356	if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
1357		return;
1358
1359	if (l2cap_chan_check_security(chan, true) &&
1360	    __l2cap_no_conn_pending(chan))
1361		l2cap_start_connection(chan);
1362}
1363
1364static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
1365{
1366	u32 local_feat_mask = l2cap_feat_mask;
1367	if (!disable_ertm)
1368		local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
1369
1370	switch (mode) {
1371	case L2CAP_MODE_ERTM:
1372		return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
1373	case L2CAP_MODE_STREAMING:
1374		return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
1375	default:
1376		return 0x00;
1377	}
1378}
1379
1380static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err)
1381{
1382	struct l2cap_conn *conn = chan->conn;
1383	struct l2cap_disconn_req req;
1384
1385	if (!conn)
1386		return;
1387
1388	if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) {
1389		__clear_retrans_timer(chan);
1390		__clear_monitor_timer(chan);
1391		__clear_ack_timer(chan);
1392	}
1393
1394	if (chan->scid == L2CAP_CID_A2MP) {
1395		l2cap_state_change(chan, BT_DISCONN);
1396		return;
1397	}
1398
1399	req.dcid = cpu_to_le16(chan->dcid);
1400	req.scid = cpu_to_le16(chan->scid);
1401	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ,
1402		       sizeof(req), &req);
1403
1404	l2cap_state_change_and_error(chan, BT_DISCONN, err);
1405}
1406
1407/* ---- L2CAP connections ---- */
1408static void l2cap_conn_start(struct l2cap_conn *conn)
1409{
1410	struct l2cap_chan *chan, *tmp;
1411
1412	BT_DBG("conn %p", conn);
1413
1414	mutex_lock(&conn->chan_lock);
1415
1416	list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
1417		l2cap_chan_lock(chan);
1418
1419		if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1420			l2cap_chan_ready(chan);
1421			l2cap_chan_unlock(chan);
1422			continue;
1423		}
1424
1425		if (chan->state == BT_CONNECT) {
1426			if (!l2cap_chan_check_security(chan, true) ||
1427			    !__l2cap_no_conn_pending(chan)) {
1428				l2cap_chan_unlock(chan);
1429				continue;
1430			}
1431
1432			if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
1433			    && test_bit(CONF_STATE2_DEVICE,
1434					&chan->conf_state)) {
1435				l2cap_chan_close(chan, ECONNRESET);
1436				l2cap_chan_unlock(chan);
1437				continue;
1438			}
1439
1440			l2cap_start_connection(chan);
1441
1442		} else if (chan->state == BT_CONNECT2) {
1443			struct l2cap_conn_rsp rsp;
1444			char buf[128];
1445			rsp.scid = cpu_to_le16(chan->dcid);
1446			rsp.dcid = cpu_to_le16(chan->scid);
1447
1448			if (l2cap_chan_check_security(chan, false)) {
1449				if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
1450					rsp.result = cpu_to_le16(L2CAP_CR_PEND);
1451					rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
1452					chan->ops->defer(chan);
1453
1454				} else {
1455					l2cap_state_change(chan, BT_CONFIG);
1456					rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
1457					rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
1458				}
1459			} else {
1460				rsp.result = cpu_to_le16(L2CAP_CR_PEND);
1461				rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
1462			}
1463
1464			l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
1465				       sizeof(rsp), &rsp);
1466
1467			if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
1468			    rsp.result != L2CAP_CR_SUCCESS) {
1469				l2cap_chan_unlock(chan);
1470				continue;
1471			}
1472
1473			set_bit(CONF_REQ_SENT, &chan->conf_state);
1474			l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
1475				       l2cap_build_conf_req(chan, buf), buf);
1476			chan->num_conf_req++;
1477		}
1478
1479		l2cap_chan_unlock(chan);
1480	}
1481
1482	mutex_unlock(&conn->chan_lock);
1483}
1484
1485static void l2cap_le_conn_ready(struct l2cap_conn *conn)
1486{
1487	struct hci_conn *hcon = conn->hcon;
1488	struct hci_dev *hdev = hcon->hdev;
1489
1490	BT_DBG("%s conn %p", hdev->name, conn);
1491
1492	/* For outgoing pairing which doesn't necessarily have an
1493	 * associated socket (e.g. mgmt_pair_device).
1494	 */
1495	if (hcon->out)
1496		smp_conn_security(hcon, hcon->pending_sec_level);
1497
1498	/* For LE slave connections, make sure the connection interval
1499	 * is in the range of the minium and maximum interval that has
1500	 * been configured for this connection. If not, then trigger
1501	 * the connection update procedure.
1502	 */
1503	if (hcon->role == HCI_ROLE_SLAVE &&
1504	    (hcon->le_conn_interval < hcon->le_conn_min_interval ||
1505	     hcon->le_conn_interval > hcon->le_conn_max_interval)) {
1506		struct l2cap_conn_param_update_req req;
1507
1508		req.min = cpu_to_le16(hcon->le_conn_min_interval);
1509		req.max = cpu_to_le16(hcon->le_conn_max_interval);
1510		req.latency = cpu_to_le16(hcon->le_conn_latency);
1511		req.to_multiplier = cpu_to_le16(hcon->le_supv_timeout);
1512
1513		l2cap_send_cmd(conn, l2cap_get_ident(conn),
1514			       L2CAP_CONN_PARAM_UPDATE_REQ, sizeof(req), &req);
1515	}
1516}
1517
1518static void l2cap_conn_ready(struct l2cap_conn *conn)
1519{
1520	struct l2cap_chan *chan;
1521	struct hci_conn *hcon = conn->hcon;
1522
1523	BT_DBG("conn %p", conn);
1524
1525	if (hcon->type == ACL_LINK)
1526		l2cap_request_info(conn);
1527
1528	mutex_lock(&conn->chan_lock);
1529
1530	list_for_each_entry(chan, &conn->chan_l, list) {
1531
1532		l2cap_chan_lock(chan);
1533
1534		if (chan->scid == L2CAP_CID_A2MP) {
1535			l2cap_chan_unlock(chan);
1536			continue;
1537		}
1538
1539		if (hcon->type == LE_LINK) {
1540			l2cap_le_start(chan);
1541		} else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
1542			if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
1543				l2cap_chan_ready(chan);
1544		} else if (chan->state == BT_CONNECT) {
1545			l2cap_do_start(chan);
1546		}
1547
1548		l2cap_chan_unlock(chan);
1549	}
1550
1551	mutex_unlock(&conn->chan_lock);
1552
1553	if (hcon->type == LE_LINK)
1554		l2cap_le_conn_ready(conn);
1555
1556	queue_work(hcon->hdev->workqueue, &conn->pending_rx_work);
1557}
1558
1559/* Notify sockets that we cannot guaranty reliability anymore */
1560static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
1561{
1562	struct l2cap_chan *chan;
1563
1564	BT_DBG("conn %p", conn);
1565
1566	mutex_lock(&conn->chan_lock);
1567
1568	list_for_each_entry(chan, &conn->chan_l, list) {
1569		if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
1570			l2cap_chan_set_err(chan, err);
1571	}
1572
1573	mutex_unlock(&conn->chan_lock);
1574}
1575
1576static void l2cap_info_timeout(struct work_struct *work)
1577{
1578	struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
1579					       info_timer.work);
1580
1581	conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1582	conn->info_ident = 0;
1583
1584	l2cap_conn_start(conn);
1585}
1586
1587/*
1588 * l2cap_user
1589 * External modules can register l2cap_user objects on l2cap_conn. The ->probe
1590 * callback is called during registration. The ->remove callback is called
1591 * during unregistration.
1592 * An l2cap_user object can either be explicitly unregistered or when the
1593 * underlying l2cap_conn object is deleted. This guarantees that l2cap->hcon,
1594 * l2cap->hchan, .. are valid as long as the remove callback hasn't been called.
1595 * External modules must own a reference to the l2cap_conn object if they intend
1596 * to call l2cap_unregister_user(). The l2cap_conn object might get destroyed at
1597 * any time if they don't.
1598 */
1599
1600int l2cap_register_user(struct l2cap_conn *conn, struct l2cap_user *user)
1601{
1602	struct hci_dev *hdev = conn->hcon->hdev;
1603	int ret;
1604
1605	/* We need to check whether l2cap_conn is registered. If it is not, we
1606	 * must not register the l2cap_user. l2cap_conn_del() is unregisters
1607	 * l2cap_conn objects, but doesn't provide its own locking. Instead, it
1608	 * relies on the parent hci_conn object to be locked. This itself relies
1609	 * on the hci_dev object to be locked. So we must lock the hci device
1610	 * here, too. */
1611
1612	hci_dev_lock(hdev);
1613
1614	if (!list_empty(&user->list)) {
1615		ret = -EINVAL;
1616		goto out_unlock;
1617	}
1618
1619	/* conn->hchan is NULL after l2cap_conn_del() was called */
1620	if (!conn->hchan) {
1621		ret = -ENODEV;
1622		goto out_unlock;
1623	}
1624
1625	ret = user->probe(conn, user);
1626	if (ret)
1627		goto out_unlock;
1628
1629	list_add(&user->list, &conn->users);
1630	ret = 0;
1631
1632out_unlock:
1633	hci_dev_unlock(hdev);
1634	return ret;
1635}
1636EXPORT_SYMBOL(l2cap_register_user);
1637
1638void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user)
1639{
1640	struct hci_dev *hdev = conn->hcon->hdev;
1641
1642	hci_dev_lock(hdev);
1643
1644	if (list_empty(&user->list))
1645		goto out_unlock;
1646
1647	list_del_init(&user->list);
1648	user->remove(conn, user);
1649
1650out_unlock:
1651	hci_dev_unlock(hdev);
1652}
1653EXPORT_SYMBOL(l2cap_unregister_user);
1654
1655static void l2cap_unregister_all_users(struct l2cap_conn *conn)
1656{
1657	struct l2cap_user *user;
1658
1659	while (!list_empty(&conn->users)) {
1660		user = list_first_entry(&conn->users, struct l2cap_user, list);
1661		list_del_init(&user->list);
1662		user->remove(conn, user);
1663	}
1664}
1665
1666static void l2cap_conn_del(struct hci_conn *hcon, int err)
1667{
1668	struct l2cap_conn *conn = hcon->l2cap_data;
1669	struct l2cap_chan *chan, *l;
1670
1671	if (!conn)
1672		return;
1673
1674	BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
1675
1676	kfree_skb(conn->rx_skb);
1677
1678	skb_queue_purge(&conn->pending_rx);
1679
1680	/* We can not call flush_work(&conn->pending_rx_work) here since we
1681	 * might block if we are running on a worker from the same workqueue
1682	 * pending_rx_work is waiting on.
1683	 */
1684	if (work_pending(&conn->pending_rx_work))
1685		cancel_work_sync(&conn->pending_rx_work);
1686
1687	if (work_pending(&conn->id_addr_update_work))
1688		cancel_work_sync(&conn->id_addr_update_work);
1689
1690	l2cap_unregister_all_users(conn);
1691
1692	/* Force the connection to be immediately dropped */
1693	hcon->disc_timeout = 0;
1694
1695	mutex_lock(&conn->chan_lock);
1696
1697	/* Kill channels */
1698	list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
1699		l2cap_chan_hold(chan);
1700		l2cap_chan_lock(chan);
1701
1702		l2cap_chan_del(chan, err);
1703
1704		l2cap_chan_unlock(chan);
1705
1706		chan->ops->close(chan);
1707		l2cap_chan_put(chan);
1708	}
1709
1710	mutex_unlock(&conn->chan_lock);
1711
1712	hci_chan_del(conn->hchan);
1713
1714	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
1715		cancel_delayed_work_sync(&conn->info_timer);
1716
1717	hcon->l2cap_data = NULL;
1718	conn->hchan = NULL;
1719	l2cap_conn_put(conn);
1720}
1721
1722static void l2cap_conn_free(struct kref *ref)
1723{
1724	struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref);
1725
1726	hci_conn_put(conn->hcon);
1727	kfree(conn);
1728}
1729
1730struct l2cap_conn *l2cap_conn_get(struct l2cap_conn *conn)
1731{
1732	kref_get(&conn->ref);
1733	return conn;
1734}
1735EXPORT_SYMBOL(l2cap_conn_get);
1736
1737void l2cap_conn_put(struct l2cap_conn *conn)
1738{
1739	kref_put(&conn->ref, l2cap_conn_free);
1740}
1741EXPORT_SYMBOL(l2cap_conn_put);
1742
1743/* ---- Socket interface ---- */
1744
1745/* Find socket with psm and source / destination bdaddr.
1746 * Returns closest match.
1747 */
1748static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
1749						   bdaddr_t *src,
1750						   bdaddr_t *dst,
1751						   u8 link_type)
1752{
1753	struct l2cap_chan *c, *c1 = NULL;
1754
1755	read_lock(&chan_list_lock);
1756
1757	list_for_each_entry(c, &chan_list, global_l) {
1758		if (state && c->state != state)
1759			continue;
1760
1761		if (link_type == ACL_LINK && c->src_type != BDADDR_BREDR)
1762			continue;
1763
1764		if (link_type == LE_LINK && c->src_type == BDADDR_BREDR)
1765			continue;
1766
1767		if (c->psm == psm) {
1768			int src_match, dst_match;
1769			int src_any, dst_any;
1770
1771			/* Exact match. */
1772			src_match = !bacmp(&c->src, src);
1773			dst_match = !bacmp(&c->dst, dst);
1774			if (src_match && dst_match) {
1775				l2cap_chan_hold(c);
1776				read_unlock(&chan_list_lock);
1777				return c;
1778			}
1779
1780			/* Closest match */
1781			src_any = !bacmp(&c->src, BDADDR_ANY);
1782			dst_any = !bacmp(&c->dst, BDADDR_ANY);
1783			if ((src_match && dst_any) || (src_any && dst_match) ||
1784			    (src_any && dst_any))
1785				c1 = c;
1786		}
1787	}
1788
1789	if (c1)
1790		l2cap_chan_hold(c1);
1791
1792	read_unlock(&chan_list_lock);
1793
1794	return c1;
1795}
1796
1797static void l2cap_monitor_timeout(struct work_struct *work)
1798{
1799	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1800					       monitor_timer.work);
1801
1802	BT_DBG("chan %p", chan);
1803
1804	l2cap_chan_lock(chan);
1805
1806	if (!chan->conn) {
1807		l2cap_chan_unlock(chan);
1808		l2cap_chan_put(chan);
1809		return;
1810	}
1811
1812	l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
1813
1814	l2cap_chan_unlock(chan);
1815	l2cap_chan_put(chan);
1816}
1817
1818static void l2cap_retrans_timeout(struct work_struct *work)
1819{
1820	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
1821					       retrans_timer.work);
1822
1823	BT_DBG("chan %p", chan);
1824
1825	l2cap_chan_lock(chan);
1826
1827	if (!chan->conn) {
1828		l2cap_chan_unlock(chan);
1829		l2cap_chan_put(chan);
1830		return;
1831	}
1832
1833	l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
1834	l2cap_chan_unlock(chan);
1835	l2cap_chan_put(chan);
1836}
1837
1838static void l2cap_streaming_send(struct l2cap_chan *chan,
1839				 struct sk_buff_head *skbs)
1840{
1841	struct sk_buff *skb;
1842	struct l2cap_ctrl *control;
1843
1844	BT_DBG("chan %p, skbs %p", chan, skbs);
1845
1846	if (__chan_is_moving(chan))
1847		return;
1848
1849	skb_queue_splice_tail_init(skbs, &chan->tx_q);
1850
1851	while (!skb_queue_empty(&chan->tx_q)) {
1852
1853		skb = skb_dequeue(&chan->tx_q);
1854
1855		bt_cb(skb)->l2cap.retries = 1;
1856		control = &bt_cb(skb)->l2cap;
1857
1858		control->reqseq = 0;
1859		control->txseq = chan->next_tx_seq;
1860
1861		__pack_control(chan, control, skb);
1862
1863		if (chan->fcs == L2CAP_FCS_CRC16) {
1864			u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1865			put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1866		}
1867
1868		l2cap_do_send(chan, skb);
1869
1870		BT_DBG("Sent txseq %u", control->txseq);
1871
1872		chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1873		chan->frames_sent++;
1874	}
1875}
1876
1877static int l2cap_ertm_send(struct l2cap_chan *chan)
1878{
1879	struct sk_buff *skb, *tx_skb;
1880	struct l2cap_ctrl *control;
1881	int sent = 0;
1882
1883	BT_DBG("chan %p", chan);
1884
1885	if (chan->state != BT_CONNECTED)
1886		return -ENOTCONN;
1887
1888	if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1889		return 0;
1890
1891	if (__chan_is_moving(chan))
1892		return 0;
1893
1894	while (chan->tx_send_head &&
1895	       chan->unacked_frames < chan->remote_tx_win &&
1896	       chan->tx_state == L2CAP_TX_STATE_XMIT) {
1897
1898		skb = chan->tx_send_head;
1899
1900		bt_cb(skb)->l2cap.retries = 1;
1901		control = &bt_cb(skb)->l2cap;
1902
1903		if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1904			control->final = 1;
1905
1906		control->reqseq = chan->buffer_seq;
1907		chan->last_acked_seq = chan->buffer_seq;
1908		control->txseq = chan->next_tx_seq;
1909
1910		__pack_control(chan, control, skb);
1911
1912		if (chan->fcs == L2CAP_FCS_CRC16) {
1913			u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
1914			put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
1915		}
1916
1917		/* Clone after data has been modified. Data is assumed to be
1918		   read-only (for locking purposes) on cloned sk_buffs.
1919		 */
1920		tx_skb = skb_clone(skb, GFP_KERNEL);
1921
1922		if (!tx_skb)
1923			break;
1924
1925		__set_retrans_timer(chan);
1926
1927		chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1928		chan->unacked_frames++;
1929		chan->frames_sent++;
1930		sent++;
1931
1932		if (skb_queue_is_last(&chan->tx_q, skb))
1933			chan->tx_send_head = NULL;
1934		else
1935			chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
1936
1937		l2cap_do_send(chan, tx_skb);
1938		BT_DBG("Sent txseq %u", control->txseq);
1939	}
1940
1941	BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent,
1942	       chan->unacked_frames, skb_queue_len(&chan->tx_q));
1943
1944	return sent;
1945}
1946
1947static void l2cap_ertm_resend(struct l2cap_chan *chan)
1948{
1949	struct l2cap_ctrl control;
1950	struct sk_buff *skb;
1951	struct sk_buff *tx_skb;
1952	u16 seq;
1953
1954	BT_DBG("chan %p", chan);
1955
1956	if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
1957		return;
1958
1959	if (__chan_is_moving(chan))
1960		return;
1961
1962	while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
1963		seq = l2cap_seq_list_pop(&chan->retrans_list);
1964
1965		skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
1966		if (!skb) {
1967			BT_DBG("Error: Can't retransmit seq %d, frame missing",
1968			       seq);
1969			continue;
1970		}
1971
1972		bt_cb(skb)->l2cap.retries++;
1973		control = bt_cb(skb)->l2cap;
1974
1975		if (chan->max_tx != 0 &&
1976		    bt_cb(skb)->l2cap.retries > chan->max_tx) {
1977			BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
1978			l2cap_send_disconn_req(chan, ECONNRESET);
1979			l2cap_seq_list_clear(&chan->retrans_list);
1980			break;
1981		}
1982
1983		control.reqseq = chan->buffer_seq;
1984		if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1985			control.final = 1;
1986		else
1987			control.final = 0;
1988
1989		if (skb_cloned(skb)) {
1990			/* Cloned sk_buffs are read-only, so we need a
1991			 * writeable copy
1992			 */
1993			tx_skb = skb_copy(skb, GFP_KERNEL);
1994		} else {
1995			tx_skb = skb_clone(skb, GFP_KERNEL);
1996		}
1997
1998		if (!tx_skb) {
1999			l2cap_seq_list_clear(&chan->retrans_list);
2000			break;
2001		}
2002
2003		/* Update skb contents */
2004		if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
2005			put_unaligned_le32(__pack_extended_control(&control),
2006					   tx_skb->data + L2CAP_HDR_SIZE);
2007		} else {
2008			put_unaligned_le16(__pack_enhanced_control(&control),
2009					   tx_skb->data + L2CAP_HDR_SIZE);
2010		}
2011
2012		/* Update FCS */
2013		if (chan->fcs == L2CAP_FCS_CRC16) {
2014			u16 fcs = crc16(0, (u8 *) tx_skb->data,
2015					tx_skb->len - L2CAP_FCS_SIZE);
2016			put_unaligned_le16(fcs, skb_tail_pointer(tx_skb) -
2017						L2CAP_FCS_SIZE);
2018		}
2019
2020		l2cap_do_send(chan, tx_skb);
2021
2022		BT_DBG("Resent txseq %d", control.txseq);
2023
2024		chan->last_acked_seq = chan->buffer_seq;
2025	}
2026}
2027
2028static void l2cap_retransmit(struct l2cap_chan *chan,
2029			     struct l2cap_ctrl *control)
2030{
2031	BT_DBG("chan %p, control %p", chan, control);
2032
2033	l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
2034	l2cap_ertm_resend(chan);
2035}
2036
2037static void l2cap_retransmit_all(struct l2cap_chan *chan,
2038				 struct l2cap_ctrl *control)
2039{
2040	struct sk_buff *skb;
2041
2042	BT_DBG("chan %p, control %p", chan, control);
2043
2044	if (control->poll)
2045		set_bit(CONN_SEND_FBIT, &chan->conn_state);
2046
2047	l2cap_seq_list_clear(&chan->retrans_list);
2048
2049	if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
2050		return;
2051
2052	if (chan->unacked_frames) {
2053		skb_queue_walk(&chan->tx_q, skb) {
2054			if (bt_cb(skb)->l2cap.txseq == control->reqseq ||
2055			    skb == chan->tx_send_head)
2056				break;
2057		}
2058
2059		skb_queue_walk_from(&chan->tx_q, skb) {
2060			if (skb == chan->tx_send_head)
2061				break;
2062
2063			l2cap_seq_list_append(&chan->retrans_list,
2064					      bt_cb(skb)->l2cap.txseq);
2065		}
2066
2067		l2cap_ertm_resend(chan);
2068	}
2069}
2070
2071static void l2cap_send_ack(struct l2cap_chan *chan)
2072{
2073	struct l2cap_ctrl control;
2074	u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
2075					 chan->last_acked_seq);
2076	int threshold;
2077
2078	BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
2079	       chan, chan->last_acked_seq, chan->buffer_seq);
2080
2081	memset(&control, 0, sizeof(control));
2082	control.sframe = 1;
2083
2084	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
2085	    chan->rx_state == L2CAP_RX_STATE_RECV) {
2086		__clear_ack_timer(chan);
2087		control.super = L2CAP_SUPER_RNR;
2088		control.reqseq = chan->buffer_seq;
2089		l2cap_send_sframe(chan, &control);
2090	} else {
2091		if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
2092			l2cap_ertm_send(chan);
2093			/* If any i-frames were sent, they included an ack */
2094			if (chan->buffer_seq == chan->last_acked_seq)
2095				frames_to_ack = 0;
2096		}
2097
2098		/* Ack now if the window is 3/4ths full.
2099		 * Calculate without mul or div
2100		 */
2101		threshold = chan->ack_win;
2102		threshold += threshold << 1;
2103		threshold >>= 2;
2104
2105		BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack,
2106		       threshold);
2107
2108		if (frames_to_ack >= threshold) {
2109			__clear_ack_timer(chan);
2110			control.super = L2CAP_SUPER_RR;
2111			control.reqseq = chan->buffer_seq;
2112			l2cap_send_sframe(chan, &control);
2113			frames_to_ack = 0;
2114		}
2115
2116		if (frames_to_ack)
2117			__set_ack_timer(chan);
2118	}
2119}
2120
2121static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
2122					 struct msghdr *msg, int len,
2123					 int count, struct sk_buff *skb)
2124{
2125	struct l2cap_conn *conn = chan->conn;
2126	struct sk_buff **frag;
2127	int sent = 0;
2128
2129	if (copy_from_iter(skb_put(skb, count), count, &msg->msg_iter) != count)
2130		return -EFAULT;
2131
2132	sent += count;
2133	len  -= count;
2134
2135	/* Continuation fragments (no L2CAP header) */
2136	frag = &skb_shinfo(skb)->frag_list;
2137	while (len) {
2138		struct sk_buff *tmp;
2139
2140		count = min_t(unsigned int, conn->mtu, len);
2141
2142		tmp = chan->ops->alloc_skb(chan, 0, count,
2143					   msg->msg_flags & MSG_DONTWAIT);
2144		if (IS_ERR(tmp))
2145			return PTR_ERR(tmp);
2146
2147		*frag = tmp;
2148
2149		if (copy_from_iter(skb_put(*frag, count), count,
2150				   &msg->msg_iter) != count)
2151			return -EFAULT;
2152
2153		sent += count;
2154		len  -= count;
2155
2156		skb->len += (*frag)->len;
2157		skb->data_len += (*frag)->len;
2158
2159		frag = &(*frag)->next;
2160	}
2161
2162	return sent;
2163}
2164
2165static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
2166						 struct msghdr *msg, size_t len)
2167{
2168	struct l2cap_conn *conn = chan->conn;
2169	struct sk_buff *skb;
2170	int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
2171	struct l2cap_hdr *lh;
2172
2173	BT_DBG("chan %p psm 0x%2.2x len %zu", chan,
2174	       __le16_to_cpu(chan->psm), len);
2175
2176	count = min_t(unsigned int, (conn->mtu - hlen), len);
2177
2178	skb = chan->ops->alloc_skb(chan, hlen, count,
2179				   msg->msg_flags & MSG_DONTWAIT);
2180	if (IS_ERR(skb))
2181		return skb;
2182
2183	/* Create L2CAP header */
2184	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2185	lh->cid = cpu_to_le16(chan->dcid);
2186	lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
2187	put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE));
2188
2189	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2190	if (unlikely(err < 0)) {
2191		kfree_skb(skb);
2192		return ERR_PTR(err);
2193	}
2194	return skb;
2195}
2196
2197static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
2198					      struct msghdr *msg, size_t len)
2199{
2200	struct l2cap_conn *conn = chan->conn;
2201	struct sk_buff *skb;
2202	int err, count;
2203	struct l2cap_hdr *lh;
2204
2205	BT_DBG("chan %p len %zu", chan, len);
2206
2207	count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
2208
2209	skb = chan->ops->alloc_skb(chan, L2CAP_HDR_SIZE, count,
2210				   msg->msg_flags & MSG_DONTWAIT);
2211	if (IS_ERR(skb))
2212		return skb;
2213
2214	/* Create L2CAP header */
2215	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2216	lh->cid = cpu_to_le16(chan->dcid);
2217	lh->len = cpu_to_le16(len);
2218
2219	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2220	if (unlikely(err < 0)) {
2221		kfree_skb(skb);
2222		return ERR_PTR(err);
2223	}
2224	return skb;
2225}
2226
2227static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
2228					       struct msghdr *msg, size_t len,
2229					       u16 sdulen)
2230{
2231	struct l2cap_conn *conn = chan->conn;
2232	struct sk_buff *skb;
2233	int err, count, hlen;
2234	struct l2cap_hdr *lh;
2235
2236	BT_DBG("chan %p len %zu", chan, len);
2237
2238	if (!conn)
2239		return ERR_PTR(-ENOTCONN);
2240
2241	hlen = __ertm_hdr_size(chan);
2242
2243	if (sdulen)
2244		hlen += L2CAP_SDULEN_SIZE;
2245
2246	if (chan->fcs == L2CAP_FCS_CRC16)
2247		hlen += L2CAP_FCS_SIZE;
2248
2249	count = min_t(unsigned int, (conn->mtu - hlen), len);
2250
2251	skb = chan->ops->alloc_skb(chan, hlen, count,
2252				   msg->msg_flags & MSG_DONTWAIT);
2253	if (IS_ERR(skb))
2254		return skb;
2255
2256	/* Create L2CAP header */
2257	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2258	lh->cid = cpu_to_le16(chan->dcid);
2259	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2260
2261	/* Control header is populated later */
2262	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2263		put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
2264	else
2265		put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
2266
2267	if (sdulen)
2268		put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2269
2270	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2271	if (unlikely(err < 0)) {
2272		kfree_skb(skb);
2273		return ERR_PTR(err);
2274	}
2275
2276	bt_cb(skb)->l2cap.fcs = chan->fcs;
2277	bt_cb(skb)->l2cap.retries = 0;
2278	return skb;
2279}
2280
2281static int l2cap_segment_sdu(struct l2cap_chan *chan,
2282			     struct sk_buff_head *seg_queue,
2283			     struct msghdr *msg, size_t len)
2284{
2285	struct sk_buff *skb;
2286	u16 sdu_len;
2287	size_t pdu_len;
2288	u8 sar;
2289
2290	BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2291
2292	/* It is critical that ERTM PDUs fit in a single HCI fragment,
2293	 * so fragmented skbs are not used.  The HCI layer's handling
2294	 * of fragmented skbs is not compatible with ERTM's queueing.
2295	 */
2296
2297	/* PDU size is derived from the HCI MTU */
2298	pdu_len = chan->conn->mtu;
2299
2300	/* Constrain PDU size for BR/EDR connections */
2301	if (!chan->hs_hcon)
2302		pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
2303
2304	/* Adjust for largest possible L2CAP overhead. */
2305	if (chan->fcs)
2306		pdu_len -= L2CAP_FCS_SIZE;
2307
2308	pdu_len -= __ertm_hdr_size(chan);
2309
2310	/* Remote device may have requested smaller PDUs */
2311	pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
2312
2313	if (len <= pdu_len) {
2314		sar = L2CAP_SAR_UNSEGMENTED;
2315		sdu_len = 0;
2316		pdu_len = len;
2317	} else {
2318		sar = L2CAP_SAR_START;
2319		sdu_len = len;
2320	}
2321
2322	while (len > 0) {
2323		skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
2324
2325		if (IS_ERR(skb)) {
2326			__skb_queue_purge(seg_queue);
2327			return PTR_ERR(skb);
2328		}
2329
2330		bt_cb(skb)->l2cap.sar = sar;
2331		__skb_queue_tail(seg_queue, skb);
2332
2333		len -= pdu_len;
2334		if (sdu_len)
2335			sdu_len = 0;
2336
2337		if (len <= pdu_len) {
2338			sar = L2CAP_SAR_END;
2339			pdu_len = len;
2340		} else {
2341			sar = L2CAP_SAR_CONTINUE;
2342		}
2343	}
2344
2345	return 0;
2346}
2347
2348static struct sk_buff *l2cap_create_le_flowctl_pdu(struct l2cap_chan *chan,
2349						   struct msghdr *msg,
2350						   size_t len, u16 sdulen)
2351{
2352	struct l2cap_conn *conn = chan->conn;
2353	struct sk_buff *skb;
2354	int err, count, hlen;
2355	struct l2cap_hdr *lh;
2356
2357	BT_DBG("chan %p len %zu", chan, len);
2358
2359	if (!conn)
2360		return ERR_PTR(-ENOTCONN);
2361
2362	hlen = L2CAP_HDR_SIZE;
2363
2364	if (sdulen)
2365		hlen += L2CAP_SDULEN_SIZE;
2366
2367	count = min_t(unsigned int, (conn->mtu - hlen), len);
2368
2369	skb = chan->ops->alloc_skb(chan, hlen, count,
2370				   msg->msg_flags & MSG_DONTWAIT);
2371	if (IS_ERR(skb))
2372		return skb;
2373
2374	/* Create L2CAP header */
2375	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2376	lh->cid = cpu_to_le16(chan->dcid);
2377	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
2378
2379	if (sdulen)
2380		put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
2381
2382	err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
2383	if (unlikely(err < 0)) {
2384		kfree_skb(skb);
2385		return ERR_PTR(err);
2386	}
2387
2388	return skb;
2389}
2390
2391static int l2cap_segment_le_sdu(struct l2cap_chan *chan,
2392				struct sk_buff_head *seg_queue,
2393				struct msghdr *msg, size_t len)
2394{
2395	struct sk_buff *skb;
2396	size_t pdu_len;
2397	u16 sdu_len;
2398
2399	BT_DBG("chan %p, msg %p, len %zu", chan, msg, len);
2400
2401	sdu_len = len;
2402	pdu_len = chan->remote_mps - L2CAP_SDULEN_SIZE;
2403
2404	while (len > 0) {
2405		if (len <= pdu_len)
2406			pdu_len = len;
2407
2408		skb = l2cap_create_le_flowctl_pdu(chan, msg, pdu_len, sdu_len);
2409		if (IS_ERR(skb)) {
2410			__skb_queue_purge(seg_queue);
2411			return PTR_ERR(skb);
2412		}
2413
2414		__skb_queue_tail(seg_queue, skb);
2415
2416		len -= pdu_len;
2417
2418		if (sdu_len) {
2419			sdu_len = 0;
2420			pdu_len += L2CAP_SDULEN_SIZE;
2421		}
2422	}
2423
2424	return 0;
2425}
2426
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
2427int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
2428{
2429	struct sk_buff *skb;
2430	int err;
2431	struct sk_buff_head seg_queue;
2432
2433	if (!chan->conn)
2434		return -ENOTCONN;
2435
2436	/* Connectionless channel */
2437	if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
2438		skb = l2cap_create_connless_pdu(chan, msg, len);
2439		if (IS_ERR(skb))
2440			return PTR_ERR(skb);
2441
2442		/* Channel lock is released before requesting new skb and then
2443		 * reacquired thus we need to recheck channel state.
2444		 */
2445		if (chan->state != BT_CONNECTED) {
2446			kfree_skb(skb);
2447			return -ENOTCONN;
2448		}
2449
2450		l2cap_do_send(chan, skb);
2451		return len;
2452	}
2453
2454	switch (chan->mode) {
2455	case L2CAP_MODE_LE_FLOWCTL:
2456		/* Check outgoing MTU */
2457		if (len > chan->omtu)
2458			return -EMSGSIZE;
2459
2460		if (!chan->tx_credits)
2461			return -EAGAIN;
2462
2463		__skb_queue_head_init(&seg_queue);
2464
2465		err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len);
2466
2467		if (chan->state != BT_CONNECTED) {
2468			__skb_queue_purge(&seg_queue);
2469			err = -ENOTCONN;
2470		}
2471
2472		if (err)
2473			return err;
2474
2475		skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
2476
2477		while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
2478			l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
2479			chan->tx_credits--;
2480		}
2481
2482		if (!chan->tx_credits)
2483			chan->ops->suspend(chan);
2484
2485		err = len;
2486
2487		break;
2488
2489	case L2CAP_MODE_BASIC:
2490		/* Check outgoing MTU */
2491		if (len > chan->omtu)
2492			return -EMSGSIZE;
2493
2494		/* Create a basic PDU */
2495		skb = l2cap_create_basic_pdu(chan, msg, len);
2496		if (IS_ERR(skb))
2497			return PTR_ERR(skb);
2498
2499		/* Channel lock is released before requesting new skb and then
2500		 * reacquired thus we need to recheck channel state.
2501		 */
2502		if (chan->state != BT_CONNECTED) {
2503			kfree_skb(skb);
2504			return -ENOTCONN;
2505		}
2506
2507		l2cap_do_send(chan, skb);
2508		err = len;
2509		break;
2510
2511	case L2CAP_MODE_ERTM:
2512	case L2CAP_MODE_STREAMING:
2513		/* Check outgoing MTU */
2514		if (len > chan->omtu) {
2515			err = -EMSGSIZE;
2516			break;
2517		}
2518
2519		__skb_queue_head_init(&seg_queue);
2520
2521		/* Do segmentation before calling in to the state machine,
2522		 * since it's possible to block while waiting for memory
2523		 * allocation.
2524		 */
2525		err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
2526
2527		/* The channel could have been closed while segmenting,
2528		 * check that it is still connected.
2529		 */
2530		if (chan->state != BT_CONNECTED) {
2531			__skb_queue_purge(&seg_queue);
2532			err = -ENOTCONN;
2533		}
2534
2535		if (err)
2536			break;
2537
2538		if (chan->mode == L2CAP_MODE_ERTM)
2539			l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
2540		else
2541			l2cap_streaming_send(chan, &seg_queue);
2542
2543		err = len;
2544
2545		/* If the skbs were not queued for sending, they'll still be in
2546		 * seg_queue and need to be purged.
2547		 */
2548		__skb_queue_purge(&seg_queue);
2549		break;
2550
2551	default:
2552		BT_DBG("bad state %1.1x", chan->mode);
2553		err = -EBADFD;
2554	}
2555
2556	return err;
2557}
2558EXPORT_SYMBOL_GPL(l2cap_chan_send);
2559
2560static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
2561{
2562	struct l2cap_ctrl control;
2563	u16 seq;
2564
2565	BT_DBG("chan %p, txseq %u", chan, txseq);
2566
2567	memset(&control, 0, sizeof(control));
2568	control.sframe = 1;
2569	control.super = L2CAP_SUPER_SREJ;
2570
2571	for (seq = chan->expected_tx_seq; seq != txseq;
2572	     seq = __next_seq(chan, seq)) {
2573		if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
2574			control.reqseq = seq;
2575			l2cap_send_sframe(chan, &control);
2576			l2cap_seq_list_append(&chan->srej_list, seq);
2577		}
2578	}
2579
2580	chan->expected_tx_seq = __next_seq(chan, txseq);
2581}
2582
2583static void l2cap_send_srej_tail(struct l2cap_chan *chan)
2584{
2585	struct l2cap_ctrl control;
2586
2587	BT_DBG("chan %p", chan);
2588
2589	if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
2590		return;
2591
2592	memset(&control, 0, sizeof(control));
2593	control.sframe = 1;
2594	control.super = L2CAP_SUPER_SREJ;
2595	control.reqseq = chan->srej_list.tail;
2596	l2cap_send_sframe(chan, &control);
2597}
2598
2599static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
2600{
2601	struct l2cap_ctrl control;
2602	u16 initial_head;
2603	u16 seq;
2604
2605	BT_DBG("chan %p, txseq %u", chan, txseq);
2606
2607	memset(&control, 0, sizeof(control));
2608	control.sframe = 1;
2609	control.super = L2CAP_SUPER_SREJ;
2610
2611	/* Capture initial list head to allow only one pass through the list. */
2612	initial_head = chan->srej_list.head;
2613
2614	do {
2615		seq = l2cap_seq_list_pop(&chan->srej_list);
2616		if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
2617			break;
2618
2619		control.reqseq = seq;
2620		l2cap_send_sframe(chan, &control);
2621		l2cap_seq_list_append(&chan->srej_list, seq);
2622	} while (chan->srej_list.head != initial_head);
2623}
2624
2625static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
2626{
2627	struct sk_buff *acked_skb;
2628	u16 ackseq;
2629
2630	BT_DBG("chan %p, reqseq %u", chan, reqseq);
2631
2632	if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
2633		return;
2634
2635	BT_DBG("expected_ack_seq %u, unacked_frames %u",
2636	       chan->expected_ack_seq, chan->unacked_frames);
2637
2638	for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
2639	     ackseq = __next_seq(chan, ackseq)) {
2640
2641		acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
2642		if (acked_skb) {
2643			skb_unlink(acked_skb, &chan->tx_q);
2644			kfree_skb(acked_skb);
2645			chan->unacked_frames--;
2646		}
2647	}
2648
2649	chan->expected_ack_seq = reqseq;
2650
2651	if (chan->unacked_frames == 0)
2652		__clear_retrans_timer(chan);
2653
2654	BT_DBG("unacked_frames %u", chan->unacked_frames);
2655}
2656
2657static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
2658{
2659	BT_DBG("chan %p", chan);
2660
2661	chan->expected_tx_seq = chan->buffer_seq;
2662	l2cap_seq_list_clear(&chan->srej_list);
2663	skb_queue_purge(&chan->srej_q);
2664	chan->rx_state = L2CAP_RX_STATE_RECV;
2665}
2666
2667static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
2668				struct l2cap_ctrl *control,
2669				struct sk_buff_head *skbs, u8 event)
2670{
2671	BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2672	       event);
2673
2674	switch (event) {
2675	case L2CAP_EV_DATA_REQUEST:
2676		if (chan->tx_send_head == NULL)
2677			chan->tx_send_head = skb_peek(skbs);
2678
2679		skb_queue_splice_tail_init(skbs, &chan->tx_q);
2680		l2cap_ertm_send(chan);
2681		break;
2682	case L2CAP_EV_LOCAL_BUSY_DETECTED:
2683		BT_DBG("Enter LOCAL_BUSY");
2684		set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2685
2686		if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2687			/* The SREJ_SENT state must be aborted if we are to
2688			 * enter the LOCAL_BUSY state.
2689			 */
2690			l2cap_abort_rx_srej_sent(chan);
2691		}
2692
2693		l2cap_send_ack(chan);
2694
2695		break;
2696	case L2CAP_EV_LOCAL_BUSY_CLEAR:
2697		BT_DBG("Exit LOCAL_BUSY");
2698		clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2699
2700		if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2701			struct l2cap_ctrl local_control;
2702
2703			memset(&local_control, 0, sizeof(local_control));
2704			local_control.sframe = 1;
2705			local_control.super = L2CAP_SUPER_RR;
2706			local_control.poll = 1;
2707			local_control.reqseq = chan->buffer_seq;
2708			l2cap_send_sframe(chan, &local_control);
2709
2710			chan->retry_count = 1;
2711			__set_monitor_timer(chan);
2712			chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2713		}
2714		break;
2715	case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2716		l2cap_process_reqseq(chan, control->reqseq);
2717		break;
2718	case L2CAP_EV_EXPLICIT_POLL:
2719		l2cap_send_rr_or_rnr(chan, 1);
2720		chan->retry_count = 1;
2721		__set_monitor_timer(chan);
2722		__clear_ack_timer(chan);
2723		chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2724		break;
2725	case L2CAP_EV_RETRANS_TO:
2726		l2cap_send_rr_or_rnr(chan, 1);
2727		chan->retry_count = 1;
2728		__set_monitor_timer(chan);
2729		chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2730		break;
2731	case L2CAP_EV_RECV_FBIT:
2732		/* Nothing to process */
2733		break;
2734	default:
2735		break;
2736	}
2737}
2738
2739static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
2740				  struct l2cap_ctrl *control,
2741				  struct sk_buff_head *skbs, u8 event)
2742{
2743	BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
2744	       event);
2745
2746	switch (event) {
2747	case L2CAP_EV_DATA_REQUEST:
2748		if (chan->tx_send_head == NULL)
2749			chan->tx_send_head = skb_peek(skbs);
2750		/* Queue data, but don't send. */
2751		skb_queue_splice_tail_init(skbs, &chan->tx_q);
2752		break;
2753	case L2CAP_EV_LOCAL_BUSY_DETECTED:
2754		BT_DBG("Enter LOCAL_BUSY");
2755		set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2756
2757		if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
2758			/* The SREJ_SENT state must be aborted if we are to
2759			 * enter the LOCAL_BUSY state.
2760			 */
2761			l2cap_abort_rx_srej_sent(chan);
2762		}
2763
2764		l2cap_send_ack(chan);
2765
2766		break;
2767	case L2CAP_EV_LOCAL_BUSY_CLEAR:
2768		BT_DBG("Exit LOCAL_BUSY");
2769		clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
2770
2771		if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
2772			struct l2cap_ctrl local_control;
2773			memset(&local_control, 0, sizeof(local_control));
2774			local_control.sframe = 1;
2775			local_control.super = L2CAP_SUPER_RR;
2776			local_control.poll = 1;
2777			local_control.reqseq = chan->buffer_seq;
2778			l2cap_send_sframe(chan, &local_control);
2779
2780			chan->retry_count = 1;
2781			__set_monitor_timer(chan);
2782			chan->tx_state = L2CAP_TX_STATE_WAIT_F;
2783		}
2784		break;
2785	case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
2786		l2cap_process_reqseq(chan, control->reqseq);
2787
2788		/* Fall through */
2789
2790	case L2CAP_EV_RECV_FBIT:
2791		if (control && control->final) {
2792			__clear_monitor_timer(chan);
2793			if (chan->unacked_frames > 0)
2794				__set_retrans_timer(chan);
2795			chan->retry_count = 0;
2796			chan->tx_state = L2CAP_TX_STATE_XMIT;
2797			BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
2798		}
2799		break;
2800	case L2CAP_EV_EXPLICIT_POLL:
2801		/* Ignore */
2802		break;
2803	case L2CAP_EV_MONITOR_TO:
2804		if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
2805			l2cap_send_rr_or_rnr(chan, 1);
2806			__set_monitor_timer(chan);
2807			chan->retry_count++;
2808		} else {
2809			l2cap_send_disconn_req(chan, ECONNABORTED);
2810		}
2811		break;
2812	default:
2813		break;
2814	}
2815}
2816
2817static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
2818		     struct sk_buff_head *skbs, u8 event)
2819{
2820	BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
2821	       chan, control, skbs, event, chan->tx_state);
2822
2823	switch (chan->tx_state) {
2824	case L2CAP_TX_STATE_XMIT:
2825		l2cap_tx_state_xmit(chan, control, skbs, event);
2826		break;
2827	case L2CAP_TX_STATE_WAIT_F:
2828		l2cap_tx_state_wait_f(chan, control, skbs, event);
2829		break;
2830	default:
2831		/* Ignore event */
2832		break;
2833	}
2834}
2835
2836static void l2cap_pass_to_tx(struct l2cap_chan *chan,
2837			     struct l2cap_ctrl *control)
2838{
2839	BT_DBG("chan %p, control %p", chan, control);
2840	l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
2841}
2842
2843static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
2844				  struct l2cap_ctrl *control)
2845{
2846	BT_DBG("chan %p, control %p", chan, control);
2847	l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
2848}
2849
2850/* Copy frame to all raw sockets on that connection */
2851static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
2852{
2853	struct sk_buff *nskb;
2854	struct l2cap_chan *chan;
2855
2856	BT_DBG("conn %p", conn);
2857
2858	mutex_lock(&conn->chan_lock);
2859
2860	list_for_each_entry(chan, &conn->chan_l, list) {
2861		if (chan->chan_type != L2CAP_CHAN_RAW)
2862			continue;
2863
2864		/* Don't send frame to the channel it came from */
2865		if (bt_cb(skb)->l2cap.chan == chan)
2866			continue;
2867
2868		nskb = skb_clone(skb, GFP_KERNEL);
2869		if (!nskb)
2870			continue;
2871		if (chan->ops->recv(chan, nskb))
2872			kfree_skb(nskb);
2873	}
2874
2875	mutex_unlock(&conn->chan_lock);
2876}
2877
2878/* ---- L2CAP signalling commands ---- */
2879static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
2880				       u8 ident, u16 dlen, void *data)
2881{
2882	struct sk_buff *skb, **frag;
2883	struct l2cap_cmd_hdr *cmd;
2884	struct l2cap_hdr *lh;
2885	int len, count;
2886
2887	BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u",
2888	       conn, code, ident, dlen);
2889
2890	if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE)
2891		return NULL;
2892
2893	len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
2894	count = min_t(unsigned int, conn->mtu, len);
2895
2896	skb = bt_skb_alloc(count, GFP_KERNEL);
2897	if (!skb)
2898		return NULL;
2899
2900	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
2901	lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
2902
2903	if (conn->hcon->type == LE_LINK)
2904		lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
2905	else
2906		lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
2907
2908	cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
2909	cmd->code  = code;
2910	cmd->ident = ident;
2911	cmd->len   = cpu_to_le16(dlen);
2912
2913	if (dlen) {
2914		count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
2915		memcpy(skb_put(skb, count), data, count);
2916		data += count;
2917	}
2918
2919	len -= skb->len;
2920
2921	/* Continuation fragments (no L2CAP header) */
2922	frag = &skb_shinfo(skb)->frag_list;
2923	while (len) {
2924		count = min_t(unsigned int, conn->mtu, len);
2925
2926		*frag = bt_skb_alloc(count, GFP_KERNEL);
2927		if (!*frag)
2928			goto fail;
2929
2930		memcpy(skb_put(*frag, count), data, count);
2931
2932		len  -= count;
2933		data += count;
2934
2935		frag = &(*frag)->next;
2936	}
2937
2938	return skb;
2939
2940fail:
2941	kfree_skb(skb);
2942	return NULL;
2943}
2944
2945static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
2946				     unsigned long *val)
2947{
2948	struct l2cap_conf_opt *opt = *ptr;
2949	int len;
2950
2951	len = L2CAP_CONF_OPT_SIZE + opt->len;
2952	*ptr += len;
2953
2954	*type = opt->type;
2955	*olen = opt->len;
2956
2957	switch (opt->len) {
2958	case 1:
2959		*val = *((u8 *) opt->val);
2960		break;
2961
2962	case 2:
2963		*val = get_unaligned_le16(opt->val);
2964		break;
2965
2966	case 4:
2967		*val = get_unaligned_le32(opt->val);
2968		break;
2969
2970	default:
2971		*val = (unsigned long) opt->val;
2972		break;
2973	}
2974
2975	BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val);
2976	return len;
2977}
2978
2979static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
2980{
2981	struct l2cap_conf_opt *opt = *ptr;
2982
2983	BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val);
2984
 
 
 
2985	opt->type = type;
2986	opt->len  = len;
2987
2988	switch (len) {
2989	case 1:
2990		*((u8 *) opt->val)  = val;
2991		break;
2992
2993	case 2:
2994		put_unaligned_le16(val, opt->val);
2995		break;
2996
2997	case 4:
2998		put_unaligned_le32(val, opt->val);
2999		break;
3000
3001	default:
3002		memcpy(opt->val, (void *) val, len);
3003		break;
3004	}
3005
3006	*ptr += L2CAP_CONF_OPT_SIZE + len;
3007}
3008
3009static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
3010{
3011	struct l2cap_conf_efs efs;
3012
3013	switch (chan->mode) {
3014	case L2CAP_MODE_ERTM:
3015		efs.id		= chan->local_id;
3016		efs.stype	= chan->local_stype;
3017		efs.msdu	= cpu_to_le16(chan->local_msdu);
3018		efs.sdu_itime	= cpu_to_le32(chan->local_sdu_itime);
3019		efs.acc_lat	= cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
3020		efs.flush_to	= cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
3021		break;
3022
3023	case L2CAP_MODE_STREAMING:
3024		efs.id		= 1;
3025		efs.stype	= L2CAP_SERV_BESTEFFORT;
3026		efs.msdu	= cpu_to_le16(chan->local_msdu);
3027		efs.sdu_itime	= cpu_to_le32(chan->local_sdu_itime);
3028		efs.acc_lat	= 0;
3029		efs.flush_to	= 0;
3030		break;
3031
3032	default:
3033		return;
3034	}
3035
3036	l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
3037			   (unsigned long) &efs);
3038}
3039
3040static void l2cap_ack_timeout(struct work_struct *work)
3041{
3042	struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
3043					       ack_timer.work);
3044	u16 frames_to_ack;
3045
3046	BT_DBG("chan %p", chan);
3047
3048	l2cap_chan_lock(chan);
3049
3050	frames_to_ack = __seq_offset(chan, chan->buffer_seq,
3051				     chan->last_acked_seq);
3052
3053	if (frames_to_ack)
3054		l2cap_send_rr_or_rnr(chan, 0);
3055
3056	l2cap_chan_unlock(chan);
3057	l2cap_chan_put(chan);
3058}
3059
3060int l2cap_ertm_init(struct l2cap_chan *chan)
3061{
3062	int err;
3063
3064	chan->next_tx_seq = 0;
3065	chan->expected_tx_seq = 0;
3066	chan->expected_ack_seq = 0;
3067	chan->unacked_frames = 0;
3068	chan->buffer_seq = 0;
3069	chan->frames_sent = 0;
3070	chan->last_acked_seq = 0;
3071	chan->sdu = NULL;
3072	chan->sdu_last_frag = NULL;
3073	chan->sdu_len = 0;
3074
3075	skb_queue_head_init(&chan->tx_q);
3076
3077	chan->local_amp_id = AMP_ID_BREDR;
3078	chan->move_id = AMP_ID_BREDR;
3079	chan->move_state = L2CAP_MOVE_STABLE;
3080	chan->move_role = L2CAP_MOVE_ROLE_NONE;
3081
3082	if (chan->mode != L2CAP_MODE_ERTM)
3083		return 0;
3084
3085	chan->rx_state = L2CAP_RX_STATE_RECV;
3086	chan->tx_state = L2CAP_TX_STATE_XMIT;
3087
3088	INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout);
3089	INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout);
3090	INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout);
3091
3092	skb_queue_head_init(&chan->srej_q);
3093
3094	err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
3095	if (err < 0)
3096		return err;
3097
3098	err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
3099	if (err < 0)
3100		l2cap_seq_list_free(&chan->srej_list);
3101
3102	return err;
3103}
3104
3105static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
3106{
3107	switch (mode) {
3108	case L2CAP_MODE_STREAMING:
3109	case L2CAP_MODE_ERTM:
3110		if (l2cap_mode_supported(mode, remote_feat_mask))
3111			return mode;
3112		/* fall through */
3113	default:
3114		return L2CAP_MODE_BASIC;
3115	}
3116}
3117
3118static inline bool __l2cap_ews_supported(struct l2cap_conn *conn)
3119{
3120	return ((conn->local_fixed_chan & L2CAP_FC_A2MP) &&
3121		(conn->feat_mask & L2CAP_FEAT_EXT_WINDOW));
3122}
3123
3124static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
3125{
3126	return ((conn->local_fixed_chan & L2CAP_FC_A2MP) &&
3127		(conn->feat_mask & L2CAP_FEAT_EXT_FLOW));
3128}
3129
3130static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
3131				      struct l2cap_conf_rfc *rfc)
3132{
3133	if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
3134		u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
3135
3136		/* Class 1 devices have must have ERTM timeouts
3137		 * exceeding the Link Supervision Timeout.  The
3138		 * default Link Supervision Timeout for AMP
3139		 * controllers is 10 seconds.
3140		 *
3141		 * Class 1 devices use 0xffffffff for their
3142		 * best-effort flush timeout, so the clamping logic
3143		 * will result in a timeout that meets the above
3144		 * requirement.  ERTM timeouts are 16-bit values, so
3145		 * the maximum timeout is 65.535 seconds.
3146		 */
3147
3148		/* Convert timeout to milliseconds and round */
3149		ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000);
3150
3151		/* This is the recommended formula for class 2 devices
3152		 * that start ERTM timers when packets are sent to the
3153		 * controller.
3154		 */
3155		ertm_to = 3 * ertm_to + 500;
3156
3157		if (ertm_to > 0xffff)
3158			ertm_to = 0xffff;
3159
3160		rfc->retrans_timeout = cpu_to_le16((u16) ertm_to);
3161		rfc->monitor_timeout = rfc->retrans_timeout;
3162	} else {
3163		rfc->retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
3164		rfc->monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
3165	}
3166}
3167
3168static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
3169{
3170	if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
3171	    __l2cap_ews_supported(chan->conn)) {
3172		/* use extended control field */
3173		set_bit(FLAG_EXT_CTRL, &chan->flags);
3174		chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3175	} else {
3176		chan->tx_win = min_t(u16, chan->tx_win,
3177				     L2CAP_DEFAULT_TX_WINDOW);
3178		chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
3179	}
3180	chan->ack_win = chan->tx_win;
3181}
3182
3183static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
3184{
3185	struct l2cap_conf_req *req = data;
3186	struct l2cap_conf_rfc rfc = { .mode = chan->mode };
3187	void *ptr = req->data;
 
3188	u16 size;
3189
3190	BT_DBG("chan %p", chan);
3191
3192	if (chan->num_conf_req || chan->num_conf_rsp)
3193		goto done;
3194
3195	switch (chan->mode) {
3196	case L2CAP_MODE_STREAMING:
3197	case L2CAP_MODE_ERTM:
3198		if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
3199			break;
3200
3201		if (__l2cap_efs_supported(chan->conn))
3202			set_bit(FLAG_EFS_ENABLE, &chan->flags);
3203
3204		/* fall through */
3205	default:
3206		chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
3207		break;
3208	}
3209
3210done:
3211	if (chan->imtu != L2CAP_DEFAULT_MTU)
3212		l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3213
3214	switch (chan->mode) {
3215	case L2CAP_MODE_BASIC:
3216		if (disable_ertm)
3217			break;
3218
3219		if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
3220		    !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
3221			break;
3222
3223		rfc.mode            = L2CAP_MODE_BASIC;
3224		rfc.txwin_size      = 0;
3225		rfc.max_transmit    = 0;
3226		rfc.retrans_timeout = 0;
3227		rfc.monitor_timeout = 0;
3228		rfc.max_pdu_size    = 0;
3229
3230		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3231				   (unsigned long) &rfc);
3232		break;
3233
3234	case L2CAP_MODE_ERTM:
3235		rfc.mode            = L2CAP_MODE_ERTM;
3236		rfc.max_transmit    = chan->max_tx;
3237
3238		__l2cap_set_ertm_timeouts(chan, &rfc);
3239
3240		size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3241			     L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3242			     L2CAP_FCS_SIZE);
3243		rfc.max_pdu_size = cpu_to_le16(size);
3244
3245		l2cap_txwin_setup(chan);
3246
3247		rfc.txwin_size = min_t(u16, chan->tx_win,
3248				       L2CAP_DEFAULT_TX_WINDOW);
3249
3250		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3251				   (unsigned long) &rfc);
3252
3253		if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3254			l2cap_add_opt_efs(&ptr, chan);
3255
3256		if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3257			l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3258					   chan->tx_win);
3259
3260		if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3261			if (chan->fcs == L2CAP_FCS_NONE ||
3262			    test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3263				chan->fcs = L2CAP_FCS_NONE;
3264				l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3265						   chan->fcs);
3266			}
3267		break;
3268
3269	case L2CAP_MODE_STREAMING:
3270		l2cap_txwin_setup(chan);
3271		rfc.mode            = L2CAP_MODE_STREAMING;
3272		rfc.txwin_size      = 0;
3273		rfc.max_transmit    = 0;
3274		rfc.retrans_timeout = 0;
3275		rfc.monitor_timeout = 0;
3276
3277		size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
3278			     L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
3279			     L2CAP_FCS_SIZE);
3280		rfc.max_pdu_size = cpu_to_le16(size);
3281
3282		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3283				   (unsigned long) &rfc);
3284
3285		if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
3286			l2cap_add_opt_efs(&ptr, chan);
3287
3288		if (chan->conn->feat_mask & L2CAP_FEAT_FCS)
3289			if (chan->fcs == L2CAP_FCS_NONE ||
3290			    test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) {
3291				chan->fcs = L2CAP_FCS_NONE;
3292				l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1,
3293						   chan->fcs);
3294			}
3295		break;
3296	}
3297
3298	req->dcid  = cpu_to_le16(chan->dcid);
3299	req->flags = cpu_to_le16(0);
3300
3301	return ptr - data;
3302}
3303
3304static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
3305{
3306	struct l2cap_conf_rsp *rsp = data;
3307	void *ptr = rsp->data;
 
3308	void *req = chan->conf_req;
3309	int len = chan->conf_len;
3310	int type, hint, olen;
3311	unsigned long val;
3312	struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3313	struct l2cap_conf_efs efs;
3314	u8 remote_efs = 0;
3315	u16 mtu = L2CAP_DEFAULT_MTU;
3316	u16 result = L2CAP_CONF_SUCCESS;
3317	u16 size;
3318
3319	BT_DBG("chan %p", chan);
3320
3321	while (len >= L2CAP_CONF_OPT_SIZE) {
3322		len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
3323
3324		hint  = type & L2CAP_CONF_HINT;
3325		type &= L2CAP_CONF_MASK;
3326
3327		switch (type) {
3328		case L2CAP_CONF_MTU:
3329			mtu = val;
3330			break;
3331
3332		case L2CAP_CONF_FLUSH_TO:
3333			chan->flush_to = val;
3334			break;
3335
3336		case L2CAP_CONF_QOS:
3337			break;
3338
3339		case L2CAP_CONF_RFC:
3340			if (olen == sizeof(rfc))
3341				memcpy(&rfc, (void *) val, olen);
3342			break;
3343
3344		case L2CAP_CONF_FCS:
3345			if (val == L2CAP_FCS_NONE)
3346				set_bit(CONF_RECV_NO_FCS, &chan->conf_state);
3347			break;
3348
3349		case L2CAP_CONF_EFS:
3350			remote_efs = 1;
3351			if (olen == sizeof(efs))
3352				memcpy(&efs, (void *) val, olen);
 
3353			break;
3354
3355		case L2CAP_CONF_EWS:
3356			if (!(chan->conn->local_fixed_chan & L2CAP_FC_A2MP))
3357				return -ECONNREFUSED;
3358
3359			set_bit(FLAG_EXT_CTRL, &chan->flags);
3360			set_bit(CONF_EWS_RECV, &chan->conf_state);
3361			chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
3362			chan->remote_tx_win = val;
3363			break;
3364
3365		default:
3366			if (hint)
3367				break;
3368
3369			result = L2CAP_CONF_UNKNOWN;
3370			*((u8 *) ptr++) = type;
3371			break;
3372		}
3373	}
3374
3375	if (chan->num_conf_rsp || chan->num_conf_req > 1)
3376		goto done;
3377
3378	switch (chan->mode) {
3379	case L2CAP_MODE_STREAMING:
3380	case L2CAP_MODE_ERTM:
3381		if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
3382			chan->mode = l2cap_select_mode(rfc.mode,
3383						       chan->conn->feat_mask);
3384			break;
3385		}
3386
3387		if (remote_efs) {
3388			if (__l2cap_efs_supported(chan->conn))
3389				set_bit(FLAG_EFS_ENABLE, &chan->flags);
3390			else
3391				return -ECONNREFUSED;
3392		}
3393
3394		if (chan->mode != rfc.mode)
3395			return -ECONNREFUSED;
3396
3397		break;
3398	}
3399
3400done:
3401	if (chan->mode != rfc.mode) {
3402		result = L2CAP_CONF_UNACCEPT;
3403		rfc.mode = chan->mode;
3404
3405		if (chan->num_conf_rsp == 1)
3406			return -ECONNREFUSED;
3407
3408		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3409				   (unsigned long) &rfc);
3410	}
3411
3412	if (result == L2CAP_CONF_SUCCESS) {
3413		/* Configure output options and let the other side know
3414		 * which ones we don't like. */
3415
3416		if (mtu < L2CAP_DEFAULT_MIN_MTU)
3417			result = L2CAP_CONF_UNACCEPT;
3418		else {
3419			chan->omtu = mtu;
3420			set_bit(CONF_MTU_DONE, &chan->conf_state);
3421		}
3422		l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
3423
3424		if (remote_efs) {
3425			if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3426			    efs.stype != L2CAP_SERV_NOTRAFIC &&
3427			    efs.stype != chan->local_stype) {
3428
3429				result = L2CAP_CONF_UNACCEPT;
3430
3431				if (chan->num_conf_req >= 1)
3432					return -ECONNREFUSED;
3433
3434				l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3435						   sizeof(efs),
3436						   (unsigned long) &efs);
3437			} else {
3438				/* Send PENDING Conf Rsp */
3439				result = L2CAP_CONF_PENDING;
3440				set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
3441			}
3442		}
3443
3444		switch (rfc.mode) {
3445		case L2CAP_MODE_BASIC:
3446			chan->fcs = L2CAP_FCS_NONE;
3447			set_bit(CONF_MODE_DONE, &chan->conf_state);
3448			break;
3449
3450		case L2CAP_MODE_ERTM:
3451			if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
3452				chan->remote_tx_win = rfc.txwin_size;
3453			else
3454				rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
3455
3456			chan->remote_max_tx = rfc.max_transmit;
3457
3458			size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3459				     chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3460				     L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3461			rfc.max_pdu_size = cpu_to_le16(size);
3462			chan->remote_mps = size;
3463
3464			__l2cap_set_ertm_timeouts(chan, &rfc);
3465
3466			set_bit(CONF_MODE_DONE, &chan->conf_state);
3467
3468			l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3469					   sizeof(rfc), (unsigned long) &rfc);
3470
3471			if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3472				chan->remote_id = efs.id;
3473				chan->remote_stype = efs.stype;
3474				chan->remote_msdu = le16_to_cpu(efs.msdu);
3475				chan->remote_flush_to =
3476					le32_to_cpu(efs.flush_to);
3477				chan->remote_acc_lat =
3478					le32_to_cpu(efs.acc_lat);
3479				chan->remote_sdu_itime =
3480					le32_to_cpu(efs.sdu_itime);
3481				l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
3482						   sizeof(efs),
3483						   (unsigned long) &efs);
3484			}
3485			break;
3486
3487		case L2CAP_MODE_STREAMING:
3488			size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
3489				     chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
3490				     L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
3491			rfc.max_pdu_size = cpu_to_le16(size);
3492			chan->remote_mps = size;
3493
3494			set_bit(CONF_MODE_DONE, &chan->conf_state);
3495
3496			l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
3497					   (unsigned long) &rfc);
3498
3499			break;
3500
3501		default:
3502			result = L2CAP_CONF_UNACCEPT;
3503
3504			memset(&rfc, 0, sizeof(rfc));
3505			rfc.mode = chan->mode;
3506		}
3507
3508		if (result == L2CAP_CONF_SUCCESS)
3509			set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
3510	}
3511	rsp->scid   = cpu_to_le16(chan->dcid);
3512	rsp->result = cpu_to_le16(result);
3513	rsp->flags  = cpu_to_le16(0);
3514
3515	return ptr - data;
3516}
3517
3518static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
3519				void *data, u16 *result)
3520{
3521	struct l2cap_conf_req *req = data;
3522	void *ptr = req->data;
 
3523	int type, olen;
3524	unsigned long val;
3525	struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
3526	struct l2cap_conf_efs efs;
3527
3528	BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
3529
3530	while (len >= L2CAP_CONF_OPT_SIZE) {
3531		len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3532
3533		switch (type) {
3534		case L2CAP_CONF_MTU:
3535			if (val < L2CAP_DEFAULT_MIN_MTU) {
3536				*result = L2CAP_CONF_UNACCEPT;
3537				chan->imtu = L2CAP_DEFAULT_MIN_MTU;
3538			} else
3539				chan->imtu = val;
3540			l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
3541			break;
3542
3543		case L2CAP_CONF_FLUSH_TO:
3544			chan->flush_to = val;
3545			l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
3546					   2, chan->flush_to);
3547			break;
3548
3549		case L2CAP_CONF_RFC:
3550			if (olen == sizeof(rfc))
3551				memcpy(&rfc, (void *)val, olen);
3552
3553			if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
3554			    rfc.mode != chan->mode)
3555				return -ECONNREFUSED;
3556
3557			chan->fcs = 0;
3558
3559			l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
3560					   sizeof(rfc), (unsigned long) &rfc);
3561			break;
3562
3563		case L2CAP_CONF_EWS:
3564			chan->ack_win = min_t(u16, val, chan->ack_win);
3565			l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
3566					   chan->tx_win);
3567			break;
3568
3569		case L2CAP_CONF_EFS:
3570			if (olen == sizeof(efs))
3571				memcpy(&efs, (void *)val, olen);
3572
3573			if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
3574			    efs.stype != L2CAP_SERV_NOTRAFIC &&
3575			    efs.stype != chan->local_stype)
3576				return -ECONNREFUSED;
3577
3578			l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
3579					   (unsigned long) &efs);
 
3580			break;
3581
3582		case L2CAP_CONF_FCS:
3583			if (*result == L2CAP_CONF_PENDING)
3584				if (val == L2CAP_FCS_NONE)
3585					set_bit(CONF_RECV_NO_FCS,
3586						&chan->conf_state);
3587			break;
3588		}
3589	}
3590
3591	if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
3592		return -ECONNREFUSED;
3593
3594	chan->mode = rfc.mode;
3595
3596	if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
3597		switch (rfc.mode) {
3598		case L2CAP_MODE_ERTM:
3599			chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3600			chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3601			chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3602			if (!test_bit(FLAG_EXT_CTRL, &chan->flags))
3603				chan->ack_win = min_t(u16, chan->ack_win,
3604						      rfc.txwin_size);
3605
3606			if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
3607				chan->local_msdu = le16_to_cpu(efs.msdu);
3608				chan->local_sdu_itime =
3609					le32_to_cpu(efs.sdu_itime);
3610				chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
3611				chan->local_flush_to =
3612					le32_to_cpu(efs.flush_to);
3613			}
3614			break;
3615
3616		case L2CAP_MODE_STREAMING:
3617			chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3618		}
3619	}
3620
3621	req->dcid   = cpu_to_le16(chan->dcid);
3622	req->flags  = cpu_to_le16(0);
3623
3624	return ptr - data;
3625}
3626
3627static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
3628				u16 result, u16 flags)
3629{
3630	struct l2cap_conf_rsp *rsp = data;
3631	void *ptr = rsp->data;
3632
3633	BT_DBG("chan %p", chan);
3634
3635	rsp->scid   = cpu_to_le16(chan->dcid);
3636	rsp->result = cpu_to_le16(result);
3637	rsp->flags  = cpu_to_le16(flags);
3638
3639	return ptr - data;
3640}
3641
3642void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
3643{
3644	struct l2cap_le_conn_rsp rsp;
3645	struct l2cap_conn *conn = chan->conn;
3646
3647	BT_DBG("chan %p", chan);
3648
3649	rsp.dcid    = cpu_to_le16(chan->scid);
3650	rsp.mtu     = cpu_to_le16(chan->imtu);
3651	rsp.mps     = cpu_to_le16(chan->mps);
3652	rsp.credits = cpu_to_le16(chan->rx_credits);
3653	rsp.result  = cpu_to_le16(L2CAP_CR_SUCCESS);
3654
3655	l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
3656		       &rsp);
3657}
3658
3659void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
3660{
3661	struct l2cap_conn_rsp rsp;
3662	struct l2cap_conn *conn = chan->conn;
3663	u8 buf[128];
3664	u8 rsp_code;
3665
3666	rsp.scid   = cpu_to_le16(chan->dcid);
3667	rsp.dcid   = cpu_to_le16(chan->scid);
3668	rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
3669	rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3670
3671	if (chan->hs_hcon)
3672		rsp_code = L2CAP_CREATE_CHAN_RSP;
3673	else
3674		rsp_code = L2CAP_CONN_RSP;
3675
3676	BT_DBG("chan %p rsp_code %u", chan, rsp_code);
3677
3678	l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp);
3679
3680	if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3681		return;
3682
3683	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3684		       l2cap_build_conf_req(chan, buf), buf);
3685	chan->num_conf_req++;
3686}
3687
3688static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
3689{
3690	int type, olen;
3691	unsigned long val;
3692	/* Use sane default values in case a misbehaving remote device
3693	 * did not send an RFC or extended window size option.
3694	 */
3695	u16 txwin_ext = chan->ack_win;
3696	struct l2cap_conf_rfc rfc = {
3697		.mode = chan->mode,
3698		.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO),
3699		.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO),
3700		.max_pdu_size = cpu_to_le16(chan->imtu),
3701		.txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW),
3702	};
3703
3704	BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
3705
3706	if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
3707		return;
3708
3709	while (len >= L2CAP_CONF_OPT_SIZE) {
3710		len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
3711
3712		switch (type) {
3713		case L2CAP_CONF_RFC:
3714			if (olen == sizeof(rfc))
3715				memcpy(&rfc, (void *)val, olen);
3716			break;
3717		case L2CAP_CONF_EWS:
3718			txwin_ext = val;
3719			break;
3720		}
3721	}
3722
3723	switch (rfc.mode) {
3724	case L2CAP_MODE_ERTM:
3725		chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
3726		chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
3727		chan->mps = le16_to_cpu(rfc.max_pdu_size);
3728		if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3729			chan->ack_win = min_t(u16, chan->ack_win, txwin_ext);
3730		else
3731			chan->ack_win = min_t(u16, chan->ack_win,
3732					      rfc.txwin_size);
3733		break;
3734	case L2CAP_MODE_STREAMING:
3735		chan->mps    = le16_to_cpu(rfc.max_pdu_size);
3736	}
3737}
3738
3739static inline int l2cap_command_rej(struct l2cap_conn *conn,
3740				    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3741				    u8 *data)
3742{
3743	struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
3744
3745	if (cmd_len < sizeof(*rej))
3746		return -EPROTO;
3747
3748	if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
3749		return 0;
3750
3751	if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
3752	    cmd->ident == conn->info_ident) {
3753		cancel_delayed_work(&conn->info_timer);
3754
3755		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
3756		conn->info_ident = 0;
3757
3758		l2cap_conn_start(conn);
3759	}
3760
3761	return 0;
3762}
3763
3764static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
3765					struct l2cap_cmd_hdr *cmd,
3766					u8 *data, u8 rsp_code, u8 amp_id)
3767{
3768	struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
3769	struct l2cap_conn_rsp rsp;
3770	struct l2cap_chan *chan = NULL, *pchan;
3771	int result, status = L2CAP_CS_NO_INFO;
3772
3773	u16 dcid = 0, scid = __le16_to_cpu(req->scid);
3774	__le16 psm = req->psm;
3775
3776	BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
3777
3778	/* Check if we have socket listening on psm */
3779	pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
3780					 &conn->hcon->dst, ACL_LINK);
3781	if (!pchan) {
3782		result = L2CAP_CR_BAD_PSM;
3783		goto sendresp;
3784	}
3785
3786	mutex_lock(&conn->chan_lock);
3787	l2cap_chan_lock(pchan);
3788
3789	/* Check if the ACL is secure enough (if not SDP) */
3790	if (psm != cpu_to_le16(L2CAP_PSM_SDP) &&
3791	    !hci_conn_check_link_mode(conn->hcon)) {
3792		conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
3793		result = L2CAP_CR_SEC_BLOCK;
3794		goto response;
3795	}
3796
3797	result = L2CAP_CR_NO_MEM;
3798
3799	/* Check if we already have channel with that dcid */
3800	if (__l2cap_get_chan_by_dcid(conn, scid))
3801		goto response;
3802
3803	chan = pchan->ops->new_connection(pchan);
3804	if (!chan)
3805		goto response;
3806
3807	/* For certain devices (ex: HID mouse), support for authentication,
3808	 * pairing and bonding is optional. For such devices, inorder to avoid
3809	 * the ACL alive for too long after L2CAP disconnection, reset the ACL
3810	 * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect.
3811	 */
3812	conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
3813
3814	bacpy(&chan->src, &conn->hcon->src);
3815	bacpy(&chan->dst, &conn->hcon->dst);
3816	chan->src_type = bdaddr_src_type(conn->hcon);
3817	chan->dst_type = bdaddr_dst_type(conn->hcon);
3818	chan->psm  = psm;
3819	chan->dcid = scid;
3820	chan->local_amp_id = amp_id;
3821
3822	__l2cap_chan_add(conn, chan);
3823
3824	dcid = chan->scid;
3825
3826	__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
3827
3828	chan->ident = cmd->ident;
3829
3830	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
3831		if (l2cap_chan_check_security(chan, false)) {
3832			if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
3833				l2cap_state_change(chan, BT_CONNECT2);
3834				result = L2CAP_CR_PEND;
3835				status = L2CAP_CS_AUTHOR_PEND;
3836				chan->ops->defer(chan);
3837			} else {
3838				/* Force pending result for AMP controllers.
3839				 * The connection will succeed after the
3840				 * physical link is up.
3841				 */
3842				if (amp_id == AMP_ID_BREDR) {
3843					l2cap_state_change(chan, BT_CONFIG);
3844					result = L2CAP_CR_SUCCESS;
3845				} else {
3846					l2cap_state_change(chan, BT_CONNECT2);
3847					result = L2CAP_CR_PEND;
3848				}
3849				status = L2CAP_CS_NO_INFO;
3850			}
3851		} else {
3852			l2cap_state_change(chan, BT_CONNECT2);
3853			result = L2CAP_CR_PEND;
3854			status = L2CAP_CS_AUTHEN_PEND;
3855		}
3856	} else {
3857		l2cap_state_change(chan, BT_CONNECT2);
3858		result = L2CAP_CR_PEND;
3859		status = L2CAP_CS_NO_INFO;
3860	}
3861
3862response:
3863	l2cap_chan_unlock(pchan);
3864	mutex_unlock(&conn->chan_lock);
3865	l2cap_chan_put(pchan);
3866
3867sendresp:
3868	rsp.scid   = cpu_to_le16(scid);
3869	rsp.dcid   = cpu_to_le16(dcid);
3870	rsp.result = cpu_to_le16(result);
3871	rsp.status = cpu_to_le16(status);
3872	l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
3873
3874	if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
3875		struct l2cap_info_req info;
3876		info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
3877
3878		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
3879		conn->info_ident = l2cap_get_ident(conn);
3880
3881		schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
3882
3883		l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
3884			       sizeof(info), &info);
3885	}
3886
3887	if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
3888	    result == L2CAP_CR_SUCCESS) {
3889		u8 buf[128];
3890		set_bit(CONF_REQ_SENT, &chan->conf_state);
3891		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3892			       l2cap_build_conf_req(chan, buf), buf);
3893		chan->num_conf_req++;
3894	}
3895
3896	return chan;
3897}
3898
3899static int l2cap_connect_req(struct l2cap_conn *conn,
3900			     struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
3901{
3902	struct hci_dev *hdev = conn->hcon->hdev;
3903	struct hci_conn *hcon = conn->hcon;
3904
3905	if (cmd_len < sizeof(struct l2cap_conn_req))
3906		return -EPROTO;
3907
3908	hci_dev_lock(hdev);
3909	if (hci_dev_test_flag(hdev, HCI_MGMT) &&
3910	    !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags))
3911		mgmt_device_connected(hdev, hcon, 0, NULL, 0);
3912	hci_dev_unlock(hdev);
3913
3914	l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
3915	return 0;
3916}
3917
3918static int l2cap_connect_create_rsp(struct l2cap_conn *conn,
3919				    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3920				    u8 *data)
3921{
3922	struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
3923	u16 scid, dcid, result, status;
3924	struct l2cap_chan *chan;
3925	u8 req[128];
3926	int err;
3927
3928	if (cmd_len < sizeof(*rsp))
3929		return -EPROTO;
3930
3931	scid   = __le16_to_cpu(rsp->scid);
3932	dcid   = __le16_to_cpu(rsp->dcid);
3933	result = __le16_to_cpu(rsp->result);
3934	status = __le16_to_cpu(rsp->status);
3935
3936	BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
3937	       dcid, scid, result, status);
3938
3939	mutex_lock(&conn->chan_lock);
3940
3941	if (scid) {
3942		chan = __l2cap_get_chan_by_scid(conn, scid);
3943		if (!chan) {
3944			err = -EBADSLT;
3945			goto unlock;
3946		}
3947	} else {
3948		chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
3949		if (!chan) {
3950			err = -EBADSLT;
3951			goto unlock;
3952		}
3953	}
3954
3955	err = 0;
3956
3957	l2cap_chan_lock(chan);
3958
3959	switch (result) {
3960	case L2CAP_CR_SUCCESS:
3961		l2cap_state_change(chan, BT_CONFIG);
3962		chan->ident = 0;
3963		chan->dcid = dcid;
3964		clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
3965
3966		if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
3967			break;
3968
3969		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
3970			       l2cap_build_conf_req(chan, req), req);
3971		chan->num_conf_req++;
3972		break;
3973
3974	case L2CAP_CR_PEND:
3975		set_bit(CONF_CONNECT_PEND, &chan->conf_state);
3976		break;
3977
3978	default:
3979		l2cap_chan_del(chan, ECONNREFUSED);
3980		break;
3981	}
3982
3983	l2cap_chan_unlock(chan);
3984
3985unlock:
3986	mutex_unlock(&conn->chan_lock);
3987
3988	return err;
3989}
3990
3991static inline void set_default_fcs(struct l2cap_chan *chan)
3992{
3993	/* FCS is enabled only in ERTM or streaming mode, if one or both
3994	 * sides request it.
3995	 */
3996	if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
3997		chan->fcs = L2CAP_FCS_NONE;
3998	else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state))
3999		chan->fcs = L2CAP_FCS_CRC16;
4000}
4001
4002static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
4003				    u8 ident, u16 flags)
4004{
4005	struct l2cap_conn *conn = chan->conn;
4006
4007	BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
4008	       flags);
4009
4010	clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
4011	set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
4012
4013	l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
4014		       l2cap_build_conf_rsp(chan, data,
4015					    L2CAP_CONF_SUCCESS, flags), data);
4016}
4017
4018static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident,
4019				   u16 scid, u16 dcid)
4020{
4021	struct l2cap_cmd_rej_cid rej;
4022
4023	rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
4024	rej.scid = __cpu_to_le16(scid);
4025	rej.dcid = __cpu_to_le16(dcid);
4026
4027	l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
4028}
4029
4030static inline int l2cap_config_req(struct l2cap_conn *conn,
4031				   struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4032				   u8 *data)
4033{
4034	struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
4035	u16 dcid, flags;
4036	u8 rsp[64];
4037	struct l2cap_chan *chan;
4038	int len, err = 0;
4039
4040	if (cmd_len < sizeof(*req))
4041		return -EPROTO;
4042
4043	dcid  = __le16_to_cpu(req->dcid);
4044	flags = __le16_to_cpu(req->flags);
4045
4046	BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
4047
4048	chan = l2cap_get_chan_by_scid(conn, dcid);
4049	if (!chan) {
4050		cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0);
4051		return 0;
4052	}
4053
4054	if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
4055		cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4056				       chan->dcid);
4057		goto unlock;
4058	}
4059
4060	/* Reject if config buffer is too small. */
4061	len = cmd_len - sizeof(*req);
4062	if (chan->conf_len + len > sizeof(chan->conf_req)) {
4063		l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4064			       l2cap_build_conf_rsp(chan, rsp,
4065			       L2CAP_CONF_REJECT, flags), rsp);
4066		goto unlock;
4067	}
4068
4069	/* Store config. */
4070	memcpy(chan->conf_req + chan->conf_len, req->data, len);
4071	chan->conf_len += len;
4072
4073	if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
4074		/* Incomplete config. Send empty response. */
4075		l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
4076			       l2cap_build_conf_rsp(chan, rsp,
4077			       L2CAP_CONF_SUCCESS, flags), rsp);
4078		goto unlock;
4079	}
4080
4081	/* Complete config. */
4082	len = l2cap_parse_conf_req(chan, rsp);
4083	if (len < 0) {
4084		l2cap_send_disconn_req(chan, ECONNRESET);
4085		goto unlock;
4086	}
4087
4088	chan->ident = cmd->ident;
4089	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
4090	chan->num_conf_rsp++;
4091
4092	/* Reset config buffer. */
4093	chan->conf_len = 0;
4094
4095	if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
4096		goto unlock;
4097
4098	if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4099		set_default_fcs(chan);
4100
4101		if (chan->mode == L2CAP_MODE_ERTM ||
4102		    chan->mode == L2CAP_MODE_STREAMING)
4103			err = l2cap_ertm_init(chan);
4104
4105		if (err < 0)
4106			l2cap_send_disconn_req(chan, -err);
4107		else
4108			l2cap_chan_ready(chan);
4109
4110		goto unlock;
4111	}
4112
4113	if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
4114		u8 buf[64];
4115		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
4116			       l2cap_build_conf_req(chan, buf), buf);
4117		chan->num_conf_req++;
4118	}
4119
4120	/* Got Conf Rsp PENDING from remote side and assume we sent
4121	   Conf Rsp PENDING in the code above */
4122	if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
4123	    test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4124
4125		/* check compatibility */
4126
4127		/* Send rsp for BR/EDR channel */
4128		if (!chan->hs_hcon)
4129			l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
4130		else
4131			chan->ident = cmd->ident;
4132	}
4133
4134unlock:
4135	l2cap_chan_unlock(chan);
4136	return err;
4137}
4138
4139static inline int l2cap_config_rsp(struct l2cap_conn *conn,
4140				   struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4141				   u8 *data)
4142{
4143	struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
4144	u16 scid, flags, result;
4145	struct l2cap_chan *chan;
4146	int len = cmd_len - sizeof(*rsp);
4147	int err = 0;
4148
4149	if (cmd_len < sizeof(*rsp))
4150		return -EPROTO;
4151
4152	scid   = __le16_to_cpu(rsp->scid);
4153	flags  = __le16_to_cpu(rsp->flags);
4154	result = __le16_to_cpu(rsp->result);
4155
4156	BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags,
4157	       result, len);
4158
4159	chan = l2cap_get_chan_by_scid(conn, scid);
4160	if (!chan)
4161		return 0;
4162
4163	switch (result) {
4164	case L2CAP_CONF_SUCCESS:
4165		l2cap_conf_rfc_get(chan, rsp->data, len);
4166		clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4167		break;
4168
4169	case L2CAP_CONF_PENDING:
4170		set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
4171
4172		if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
4173			char buf[64];
4174
4175			len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4176						   buf, &result);
4177			if (len < 0) {
4178				l2cap_send_disconn_req(chan, ECONNRESET);
4179				goto done;
4180			}
4181
4182			if (!chan->hs_hcon) {
4183				l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
4184							0);
4185			} else {
4186				if (l2cap_check_efs(chan)) {
4187					amp_create_logical_link(chan);
4188					chan->ident = cmd->ident;
4189				}
4190			}
4191		}
4192		goto done;
4193
4194	case L2CAP_CONF_UNACCEPT:
4195		if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
4196			char req[64];
4197
4198			if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
4199				l2cap_send_disconn_req(chan, ECONNRESET);
4200				goto done;
4201			}
4202
4203			/* throw out any old stored conf requests */
4204			result = L2CAP_CONF_SUCCESS;
4205			len = l2cap_parse_conf_rsp(chan, rsp->data, len,
4206						   req, &result);
4207			if (len < 0) {
4208				l2cap_send_disconn_req(chan, ECONNRESET);
4209				goto done;
4210			}
4211
4212			l2cap_send_cmd(conn, l2cap_get_ident(conn),
4213				       L2CAP_CONF_REQ, len, req);
4214			chan->num_conf_req++;
4215			if (result != L2CAP_CONF_SUCCESS)
4216				goto done;
4217			break;
4218		}
4219
4220	default:
4221		l2cap_chan_set_err(chan, ECONNRESET);
4222
4223		__set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT);
4224		l2cap_send_disconn_req(chan, ECONNRESET);
4225		goto done;
4226	}
4227
4228	if (flags & L2CAP_CONF_FLAG_CONTINUATION)
4229		goto done;
4230
4231	set_bit(CONF_INPUT_DONE, &chan->conf_state);
4232
4233	if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
4234		set_default_fcs(chan);
4235
4236		if (chan->mode == L2CAP_MODE_ERTM ||
4237		    chan->mode == L2CAP_MODE_STREAMING)
4238			err = l2cap_ertm_init(chan);
4239
4240		if (err < 0)
4241			l2cap_send_disconn_req(chan, -err);
4242		else
4243			l2cap_chan_ready(chan);
4244	}
4245
4246done:
4247	l2cap_chan_unlock(chan);
4248	return err;
4249}
4250
4251static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
4252				       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4253				       u8 *data)
4254{
4255	struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
4256	struct l2cap_disconn_rsp rsp;
4257	u16 dcid, scid;
4258	struct l2cap_chan *chan;
4259
4260	if (cmd_len != sizeof(*req))
4261		return -EPROTO;
4262
4263	scid = __le16_to_cpu(req->scid);
4264	dcid = __le16_to_cpu(req->dcid);
4265
4266	BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
4267
4268	mutex_lock(&conn->chan_lock);
4269
4270	chan = __l2cap_get_chan_by_scid(conn, dcid);
4271	if (!chan) {
4272		mutex_unlock(&conn->chan_lock);
4273		cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
4274		return 0;
4275	}
4276
4277	l2cap_chan_lock(chan);
4278
4279	rsp.dcid = cpu_to_le16(chan->scid);
4280	rsp.scid = cpu_to_le16(chan->dcid);
4281	l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
4282
4283	chan->ops->set_shutdown(chan);
4284
4285	l2cap_chan_hold(chan);
4286	l2cap_chan_del(chan, ECONNRESET);
4287
4288	l2cap_chan_unlock(chan);
4289
4290	chan->ops->close(chan);
4291	l2cap_chan_put(chan);
4292
4293	mutex_unlock(&conn->chan_lock);
4294
4295	return 0;
4296}
4297
4298static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
4299				       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4300				       u8 *data)
4301{
4302	struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
4303	u16 dcid, scid;
4304	struct l2cap_chan *chan;
4305
4306	if (cmd_len != sizeof(*rsp))
4307		return -EPROTO;
4308
4309	scid = __le16_to_cpu(rsp->scid);
4310	dcid = __le16_to_cpu(rsp->dcid);
4311
4312	BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
4313
4314	mutex_lock(&conn->chan_lock);
4315
4316	chan = __l2cap_get_chan_by_scid(conn, scid);
4317	if (!chan) {
4318		mutex_unlock(&conn->chan_lock);
4319		return 0;
4320	}
4321
4322	l2cap_chan_lock(chan);
4323
4324	l2cap_chan_hold(chan);
4325	l2cap_chan_del(chan, 0);
4326
4327	l2cap_chan_unlock(chan);
4328
4329	chan->ops->close(chan);
4330	l2cap_chan_put(chan);
4331
4332	mutex_unlock(&conn->chan_lock);
4333
4334	return 0;
4335}
4336
4337static inline int l2cap_information_req(struct l2cap_conn *conn,
4338					struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4339					u8 *data)
4340{
4341	struct l2cap_info_req *req = (struct l2cap_info_req *) data;
4342	u16 type;
4343
4344	if (cmd_len != sizeof(*req))
4345		return -EPROTO;
4346
4347	type = __le16_to_cpu(req->type);
4348
4349	BT_DBG("type 0x%4.4x", type);
4350
4351	if (type == L2CAP_IT_FEAT_MASK) {
4352		u8 buf[8];
4353		u32 feat_mask = l2cap_feat_mask;
4354		struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4355		rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
4356		rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4357		if (!disable_ertm)
4358			feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
4359				| L2CAP_FEAT_FCS;
4360		if (conn->local_fixed_chan & L2CAP_FC_A2MP)
4361			feat_mask |= L2CAP_FEAT_EXT_FLOW
4362				| L2CAP_FEAT_EXT_WINDOW;
4363
4364		put_unaligned_le32(feat_mask, rsp->data);
4365		l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4366			       buf);
4367	} else if (type == L2CAP_IT_FIXED_CHAN) {
4368		u8 buf[12];
4369		struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
4370
4371		rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4372		rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
4373		rsp->data[0] = conn->local_fixed_chan;
4374		memset(rsp->data + 1, 0, 7);
4375		l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
4376			       buf);
4377	} else {
4378		struct l2cap_info_rsp rsp;
4379		rsp.type   = cpu_to_le16(type);
4380		rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
4381		l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
4382			       &rsp);
4383	}
4384
4385	return 0;
4386}
4387
4388static inline int l2cap_information_rsp(struct l2cap_conn *conn,
4389					struct l2cap_cmd_hdr *cmd, u16 cmd_len,
4390					u8 *data)
4391{
4392	struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
4393	u16 type, result;
4394
4395	if (cmd_len < sizeof(*rsp))
4396		return -EPROTO;
4397
4398	type   = __le16_to_cpu(rsp->type);
4399	result = __le16_to_cpu(rsp->result);
4400
4401	BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
4402
4403	/* L2CAP Info req/rsp are unbound to channels, add extra checks */
4404	if (cmd->ident != conn->info_ident ||
4405	    conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
4406		return 0;
4407
4408	cancel_delayed_work(&conn->info_timer);
4409
4410	if (result != L2CAP_IR_SUCCESS) {
4411		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4412		conn->info_ident = 0;
4413
4414		l2cap_conn_start(conn);
4415
4416		return 0;
4417	}
4418
4419	switch (type) {
4420	case L2CAP_IT_FEAT_MASK:
4421		conn->feat_mask = get_unaligned_le32(rsp->data);
4422
4423		if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
4424			struct l2cap_info_req req;
4425			req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
4426
4427			conn->info_ident = l2cap_get_ident(conn);
4428
4429			l2cap_send_cmd(conn, conn->info_ident,
4430				       L2CAP_INFO_REQ, sizeof(req), &req);
4431		} else {
4432			conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4433			conn->info_ident = 0;
4434
4435			l2cap_conn_start(conn);
4436		}
4437		break;
4438
4439	case L2CAP_IT_FIXED_CHAN:
4440		conn->remote_fixed_chan = rsp->data[0];
4441		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
4442		conn->info_ident = 0;
4443
4444		l2cap_conn_start(conn);
4445		break;
4446	}
4447
4448	return 0;
4449}
4450
4451static int l2cap_create_channel_req(struct l2cap_conn *conn,
4452				    struct l2cap_cmd_hdr *cmd,
4453				    u16 cmd_len, void *data)
4454{
4455	struct l2cap_create_chan_req *req = data;
4456	struct l2cap_create_chan_rsp rsp;
4457	struct l2cap_chan *chan;
4458	struct hci_dev *hdev;
4459	u16 psm, scid;
4460
4461	if (cmd_len != sizeof(*req))
4462		return -EPROTO;
4463
4464	if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4465		return -EINVAL;
4466
4467	psm = le16_to_cpu(req->psm);
4468	scid = le16_to_cpu(req->scid);
4469
4470	BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
4471
4472	/* For controller id 0 make BR/EDR connection */
4473	if (req->amp_id == AMP_ID_BREDR) {
4474		l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4475			      req->amp_id);
4476		return 0;
4477	}
4478
4479	/* Validate AMP controller id */
4480	hdev = hci_dev_get(req->amp_id);
4481	if (!hdev)
4482		goto error;
4483
4484	if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) {
4485		hci_dev_put(hdev);
4486		goto error;
4487	}
4488
4489	chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
4490			     req->amp_id);
4491	if (chan) {
4492		struct amp_mgr *mgr = conn->hcon->amp_mgr;
4493		struct hci_conn *hs_hcon;
4494
4495		hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
4496						  &conn->hcon->dst);
4497		if (!hs_hcon) {
4498			hci_dev_put(hdev);
4499			cmd_reject_invalid_cid(conn, cmd->ident, chan->scid,
4500					       chan->dcid);
4501			return 0;
4502		}
4503
4504		BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon);
4505
4506		mgr->bredr_chan = chan;
4507		chan->hs_hcon = hs_hcon;
4508		chan->fcs = L2CAP_FCS_NONE;
4509		conn->mtu = hdev->block_mtu;
4510	}
4511
4512	hci_dev_put(hdev);
4513
4514	return 0;
4515
4516error:
4517	rsp.dcid = 0;
4518	rsp.scid = cpu_to_le16(scid);
4519	rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP);
4520	rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4521
4522	l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
4523		       sizeof(rsp), &rsp);
4524
4525	return 0;
4526}
4527
4528static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id)
4529{
4530	struct l2cap_move_chan_req req;
4531	u8 ident;
4532
4533	BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id);
4534
4535	ident = l2cap_get_ident(chan->conn);
4536	chan->ident = ident;
4537
4538	req.icid = cpu_to_le16(chan->scid);
4539	req.dest_amp_id = dest_amp_id;
4540
4541	l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req),
4542		       &req);
4543
4544	__set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4545}
4546
4547static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result)
4548{
4549	struct l2cap_move_chan_rsp rsp;
4550
4551	BT_DBG("chan %p, result 0x%4.4x", chan, result);
4552
4553	rsp.icid = cpu_to_le16(chan->dcid);
4554	rsp.result = cpu_to_le16(result);
4555
4556	l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP,
4557		       sizeof(rsp), &rsp);
4558}
4559
4560static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result)
4561{
4562	struct l2cap_move_chan_cfm cfm;
4563
4564	BT_DBG("chan %p, result 0x%4.4x", chan, result);
4565
4566	chan->ident = l2cap_get_ident(chan->conn);
4567
4568	cfm.icid = cpu_to_le16(chan->scid);
4569	cfm.result = cpu_to_le16(result);
4570
4571	l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM,
4572		       sizeof(cfm), &cfm);
4573
4574	__set_chan_timer(chan, L2CAP_MOVE_TIMEOUT);
4575}
4576
4577static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid)
4578{
4579	struct l2cap_move_chan_cfm cfm;
4580
4581	BT_DBG("conn %p, icid 0x%4.4x", conn, icid);
4582
4583	cfm.icid = cpu_to_le16(icid);
4584	cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED);
4585
4586	l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM,
4587		       sizeof(cfm), &cfm);
4588}
4589
4590static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
4591					 u16 icid)
4592{
4593	struct l2cap_move_chan_cfm_rsp rsp;
4594
4595	BT_DBG("icid 0x%4.4x", icid);
4596
4597	rsp.icid = cpu_to_le16(icid);
4598	l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
4599}
4600
4601static void __release_logical_link(struct l2cap_chan *chan)
4602{
4603	chan->hs_hchan = NULL;
4604	chan->hs_hcon = NULL;
4605
4606	/* Placeholder - release the logical link */
4607}
4608
4609static void l2cap_logical_fail(struct l2cap_chan *chan)
4610{
4611	/* Logical link setup failed */
4612	if (chan->state != BT_CONNECTED) {
4613		/* Create channel failure, disconnect */
4614		l2cap_send_disconn_req(chan, ECONNRESET);
4615		return;
4616	}
4617
4618	switch (chan->move_role) {
4619	case L2CAP_MOVE_ROLE_RESPONDER:
4620		l2cap_move_done(chan);
4621		l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP);
4622		break;
4623	case L2CAP_MOVE_ROLE_INITIATOR:
4624		if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP ||
4625		    chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) {
4626			/* Remote has only sent pending or
4627			 * success responses, clean up
4628			 */
4629			l2cap_move_done(chan);
4630		}
4631
4632		/* Other amp move states imply that the move
4633		 * has already aborted
4634		 */
4635		l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
4636		break;
4637	}
4638}
4639
4640static void l2cap_logical_finish_create(struct l2cap_chan *chan,
4641					struct hci_chan *hchan)
4642{
4643	struct l2cap_conf_rsp rsp;
4644
4645	chan->hs_hchan = hchan;
4646	chan->hs_hcon->l2cap_data = chan->conn;
4647
4648	l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0);
4649
4650	if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
4651		int err;
4652
4653		set_default_fcs(chan);
4654
4655		err = l2cap_ertm_init(chan);
4656		if (err < 0)
4657			l2cap_send_disconn_req(chan, -err);
4658		else
4659			l2cap_chan_ready(chan);
4660	}
4661}
4662
4663static void l2cap_logical_finish_move(struct l2cap_chan *chan,
4664				      struct hci_chan *hchan)
4665{
4666	chan->hs_hcon = hchan->conn;
4667	chan->hs_hcon->l2cap_data = chan->conn;
4668
4669	BT_DBG("move_state %d", chan->move_state);
4670
4671	switch (chan->move_state) {
4672	case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4673		/* Move confirm will be sent after a success
4674		 * response is received
4675		 */
4676		chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4677		break;
4678	case L2CAP_MOVE_WAIT_LOGICAL_CFM:
4679		if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4680			chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4681		} else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
4682			chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
4683			l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
4684		} else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4685			chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4686			l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4687		}
4688		break;
4689	default:
4690		/* Move was not in expected state, free the channel */
4691		__release_logical_link(chan);
4692
4693		chan->move_state = L2CAP_MOVE_STABLE;
4694	}
4695}
4696
4697/* Call with chan locked */
4698void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
4699		       u8 status)
4700{
4701	BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status);
4702
4703	if (status) {
4704		l2cap_logical_fail(chan);
4705		__release_logical_link(chan);
4706		return;
4707	}
4708
4709	if (chan->state != BT_CONNECTED) {
4710		/* Ignore logical link if channel is on BR/EDR */
4711		if (chan->local_amp_id != AMP_ID_BREDR)
4712			l2cap_logical_finish_create(chan, hchan);
4713	} else {
4714		l2cap_logical_finish_move(chan, hchan);
4715	}
4716}
4717
4718void l2cap_move_start(struct l2cap_chan *chan)
4719{
4720	BT_DBG("chan %p", chan);
4721
4722	if (chan->local_amp_id == AMP_ID_BREDR) {
4723		if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
4724			return;
4725		chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4726		chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4727		/* Placeholder - start physical link setup */
4728	} else {
4729		chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
4730		chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
4731		chan->move_id = 0;
4732		l2cap_move_setup(chan);
4733		l2cap_send_move_chan_req(chan, 0);
4734	}
4735}
4736
4737static void l2cap_do_create(struct l2cap_chan *chan, int result,
4738			    u8 local_amp_id, u8 remote_amp_id)
4739{
4740	BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state),
4741	       local_amp_id, remote_amp_id);
4742
4743	chan->fcs = L2CAP_FCS_NONE;
4744
4745	/* Outgoing channel on AMP */
4746	if (chan->state == BT_CONNECT) {
4747		if (result == L2CAP_CR_SUCCESS) {
4748			chan->local_amp_id = local_amp_id;
4749			l2cap_send_create_chan_req(chan, remote_amp_id);
4750		} else {
4751			/* Revert to BR/EDR connect */
4752			l2cap_send_conn_req(chan);
4753		}
4754
4755		return;
4756	}
4757
4758	/* Incoming channel on AMP */
4759	if (__l2cap_no_conn_pending(chan)) {
4760		struct l2cap_conn_rsp rsp;
4761		char buf[128];
4762		rsp.scid = cpu_to_le16(chan->dcid);
4763		rsp.dcid = cpu_to_le16(chan->scid);
4764
4765		if (result == L2CAP_CR_SUCCESS) {
4766			/* Send successful response */
4767			rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
4768			rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4769		} else {
4770			/* Send negative response */
4771			rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM);
4772			rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
4773		}
4774
4775		l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP,
4776			       sizeof(rsp), &rsp);
4777
4778		if (result == L2CAP_CR_SUCCESS) {
4779			l2cap_state_change(chan, BT_CONFIG);
4780			set_bit(CONF_REQ_SENT, &chan->conf_state);
4781			l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn),
4782				       L2CAP_CONF_REQ,
4783				       l2cap_build_conf_req(chan, buf), buf);
4784			chan->num_conf_req++;
4785		}
4786	}
4787}
4788
4789static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id,
4790				   u8 remote_amp_id)
4791{
4792	l2cap_move_setup(chan);
4793	chan->move_id = local_amp_id;
4794	chan->move_state = L2CAP_MOVE_WAIT_RSP;
4795
4796	l2cap_send_move_chan_req(chan, remote_amp_id);
4797}
4798
4799static void l2cap_do_move_respond(struct l2cap_chan *chan, int result)
4800{
4801	struct hci_chan *hchan = NULL;
4802
4803	/* Placeholder - get hci_chan for logical link */
4804
4805	if (hchan) {
4806		if (hchan->state == BT_CONNECTED) {
4807			/* Logical link is ready to go */
4808			chan->hs_hcon = hchan->conn;
4809			chan->hs_hcon->l2cap_data = chan->conn;
4810			chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4811			l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS);
4812
4813			l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
4814		} else {
4815			/* Wait for logical link to be ready */
4816			chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4817		}
4818	} else {
4819		/* Logical link not available */
4820		l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED);
4821	}
4822}
4823
4824static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result)
4825{
4826	if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) {
4827		u8 rsp_result;
4828		if (result == -EINVAL)
4829			rsp_result = L2CAP_MR_BAD_ID;
4830		else
4831			rsp_result = L2CAP_MR_NOT_ALLOWED;
4832
4833		l2cap_send_move_chan_rsp(chan, rsp_result);
4834	}
4835
4836	chan->move_role = L2CAP_MOVE_ROLE_NONE;
4837	chan->move_state = L2CAP_MOVE_STABLE;
4838
4839	/* Restart data transmission */
4840	l2cap_ertm_send(chan);
4841}
4842
4843/* Invoke with locked chan */
4844void __l2cap_physical_cfm(struct l2cap_chan *chan, int result)
4845{
4846	u8 local_amp_id = chan->local_amp_id;
4847	u8 remote_amp_id = chan->remote_amp_id;
4848
4849	BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d",
4850	       chan, result, local_amp_id, remote_amp_id);
4851
4852	if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) {
4853		l2cap_chan_unlock(chan);
4854		return;
4855	}
4856
4857	if (chan->state != BT_CONNECTED) {
4858		l2cap_do_create(chan, result, local_amp_id, remote_amp_id);
4859	} else if (result != L2CAP_MR_SUCCESS) {
4860		l2cap_do_move_cancel(chan, result);
4861	} else {
4862		switch (chan->move_role) {
4863		case L2CAP_MOVE_ROLE_INITIATOR:
4864			l2cap_do_move_initiate(chan, local_amp_id,
4865					       remote_amp_id);
4866			break;
4867		case L2CAP_MOVE_ROLE_RESPONDER:
4868			l2cap_do_move_respond(chan, result);
4869			break;
4870		default:
4871			l2cap_do_move_cancel(chan, result);
4872			break;
4873		}
4874	}
4875}
4876
4877static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
4878					 struct l2cap_cmd_hdr *cmd,
4879					 u16 cmd_len, void *data)
4880{
4881	struct l2cap_move_chan_req *req = data;
4882	struct l2cap_move_chan_rsp rsp;
4883	struct l2cap_chan *chan;
4884	u16 icid = 0;
4885	u16 result = L2CAP_MR_NOT_ALLOWED;
4886
4887	if (cmd_len != sizeof(*req))
4888		return -EPROTO;
4889
4890	icid = le16_to_cpu(req->icid);
4891
4892	BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id);
4893
4894	if (!(conn->local_fixed_chan & L2CAP_FC_A2MP))
4895		return -EINVAL;
4896
4897	chan = l2cap_get_chan_by_dcid(conn, icid);
4898	if (!chan) {
4899		rsp.icid = cpu_to_le16(icid);
4900		rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED);
4901		l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP,
4902			       sizeof(rsp), &rsp);
4903		return 0;
4904	}
4905
4906	chan->ident = cmd->ident;
4907
4908	if (chan->scid < L2CAP_CID_DYN_START ||
4909	    chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY ||
4910	    (chan->mode != L2CAP_MODE_ERTM &&
4911	     chan->mode != L2CAP_MODE_STREAMING)) {
4912		result = L2CAP_MR_NOT_ALLOWED;
4913		goto send_move_response;
4914	}
4915
4916	if (chan->local_amp_id == req->dest_amp_id) {
4917		result = L2CAP_MR_SAME_ID;
4918		goto send_move_response;
4919	}
4920
4921	if (req->dest_amp_id != AMP_ID_BREDR) {
4922		struct hci_dev *hdev;
4923		hdev = hci_dev_get(req->dest_amp_id);
4924		if (!hdev || hdev->dev_type != HCI_AMP ||
4925		    !test_bit(HCI_UP, &hdev->flags)) {
4926			if (hdev)
4927				hci_dev_put(hdev);
4928
4929			result = L2CAP_MR_BAD_ID;
4930			goto send_move_response;
4931		}
4932		hci_dev_put(hdev);
4933	}
4934
4935	/* Detect a move collision.  Only send a collision response
4936	 * if this side has "lost", otherwise proceed with the move.
4937	 * The winner has the larger bd_addr.
4938	 */
4939	if ((__chan_is_moving(chan) ||
4940	     chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
4941	    bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
4942		result = L2CAP_MR_COLLISION;
4943		goto send_move_response;
4944	}
4945
4946	chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
4947	l2cap_move_setup(chan);
4948	chan->move_id = req->dest_amp_id;
4949	icid = chan->dcid;
4950
4951	if (req->dest_amp_id == AMP_ID_BREDR) {
4952		/* Moving to BR/EDR */
4953		if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
4954			chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
4955			result = L2CAP_MR_PEND;
4956		} else {
4957			chan->move_state = L2CAP_MOVE_WAIT_CONFIRM;
4958			result = L2CAP_MR_SUCCESS;
4959		}
4960	} else {
4961		chan->move_state = L2CAP_MOVE_WAIT_PREPARE;
4962		/* Placeholder - uncomment when amp functions are available */
4963		/*amp_accept_physical(chan, req->dest_amp_id);*/
4964		result = L2CAP_MR_PEND;
4965	}
4966
4967send_move_response:
4968	l2cap_send_move_chan_rsp(chan, result);
4969
4970	l2cap_chan_unlock(chan);
4971
4972	return 0;
4973}
4974
4975static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result)
4976{
4977	struct l2cap_chan *chan;
4978	struct hci_chan *hchan = NULL;
4979
4980	chan = l2cap_get_chan_by_scid(conn, icid);
4981	if (!chan) {
4982		l2cap_send_move_chan_cfm_icid(conn, icid);
4983		return;
4984	}
4985
4986	__clear_chan_timer(chan);
4987	if (result == L2CAP_MR_PEND)
4988		__set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT);
4989
4990	switch (chan->move_state) {
4991	case L2CAP_MOVE_WAIT_LOGICAL_COMP:
4992		/* Move confirm will be sent when logical link
4993		 * is complete.
4994		 */
4995		chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
4996		break;
4997	case L2CAP_MOVE_WAIT_RSP_SUCCESS:
4998		if (result == L2CAP_MR_PEND) {
4999			break;
5000		} else if (test_bit(CONN_LOCAL_BUSY,
5001				    &chan->conn_state)) {
5002			chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
5003		} else {
5004			/* Logical link is up or moving to BR/EDR,
5005			 * proceed with move
5006			 */
5007			chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP;
5008			l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5009		}
5010		break;
5011	case L2CAP_MOVE_WAIT_RSP:
5012		/* Moving to AMP */
5013		if (result == L2CAP_MR_SUCCESS) {
5014			/* Remote is ready, send confirm immediately
5015			 * after logical link is ready
5016			 */
5017			chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM;
5018		} else {
5019			/* Both logical link and move success
5020			 * are required to confirm
5021			 */
5022			chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP;
5023		}
5024
5025		/* Placeholder - get hci_chan for logical link */
5026		if (!hchan) {
5027			/* Logical link not available */
5028			l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5029			break;
5030		}
5031
5032		/* If the logical link is not yet connected, do not
5033		 * send confirmation.
5034		 */
5035		if (hchan->state != BT_CONNECTED)
5036			break;
5037
5038		/* Logical link is already ready to go */
5039
5040		chan->hs_hcon = hchan->conn;
5041		chan->hs_hcon->l2cap_data = chan->conn;
5042
5043		if (result == L2CAP_MR_SUCCESS) {
5044			/* Can confirm now */
5045			l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED);
5046		} else {
5047			/* Now only need move success
5048			 * to confirm
5049			 */
5050			chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS;
5051		}
5052
5053		l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS);
5054		break;
5055	default:
5056		/* Any other amp move state means the move failed. */
5057		chan->move_id = chan->local_amp_id;
5058		l2cap_move_done(chan);
5059		l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5060	}
5061
5062	l2cap_chan_unlock(chan);
5063}
5064
5065static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid,
5066			    u16 result)
5067{
5068	struct l2cap_chan *chan;
5069
5070	chan = l2cap_get_chan_by_ident(conn, ident);
5071	if (!chan) {
5072		/* Could not locate channel, icid is best guess */
5073		l2cap_send_move_chan_cfm_icid(conn, icid);
5074		return;
5075	}
5076
5077	__clear_chan_timer(chan);
5078
5079	if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) {
5080		if (result == L2CAP_MR_COLLISION) {
5081			chan->move_role = L2CAP_MOVE_ROLE_RESPONDER;
5082		} else {
5083			/* Cleanup - cancel move */
5084			chan->move_id = chan->local_amp_id;
5085			l2cap_move_done(chan);
5086		}
5087	}
5088
5089	l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED);
5090
5091	l2cap_chan_unlock(chan);
5092}
5093
5094static int l2cap_move_channel_rsp(struct l2cap_conn *conn,
5095				  struct l2cap_cmd_hdr *cmd,
5096				  u16 cmd_len, void *data)
5097{
5098	struct l2cap_move_chan_rsp *rsp = data;
5099	u16 icid, result;
5100
5101	if (cmd_len != sizeof(*rsp))
5102		return -EPROTO;
5103
5104	icid = le16_to_cpu(rsp->icid);
5105	result = le16_to_cpu(rsp->result);
5106
5107	BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5108
5109	if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND)
5110		l2cap_move_continue(conn, icid, result);
5111	else
5112		l2cap_move_fail(conn, cmd->ident, icid, result);
5113
5114	return 0;
5115}
5116
5117static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
5118				      struct l2cap_cmd_hdr *cmd,
5119				      u16 cmd_len, void *data)
5120{
5121	struct l2cap_move_chan_cfm *cfm = data;
5122	struct l2cap_chan *chan;
5123	u16 icid, result;
5124
5125	if (cmd_len != sizeof(*cfm))
5126		return -EPROTO;
5127
5128	icid = le16_to_cpu(cfm->icid);
5129	result = le16_to_cpu(cfm->result);
5130
5131	BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result);
5132
5133	chan = l2cap_get_chan_by_dcid(conn, icid);
5134	if (!chan) {
5135		/* Spec requires a response even if the icid was not found */
5136		l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5137		return 0;
5138	}
5139
5140	if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
5141		if (result == L2CAP_MC_CONFIRMED) {
5142			chan->local_amp_id = chan->move_id;
5143			if (chan->local_amp_id == AMP_ID_BREDR)
5144				__release_logical_link(chan);
5145		} else {
5146			chan->move_id = chan->local_amp_id;
5147		}
5148
5149		l2cap_move_done(chan);
5150	}
5151
5152	l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
5153
5154	l2cap_chan_unlock(chan);
5155
5156	return 0;
5157}
5158
5159static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
5160						 struct l2cap_cmd_hdr *cmd,
5161						 u16 cmd_len, void *data)
5162{
5163	struct l2cap_move_chan_cfm_rsp *rsp = data;
5164	struct l2cap_chan *chan;
5165	u16 icid;
5166
5167	if (cmd_len != sizeof(*rsp))
5168		return -EPROTO;
5169
5170	icid = le16_to_cpu(rsp->icid);
5171
5172	BT_DBG("icid 0x%4.4x", icid);
5173
5174	chan = l2cap_get_chan_by_scid(conn, icid);
5175	if (!chan)
5176		return 0;
5177
5178	__clear_chan_timer(chan);
5179
5180	if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
5181		chan->local_amp_id = chan->move_id;
5182
5183		if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
5184			__release_logical_link(chan);
5185
5186		l2cap_move_done(chan);
5187	}
5188
5189	l2cap_chan_unlock(chan);
5190
5191	return 0;
5192}
5193
5194static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
5195					      struct l2cap_cmd_hdr *cmd,
5196					      u16 cmd_len, u8 *data)
5197{
5198	struct hci_conn *hcon = conn->hcon;
5199	struct l2cap_conn_param_update_req *req;
5200	struct l2cap_conn_param_update_rsp rsp;
5201	u16 min, max, latency, to_multiplier;
5202	int err;
5203
5204	if (hcon->role != HCI_ROLE_MASTER)
5205		return -EINVAL;
5206
5207	if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
5208		return -EPROTO;
5209
5210	req = (struct l2cap_conn_param_update_req *) data;
5211	min		= __le16_to_cpu(req->min);
5212	max		= __le16_to_cpu(req->max);
5213	latency		= __le16_to_cpu(req->latency);
5214	to_multiplier	= __le16_to_cpu(req->to_multiplier);
5215
5216	BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
5217	       min, max, latency, to_multiplier);
5218
5219	memset(&rsp, 0, sizeof(rsp));
5220
5221	err = hci_check_conn_params(min, max, latency, to_multiplier);
5222	if (err)
5223		rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
5224	else
5225		rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
5226
5227	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
5228		       sizeof(rsp), &rsp);
5229
5230	if (!err) {
5231		u8 store_hint;
5232
5233		store_hint = hci_le_conn_update(hcon, min, max, latency,
5234						to_multiplier);
5235		mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type,
5236				    store_hint, min, max, latency,
5237				    to_multiplier);
5238
5239	}
5240
5241	return 0;
5242}
5243
5244static int l2cap_le_connect_rsp(struct l2cap_conn *conn,
5245				struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5246				u8 *data)
5247{
5248	struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data;
5249	struct hci_conn *hcon = conn->hcon;
5250	u16 dcid, mtu, mps, credits, result;
5251	struct l2cap_chan *chan;
5252	int err, sec_level;
5253
5254	if (cmd_len < sizeof(*rsp))
5255		return -EPROTO;
5256
5257	dcid    = __le16_to_cpu(rsp->dcid);
5258	mtu     = __le16_to_cpu(rsp->mtu);
5259	mps     = __le16_to_cpu(rsp->mps);
5260	credits = __le16_to_cpu(rsp->credits);
5261	result  = __le16_to_cpu(rsp->result);
5262
5263	if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23 ||
5264					   dcid < L2CAP_CID_DYN_START ||
5265					   dcid > L2CAP_CID_LE_DYN_END))
5266		return -EPROTO;
5267
5268	BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x",
5269	       dcid, mtu, mps, credits, result);
5270
5271	mutex_lock(&conn->chan_lock);
5272
5273	chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5274	if (!chan) {
5275		err = -EBADSLT;
5276		goto unlock;
5277	}
5278
5279	err = 0;
5280
5281	l2cap_chan_lock(chan);
5282
5283	switch (result) {
5284	case L2CAP_CR_SUCCESS:
5285		if (__l2cap_get_chan_by_dcid(conn, dcid)) {
5286			err = -EBADSLT;
5287			break;
5288		}
5289
5290		chan->ident = 0;
5291		chan->dcid = dcid;
5292		chan->omtu = mtu;
5293		chan->remote_mps = mps;
5294		chan->tx_credits = credits;
5295		l2cap_chan_ready(chan);
5296		break;
5297
5298	case L2CAP_CR_AUTHENTICATION:
5299	case L2CAP_CR_ENCRYPTION:
5300		/* If we already have MITM protection we can't do
5301		 * anything.
5302		 */
5303		if (hcon->sec_level > BT_SECURITY_MEDIUM) {
5304			l2cap_chan_del(chan, ECONNREFUSED);
5305			break;
5306		}
5307
5308		sec_level = hcon->sec_level + 1;
5309		if (chan->sec_level < sec_level)
5310			chan->sec_level = sec_level;
5311
5312		/* We'll need to send a new Connect Request */
5313		clear_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags);
5314
5315		smp_conn_security(hcon, chan->sec_level);
5316		break;
5317
5318	default:
5319		l2cap_chan_del(chan, ECONNREFUSED);
5320		break;
5321	}
5322
5323	l2cap_chan_unlock(chan);
5324
5325unlock:
5326	mutex_unlock(&conn->chan_lock);
5327
5328	return err;
5329}
5330
5331static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
5332				      struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5333				      u8 *data)
5334{
5335	int err = 0;
5336
5337	switch (cmd->code) {
5338	case L2CAP_COMMAND_REJ:
5339		l2cap_command_rej(conn, cmd, cmd_len, data);
5340		break;
5341
5342	case L2CAP_CONN_REQ:
5343		err = l2cap_connect_req(conn, cmd, cmd_len, data);
5344		break;
5345
5346	case L2CAP_CONN_RSP:
5347	case L2CAP_CREATE_CHAN_RSP:
5348		l2cap_connect_create_rsp(conn, cmd, cmd_len, data);
5349		break;
5350
5351	case L2CAP_CONF_REQ:
5352		err = l2cap_config_req(conn, cmd, cmd_len, data);
5353		break;
5354
5355	case L2CAP_CONF_RSP:
5356		l2cap_config_rsp(conn, cmd, cmd_len, data);
5357		break;
5358
5359	case L2CAP_DISCONN_REQ:
5360		err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5361		break;
5362
5363	case L2CAP_DISCONN_RSP:
5364		l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5365		break;
5366
5367	case L2CAP_ECHO_REQ:
5368		l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
5369		break;
5370
5371	case L2CAP_ECHO_RSP:
5372		break;
5373
5374	case L2CAP_INFO_REQ:
5375		err = l2cap_information_req(conn, cmd, cmd_len, data);
5376		break;
5377
5378	case L2CAP_INFO_RSP:
5379		l2cap_information_rsp(conn, cmd, cmd_len, data);
5380		break;
5381
5382	case L2CAP_CREATE_CHAN_REQ:
5383		err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
5384		break;
5385
5386	case L2CAP_MOVE_CHAN_REQ:
5387		err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
5388		break;
5389
5390	case L2CAP_MOVE_CHAN_RSP:
5391		l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
5392		break;
5393
5394	case L2CAP_MOVE_CHAN_CFM:
5395		err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
5396		break;
5397
5398	case L2CAP_MOVE_CHAN_CFM_RSP:
5399		l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
5400		break;
5401
5402	default:
5403		BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
5404		err = -EINVAL;
5405		break;
5406	}
5407
5408	return err;
5409}
5410
5411static int l2cap_le_connect_req(struct l2cap_conn *conn,
5412				struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5413				u8 *data)
5414{
5415	struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
5416	struct l2cap_le_conn_rsp rsp;
5417	struct l2cap_chan *chan, *pchan;
5418	u16 dcid, scid, credits, mtu, mps;
5419	__le16 psm;
5420	u8 result;
5421
5422	if (cmd_len != sizeof(*req))
5423		return -EPROTO;
5424
5425	scid = __le16_to_cpu(req->scid);
5426	mtu  = __le16_to_cpu(req->mtu);
5427	mps  = __le16_to_cpu(req->mps);
5428	psm  = req->psm;
5429	dcid = 0;
5430	credits = 0;
5431
5432	if (mtu < 23 || mps < 23)
5433		return -EPROTO;
5434
5435	BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
5436	       scid, mtu, mps);
5437
5438	/* Check if we have socket listening on psm */
5439	pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
5440					 &conn->hcon->dst, LE_LINK);
5441	if (!pchan) {
5442		result = L2CAP_CR_BAD_PSM;
5443		chan = NULL;
5444		goto response;
5445	}
5446
5447	mutex_lock(&conn->chan_lock);
5448	l2cap_chan_lock(pchan);
5449
5450	if (!smp_sufficient_security(conn->hcon, pchan->sec_level,
5451				     SMP_ALLOW_STK)) {
5452		result = L2CAP_CR_AUTHENTICATION;
5453		chan = NULL;
5454		goto response_unlock;
5455	}
5456
5457	/* Check for valid dynamic CID range */
5458	if (scid < L2CAP_CID_DYN_START || scid > L2CAP_CID_LE_DYN_END) {
5459		result = L2CAP_CR_INVALID_SCID;
5460		chan = NULL;
5461		goto response_unlock;
5462	}
5463
5464	/* Check if we already have channel with that dcid */
5465	if (__l2cap_get_chan_by_dcid(conn, scid)) {
5466		result = L2CAP_CR_SCID_IN_USE;
5467		chan = NULL;
5468		goto response_unlock;
5469	}
5470
5471	chan = pchan->ops->new_connection(pchan);
5472	if (!chan) {
5473		result = L2CAP_CR_NO_MEM;
5474		goto response_unlock;
5475	}
5476
5477	l2cap_le_flowctl_init(chan);
5478
5479	bacpy(&chan->src, &conn->hcon->src);
5480	bacpy(&chan->dst, &conn->hcon->dst);
5481	chan->src_type = bdaddr_src_type(conn->hcon);
5482	chan->dst_type = bdaddr_dst_type(conn->hcon);
5483	chan->psm  = psm;
5484	chan->dcid = scid;
5485	chan->omtu = mtu;
5486	chan->remote_mps = mps;
5487	chan->tx_credits = __le16_to_cpu(req->credits);
5488
5489	__l2cap_chan_add(conn, chan);
5490	dcid = chan->scid;
5491	credits = chan->rx_credits;
5492
5493	__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
5494
5495	chan->ident = cmd->ident;
5496
5497	if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
5498		l2cap_state_change(chan, BT_CONNECT2);
5499		/* The following result value is actually not defined
5500		 * for LE CoC but we use it to let the function know
5501		 * that it should bail out after doing its cleanup
5502		 * instead of sending a response.
5503		 */
5504		result = L2CAP_CR_PEND;
5505		chan->ops->defer(chan);
5506	} else {
5507		l2cap_chan_ready(chan);
5508		result = L2CAP_CR_SUCCESS;
5509	}
5510
5511response_unlock:
5512	l2cap_chan_unlock(pchan);
5513	mutex_unlock(&conn->chan_lock);
5514	l2cap_chan_put(pchan);
5515
5516	if (result == L2CAP_CR_PEND)
5517		return 0;
5518
5519response:
5520	if (chan) {
5521		rsp.mtu = cpu_to_le16(chan->imtu);
5522		rsp.mps = cpu_to_le16(chan->mps);
5523	} else {
5524		rsp.mtu = 0;
5525		rsp.mps = 0;
5526	}
5527
5528	rsp.dcid    = cpu_to_le16(dcid);
5529	rsp.credits = cpu_to_le16(credits);
5530	rsp.result  = cpu_to_le16(result);
5531
5532	l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
5533
5534	return 0;
5535}
5536
5537static inline int l2cap_le_credits(struct l2cap_conn *conn,
5538				   struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5539				   u8 *data)
5540{
5541	struct l2cap_le_credits *pkt;
5542	struct l2cap_chan *chan;
5543	u16 cid, credits, max_credits;
5544
5545	if (cmd_len != sizeof(*pkt))
5546		return -EPROTO;
5547
5548	pkt = (struct l2cap_le_credits *) data;
5549	cid	= __le16_to_cpu(pkt->cid);
5550	credits	= __le16_to_cpu(pkt->credits);
5551
5552	BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits);
5553
5554	chan = l2cap_get_chan_by_dcid(conn, cid);
5555	if (!chan)
5556		return -EBADSLT;
5557
5558	max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits;
5559	if (credits > max_credits) {
5560		BT_ERR("LE credits overflow");
5561		l2cap_send_disconn_req(chan, ECONNRESET);
5562		l2cap_chan_unlock(chan);
5563
5564		/* Return 0 so that we don't trigger an unnecessary
5565		 * command reject packet.
5566		 */
5567		return 0;
5568	}
5569
5570	chan->tx_credits += credits;
5571
5572	while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) {
5573		l2cap_do_send(chan, skb_dequeue(&chan->tx_q));
5574		chan->tx_credits--;
5575	}
5576
5577	if (chan->tx_credits)
5578		chan->ops->resume(chan);
5579
5580	l2cap_chan_unlock(chan);
5581
5582	return 0;
5583}
5584
5585static inline int l2cap_le_command_rej(struct l2cap_conn *conn,
5586				       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5587				       u8 *data)
5588{
5589	struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
5590	struct l2cap_chan *chan;
5591
5592	if (cmd_len < sizeof(*rej))
5593		return -EPROTO;
5594
5595	mutex_lock(&conn->chan_lock);
5596
5597	chan = __l2cap_get_chan_by_ident(conn, cmd->ident);
5598	if (!chan)
5599		goto done;
5600
5601	l2cap_chan_lock(chan);
5602	l2cap_chan_del(chan, ECONNREFUSED);
5603	l2cap_chan_unlock(chan);
5604
5605done:
5606	mutex_unlock(&conn->chan_lock);
5607	return 0;
5608}
5609
5610static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
5611				   struct l2cap_cmd_hdr *cmd, u16 cmd_len,
5612				   u8 *data)
5613{
5614	int err = 0;
5615
5616	switch (cmd->code) {
5617	case L2CAP_COMMAND_REJ:
5618		l2cap_le_command_rej(conn, cmd, cmd_len, data);
5619		break;
5620
5621	case L2CAP_CONN_PARAM_UPDATE_REQ:
5622		err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data);
5623		break;
5624
5625	case L2CAP_CONN_PARAM_UPDATE_RSP:
5626		break;
5627
5628	case L2CAP_LE_CONN_RSP:
5629		l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
5630		break;
5631
5632	case L2CAP_LE_CONN_REQ:
5633		err = l2cap_le_connect_req(conn, cmd, cmd_len, data);
5634		break;
5635
5636	case L2CAP_LE_CREDITS:
5637		err = l2cap_le_credits(conn, cmd, cmd_len, data);
5638		break;
5639
5640	case L2CAP_DISCONN_REQ:
5641		err = l2cap_disconnect_req(conn, cmd, cmd_len, data);
5642		break;
5643
5644	case L2CAP_DISCONN_RSP:
5645		l2cap_disconnect_rsp(conn, cmd, cmd_len, data);
5646		break;
5647
5648	default:
5649		BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
5650		err = -EINVAL;
5651		break;
5652	}
5653
5654	return err;
5655}
5656
5657static inline void l2cap_le_sig_channel(struct l2cap_conn *conn,
5658					struct sk_buff *skb)
5659{
5660	struct hci_conn *hcon = conn->hcon;
5661	struct l2cap_cmd_hdr *cmd;
5662	u16 len;
5663	int err;
5664
5665	if (hcon->type != LE_LINK)
5666		goto drop;
5667
5668	if (skb->len < L2CAP_CMD_HDR_SIZE)
5669		goto drop;
5670
5671	cmd = (void *) skb->data;
5672	skb_pull(skb, L2CAP_CMD_HDR_SIZE);
5673
5674	len = le16_to_cpu(cmd->len);
5675
5676	BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident);
5677
5678	if (len != skb->len || !cmd->ident) {
5679		BT_DBG("corrupted command");
5680		goto drop;
5681	}
5682
5683	err = l2cap_le_sig_cmd(conn, cmd, len, skb->data);
5684	if (err) {
5685		struct l2cap_cmd_rej_unk rej;
5686
5687		BT_ERR("Wrong link type (%d)", err);
5688
5689		rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5690		l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
5691			       sizeof(rej), &rej);
5692	}
5693
5694drop:
5695	kfree_skb(skb);
5696}
5697
5698static inline void l2cap_sig_channel(struct l2cap_conn *conn,
5699				     struct sk_buff *skb)
5700{
5701	struct hci_conn *hcon = conn->hcon;
5702	u8 *data = skb->data;
5703	int len = skb->len;
5704	struct l2cap_cmd_hdr cmd;
5705	int err;
5706
5707	l2cap_raw_recv(conn, skb);
5708
5709	if (hcon->type != ACL_LINK)
5710		goto drop;
5711
5712	while (len >= L2CAP_CMD_HDR_SIZE) {
5713		u16 cmd_len;
5714		memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
5715		data += L2CAP_CMD_HDR_SIZE;
5716		len  -= L2CAP_CMD_HDR_SIZE;
5717
5718		cmd_len = le16_to_cpu(cmd.len);
5719
5720		BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
5721		       cmd.ident);
5722
5723		if (cmd_len > len || !cmd.ident) {
5724			BT_DBG("corrupted command");
5725			break;
5726		}
5727
5728		err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
5729		if (err) {
5730			struct l2cap_cmd_rej_unk rej;
5731
5732			BT_ERR("Wrong link type (%d)", err);
5733
5734			rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
5735			l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
5736				       sizeof(rej), &rej);
5737		}
5738
5739		data += cmd_len;
5740		len  -= cmd_len;
5741	}
5742
5743drop:
5744	kfree_skb(skb);
5745}
5746
5747static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
5748{
5749	u16 our_fcs, rcv_fcs;
5750	int hdr_size;
5751
5752	if (test_bit(FLAG_EXT_CTRL, &chan->flags))
5753		hdr_size = L2CAP_EXT_HDR_SIZE;
5754	else
5755		hdr_size = L2CAP_ENH_HDR_SIZE;
5756
5757	if (chan->fcs == L2CAP_FCS_CRC16) {
5758		skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
5759		rcv_fcs = get_unaligned_le16(skb->data + skb->len);
5760		our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
5761
5762		if (our_fcs != rcv_fcs)
5763			return -EBADMSG;
5764	}
5765	return 0;
5766}
5767
5768static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
5769{
5770	struct l2cap_ctrl control;
5771
5772	BT_DBG("chan %p", chan);
5773
5774	memset(&control, 0, sizeof(control));
5775	control.sframe = 1;
5776	control.final = 1;
5777	control.reqseq = chan->buffer_seq;
5778	set_bit(CONN_SEND_FBIT, &chan->conn_state);
5779
5780	if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5781		control.super = L2CAP_SUPER_RNR;
5782		l2cap_send_sframe(chan, &control);
5783	}
5784
5785	if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
5786	    chan->unacked_frames > 0)
5787		__set_retrans_timer(chan);
5788
5789	/* Send pending iframes */
5790	l2cap_ertm_send(chan);
5791
5792	if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
5793	    test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
5794		/* F-bit wasn't sent in an s-frame or i-frame yet, so
5795		 * send it now.
5796		 */
5797		control.super = L2CAP_SUPER_RR;
5798		l2cap_send_sframe(chan, &control);
5799	}
5800}
5801
5802static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
5803			    struct sk_buff **last_frag)
5804{
5805	/* skb->len reflects data in skb as well as all fragments
5806	 * skb->data_len reflects only data in fragments
5807	 */
5808	if (!skb_has_frag_list(skb))
5809		skb_shinfo(skb)->frag_list = new_frag;
5810
5811	new_frag->next = NULL;
5812
5813	(*last_frag)->next = new_frag;
5814	*last_frag = new_frag;
5815
5816	skb->len += new_frag->len;
5817	skb->data_len += new_frag->len;
5818	skb->truesize += new_frag->truesize;
5819}
5820
5821static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
5822				struct l2cap_ctrl *control)
5823{
5824	int err = -EINVAL;
5825
5826	switch (control->sar) {
5827	case L2CAP_SAR_UNSEGMENTED:
5828		if (chan->sdu)
5829			break;
5830
5831		err = chan->ops->recv(chan, skb);
5832		break;
5833
5834	case L2CAP_SAR_START:
5835		if (chan->sdu)
5836			break;
5837
 
 
 
5838		chan->sdu_len = get_unaligned_le16(skb->data);
5839		skb_pull(skb, L2CAP_SDULEN_SIZE);
5840
5841		if (chan->sdu_len > chan->imtu) {
5842			err = -EMSGSIZE;
5843			break;
5844		}
5845
5846		if (skb->len >= chan->sdu_len)
5847			break;
5848
5849		chan->sdu = skb;
5850		chan->sdu_last_frag = skb;
5851
5852		skb = NULL;
5853		err = 0;
5854		break;
5855
5856	case L2CAP_SAR_CONTINUE:
5857		if (!chan->sdu)
5858			break;
5859
5860		append_skb_frag(chan->sdu, skb,
5861				&chan->sdu_last_frag);
5862		skb = NULL;
5863
5864		if (chan->sdu->len >= chan->sdu_len)
5865			break;
5866
5867		err = 0;
5868		break;
5869
5870	case L2CAP_SAR_END:
5871		if (!chan->sdu)
5872			break;
5873
5874		append_skb_frag(chan->sdu, skb,
5875				&chan->sdu_last_frag);
5876		skb = NULL;
5877
5878		if (chan->sdu->len != chan->sdu_len)
5879			break;
5880
5881		err = chan->ops->recv(chan, chan->sdu);
5882
5883		if (!err) {
5884			/* Reassembly complete */
5885			chan->sdu = NULL;
5886			chan->sdu_last_frag = NULL;
5887			chan->sdu_len = 0;
5888		}
5889		break;
5890	}
5891
5892	if (err) {
5893		kfree_skb(skb);
5894		kfree_skb(chan->sdu);
5895		chan->sdu = NULL;
5896		chan->sdu_last_frag = NULL;
5897		chan->sdu_len = 0;
5898	}
5899
5900	return err;
5901}
5902
5903static int l2cap_resegment(struct l2cap_chan *chan)
5904{
5905	/* Placeholder */
5906	return 0;
5907}
5908
5909void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
5910{
5911	u8 event;
5912
5913	if (chan->mode != L2CAP_MODE_ERTM)
5914		return;
5915
5916	event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
5917	l2cap_tx(chan, NULL, NULL, event);
5918}
5919
5920static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
5921{
5922	int err = 0;
5923	/* Pass sequential frames to l2cap_reassemble_sdu()
5924	 * until a gap is encountered.
5925	 */
5926
5927	BT_DBG("chan %p", chan);
5928
5929	while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
5930		struct sk_buff *skb;
5931		BT_DBG("Searching for skb with txseq %d (queue len %d)",
5932		       chan->buffer_seq, skb_queue_len(&chan->srej_q));
5933
5934		skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
5935
5936		if (!skb)
5937			break;
5938
5939		skb_unlink(skb, &chan->srej_q);
5940		chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
5941		err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->l2cap);
5942		if (err)
5943			break;
5944	}
5945
5946	if (skb_queue_empty(&chan->srej_q)) {
5947		chan->rx_state = L2CAP_RX_STATE_RECV;
5948		l2cap_send_ack(chan);
5949	}
5950
5951	return err;
5952}
5953
5954static void l2cap_handle_srej(struct l2cap_chan *chan,
5955			      struct l2cap_ctrl *control)
5956{
5957	struct sk_buff *skb;
5958
5959	BT_DBG("chan %p, control %p", chan, control);
5960
5961	if (control->reqseq == chan->next_tx_seq) {
5962		BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
5963		l2cap_send_disconn_req(chan, ECONNRESET);
5964		return;
5965	}
5966
5967	skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
5968
5969	if (skb == NULL) {
5970		BT_DBG("Seq %d not available for retransmission",
5971		       control->reqseq);
5972		return;
5973	}
5974
5975	if (chan->max_tx != 0 && bt_cb(skb)->l2cap.retries >= chan->max_tx) {
5976		BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
5977		l2cap_send_disconn_req(chan, ECONNRESET);
5978		return;
5979	}
5980
5981	clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
5982
5983	if (control->poll) {
5984		l2cap_pass_to_tx(chan, control);
5985
5986		set_bit(CONN_SEND_FBIT, &chan->conn_state);
5987		l2cap_retransmit(chan, control);
5988		l2cap_ertm_send(chan);
5989
5990		if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
5991			set_bit(CONN_SREJ_ACT, &chan->conn_state);
5992			chan->srej_save_reqseq = control->reqseq;
5993		}
5994	} else {
5995		l2cap_pass_to_tx_fbit(chan, control);
5996
5997		if (control->final) {
5998			if (chan->srej_save_reqseq != control->reqseq ||
5999			    !test_and_clear_bit(CONN_SREJ_ACT,
6000						&chan->conn_state))
6001				l2cap_retransmit(chan, control);
6002		} else {
6003			l2cap_retransmit(chan, control);
6004			if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
6005				set_bit(CONN_SREJ_ACT, &chan->conn_state);
6006				chan->srej_save_reqseq = control->reqseq;
6007			}
6008		}
6009	}
6010}
6011
6012static void l2cap_handle_rej(struct l2cap_chan *chan,
6013			     struct l2cap_ctrl *control)
6014{
6015	struct sk_buff *skb;
6016
6017	BT_DBG("chan %p, control %p", chan, control);
6018
6019	if (control->reqseq == chan->next_tx_seq) {
6020		BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
6021		l2cap_send_disconn_req(chan, ECONNRESET);
6022		return;
6023	}
6024
6025	skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
6026
6027	if (chan->max_tx && skb &&
6028	    bt_cb(skb)->l2cap.retries >= chan->max_tx) {
6029		BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
6030		l2cap_send_disconn_req(chan, ECONNRESET);
6031		return;
6032	}
6033
6034	clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6035
6036	l2cap_pass_to_tx(chan, control);
6037
6038	if (control->final) {
6039		if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
6040			l2cap_retransmit_all(chan, control);
6041	} else {
6042		l2cap_retransmit_all(chan, control);
6043		l2cap_ertm_send(chan);
6044		if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
6045			set_bit(CONN_REJ_ACT, &chan->conn_state);
6046	}
6047}
6048
6049static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
6050{
6051	BT_DBG("chan %p, txseq %d", chan, txseq);
6052
6053	BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
6054	       chan->expected_tx_seq);
6055
6056	if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
6057		if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6058		    chan->tx_win) {
6059			/* See notes below regarding "double poll" and
6060			 * invalid packets.
6061			 */
6062			if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6063				BT_DBG("Invalid/Ignore - after SREJ");
6064				return L2CAP_TXSEQ_INVALID_IGNORE;
6065			} else {
6066				BT_DBG("Invalid - in window after SREJ sent");
6067				return L2CAP_TXSEQ_INVALID;
6068			}
6069		}
6070
6071		if (chan->srej_list.head == txseq) {
6072			BT_DBG("Expected SREJ");
6073			return L2CAP_TXSEQ_EXPECTED_SREJ;
6074		}
6075
6076		if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
6077			BT_DBG("Duplicate SREJ - txseq already stored");
6078			return L2CAP_TXSEQ_DUPLICATE_SREJ;
6079		}
6080
6081		if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
6082			BT_DBG("Unexpected SREJ - not requested");
6083			return L2CAP_TXSEQ_UNEXPECTED_SREJ;
6084		}
6085	}
6086
6087	if (chan->expected_tx_seq == txseq) {
6088		if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
6089		    chan->tx_win) {
6090			BT_DBG("Invalid - txseq outside tx window");
6091			return L2CAP_TXSEQ_INVALID;
6092		} else {
6093			BT_DBG("Expected");
6094			return L2CAP_TXSEQ_EXPECTED;
6095		}
6096	}
6097
6098	if (__seq_offset(chan, txseq, chan->last_acked_seq) <
6099	    __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
6100		BT_DBG("Duplicate - expected_tx_seq later than txseq");
6101		return L2CAP_TXSEQ_DUPLICATE;
6102	}
6103
6104	if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
6105		/* A source of invalid packets is a "double poll" condition,
6106		 * where delays cause us to send multiple poll packets.  If
6107		 * the remote stack receives and processes both polls,
6108		 * sequence numbers can wrap around in such a way that a
6109		 * resent frame has a sequence number that looks like new data
6110		 * with a sequence gap.  This would trigger an erroneous SREJ
6111		 * request.
6112		 *
6113		 * Fortunately, this is impossible with a tx window that's
6114		 * less than half of the maximum sequence number, which allows
6115		 * invalid frames to be safely ignored.
6116		 *
6117		 * With tx window sizes greater than half of the tx window
6118		 * maximum, the frame is invalid and cannot be ignored.  This
6119		 * causes a disconnect.
6120		 */
6121
6122		if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
6123			BT_DBG("Invalid/Ignore - txseq outside tx window");
6124			return L2CAP_TXSEQ_INVALID_IGNORE;
6125		} else {
6126			BT_DBG("Invalid - txseq outside tx window");
6127			return L2CAP_TXSEQ_INVALID;
6128		}
6129	} else {
6130		BT_DBG("Unexpected - txseq indicates missing frames");
6131		return L2CAP_TXSEQ_UNEXPECTED;
6132	}
6133}
6134
6135static int l2cap_rx_state_recv(struct l2cap_chan *chan,
6136			       struct l2cap_ctrl *control,
6137			       struct sk_buff *skb, u8 event)
6138{
6139	int err = 0;
6140	bool skb_in_use = false;
6141
6142	BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6143	       event);
6144
6145	switch (event) {
6146	case L2CAP_EV_RECV_IFRAME:
6147		switch (l2cap_classify_txseq(chan, control->txseq)) {
6148		case L2CAP_TXSEQ_EXPECTED:
6149			l2cap_pass_to_tx(chan, control);
6150
6151			if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6152				BT_DBG("Busy, discarding expected seq %d",
6153				       control->txseq);
6154				break;
6155			}
6156
6157			chan->expected_tx_seq = __next_seq(chan,
6158							   control->txseq);
6159
6160			chan->buffer_seq = chan->expected_tx_seq;
6161			skb_in_use = true;
6162
6163			err = l2cap_reassemble_sdu(chan, skb, control);
6164			if (err)
6165				break;
6166
6167			if (control->final) {
6168				if (!test_and_clear_bit(CONN_REJ_ACT,
6169							&chan->conn_state)) {
6170					control->final = 0;
6171					l2cap_retransmit_all(chan, control);
6172					l2cap_ertm_send(chan);
6173				}
6174			}
6175
6176			if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
6177				l2cap_send_ack(chan);
6178			break;
6179		case L2CAP_TXSEQ_UNEXPECTED:
6180			l2cap_pass_to_tx(chan, control);
6181
6182			/* Can't issue SREJ frames in the local busy state.
6183			 * Drop this frame, it will be seen as missing
6184			 * when local busy is exited.
6185			 */
6186			if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
6187				BT_DBG("Busy, discarding unexpected seq %d",
6188				       control->txseq);
6189				break;
6190			}
6191
6192			/* There was a gap in the sequence, so an SREJ
6193			 * must be sent for each missing frame.  The
6194			 * current frame is stored for later use.
6195			 */
6196			skb_queue_tail(&chan->srej_q, skb);
6197			skb_in_use = true;
6198			BT_DBG("Queued %p (queue len %d)", skb,
6199			       skb_queue_len(&chan->srej_q));
6200
6201			clear_bit(CONN_SREJ_ACT, &chan->conn_state);
6202			l2cap_seq_list_clear(&chan->srej_list);
6203			l2cap_send_srej(chan, control->txseq);
6204
6205			chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
6206			break;
6207		case L2CAP_TXSEQ_DUPLICATE:
6208			l2cap_pass_to_tx(chan, control);
6209			break;
6210		case L2CAP_TXSEQ_INVALID_IGNORE:
6211			break;
6212		case L2CAP_TXSEQ_INVALID:
6213		default:
6214			l2cap_send_disconn_req(chan, ECONNRESET);
6215			break;
6216		}
6217		break;
6218	case L2CAP_EV_RECV_RR:
6219		l2cap_pass_to_tx(chan, control);
6220		if (control->final) {
6221			clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6222
6223			if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) &&
6224			    !__chan_is_moving(chan)) {
6225				control->final = 0;
6226				l2cap_retransmit_all(chan, control);
6227			}
6228
6229			l2cap_ertm_send(chan);
6230		} else if (control->poll) {
6231			l2cap_send_i_or_rr_or_rnr(chan);
6232		} else {
6233			if (test_and_clear_bit(CONN_REMOTE_BUSY,
6234					       &chan->conn_state) &&
6235			    chan->unacked_frames)
6236				__set_retrans_timer(chan);
6237
6238			l2cap_ertm_send(chan);
6239		}
6240		break;
6241	case L2CAP_EV_RECV_RNR:
6242		set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6243		l2cap_pass_to_tx(chan, control);
6244		if (control && control->poll) {
6245			set_bit(CONN_SEND_FBIT, &chan->conn_state);
6246			l2cap_send_rr_or_rnr(chan, 0);
6247		}
6248		__clear_retrans_timer(chan);
6249		l2cap_seq_list_clear(&chan->retrans_list);
6250		break;
6251	case L2CAP_EV_RECV_REJ:
6252		l2cap_handle_rej(chan, control);
6253		break;
6254	case L2CAP_EV_RECV_SREJ:
6255		l2cap_handle_srej(chan, control);
6256		break;
6257	default:
6258		break;
6259	}
6260
6261	if (skb && !skb_in_use) {
6262		BT_DBG("Freeing %p", skb);
6263		kfree_skb(skb);
6264	}
6265
6266	return err;
6267}
6268
6269static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
6270				    struct l2cap_ctrl *control,
6271				    struct sk_buff *skb, u8 event)
6272{
6273	int err = 0;
6274	u16 txseq = control->txseq;
6275	bool skb_in_use = false;
6276
6277	BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6278	       event);
6279
6280	switch (event) {
6281	case L2CAP_EV_RECV_IFRAME:
6282		switch (l2cap_classify_txseq(chan, txseq)) {
6283		case L2CAP_TXSEQ_EXPECTED:
6284			/* Keep frame for reassembly later */
6285			l2cap_pass_to_tx(chan, control);
6286			skb_queue_tail(&chan->srej_q, skb);
6287			skb_in_use = true;
6288			BT_DBG("Queued %p (queue len %d)", skb,
6289			       skb_queue_len(&chan->srej_q));
6290
6291			chan->expected_tx_seq = __next_seq(chan, txseq);
6292			break;
6293		case L2CAP_TXSEQ_EXPECTED_SREJ:
6294			l2cap_seq_list_pop(&chan->srej_list);
6295
6296			l2cap_pass_to_tx(chan, control);
6297			skb_queue_tail(&chan->srej_q, skb);
6298			skb_in_use = true;
6299			BT_DBG("Queued %p (queue len %d)", skb,
6300			       skb_queue_len(&chan->srej_q));
6301
6302			err = l2cap_rx_queued_iframes(chan);
6303			if (err)
6304				break;
6305
6306			break;
6307		case L2CAP_TXSEQ_UNEXPECTED:
6308			/* Got a frame that can't be reassembled yet.
6309			 * Save it for later, and send SREJs to cover
6310			 * the missing frames.
6311			 */
6312			skb_queue_tail(&chan->srej_q, skb);
6313			skb_in_use = true;
6314			BT_DBG("Queued %p (queue len %d)", skb,
6315			       skb_queue_len(&chan->srej_q));
6316
6317			l2cap_pass_to_tx(chan, control);
6318			l2cap_send_srej(chan, control->txseq);
6319			break;
6320		case L2CAP_TXSEQ_UNEXPECTED_SREJ:
6321			/* This frame was requested with an SREJ, but
6322			 * some expected retransmitted frames are
6323			 * missing.  Request retransmission of missing
6324			 * SREJ'd frames.
6325			 */
6326			skb_queue_tail(&chan->srej_q, skb);
6327			skb_in_use = true;
6328			BT_DBG("Queued %p (queue len %d)", skb,
6329			       skb_queue_len(&chan->srej_q));
6330
6331			l2cap_pass_to_tx(chan, control);
6332			l2cap_send_srej_list(chan, control->txseq);
6333			break;
6334		case L2CAP_TXSEQ_DUPLICATE_SREJ:
6335			/* We've already queued this frame.  Drop this copy. */
6336			l2cap_pass_to_tx(chan, control);
6337			break;
6338		case L2CAP_TXSEQ_DUPLICATE:
6339			/* Expecting a later sequence number, so this frame
6340			 * was already received.  Ignore it completely.
6341			 */
6342			break;
6343		case L2CAP_TXSEQ_INVALID_IGNORE:
6344			break;
6345		case L2CAP_TXSEQ_INVALID:
6346		default:
6347			l2cap_send_disconn_req(chan, ECONNRESET);
6348			break;
6349		}
6350		break;
6351	case L2CAP_EV_RECV_RR:
6352		l2cap_pass_to_tx(chan, control);
6353		if (control->final) {
6354			clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6355
6356			if (!test_and_clear_bit(CONN_REJ_ACT,
6357						&chan->conn_state)) {
6358				control->final = 0;
6359				l2cap_retransmit_all(chan, control);
6360			}
6361
6362			l2cap_ertm_send(chan);
6363		} else if (control->poll) {
6364			if (test_and_clear_bit(CONN_REMOTE_BUSY,
6365					       &chan->conn_state) &&
6366			    chan->unacked_frames) {
6367				__set_retrans_timer(chan);
6368			}
6369
6370			set_bit(CONN_SEND_FBIT, &chan->conn_state);
6371			l2cap_send_srej_tail(chan);
6372		} else {
6373			if (test_and_clear_bit(CONN_REMOTE_BUSY,
6374					       &chan->conn_state) &&
6375			    chan->unacked_frames)
6376				__set_retrans_timer(chan);
6377
6378			l2cap_send_ack(chan);
6379		}
6380		break;
6381	case L2CAP_EV_RECV_RNR:
6382		set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6383		l2cap_pass_to_tx(chan, control);
6384		if (control->poll) {
6385			l2cap_send_srej_tail(chan);
6386		} else {
6387			struct l2cap_ctrl rr_control;
6388			memset(&rr_control, 0, sizeof(rr_control));
6389			rr_control.sframe = 1;
6390			rr_control.super = L2CAP_SUPER_RR;
6391			rr_control.reqseq = chan->buffer_seq;
6392			l2cap_send_sframe(chan, &rr_control);
6393		}
6394
6395		break;
6396	case L2CAP_EV_RECV_REJ:
6397		l2cap_handle_rej(chan, control);
6398		break;
6399	case L2CAP_EV_RECV_SREJ:
6400		l2cap_handle_srej(chan, control);
6401		break;
6402	}
6403
6404	if (skb && !skb_in_use) {
6405		BT_DBG("Freeing %p", skb);
6406		kfree_skb(skb);
6407	}
6408
6409	return err;
6410}
6411
6412static int l2cap_finish_move(struct l2cap_chan *chan)
6413{
6414	BT_DBG("chan %p", chan);
6415
6416	chan->rx_state = L2CAP_RX_STATE_RECV;
6417
6418	if (chan->hs_hcon)
6419		chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6420	else
6421		chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6422
6423	return l2cap_resegment(chan);
6424}
6425
6426static int l2cap_rx_state_wait_p(struct l2cap_chan *chan,
6427				 struct l2cap_ctrl *control,
6428				 struct sk_buff *skb, u8 event)
6429{
6430	int err;
6431
6432	BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
6433	       event);
6434
6435	if (!control->poll)
6436		return -EPROTO;
6437
6438	l2cap_process_reqseq(chan, control->reqseq);
6439
6440	if (!skb_queue_empty(&chan->tx_q))
6441		chan->tx_send_head = skb_peek(&chan->tx_q);
6442	else
6443		chan->tx_send_head = NULL;
6444
6445	/* Rewind next_tx_seq to the point expected
6446	 * by the receiver.
6447	 */
6448	chan->next_tx_seq = control->reqseq;
6449	chan->unacked_frames = 0;
6450
6451	err = l2cap_finish_move(chan);
6452	if (err)
6453		return err;
6454
6455	set_bit(CONN_SEND_FBIT, &chan->conn_state);
6456	l2cap_send_i_or_rr_or_rnr(chan);
6457
6458	if (event == L2CAP_EV_RECV_IFRAME)
6459		return -EPROTO;
6460
6461	return l2cap_rx_state_recv(chan, control, NULL, event);
6462}
6463
6464static int l2cap_rx_state_wait_f(struct l2cap_chan *chan,
6465				 struct l2cap_ctrl *control,
6466				 struct sk_buff *skb, u8 event)
6467{
6468	int err;
6469
6470	if (!control->final)
6471		return -EPROTO;
6472
6473	clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
6474
6475	chan->rx_state = L2CAP_RX_STATE_RECV;
6476	l2cap_process_reqseq(chan, control->reqseq);
6477
6478	if (!skb_queue_empty(&chan->tx_q))
6479		chan->tx_send_head = skb_peek(&chan->tx_q);
6480	else
6481		chan->tx_send_head = NULL;
6482
6483	/* Rewind next_tx_seq to the point expected
6484	 * by the receiver.
6485	 */
6486	chan->next_tx_seq = control->reqseq;
6487	chan->unacked_frames = 0;
6488
6489	if (chan->hs_hcon)
6490		chan->conn->mtu = chan->hs_hcon->hdev->block_mtu;
6491	else
6492		chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu;
6493
6494	err = l2cap_resegment(chan);
6495
6496	if (!err)
6497		err = l2cap_rx_state_recv(chan, control, skb, event);
6498
6499	return err;
6500}
6501
6502static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
6503{
6504	/* Make sure reqseq is for a packet that has been sent but not acked */
6505	u16 unacked;
6506
6507	unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
6508	return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
6509}
6510
6511static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6512		    struct sk_buff *skb, u8 event)
6513{
6514	int err = 0;
6515
6516	BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
6517	       control, skb, event, chan->rx_state);
6518
6519	if (__valid_reqseq(chan, control->reqseq)) {
6520		switch (chan->rx_state) {
6521		case L2CAP_RX_STATE_RECV:
6522			err = l2cap_rx_state_recv(chan, control, skb, event);
6523			break;
6524		case L2CAP_RX_STATE_SREJ_SENT:
6525			err = l2cap_rx_state_srej_sent(chan, control, skb,
6526						       event);
6527			break;
6528		case L2CAP_RX_STATE_WAIT_P:
6529			err = l2cap_rx_state_wait_p(chan, control, skb, event);
6530			break;
6531		case L2CAP_RX_STATE_WAIT_F:
6532			err = l2cap_rx_state_wait_f(chan, control, skb, event);
6533			break;
6534		default:
6535			/* shut it down */
6536			break;
6537		}
6538	} else {
6539		BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
6540		       control->reqseq, chan->next_tx_seq,
6541		       chan->expected_ack_seq);
6542		l2cap_send_disconn_req(chan, ECONNRESET);
6543	}
6544
6545	return err;
6546}
6547
6548static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
6549			   struct sk_buff *skb)
6550{
6551	BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
6552	       chan->rx_state);
6553
6554	if (l2cap_classify_txseq(chan, control->txseq) ==
6555	    L2CAP_TXSEQ_EXPECTED) {
6556		l2cap_pass_to_tx(chan, control);
6557
6558		BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
6559		       __next_seq(chan, chan->buffer_seq));
6560
6561		chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
6562
6563		l2cap_reassemble_sdu(chan, skb, control);
6564	} else {
6565		if (chan->sdu) {
6566			kfree_skb(chan->sdu);
6567			chan->sdu = NULL;
6568		}
6569		chan->sdu_last_frag = NULL;
6570		chan->sdu_len = 0;
6571
6572		if (skb) {
6573			BT_DBG("Freeing %p", skb);
6574			kfree_skb(skb);
6575		}
6576	}
6577
6578	chan->last_acked_seq = control->txseq;
6579	chan->expected_tx_seq = __next_seq(chan, control->txseq);
6580
6581	return 0;
6582}
6583
6584static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6585{
6586	struct l2cap_ctrl *control = &bt_cb(skb)->l2cap;
6587	u16 len;
6588	u8 event;
6589
6590	__unpack_control(chan, skb);
6591
6592	len = skb->len;
6593
6594	/*
6595	 * We can just drop the corrupted I-frame here.
6596	 * Receiver will miss it and start proper recovery
6597	 * procedures and ask for retransmission.
6598	 */
6599	if (l2cap_check_fcs(chan, skb))
6600		goto drop;
6601
6602	if (!control->sframe && control->sar == L2CAP_SAR_START)
6603		len -= L2CAP_SDULEN_SIZE;
6604
6605	if (chan->fcs == L2CAP_FCS_CRC16)
6606		len -= L2CAP_FCS_SIZE;
6607
6608	if (len > chan->mps) {
6609		l2cap_send_disconn_req(chan, ECONNRESET);
6610		goto drop;
6611	}
6612
 
 
 
 
6613	if (!control->sframe) {
6614		int err;
6615
6616		BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
6617		       control->sar, control->reqseq, control->final,
6618		       control->txseq);
6619
6620		/* Validate F-bit - F=0 always valid, F=1 only
6621		 * valid in TX WAIT_F
6622		 */
6623		if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
6624			goto drop;
6625
6626		if (chan->mode != L2CAP_MODE_STREAMING) {
6627			event = L2CAP_EV_RECV_IFRAME;
6628			err = l2cap_rx(chan, control, skb, event);
6629		} else {
6630			err = l2cap_stream_rx(chan, control, skb);
6631		}
6632
6633		if (err)
6634			l2cap_send_disconn_req(chan, ECONNRESET);
6635	} else {
6636		const u8 rx_func_to_event[4] = {
6637			L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
6638			L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
6639		};
6640
6641		/* Only I-frames are expected in streaming mode */
6642		if (chan->mode == L2CAP_MODE_STREAMING)
6643			goto drop;
6644
6645		BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
6646		       control->reqseq, control->final, control->poll,
6647		       control->super);
6648
6649		if (len != 0) {
6650			BT_ERR("Trailing bytes: %d in sframe", len);
6651			l2cap_send_disconn_req(chan, ECONNRESET);
6652			goto drop;
6653		}
6654
6655		/* Validate F and P bits */
6656		if (control->final && (control->poll ||
6657				       chan->tx_state != L2CAP_TX_STATE_WAIT_F))
6658			goto drop;
6659
6660		event = rx_func_to_event[control->super];
6661		if (l2cap_rx(chan, control, skb, event))
6662			l2cap_send_disconn_req(chan, ECONNRESET);
6663	}
6664
6665	return 0;
6666
6667drop:
6668	kfree_skb(skb);
6669	return 0;
6670}
6671
6672static void l2cap_chan_le_send_credits(struct l2cap_chan *chan)
6673{
6674	struct l2cap_conn *conn = chan->conn;
6675	struct l2cap_le_credits pkt;
6676	u16 return_credits;
6677
6678	/* We return more credits to the sender only after the amount of
6679	 * credits falls below half of the initial amount.
6680	 */
6681	if (chan->rx_credits >= (le_max_credits + 1) / 2)
6682		return;
6683
6684	return_credits = le_max_credits - chan->rx_credits;
6685
6686	BT_DBG("chan %p returning %u credits to sender", chan, return_credits);
6687
6688	chan->rx_credits += return_credits;
6689
6690	pkt.cid     = cpu_to_le16(chan->scid);
6691	pkt.credits = cpu_to_le16(return_credits);
6692
6693	chan->ident = l2cap_get_ident(conn);
6694
6695	l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt);
6696}
6697
6698static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
6699{
6700	int err;
6701
6702	if (!chan->rx_credits) {
6703		BT_ERR("No credits to receive LE L2CAP data");
6704		l2cap_send_disconn_req(chan, ECONNRESET);
6705		return -ENOBUFS;
6706	}
6707
6708	if (chan->imtu < skb->len) {
6709		BT_ERR("Too big LE L2CAP PDU");
6710		return -ENOBUFS;
6711	}
6712
6713	chan->rx_credits--;
6714	BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits);
6715
6716	l2cap_chan_le_send_credits(chan);
6717
6718	err = 0;
6719
6720	if (!chan->sdu) {
6721		u16 sdu_len;
6722
6723		sdu_len = get_unaligned_le16(skb->data);
6724		skb_pull(skb, L2CAP_SDULEN_SIZE);
6725
6726		BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u",
6727		       sdu_len, skb->len, chan->imtu);
6728
6729		if (sdu_len > chan->imtu) {
6730			BT_ERR("Too big LE L2CAP SDU length received");
6731			err = -EMSGSIZE;
6732			goto failed;
6733		}
6734
6735		if (skb->len > sdu_len) {
6736			BT_ERR("Too much LE L2CAP data received");
6737			err = -EINVAL;
6738			goto failed;
6739		}
6740
6741		if (skb->len == sdu_len)
6742			return chan->ops->recv(chan, skb);
6743
6744		chan->sdu = skb;
6745		chan->sdu_len = sdu_len;
6746		chan->sdu_last_frag = skb;
6747
6748		return 0;
6749	}
6750
6751	BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u",
6752	       chan->sdu->len, skb->len, chan->sdu_len);
6753
6754	if (chan->sdu->len + skb->len > chan->sdu_len) {
6755		BT_ERR("Too much LE L2CAP data received");
6756		err = -EINVAL;
6757		goto failed;
6758	}
6759
6760	append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag);
6761	skb = NULL;
6762
6763	if (chan->sdu->len == chan->sdu_len) {
6764		err = chan->ops->recv(chan, chan->sdu);
6765		if (!err) {
6766			chan->sdu = NULL;
6767			chan->sdu_last_frag = NULL;
6768			chan->sdu_len = 0;
6769		}
6770	}
6771
6772failed:
6773	if (err) {
6774		kfree_skb(skb);
6775		kfree_skb(chan->sdu);
6776		chan->sdu = NULL;
6777		chan->sdu_last_frag = NULL;
6778		chan->sdu_len = 0;
6779	}
6780
6781	/* We can't return an error here since we took care of the skb
6782	 * freeing internally. An error return would cause the caller to
6783	 * do a double-free of the skb.
6784	 */
6785	return 0;
6786}
6787
6788static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
6789			       struct sk_buff *skb)
6790{
6791	struct l2cap_chan *chan;
6792
6793	chan = l2cap_get_chan_by_scid(conn, cid);
6794	if (!chan) {
6795		if (cid == L2CAP_CID_A2MP) {
6796			chan = a2mp_channel_create(conn, skb);
6797			if (!chan) {
6798				kfree_skb(skb);
6799				return;
6800			}
6801
6802			l2cap_chan_lock(chan);
6803		} else {
6804			BT_DBG("unknown cid 0x%4.4x", cid);
6805			/* Drop packet and return */
6806			kfree_skb(skb);
6807			return;
6808		}
6809	}
6810
6811	BT_DBG("chan %p, len %d", chan, skb->len);
6812
6813	/* If we receive data on a fixed channel before the info req/rsp
6814	 * procdure is done simply assume that the channel is supported
6815	 * and mark it as ready.
6816	 */
6817	if (chan->chan_type == L2CAP_CHAN_FIXED)
6818		l2cap_chan_ready(chan);
6819
6820	if (chan->state != BT_CONNECTED)
6821		goto drop;
6822
6823	switch (chan->mode) {
6824	case L2CAP_MODE_LE_FLOWCTL:
6825		if (l2cap_le_data_rcv(chan, skb) < 0)
6826			goto drop;
6827
6828		goto done;
6829
6830	case L2CAP_MODE_BASIC:
6831		/* If socket recv buffers overflows we drop data here
6832		 * which is *bad* because L2CAP has to be reliable.
6833		 * But we don't have any other choice. L2CAP doesn't
6834		 * provide flow control mechanism. */
6835
6836		if (chan->imtu < skb->len) {
6837			BT_ERR("Dropping L2CAP data: receive buffer overflow");
6838			goto drop;
6839		}
6840
6841		if (!chan->ops->recv(chan, skb))
6842			goto done;
6843		break;
6844
6845	case L2CAP_MODE_ERTM:
6846	case L2CAP_MODE_STREAMING:
6847		l2cap_data_rcv(chan, skb);
6848		goto done;
6849
6850	default:
6851		BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
6852		break;
6853	}
6854
6855drop:
6856	kfree_skb(skb);
6857
6858done:
6859	l2cap_chan_unlock(chan);
6860}
6861
6862static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
6863				  struct sk_buff *skb)
6864{
6865	struct hci_conn *hcon = conn->hcon;
6866	struct l2cap_chan *chan;
6867
6868	if (hcon->type != ACL_LINK)
6869		goto free_skb;
6870
6871	chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst,
6872					ACL_LINK);
6873	if (!chan)
6874		goto free_skb;
6875
6876	BT_DBG("chan %p, len %d", chan, skb->len);
6877
6878	if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
6879		goto drop;
6880
6881	if (chan->imtu < skb->len)
6882		goto drop;
6883
6884	/* Store remote BD_ADDR and PSM for msg_name */
6885	bacpy(&bt_cb(skb)->l2cap.bdaddr, &hcon->dst);
6886	bt_cb(skb)->l2cap.psm = psm;
6887
6888	if (!chan->ops->recv(chan, skb)) {
6889		l2cap_chan_put(chan);
6890		return;
6891	}
6892
6893drop:
6894	l2cap_chan_put(chan);
6895free_skb:
6896	kfree_skb(skb);
6897}
6898
6899static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
6900{
6901	struct l2cap_hdr *lh = (void *) skb->data;
6902	struct hci_conn *hcon = conn->hcon;
6903	u16 cid, len;
6904	__le16 psm;
6905
6906	if (hcon->state != BT_CONNECTED) {
6907		BT_DBG("queueing pending rx skb");
6908		skb_queue_tail(&conn->pending_rx, skb);
6909		return;
6910	}
6911
6912	skb_pull(skb, L2CAP_HDR_SIZE);
6913	cid = __le16_to_cpu(lh->cid);
6914	len = __le16_to_cpu(lh->len);
6915
6916	if (len != skb->len) {
6917		kfree_skb(skb);
6918		return;
6919	}
6920
6921	/* Since we can't actively block incoming LE connections we must
6922	 * at least ensure that we ignore incoming data from them.
6923	 */
6924	if (hcon->type == LE_LINK &&
6925	    hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst,
6926				   bdaddr_dst_type(hcon))) {
6927		kfree_skb(skb);
6928		return;
6929	}
6930
6931	BT_DBG("len %d, cid 0x%4.4x", len, cid);
6932
6933	switch (cid) {
6934	case L2CAP_CID_SIGNALING:
6935		l2cap_sig_channel(conn, skb);
6936		break;
6937
6938	case L2CAP_CID_CONN_LESS:
6939		psm = get_unaligned((__le16 *) skb->data);
6940		skb_pull(skb, L2CAP_PSMLEN_SIZE);
6941		l2cap_conless_channel(conn, psm, skb);
6942		break;
6943
6944	case L2CAP_CID_LE_SIGNALING:
6945		l2cap_le_sig_channel(conn, skb);
6946		break;
6947
6948	default:
6949		l2cap_data_channel(conn, cid, skb);
6950		break;
6951	}
6952}
6953
6954static void process_pending_rx(struct work_struct *work)
6955{
6956	struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
6957					       pending_rx_work);
6958	struct sk_buff *skb;
6959
6960	BT_DBG("");
6961
6962	while ((skb = skb_dequeue(&conn->pending_rx)))
6963		l2cap_recv_frame(conn, skb);
6964}
6965
6966static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
6967{
6968	struct l2cap_conn *conn = hcon->l2cap_data;
6969	struct hci_chan *hchan;
6970
6971	if (conn)
6972		return conn;
6973
6974	hchan = hci_chan_create(hcon);
6975	if (!hchan)
6976		return NULL;
6977
6978	conn = kzalloc(sizeof(*conn), GFP_KERNEL);
6979	if (!conn) {
6980		hci_chan_del(hchan);
6981		return NULL;
6982	}
6983
6984	kref_init(&conn->ref);
6985	hcon->l2cap_data = conn;
6986	conn->hcon = hci_conn_get(hcon);
6987	conn->hchan = hchan;
6988
6989	BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
6990
6991	switch (hcon->type) {
6992	case LE_LINK:
6993		if (hcon->hdev->le_mtu) {
6994			conn->mtu = hcon->hdev->le_mtu;
6995			break;
6996		}
6997		/* fall through */
6998	default:
6999		conn->mtu = hcon->hdev->acl_mtu;
7000		break;
7001	}
7002
7003	conn->feat_mask = 0;
7004
7005	conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS;
7006
7007	if (hcon->type == ACL_LINK &&
7008	    hci_dev_test_flag(hcon->hdev, HCI_HS_ENABLED))
7009		conn->local_fixed_chan |= L2CAP_FC_A2MP;
7010
7011	if (hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED) &&
7012	    (bredr_sc_enabled(hcon->hdev) ||
7013	     hci_dev_test_flag(hcon->hdev, HCI_FORCE_BREDR_SMP)))
7014		conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR;
7015
7016	mutex_init(&conn->ident_lock);
7017	mutex_init(&conn->chan_lock);
7018
7019	INIT_LIST_HEAD(&conn->chan_l);
7020	INIT_LIST_HEAD(&conn->users);
7021
7022	INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout);
7023
7024	skb_queue_head_init(&conn->pending_rx);
7025	INIT_WORK(&conn->pending_rx_work, process_pending_rx);
7026	INIT_WORK(&conn->id_addr_update_work, l2cap_conn_update_id_addr);
7027
7028	conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
7029
7030	return conn;
7031}
7032
7033static bool is_valid_psm(u16 psm, u8 dst_type) {
7034	if (!psm)
7035		return false;
7036
7037	if (bdaddr_type_is_le(dst_type))
7038		return (psm <= 0x00ff);
7039
7040	/* PSM must be odd and lsb of upper byte must be 0 */
7041	return ((psm & 0x0101) == 0x0001);
7042}
7043
7044int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
7045		       bdaddr_t *dst, u8 dst_type)
7046{
7047	struct l2cap_conn *conn;
7048	struct hci_conn *hcon;
7049	struct hci_dev *hdev;
7050	int err;
7051
7052	BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
7053	       dst_type, __le16_to_cpu(psm));
7054
7055	hdev = hci_get_route(dst, &chan->src);
7056	if (!hdev)
7057		return -EHOSTUNREACH;
7058
7059	hci_dev_lock(hdev);
7060
7061	if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid &&
7062	    chan->chan_type != L2CAP_CHAN_RAW) {
7063		err = -EINVAL;
7064		goto done;
7065	}
7066
7067	if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) {
7068		err = -EINVAL;
7069		goto done;
7070	}
7071
7072	if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) {
7073		err = -EINVAL;
7074		goto done;
7075	}
7076
7077	switch (chan->mode) {
7078	case L2CAP_MODE_BASIC:
7079		break;
7080	case L2CAP_MODE_LE_FLOWCTL:
7081		l2cap_le_flowctl_init(chan);
7082		break;
7083	case L2CAP_MODE_ERTM:
7084	case L2CAP_MODE_STREAMING:
7085		if (!disable_ertm)
7086			break;
7087		/* fall through */
7088	default:
7089		err = -EOPNOTSUPP;
7090		goto done;
7091	}
7092
7093	switch (chan->state) {
7094	case BT_CONNECT:
7095	case BT_CONNECT2:
7096	case BT_CONFIG:
7097		/* Already connecting */
7098		err = 0;
7099		goto done;
7100
7101	case BT_CONNECTED:
7102		/* Already connected */
7103		err = -EISCONN;
7104		goto done;
7105
7106	case BT_OPEN:
7107	case BT_BOUND:
7108		/* Can connect */
7109		break;
7110
7111	default:
7112		err = -EBADFD;
7113		goto done;
7114	}
7115
7116	/* Set destination address and psm */
7117	bacpy(&chan->dst, dst);
7118	chan->dst_type = dst_type;
7119
7120	chan->psm = psm;
7121	chan->dcid = cid;
7122
7123	if (bdaddr_type_is_le(dst_type)) {
7124		/* Convert from L2CAP channel address type to HCI address type
7125		 */
7126		if (dst_type == BDADDR_LE_PUBLIC)
7127			dst_type = ADDR_LE_DEV_PUBLIC;
7128		else
7129			dst_type = ADDR_LE_DEV_RANDOM;
7130
7131		if (hci_dev_test_flag(hdev, HCI_ADVERTISING))
7132			hcon = hci_connect_le(hdev, dst, dst_type,
7133					      chan->sec_level,
7134					      HCI_LE_CONN_TIMEOUT,
7135					      HCI_ROLE_SLAVE);
7136		else
7137			hcon = hci_connect_le_scan(hdev, dst, dst_type,
7138						   chan->sec_level,
7139						   HCI_LE_CONN_TIMEOUT);
7140
7141	} else {
7142		u8 auth_type = l2cap_get_auth_type(chan);
7143		hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type);
7144	}
7145
7146	if (IS_ERR(hcon)) {
7147		err = PTR_ERR(hcon);
7148		goto done;
7149	}
7150
7151	conn = l2cap_conn_add(hcon);
7152	if (!conn) {
7153		hci_conn_drop(hcon);
7154		err = -ENOMEM;
7155		goto done;
7156	}
7157
7158	mutex_lock(&conn->chan_lock);
7159	l2cap_chan_lock(chan);
7160
7161	if (cid && __l2cap_get_chan_by_dcid(conn, cid)) {
7162		hci_conn_drop(hcon);
7163		err = -EBUSY;
7164		goto chan_unlock;
7165	}
7166
7167	/* Update source addr of the socket */
7168	bacpy(&chan->src, &hcon->src);
7169	chan->src_type = bdaddr_src_type(hcon);
7170
7171	__l2cap_chan_add(conn, chan);
7172
7173	/* l2cap_chan_add takes its own ref so we can drop this one */
7174	hci_conn_drop(hcon);
7175
7176	l2cap_state_change(chan, BT_CONNECT);
7177	__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
7178
7179	/* Release chan->sport so that it can be reused by other
7180	 * sockets (as it's only used for listening sockets).
7181	 */
7182	write_lock(&chan_list_lock);
7183	chan->sport = 0;
7184	write_unlock(&chan_list_lock);
7185
7186	if (hcon->state == BT_CONNECTED) {
7187		if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
7188			__clear_chan_timer(chan);
7189			if (l2cap_chan_check_security(chan, true))
7190				l2cap_state_change(chan, BT_CONNECTED);
7191		} else
7192			l2cap_do_start(chan);
7193	}
7194
7195	err = 0;
7196
7197chan_unlock:
7198	l2cap_chan_unlock(chan);
7199	mutex_unlock(&conn->chan_lock);
7200done:
7201	hci_dev_unlock(hdev);
7202	hci_dev_put(hdev);
7203	return err;
7204}
7205EXPORT_SYMBOL_GPL(l2cap_chan_connect);
7206
7207/* ---- L2CAP interface with lower layer (HCI) ---- */
7208
7209int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
7210{
7211	int exact = 0, lm1 = 0, lm2 = 0;
7212	struct l2cap_chan *c;
7213
7214	BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
7215
7216	/* Find listening sockets and check their link_mode */
7217	read_lock(&chan_list_lock);
7218	list_for_each_entry(c, &chan_list, global_l) {
7219		if (c->state != BT_LISTEN)
7220			continue;
7221
7222		if (!bacmp(&c->src, &hdev->bdaddr)) {
7223			lm1 |= HCI_LM_ACCEPT;
7224			if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7225				lm1 |= HCI_LM_MASTER;
7226			exact++;
7227		} else if (!bacmp(&c->src, BDADDR_ANY)) {
7228			lm2 |= HCI_LM_ACCEPT;
7229			if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
7230				lm2 |= HCI_LM_MASTER;
7231		}
7232	}
7233	read_unlock(&chan_list_lock);
7234
7235	return exact ? lm1 : lm2;
7236}
7237
7238/* Find the next fixed channel in BT_LISTEN state, continue iteration
7239 * from an existing channel in the list or from the beginning of the
7240 * global list (by passing NULL as first parameter).
7241 */
7242static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
7243						  struct hci_conn *hcon)
7244{
7245	u8 src_type = bdaddr_src_type(hcon);
7246
7247	read_lock(&chan_list_lock);
7248
7249	if (c)
7250		c = list_next_entry(c, global_l);
7251	else
7252		c = list_entry(chan_list.next, typeof(*c), global_l);
7253
7254	list_for_each_entry_from(c, &chan_list, global_l) {
7255		if (c->chan_type != L2CAP_CHAN_FIXED)
7256			continue;
7257		if (c->state != BT_LISTEN)
7258			continue;
7259		if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY))
7260			continue;
7261		if (src_type != c->src_type)
7262			continue;
7263
7264		l2cap_chan_hold(c);
7265		read_unlock(&chan_list_lock);
7266		return c;
7267	}
7268
7269	read_unlock(&chan_list_lock);
7270
7271	return NULL;
7272}
7273
7274static void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
7275{
7276	struct hci_dev *hdev = hcon->hdev;
7277	struct l2cap_conn *conn;
7278	struct l2cap_chan *pchan;
7279	u8 dst_type;
7280
7281	if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7282		return;
7283
7284	BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
7285
7286	if (status) {
7287		l2cap_conn_del(hcon, bt_to_errno(status));
7288		return;
7289	}
7290
7291	conn = l2cap_conn_add(hcon);
7292	if (!conn)
7293		return;
7294
7295	dst_type = bdaddr_dst_type(hcon);
7296
7297	/* If device is blocked, do not create channels for it */
7298	if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type))
7299		return;
7300
7301	/* Find fixed channels and notify them of the new connection. We
7302	 * use multiple individual lookups, continuing each time where
7303	 * we left off, because the list lock would prevent calling the
7304	 * potentially sleeping l2cap_chan_lock() function.
7305	 */
7306	pchan = l2cap_global_fixed_chan(NULL, hcon);
7307	while (pchan) {
7308		struct l2cap_chan *chan, *next;
7309
7310		/* Client fixed channels should override server ones */
7311		if (__l2cap_get_chan_by_dcid(conn, pchan->scid))
7312			goto next;
7313
7314		l2cap_chan_lock(pchan);
7315		chan = pchan->ops->new_connection(pchan);
7316		if (chan) {
7317			bacpy(&chan->src, &hcon->src);
7318			bacpy(&chan->dst, &hcon->dst);
7319			chan->src_type = bdaddr_src_type(hcon);
7320			chan->dst_type = dst_type;
7321
7322			__l2cap_chan_add(conn, chan);
7323		}
7324
7325		l2cap_chan_unlock(pchan);
7326next:
7327		next = l2cap_global_fixed_chan(pchan, hcon);
7328		l2cap_chan_put(pchan);
7329		pchan = next;
7330	}
7331
7332	l2cap_conn_ready(conn);
7333}
7334
7335int l2cap_disconn_ind(struct hci_conn *hcon)
7336{
7337	struct l2cap_conn *conn = hcon->l2cap_data;
7338
7339	BT_DBG("hcon %p", hcon);
7340
7341	if (!conn)
7342		return HCI_ERROR_REMOTE_USER_TERM;
7343	return conn->disc_reason;
7344}
7345
7346static void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
7347{
7348	if (hcon->type != ACL_LINK && hcon->type != LE_LINK)
7349		return;
7350
7351	BT_DBG("hcon %p reason %d", hcon, reason);
7352
7353	l2cap_conn_del(hcon, bt_to_errno(reason));
7354}
7355
7356static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
7357{
7358	if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
7359		return;
7360
7361	if (encrypt == 0x00) {
7362		if (chan->sec_level == BT_SECURITY_MEDIUM) {
7363			__set_chan_timer(chan, L2CAP_ENC_TIMEOUT);
7364		} else if (chan->sec_level == BT_SECURITY_HIGH ||
7365			   chan->sec_level == BT_SECURITY_FIPS)
7366			l2cap_chan_close(chan, ECONNREFUSED);
7367	} else {
7368		if (chan->sec_level == BT_SECURITY_MEDIUM)
7369			__clear_chan_timer(chan);
7370	}
7371}
7372
7373static void l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
7374{
7375	struct l2cap_conn *conn = hcon->l2cap_data;
7376	struct l2cap_chan *chan;
7377
7378	if (!conn)
7379		return;
7380
7381	BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt);
7382
7383	mutex_lock(&conn->chan_lock);
7384
7385	list_for_each_entry(chan, &conn->chan_l, list) {
7386		l2cap_chan_lock(chan);
7387
7388		BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
7389		       state_to_string(chan->state));
7390
7391		if (chan->scid == L2CAP_CID_A2MP) {
7392			l2cap_chan_unlock(chan);
7393			continue;
7394		}
7395
7396		if (!status && encrypt)
7397			chan->sec_level = hcon->sec_level;
7398
7399		if (!__l2cap_no_conn_pending(chan)) {
7400			l2cap_chan_unlock(chan);
7401			continue;
7402		}
7403
7404		if (!status && (chan->state == BT_CONNECTED ||
7405				chan->state == BT_CONFIG)) {
7406			chan->ops->resume(chan);
7407			l2cap_check_encryption(chan, encrypt);
7408			l2cap_chan_unlock(chan);
7409			continue;
7410		}
7411
7412		if (chan->state == BT_CONNECT) {
7413			if (!status)
7414				l2cap_start_connection(chan);
7415			else
7416				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7417		} else if (chan->state == BT_CONNECT2 &&
7418			   chan->mode != L2CAP_MODE_LE_FLOWCTL) {
7419			struct l2cap_conn_rsp rsp;
7420			__u16 res, stat;
7421
7422			if (!status) {
7423				if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
7424					res = L2CAP_CR_PEND;
7425					stat = L2CAP_CS_AUTHOR_PEND;
7426					chan->ops->defer(chan);
7427				} else {
7428					l2cap_state_change(chan, BT_CONFIG);
7429					res = L2CAP_CR_SUCCESS;
7430					stat = L2CAP_CS_NO_INFO;
7431				}
7432			} else {
7433				l2cap_state_change(chan, BT_DISCONN);
7434				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
7435				res = L2CAP_CR_SEC_BLOCK;
7436				stat = L2CAP_CS_NO_INFO;
7437			}
7438
7439			rsp.scid   = cpu_to_le16(chan->dcid);
7440			rsp.dcid   = cpu_to_le16(chan->scid);
7441			rsp.result = cpu_to_le16(res);
7442			rsp.status = cpu_to_le16(stat);
7443			l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
7444				       sizeof(rsp), &rsp);
7445
7446			if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
7447			    res == L2CAP_CR_SUCCESS) {
7448				char buf[128];
7449				set_bit(CONF_REQ_SENT, &chan->conf_state);
7450				l2cap_send_cmd(conn, l2cap_get_ident(conn),
7451					       L2CAP_CONF_REQ,
7452					       l2cap_build_conf_req(chan, buf),
7453					       buf);
7454				chan->num_conf_req++;
7455			}
7456		}
7457
7458		l2cap_chan_unlock(chan);
7459	}
7460
7461	mutex_unlock(&conn->chan_lock);
7462}
7463
7464void l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
7465{
7466	struct l2cap_conn *conn = hcon->l2cap_data;
7467	struct l2cap_hdr *hdr;
7468	int len;
7469
7470	/* For AMP controller do not create l2cap conn */
7471	if (!conn && hcon->hdev->dev_type != HCI_BREDR)
7472		goto drop;
7473
7474	if (!conn)
7475		conn = l2cap_conn_add(hcon);
7476
7477	if (!conn)
7478		goto drop;
7479
7480	BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
7481
7482	switch (flags) {
7483	case ACL_START:
7484	case ACL_START_NO_FLUSH:
7485	case ACL_COMPLETE:
7486		if (conn->rx_len) {
7487			BT_ERR("Unexpected start frame (len %d)", skb->len);
7488			kfree_skb(conn->rx_skb);
7489			conn->rx_skb = NULL;
7490			conn->rx_len = 0;
7491			l2cap_conn_unreliable(conn, ECOMM);
7492		}
7493
7494		/* Start fragment always begin with Basic L2CAP header */
7495		if (skb->len < L2CAP_HDR_SIZE) {
7496			BT_ERR("Frame is too short (len %d)", skb->len);
7497			l2cap_conn_unreliable(conn, ECOMM);
7498			goto drop;
7499		}
7500
7501		hdr = (struct l2cap_hdr *) skb->data;
7502		len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
7503
7504		if (len == skb->len) {
7505			/* Complete frame received */
7506			l2cap_recv_frame(conn, skb);
7507			return;
7508		}
7509
7510		BT_DBG("Start: total len %d, frag len %d", len, skb->len);
7511
7512		if (skb->len > len) {
7513			BT_ERR("Frame is too long (len %d, expected len %d)",
7514			       skb->len, len);
7515			l2cap_conn_unreliable(conn, ECOMM);
7516			goto drop;
7517		}
7518
7519		/* Allocate skb for the complete frame (with header) */
7520		conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
7521		if (!conn->rx_skb)
7522			goto drop;
7523
7524		skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7525					  skb->len);
7526		conn->rx_len = len - skb->len;
7527		break;
7528
7529	case ACL_CONT:
7530		BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
7531
7532		if (!conn->rx_len) {
7533			BT_ERR("Unexpected continuation frame (len %d)", skb->len);
7534			l2cap_conn_unreliable(conn, ECOMM);
7535			goto drop;
7536		}
7537
7538		if (skb->len > conn->rx_len) {
7539			BT_ERR("Fragment is too long (len %d, expected %d)",
7540			       skb->len, conn->rx_len);
7541			kfree_skb(conn->rx_skb);
7542			conn->rx_skb = NULL;
7543			conn->rx_len = 0;
7544			l2cap_conn_unreliable(conn, ECOMM);
7545			goto drop;
7546		}
7547
7548		skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
7549					  skb->len);
7550		conn->rx_len -= skb->len;
7551
7552		if (!conn->rx_len) {
7553			/* Complete frame received. l2cap_recv_frame
7554			 * takes ownership of the skb so set the global
7555			 * rx_skb pointer to NULL first.
7556			 */
7557			struct sk_buff *rx_skb = conn->rx_skb;
7558			conn->rx_skb = NULL;
7559			l2cap_recv_frame(conn, rx_skb);
7560		}
7561		break;
7562	}
7563
7564drop:
7565	kfree_skb(skb);
7566}
7567
7568static struct hci_cb l2cap_cb = {
7569	.name		= "L2CAP",
7570	.connect_cfm	= l2cap_connect_cfm,
7571	.disconn_cfm	= l2cap_disconn_cfm,
7572	.security_cfm	= l2cap_security_cfm,
7573};
7574
7575static int l2cap_debugfs_show(struct seq_file *f, void *p)
7576{
7577	struct l2cap_chan *c;
7578
7579	read_lock(&chan_list_lock);
7580
7581	list_for_each_entry(c, &chan_list, global_l) {
7582		seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
7583			   &c->src, c->src_type, &c->dst, c->dst_type,
7584			   c->state, __le16_to_cpu(c->psm),
7585			   c->scid, c->dcid, c->imtu, c->omtu,
7586			   c->sec_level, c->mode);
7587	}
7588
7589	read_unlock(&chan_list_lock);
7590
7591	return 0;
7592}
7593
7594static int l2cap_debugfs_open(struct inode *inode, struct file *file)
7595{
7596	return single_open(file, l2cap_debugfs_show, inode->i_private);
7597}
7598
7599static const struct file_operations l2cap_debugfs_fops = {
7600	.open		= l2cap_debugfs_open,
7601	.read		= seq_read,
7602	.llseek		= seq_lseek,
7603	.release	= single_release,
7604};
7605
7606static struct dentry *l2cap_debugfs;
7607
7608int __init l2cap_init(void)
7609{
7610	int err;
7611
7612	err = l2cap_init_sockets();
7613	if (err < 0)
7614		return err;
7615
7616	hci_register_cb(&l2cap_cb);
7617
7618	if (IS_ERR_OR_NULL(bt_debugfs))
7619		return 0;
7620
7621	l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
7622					    NULL, &l2cap_debugfs_fops);
7623
7624	debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs,
7625			   &le_max_credits);
7626	debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs,
7627			   &le_default_mps);
7628
7629	return 0;
7630}
7631
7632void l2cap_exit(void)
7633{
7634	debugfs_remove(l2cap_debugfs);
7635	hci_unregister_cb(&l2cap_cb);
7636	l2cap_cleanup_sockets();
7637}
7638
7639module_param(disable_ertm, bool, 0644);
7640MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");