l2cap_core.c 91.4 KB
Newer Older
1
/*
L
Linus Torvalds 已提交
2 3
   BlueZ - Bluetooth protocol stack for Linux
   Copyright (C) 2000-2001 Qualcomm Incorporated
4
   Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
5
   Copyright (C) 2010 Google Inc.
L
Linus Torvalds 已提交
6 7 8 9 10 11 12 13 14 15 16

   Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License version 2 as
   published by the Free Software Foundation;

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
   IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
17 18 19
   CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
   WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
   ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
L
Linus Torvalds 已提交
20 21
   OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

22 23
   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
L
Linus Torvalds 已提交
24 25 26
   SOFTWARE IS DISCLAIMED.
*/

27
/* Bluetooth L2CAP core. */
L
Linus Torvalds 已提交
28 29 30 31

#include <linux/module.h>

#include <linux/types.h>
32
#include <linux/capability.h>
L
Linus Torvalds 已提交
33 34 35 36 37 38 39 40 41 42 43
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/poll.h>
#include <linux/fcntl.h>
#include <linux/init.h>
#include <linux/interrupt.h>
#include <linux/socket.h>
#include <linux/skbuff.h>
#include <linux/list.h>
44
#include <linux/device.h>
45 46
#include <linux/debugfs.h>
#include <linux/seq_file.h>
47
#include <linux/uaccess.h>
48
#include <linux/crc16.h>
L
Linus Torvalds 已提交
49 50 51 52 53 54 55 56 57
#include <net/sock.h>

#include <asm/system.h>
#include <asm/unaligned.h>

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>

58
int disable_ertm;
59

60
static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
61
static u8 l2cap_fixed_chan[8] = { 0x02, };
L
Linus Torvalds 已提交
62

63 64
static struct workqueue_struct *_busy_wq;

65
struct bt_sock_list l2cap_sk_list = {
66
	.lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
L
Linus Torvalds 已提交
67 68
};

69 70
static void l2cap_busy_work(struct work_struct *work);

L
Linus Torvalds 已提交
71 72 73
static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
				u8 code, u8 ident, u16 dlen, void *data);

74 75
static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);

76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103
/* ---- L2CAP channels ---- */
static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid)
{
	struct sock *s;
	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->dcid == cid)
			break;
	}
	return s;
}

static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
{
	struct sock *s;
	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->scid == cid)
			break;
	}
	return s;
}

/* Find channel with given SCID.
 * Returns locked socket */
static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid)
{
	struct sock *s;
	read_lock(&l->lock);
	s = __l2cap_get_chan_by_scid(l, cid);
104 105
	if (s)
		bh_lock_sock(s);
106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124
	read_unlock(&l->lock);
	return s;
}

static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
{
	struct sock *s;
	for (s = l->head; s; s = l2cap_pi(s)->next_c) {
		if (l2cap_pi(s)->ident == ident)
			break;
	}
	return s;
}

static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident)
{
	struct sock *s;
	read_lock(&l->lock);
	s = __l2cap_get_chan_by_ident(l, ident);
125 126
	if (s)
		bh_lock_sock(s);
127 128 129 130 131 132
	read_unlock(&l->lock);
	return s;
}

static u16 l2cap_alloc_cid(struct l2cap_chan_list *l)
{
133
	u16 cid = L2CAP_CID_DYN_START;
134

135
	for (; cid < L2CAP_CID_DYN_END; cid++) {
136
		if (!__l2cap_get_chan_by_scid(l, cid))
137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158
			return cid;
	}

	return 0;
}

static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk)
{
	sock_hold(sk);

	if (l->head)
		l2cap_pi(l->head)->prev_c = sk;

	l2cap_pi(sk)->next_c = l->head;
	l2cap_pi(sk)->prev_c = NULL;
	l->head = sk;
}

static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk)
{
	struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c;

159
	write_lock_bh(&l->lock);
160 161 162 163 164 165 166
	if (sk == l->head)
		l->head = next;

	if (next)
		l2cap_pi(next)->prev_c = prev;
	if (prev)
		l2cap_pi(prev)->next_c = next;
167
	write_unlock_bh(&l->lock);
168 169 170 171 172 173 174 175

	__sock_put(sk);
}

static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
{
	struct l2cap_chan_list *l = &conn->chan_list;

176 177
	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
			l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
178

179 180
	conn->disc_reason = 0x13;

181 182
	l2cap_pi(sk)->conn = conn;

183
	if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
184 185 186 187 188 189 190 191 192 193
		if (conn->hcon->type == LE_LINK) {
			/* LE connection */
			l2cap_pi(sk)->omtu = L2CAP_LE_DEFAULT_MTU;
			l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA;
			l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA;
		} else {
			/* Alloc CID for connection-oriented socket */
			l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
			l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
		}
194 195
	} else if (sk->sk_type == SOCK_DGRAM) {
		/* Connectionless socket */
196 197
		l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
		l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS;
198 199 200
		l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
	} else {
		/* Raw socket can send/recv signalling messages only */
201 202
		l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING;
		l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING;
203 204 205 206 207 208 209 210 211
		l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
	}

	__l2cap_chan_link(l, sk);

	if (parent)
		bt_accept_enqueue(parent, sk);
}

212
/* Delete channel.
213
 * Must be called on the locked socket. */
214
void l2cap_chan_del(struct sock *sk, int err)
215 216 217 218 219 220 221 222
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	struct sock *parent = bt_sk(sk)->parent;

	l2cap_sock_clear_timer(sk);

	BT_DBG("sk %p, conn %p, err %d", sk, conn, err);

223
	if (conn) {
224 225 226 227 228 229
		/* Unlink from channel list */
		l2cap_chan_unlink(&conn->chan_list, sk);
		l2cap_pi(sk)->conn = NULL;
		hci_conn_put(conn->hcon);
	}

230
	sk->sk_state = BT_CLOSED;
231 232 233 234 235 236 237 238 239 240
	sock_set_flag(sk, SOCK_ZAPPED);

	if (err)
		sk->sk_err = err;

	if (parent) {
		bt_accept_unlink(sk);
		parent->sk_data_ready(parent, 0);
	} else
		sk->sk_state_change(sk);
241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258

	skb_queue_purge(TX_QUEUE(sk));

	if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
		struct srej_list *l, *tmp;

		del_timer(&l2cap_pi(sk)->retrans_timer);
		del_timer(&l2cap_pi(sk)->monitor_timer);
		del_timer(&l2cap_pi(sk)->ack_timer);

		skb_queue_purge(SREJ_QUEUE(sk));
		skb_queue_purge(BUSY_QUEUE(sk));

		list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
			list_del(&l->list);
			kfree(l);
		}
	}
259 260
}

261
static inline u8 l2cap_get_auth_type(struct sock *sk)
262
{
263 264 265 266 267 268 269 270 271 272 273 274
	if (sk->sk_type == SOCK_RAW) {
		switch (l2cap_pi(sk)->sec_level) {
		case BT_SECURITY_HIGH:
			return HCI_AT_DEDICATED_BONDING_MITM;
		case BT_SECURITY_MEDIUM:
			return HCI_AT_DEDICATED_BONDING;
		default:
			return HCI_AT_NO_BONDING;
		}
	} else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
		if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW)
			l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
275

276
		if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
277
			return HCI_AT_NO_BONDING_MITM;
278
		else
279
			return HCI_AT_NO_BONDING;
280 281 282
	} else {
		switch (l2cap_pi(sk)->sec_level) {
		case BT_SECURITY_HIGH:
283
			return HCI_AT_GENERAL_BONDING_MITM;
284
		case BT_SECURITY_MEDIUM:
285
			return HCI_AT_GENERAL_BONDING;
286
		default:
287
			return HCI_AT_NO_BONDING;
288
		}
289
	}
290 291 292 293 294 295 296 297 298
}

/* Service level security */
static inline int l2cap_check_security(struct sock *sk)
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	__u8 auth_type;

	auth_type = l2cap_get_auth_type(sk);
299

300 301
	return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level,
								auth_type);
302 303
}

304
u8 l2cap_get_ident(struct l2cap_conn *conn)
305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325
{
	u8 id;

	/* Get next available identificator.
	 *    1 - 128 are used by kernel.
	 *  129 - 199 are reserved.
	 *  200 - 254 are used by utilities like l2ping, etc.
	 */

	spin_lock_bh(&conn->lock);

	if (++conn->tx_ident > 128)
		conn->tx_ident = 1;

	id = conn->tx_ident;

	spin_unlock_bh(&conn->lock);

	return id;
}

326
void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
327 328
{
	struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
329
	u8 flags;
330 331 332 333

	BT_DBG("code 0x%2.2x", code);

	if (!skb)
334
		return;
335

336 337 338 339 340 341
	if (lmp_no_flush_capable(conn->hcon->hdev))
		flags = ACL_START_NO_FLUSH;
	else
		flags = ACL_START;

	hci_send_acl(conn->hcon, skb, flags);
342 343
}

344
static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
345 346 347 348
{
	struct sk_buff *skb;
	struct l2cap_hdr *lh;
	struct l2cap_conn *conn = pi->conn;
349
	struct sock *sk = (struct sock *)pi;
350
	int count, hlen = L2CAP_HDR_SIZE + 2;
351
	u8 flags;
352

353 354 355
	if (sk->sk_state != BT_CONNECTED)
		return;

356 357
	if (pi->fcs == L2CAP_FCS_CRC16)
		hlen += 2;
358 359 360

	BT_DBG("pi %p, control 0x%2.2x", pi, control);

361
	count = min_t(unsigned int, conn->mtu, hlen);
362 363
	control |= L2CAP_CTRL_FRAME_TYPE;

364 365 366 367 368
	if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
		control |= L2CAP_CTRL_FINAL;
		pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
	}

369 370 371 372 373
	if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
		control |= L2CAP_CTRL_POLL;
		pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
	}

374 375
	skb = bt_skb_alloc(count, GFP_ATOMIC);
	if (!skb)
376
		return;
377 378

	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
379
	lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
380 381 382
	lh->cid = cpu_to_le16(pi->dcid);
	put_unaligned_le16(control, skb_put(skb, 2));

383 384 385 386 387
	if (pi->fcs == L2CAP_FCS_CRC16) {
		u16 fcs = crc16(0, (u8 *)lh, count - 2);
		put_unaligned_le16(fcs, skb_put(skb, 2));
	}

388 389 390 391 392 393
	if (lmp_no_flush_capable(conn->hcon->hdev))
		flags = ACL_START_NO_FLUSH;
	else
		flags = ACL_START;

	hci_send_acl(pi->conn->hcon, skb, flags);
394 395
}

396
static inline void l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
397
{
398
	if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
399
		control |= L2CAP_SUPER_RCV_NOT_READY;
400 401
		pi->conn_state |= L2CAP_CONN_RNR_SENT;
	} else
402 403
		control |= L2CAP_SUPER_RCV_READY;

404 405
	control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;

406
	l2cap_send_sframe(pi, control);
407 408
}

409 410 411 412 413
static inline int __l2cap_no_conn_pending(struct sock *sk)
{
	return !(l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND);
}

414 415 416 417 418
static void l2cap_do_start(struct sock *sk)
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;

	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
419 420 421
		if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
			return;

422
		if (l2cap_check_security(sk) && __l2cap_no_conn_pending(sk)) {
423 424 425
			struct l2cap_conn_req req;
			req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
			req.psm  = l2cap_pi(sk)->psm;
426

427
			l2cap_pi(sk)->ident = l2cap_get_ident(conn);
428
			l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
429

430
			l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
431
					L2CAP_CONN_REQ, sizeof(req), &req);
432
		}
433 434 435 436 437 438 439 440 441 442 443 444 445 446 447
	} else {
		struct l2cap_info_req req;
		req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);

		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
		conn->info_ident = l2cap_get_ident(conn);

		mod_timer(&conn->info_timer, jiffies +
					msecs_to_jiffies(L2CAP_INFO_TIMEOUT));

		l2cap_send_cmd(conn, conn->info_ident,
					L2CAP_INFO_REQ, sizeof(req), &req);
	}
}

448 449 450
static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
{
	u32 local_feat_mask = l2cap_feat_mask;
451
	if (!disable_ertm)
452 453 454 455 456 457 458 459 460 461 462 463
		local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;

	switch (mode) {
	case L2CAP_MODE_ERTM:
		return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
	case L2CAP_MODE_STREAMING:
		return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
	default:
		return 0x00;
	}
}

464
void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int err)
465 466 467
{
	struct l2cap_disconn_req req;

468 469 470 471 472 473 474 475 476 477 478
	if (!conn)
		return;

	skb_queue_purge(TX_QUEUE(sk));

	if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
		del_timer(&l2cap_pi(sk)->retrans_timer);
		del_timer(&l2cap_pi(sk)->monitor_timer);
		del_timer(&l2cap_pi(sk)->ack_timer);
	}

479 480 481 482
	req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
	req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
	l2cap_send_cmd(conn, l2cap_get_ident(conn),
			L2CAP_DISCONN_REQ, sizeof(req), &req);
483 484

	sk->sk_state = BT_DISCONN;
485
	sk->sk_err = err;
486 487
}

L
Linus Torvalds 已提交
488
/* ---- L2CAP connections ---- */
489 490 491
static void l2cap_conn_start(struct l2cap_conn *conn)
{
	struct l2cap_chan_list *l = &conn->chan_list;
492
	struct sock_del_list del, *tmp1, *tmp2;
493 494 495 496
	struct sock *sk;

	BT_DBG("conn %p", conn);

497 498
	INIT_LIST_HEAD(&del.list);

499 500 501 502 503
	read_lock(&l->lock);

	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
		bh_lock_sock(sk);

504 505
		if (sk->sk_type != SOCK_SEQPACKET &&
				sk->sk_type != SOCK_STREAM) {
506 507 508 509 510
			bh_unlock_sock(sk);
			continue;
		}

		if (sk->sk_state == BT_CONNECT) {
511
			struct l2cap_conn_req req;
512

513 514 515 516 517
			if (!l2cap_check_security(sk) ||
					!__l2cap_no_conn_pending(sk)) {
				bh_unlock_sock(sk);
				continue;
			}
518

519 520 521 522 523 524 525 526 527 528
			if (!l2cap_mode_supported(l2cap_pi(sk)->mode,
					conn->feat_mask)
					&& l2cap_pi(sk)->conf_state &
					L2CAP_CONF_STATE2_DEVICE) {
				tmp1 = kzalloc(sizeof(struct sock_del_list),
						GFP_ATOMIC);
				tmp1->sk = sk;
				list_add_tail(&tmp1->list, &del.list);
				bh_unlock_sock(sk);
				continue;
529
			}
530 531 532 533 534 535 536 537 538 539

			req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
			req.psm  = l2cap_pi(sk)->psm;

			l2cap_pi(sk)->ident = l2cap_get_ident(conn);
			l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;

			l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
				L2CAP_CONN_REQ, sizeof(req), &req);

540 541
		} else if (sk->sk_state == BT_CONNECT2) {
			struct l2cap_conn_rsp rsp;
542
			char buf[128];
543 544 545
			rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
			rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);

546
			if (l2cap_check_security(sk)) {
547 548 549 550 551 552 553 554 555 556 557
				if (bt_sk(sk)->defer_setup) {
					struct sock *parent = bt_sk(sk)->parent;
					rsp.result = cpu_to_le16(L2CAP_CR_PEND);
					rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
					parent->sk_data_ready(parent, 0);

				} else {
					sk->sk_state = BT_CONFIG;
					rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
					rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
				}
558 559 560 561 562 563 564
			} else {
				rsp.result = cpu_to_le16(L2CAP_CR_PEND);
				rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
			}

			l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
					L2CAP_CONN_RSP, sizeof(rsp), &rsp);
565 566 567 568 569 570 571 572 573 574 575

			if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT ||
					rsp.result != L2CAP_CR_SUCCESS) {
				bh_unlock_sock(sk);
				continue;
			}

			l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
			l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
						l2cap_build_conf_req(sk, buf), buf);
			l2cap_pi(sk)->num_conf_req++;
576 577 578 579 580 581
		}

		bh_unlock_sock(sk);
	}

	read_unlock(&l->lock);
582 583 584 585 586 587 588 589

	list_for_each_entry_safe(tmp1, tmp2, &del.list, list) {
		bh_lock_sock(tmp1->sk);
		__l2cap_sock_close(tmp1->sk, ECONNRESET);
		bh_unlock_sock(tmp1->sk);
		list_del(&tmp1->list);
		kfree(tmp1);
	}
590 591
}

592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667
/* Find socket with cid and source bdaddr.
 * Returns closest match, locked.
 */
static struct sock *l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t *src)
{
	struct sock *s, *sk = NULL, *sk1 = NULL;
	struct hlist_node *node;

	read_lock(&l2cap_sk_list.lock);

	sk_for_each(sk, node, &l2cap_sk_list.head) {
		if (state && sk->sk_state != state)
			continue;

		if (l2cap_pi(sk)->scid == cid) {
			/* Exact match. */
			if (!bacmp(&bt_sk(sk)->src, src))
				break;

			/* Closest match */
			if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
				sk1 = sk;
		}
	}
	s = node ? sk : sk1;
	if (s)
		bh_lock_sock(s);
	read_unlock(&l2cap_sk_list.lock);

	return s;
}

static void l2cap_le_conn_ready(struct l2cap_conn *conn)
{
	struct l2cap_chan_list *list = &conn->chan_list;
	struct sock *parent, *uninitialized_var(sk);

	BT_DBG("");

	/* Check if we have socket listening on cid */
	parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
							conn->src);
	if (!parent)
		return;

	/* Check for backlog size */
	if (sk_acceptq_is_full(parent)) {
		BT_DBG("backlog full %d", parent->sk_ack_backlog);
		goto clean;
	}

	sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
	if (!sk)
		goto clean;

	write_lock_bh(&list->lock);

	hci_conn_hold(conn->hcon);

	l2cap_sock_init(sk, parent);
	bacpy(&bt_sk(sk)->src, conn->src);
	bacpy(&bt_sk(sk)->dst, conn->dst);

	__l2cap_chan_add(conn, sk, parent);

	l2cap_sock_set_timer(sk, sk->sk_sndtimeo);

	sk->sk_state = BT_CONNECTED;
	parent->sk_data_ready(parent, 0);

	write_unlock_bh(&list->lock);

clean:
	bh_unlock_sock(parent);
}

668 669
static void l2cap_conn_ready(struct l2cap_conn *conn)
{
670 671
	struct l2cap_chan_list *l = &conn->chan_list;
	struct sock *sk;
672

673
	BT_DBG("conn %p", conn);
674

675 676 677
	if (!conn->hcon->out && conn->hcon->type == LE_LINK)
		l2cap_le_conn_ready(conn);

678
	read_lock(&l->lock);
679

680 681
	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
		bh_lock_sock(sk);
682

683 684 685 686 687 688
		if (conn->hcon->type == LE_LINK) {
			l2cap_sock_clear_timer(sk);
			sk->sk_state = BT_CONNECTED;
			sk->sk_state_change(sk);
		}

689 690
		if (sk->sk_type != SOCK_SEQPACKET &&
				sk->sk_type != SOCK_STREAM) {
691 692 693 694 695
			l2cap_sock_clear_timer(sk);
			sk->sk_state = BT_CONNECTED;
			sk->sk_state_change(sk);
		} else if (sk->sk_state == BT_CONNECT)
			l2cap_do_start(sk);
696

697
		bh_unlock_sock(sk);
698
	}
699 700

	read_unlock(&l->lock);
701 702 703 704 705 706 707 708 709 710 711 712 713
}

/* Notify sockets that we cannot guaranty reliability anymore */
static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
{
	struct l2cap_chan_list *l = &conn->chan_list;
	struct sock *sk;

	BT_DBG("conn %p", conn);

	read_lock(&l->lock);

	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
714
		if (l2cap_pi(sk)->force_reliable)
715 716 717 718 719 720 721 722 723 724
			sk->sk_err = err;
	}

	read_unlock(&l->lock);
}

static void l2cap_info_timeout(unsigned long arg)
{
	struct l2cap_conn *conn = (void *) arg;

725
	conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
726
	conn->info_ident = 0;
727

728 729 730
	l2cap_conn_start(conn);
}

L
Linus Torvalds 已提交
731 732
static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
{
733
	struct l2cap_conn *conn = hcon->l2cap_data;
L
Linus Torvalds 已提交
734

735
	if (conn || status)
L
Linus Torvalds 已提交
736 737
		return conn;

738 739
	conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
	if (!conn)
L
Linus Torvalds 已提交
740 741 742 743 744
		return NULL;

	hcon->l2cap_data = conn;
	conn->hcon = hcon;

745 746
	BT_DBG("hcon %p conn %p", hcon, conn);

747 748 749 750 751
	if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
		conn->mtu = hcon->hdev->le_mtu;
	else
		conn->mtu = hcon->hdev->acl_mtu;

L
Linus Torvalds 已提交
752 753 754
	conn->src = &hcon->hdev->bdaddr;
	conn->dst = &hcon->dst;

755 756
	conn->feat_mask = 0;

L
Linus Torvalds 已提交
757 758 759
	spin_lock_init(&conn->lock);
	rwlock_init(&conn->chan_list.lock);

760 761
	if (hcon->type != LE_LINK)
		setup_timer(&conn->info_timer, l2cap_info_timeout,
D
Dave Young 已提交
762 763
						(unsigned long) conn);

764 765
	conn->disc_reason = 0x13;

L
Linus Torvalds 已提交
766 767 768
	return conn;
}

769
static void l2cap_conn_del(struct hci_conn *hcon, int err)
L
Linus Torvalds 已提交
770
{
771
	struct l2cap_conn *conn = hcon->l2cap_data;
L
Linus Torvalds 已提交
772 773
	struct sock *sk;

774 775
	if (!conn)
		return;
L
Linus Torvalds 已提交
776 777 778

	BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);

779
	kfree_skb(conn->rx_skb);
L
Linus Torvalds 已提交
780 781 782 783 784 785 786 787 788

	/* Kill channels */
	while ((sk = conn->chan_list.head)) {
		bh_lock_sock(sk);
		l2cap_chan_del(sk, err);
		bh_unlock_sock(sk);
		l2cap_sock_kill(sk);
	}

789 790
	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
		del_timer_sync(&conn->info_timer);
791

L
Linus Torvalds 已提交
792 793 794 795 796 797 798
	hcon->l2cap_data = NULL;
	kfree(conn);
}

static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent)
{
	struct l2cap_chan_list *l = &conn->chan_list;
799
	write_lock_bh(&l->lock);
L
Linus Torvalds 已提交
800
	__l2cap_chan_add(conn, sk, parent);
801
	write_unlock_bh(&l->lock);
L
Linus Torvalds 已提交
802 803 804 805 806 807 808
}

/* ---- Socket interface ---- */

/* Find socket with psm and source bdaddr.
 * Returns closest match.
 */
809
static struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
L
Linus Torvalds 已提交
810 811 812 813
{
	struct sock *sk = NULL, *sk1 = NULL;
	struct hlist_node *node;

814 815
	read_lock(&l2cap_sk_list.lock);

L
Linus Torvalds 已提交
816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831
	sk_for_each(sk, node, &l2cap_sk_list.head) {
		if (state && sk->sk_state != state)
			continue;

		if (l2cap_pi(sk)->psm == psm) {
			/* Exact match. */
			if (!bacmp(&bt_sk(sk)->src, src))
				break;

			/* Closest match */
			if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
				sk1 = sk;
		}
	}

	read_unlock(&l2cap_sk_list.lock);
832 833

	return node ? sk : sk1;
L
Linus Torvalds 已提交
834 835
}

836
int l2cap_do_connect(struct sock *sk)
L
Linus Torvalds 已提交
837 838 839 840 841 842
{
	bdaddr_t *src = &bt_sk(sk)->src;
	bdaddr_t *dst = &bt_sk(sk)->dst;
	struct l2cap_conn *conn;
	struct hci_conn *hcon;
	struct hci_dev *hdev;
843
	__u8 auth_type;
844
	int err;
L
Linus Torvalds 已提交
845

846 847
	BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
							l2cap_pi(sk)->psm);
L
Linus Torvalds 已提交
848

849 850
	hdev = hci_get_route(dst, src);
	if (!hdev)
L
Linus Torvalds 已提交
851 852 853 854 855 856
		return -EHOSTUNREACH;

	hci_dev_lock_bh(hdev);

	err = -ENOMEM;

857
	auth_type = l2cap_get_auth_type(sk);
858

859 860
	if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA)
		hcon = hci_connect(hdev, LE_LINK, dst,
861
					l2cap_pi(sk)->sec_level, auth_type);
862 863 864 865
	else
		hcon = hci_connect(hdev, ACL_LINK, dst,
					l2cap_pi(sk)->sec_level, auth_type);

L
Linus Torvalds 已提交
866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885
	if (!hcon)
		goto done;

	conn = l2cap_conn_add(hcon, 0);
	if (!conn) {
		hci_conn_put(hcon);
		goto done;
	}

	err = 0;

	/* Update source addr of the socket */
	bacpy(src, conn->src);

	l2cap_chan_add(conn, sk, NULL);

	sk->sk_state = BT_CONNECT;
	l2cap_sock_set_timer(sk, sk->sk_sndtimeo);

	if (hcon->state == BT_CONNECTED) {
886 887
		if (sk->sk_type != SOCK_SEQPACKET &&
				sk->sk_type != SOCK_STREAM) {
L
Linus Torvalds 已提交
888
			l2cap_sock_clear_timer(sk);
889 890
			if (l2cap_check_security(sk))
				sk->sk_state = BT_CONNECTED;
891 892
		} else
			l2cap_do_start(sk);
L
Linus Torvalds 已提交
893 894 895 896 897 898 899 900
	}

done:
	hci_dev_unlock_bh(hdev);
	hci_dev_put(hdev);
	return err;
}

901
int __l2cap_wait_ack(struct sock *sk)
902 903 904 905 906
{
	DECLARE_WAITQUEUE(wait, current);
	int err = 0;
	int timeo = HZ/5;

907
	add_wait_queue(sk_sleep(sk), &wait);
908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927
	while ((l2cap_pi(sk)->unacked_frames > 0 && l2cap_pi(sk)->conn)) {
		set_current_state(TASK_INTERRUPTIBLE);

		if (!timeo)
			timeo = HZ/5;

		if (signal_pending(current)) {
			err = sock_intr_errno(timeo);
			break;
		}

		release_sock(sk);
		timeo = schedule_timeout(timeo);
		lock_sock(sk);

		err = sock_error(sk);
		if (err)
			break;
	}
	set_current_state(TASK_RUNNING);
928
	remove_wait_queue(sk_sleep(sk), &wait);
929 930 931
	return err;
}

932 933 934 935
static void l2cap_monitor_timeout(unsigned long arg)
{
	struct sock *sk = (void *) arg;

936 937
	BT_DBG("sk %p", sk);

938
	bh_lock_sock(sk);
939
	if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
940
		l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk, ECONNABORTED);
941
		bh_unlock_sock(sk);
942 943 944 945 946 947
		return;
	}

	l2cap_pi(sk)->retry_count++;
	__mod_monitor_timer();

948
	l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
949
	bh_unlock_sock(sk);
950 951 952 953 954 955
}

static void l2cap_retrans_timeout(unsigned long arg)
{
	struct sock *sk = (void *) arg;

956 957
	BT_DBG("sk %p", sk);

958
	bh_lock_sock(sk);
959 960 961 962 963
	l2cap_pi(sk)->retry_count = 1;
	__mod_monitor_timer();

	l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;

964
	l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
965
	bh_unlock_sock(sk);
966 967
}

968
static void l2cap_drop_acked_frames(struct sock *sk)
L
Linus Torvalds 已提交
969
{
970
	struct sk_buff *skb;
L
Linus Torvalds 已提交
971

972 973
	while ((skb = skb_peek(TX_QUEUE(sk))) &&
			l2cap_pi(sk)->unacked_frames) {
974 975
		if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
			break;
L
Linus Torvalds 已提交
976

977 978
		skb = skb_dequeue(TX_QUEUE(sk));
		kfree_skb(skb);
L
Linus Torvalds 已提交
979

980 981
		l2cap_pi(sk)->unacked_frames--;
	}
L
Linus Torvalds 已提交
982

983 984
	if (!l2cap_pi(sk)->unacked_frames)
		del_timer(&l2cap_pi(sk)->retrans_timer);
985
}
L
Linus Torvalds 已提交
986

987
void l2cap_do_send(struct sock *sk, struct sk_buff *skb)
988 989
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
990 991
	struct hci_conn *hcon = pi->conn->hcon;
	u16 flags;
992 993

	BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
L
Linus Torvalds 已提交
994

995 996 997 998 999 1000
	if (!pi->flushable && lmp_no_flush_capable(hcon->hdev))
		flags = ACL_START_NO_FLUSH;
	else
		flags = ACL_START;

	hci_send_acl(hcon, skb, flags);
1001 1002
}

1003
void l2cap_streaming_send(struct sock *sk)
1004
{
1005
	struct sk_buff *skb;
1006
	struct l2cap_pinfo *pi = l2cap_pi(sk);
1007
	u16 control, fcs;
1008

1009 1010
	while ((skb = skb_dequeue(TX_QUEUE(sk)))) {
		control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE);
1011
		control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
1012
		put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE);
1013

1014
		if (pi->fcs == L2CAP_FCS_CRC16) {
1015 1016
			fcs = crc16(0, (u8 *)skb->data, skb->len - 2);
			put_unaligned_le16(fcs, skb->data + skb->len - 2);
1017 1018
		}

1019
		l2cap_do_send(sk, skb);
1020 1021 1022 1023 1024

		pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
	}
}

1025
static void l2cap_retransmit_one_frame(struct sock *sk, u8 tx_seq)
1026 1027 1028 1029 1030 1031
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct sk_buff *skb, *tx_skb;
	u16 control, fcs;

	skb = skb_peek(TX_QUEUE(sk));
1032 1033
	if (!skb)
		return;
1034

1035 1036
	do {
		if (bt_cb(skb)->tx_seq == tx_seq)
1037 1038
			break;

1039 1040
		if (skb_queue_is_last(TX_QUEUE(sk), skb))
			return;
1041

1042
	} while ((skb = skb_queue_next(TX_QUEUE(sk), skb)));
1043

1044 1045
	if (pi->remote_max_tx &&
			bt_cb(skb)->retries == pi->remote_max_tx) {
1046
		l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED);
1047 1048 1049 1050 1051 1052
		return;
	}

	tx_skb = skb_clone(skb, GFP_ATOMIC);
	bt_cb(skb)->retries++;
	control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1053 1054 1055 1056 1057

	if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
		control |= L2CAP_CTRL_FINAL;
		pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
	}
1058

1059 1060
	control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
			| (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1061

1062 1063 1064 1065 1066 1067 1068 1069
	put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);

	if (pi->fcs == L2CAP_FCS_CRC16) {
		fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
		put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
	}

	l2cap_do_send(sk, tx_skb);
1070 1071
}

1072
int l2cap_ertm_send(struct sock *sk)
1073 1074 1075
{
	struct sk_buff *skb, *tx_skb;
	struct l2cap_pinfo *pi = l2cap_pi(sk);
1076
	u16 control, fcs;
1077
	int nsent = 0;
1078

1079 1080
	if (sk->sk_state != BT_CONNECTED)
		return -ENOTCONN;
1081

1082
	while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
1083

1084 1085
		if (pi->remote_max_tx &&
				bt_cb(skb)->retries == pi->remote_max_tx) {
1086
			l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED);
1087 1088 1089
			break;
		}

1090 1091
		tx_skb = skb_clone(skb, GFP_ATOMIC);

1092 1093
		bt_cb(skb)->retries++;

1094
		control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1095 1096
		control &= L2CAP_CTRL_SAR;

1097 1098 1099 1100
		if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
			control |= L2CAP_CTRL_FINAL;
			pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
		}
1101
		control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1102 1103 1104
				| (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
		put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);

1105

1106
		if (pi->fcs == L2CAP_FCS_CRC16) {
1107 1108 1109 1110
			fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
			put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
		}

1111 1112
		l2cap_do_send(sk, tx_skb);

1113
		__mod_retrans_timer();
1114 1115 1116 1117 1118

		bt_cb(skb)->tx_seq = pi->next_tx_seq;
		pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;

		pi->unacked_frames++;
1119
		pi->frames_sent++;
1120 1121 1122 1123 1124

		if (skb_queue_is_last(TX_QUEUE(sk), skb))
			sk->sk_send_head = NULL;
		else
			sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
1125 1126

		nsent++;
1127 1128
	}

1129 1130 1131
	return nsent;
}

1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144
static int l2cap_retransmit_frames(struct sock *sk)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	int ret;

	if (!skb_queue_empty(TX_QUEUE(sk)))
		sk->sk_send_head = TX_QUEUE(sk)->next;

	pi->next_tx_seq = pi->expected_ack_seq;
	ret = l2cap_ertm_send(sk);
	return ret;
}

1145
static void l2cap_send_ack(struct l2cap_pinfo *pi)
1146 1147 1148 1149 1150 1151 1152 1153
{
	struct sock *sk = (struct sock *)pi;
	u16 control = 0;

	control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;

	if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
		control |= L2CAP_SUPER_RCV_NOT_READY;
1154
		pi->conn_state |= L2CAP_CONN_RNR_SENT;
1155 1156
		l2cap_send_sframe(pi, control);
		return;
1157
	}
1158

1159
	if (l2cap_ertm_send(sk) > 0)
1160 1161 1162 1163
		return;

	control |= L2CAP_SUPER_RCV_READY;
	l2cap_send_sframe(pi, control);
1164 1165
}

1166
static void l2cap_send_srejtail(struct sock *sk)
1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179
{
	struct srej_list *tail;
	u16 control;

	control = L2CAP_SUPER_SELECT_REJECT;
	control |= L2CAP_CTRL_FINAL;

	tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list);
	control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;

	l2cap_send_sframe(l2cap_pi(sk), control);
}

1180 1181 1182 1183 1184
static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	struct sk_buff **frag;
	int err, sent = 0;
L
Linus Torvalds 已提交
1185

1186
	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1187
		return -EFAULT;
L
Linus Torvalds 已提交
1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198

	sent += count;
	len  -= count;

	/* Continuation fragments (no L2CAP header) */
	frag = &skb_shinfo(skb)->frag_list;
	while (len) {
		count = min_t(unsigned int, conn->mtu, len);

		*frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
		if (!*frag)
1199
			return err;
1200 1201
		if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
			return -EFAULT;
L
Linus Torvalds 已提交
1202 1203 1204 1205 1206 1207 1208 1209

		sent += count;
		len  -= count;

		frag = &(*frag)->next;
	}

	return sent;
1210
}
L
Linus Torvalds 已提交
1211

1212
struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	struct sk_buff *skb;
	int err, count, hlen = L2CAP_HDR_SIZE + 2;
	struct l2cap_hdr *lh;

	BT_DBG("sk %p len %d", sk, (int)len);

	count = min_t(unsigned int, (conn->mtu - hlen), len);
	skb = bt_skb_send_alloc(sk, count + hlen,
			msg->msg_flags & MSG_DONTWAIT, &err);
	if (!skb)
1225
		return ERR_PTR(err);
1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240

	/* Create L2CAP header */
	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
	put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));

	err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
	if (unlikely(err < 0)) {
		kfree_skb(skb);
		return ERR_PTR(err);
	}
	return skb;
}

1241
struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	struct sk_buff *skb;
	int err, count, hlen = L2CAP_HDR_SIZE;
	struct l2cap_hdr *lh;

	BT_DBG("sk %p len %d", sk, (int)len);

	count = min_t(unsigned int, (conn->mtu - hlen), len);
	skb = bt_skb_send_alloc(sk, count + hlen,
			msg->msg_flags & MSG_DONTWAIT, &err);
	if (!skb)
1254
		return ERR_PTR(err);
1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267 1268

	/* Create L2CAP header */
	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));

	err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
	if (unlikely(err < 0)) {
		kfree_skb(skb);
		return ERR_PTR(err);
	}
	return skb;
}

1269
struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1270 1271 1272 1273 1274 1275 1276 1277
{
	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
	struct sk_buff *skb;
	int err, count, hlen = L2CAP_HDR_SIZE + 2;
	struct l2cap_hdr *lh;

	BT_DBG("sk %p len %d", sk, (int)len);

1278 1279 1280
	if (!conn)
		return ERR_PTR(-ENOTCONN);

1281 1282 1283
	if (sdulen)
		hlen += 2;

1284 1285 1286
	if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
		hlen += 2;

1287 1288 1289 1290
	count = min_t(unsigned int, (conn->mtu - hlen), len);
	skb = bt_skb_send_alloc(sk, count + hlen,
			msg->msg_flags & MSG_DONTWAIT, &err);
	if (!skb)
1291
		return ERR_PTR(err);
1292 1293 1294 1295 1296 1297

	/* Create L2CAP header */
	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
	put_unaligned_le16(control, skb_put(skb, 2));
1298 1299
	if (sdulen)
		put_unaligned_le16(sdulen, skb_put(skb, 2));
1300 1301 1302 1303 1304 1305

	err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
	if (unlikely(err < 0)) {
		kfree_skb(skb);
		return ERR_PTR(err);
	}
1306

1307 1308 1309
	if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
		put_unaligned_le16(0, skb_put(skb, 2));

1310
	bt_cb(skb)->retries = 0;
1311
	return skb;
L
Linus Torvalds 已提交
1312 1313
}

1314
int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
1315 1316 1317 1318 1319 1320 1321
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct sk_buff *skb;
	struct sk_buff_head sar_queue;
	u16 control;
	size_t size = 0;

1322
	skb_queue_head_init(&sar_queue);
1323
	control = L2CAP_SDU_START;
1324
	skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len);
1325 1326 1327 1328
	if (IS_ERR(skb))
		return PTR_ERR(skb);

	__skb_queue_tail(&sar_queue, skb);
1329 1330
	len -= pi->remote_mps;
	size += pi->remote_mps;
1331 1332 1333 1334

	while (len > 0) {
		size_t buflen;

1335
		if (len > pi->remote_mps) {
1336
			control = L2CAP_SDU_CONTINUE;
1337
			buflen = pi->remote_mps;
1338
		} else {
1339
			control = L2CAP_SDU_END;
1340 1341 1342
			buflen = len;
		}

1343
		skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359
		if (IS_ERR(skb)) {
			skb_queue_purge(&sar_queue);
			return PTR_ERR(skb);
		}

		__skb_queue_tail(&sar_queue, skb);
		len -= buflen;
		size += buflen;
	}
	skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
	if (sk->sk_send_head == NULL)
		sk->sk_send_head = sar_queue.next;

	return size;
}

L
Linus Torvalds 已提交
1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387
static void l2cap_chan_ready(struct sock *sk)
{
	struct sock *parent = bt_sk(sk)->parent;

	BT_DBG("sk %p, parent %p", sk, parent);

	l2cap_pi(sk)->conf_state = 0;
	l2cap_sock_clear_timer(sk);

	if (!parent) {
		/* Outgoing channel.
		 * Wake up socket sleeping on connect.
		 */
		sk->sk_state = BT_CONNECTED;
		sk->sk_state_change(sk);
	} else {
		/* Incoming channel.
		 * Wake up socket sleeping on accept.
		 */
		parent->sk_data_ready(parent, 0);
	}
}

/* Copy frame to all raw sockets on that connection */
static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct l2cap_chan_list *l = &conn->chan_list;
	struct sk_buff *nskb;
1388
	struct sock *sk;
L
Linus Torvalds 已提交
1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399

	BT_DBG("conn %p", conn);

	read_lock(&l->lock);
	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
		if (sk->sk_type != SOCK_RAW)
			continue;

		/* Don't send frame to the socket it came from */
		if (skb->sk == sk)
			continue;
1400 1401
		nskb = skb_clone(skb, GFP_ATOMIC);
		if (!nskb)
L
Linus Torvalds 已提交
1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414 1415 1416 1417 1418
			continue;

		if (sock_queue_rcv_skb(sk, nskb))
			kfree_skb(nskb);
	}
	read_unlock(&l->lock);
}

/* ---- L2CAP signalling commands ---- */
static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
				u8 code, u8 ident, u16 dlen, void *data)
{
	struct sk_buff *skb, **frag;
	struct l2cap_cmd_hdr *cmd;
	struct l2cap_hdr *lh;
	int len, count;

1419 1420
	BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
			conn, code, ident, dlen);
L
Linus Torvalds 已提交
1421 1422 1423 1424 1425 1426 1427 1428 1429

	len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
	count = min_t(unsigned int, conn->mtu, len);

	skb = bt_skb_alloc(count, GFP_ATOMIC);
	if (!skb)
		return NULL;

	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1430
	lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
1431 1432 1433 1434 1435

	if (conn->hcon->type == LE_LINK)
		lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
	else
		lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
L
Linus Torvalds 已提交
1436 1437 1438 1439

	cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
	cmd->code  = code;
	cmd->ident = ident;
1440
	cmd->len   = cpu_to_le16(dlen);
L
Linus Torvalds 已提交
1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465 1466 1467 1468 1469 1470 1471 1472 1473 1474 1475 1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490

	if (dlen) {
		count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
		memcpy(skb_put(skb, count), data, count);
		data += count;
	}

	len -= skb->len;

	/* Continuation fragments (no L2CAP header) */
	frag = &skb_shinfo(skb)->frag_list;
	while (len) {
		count = min_t(unsigned int, conn->mtu, len);

		*frag = bt_skb_alloc(count, GFP_ATOMIC);
		if (!*frag)
			goto fail;

		memcpy(skb_put(*frag, count), data, count);

		len  -= count;
		data += count;

		frag = &(*frag)->next;
	}

	return skb;

fail:
	kfree_skb(skb);
	return NULL;
}

static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
{
	struct l2cap_conf_opt *opt = *ptr;
	int len;

	len = L2CAP_CONF_OPT_SIZE + opt->len;
	*ptr += len;

	*type = opt->type;
	*olen = opt->len;

	switch (opt->len) {
	case 1:
		*val = *((u8 *) opt->val);
		break;

	case 2:
1491
		*val = get_unaligned_le16(opt->val);
L
Linus Torvalds 已提交
1492 1493 1494
		break;

	case 4:
1495
		*val = get_unaligned_le32(opt->val);
L
Linus Torvalds 已提交
1496 1497 1498 1499 1500 1501 1502 1503 1504 1505 1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520 1521
		break;

	default:
		*val = (unsigned long) opt->val;
		break;
	}

	BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
	return len;
}

static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
{
	struct l2cap_conf_opt *opt = *ptr;

	BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);

	opt->type = type;
	opt->len  = len;

	switch (len) {
	case 1:
		*((u8 *) opt->val)  = val;
		break;

	case 2:
1522
		put_unaligned_le16(val, opt->val);
L
Linus Torvalds 已提交
1523 1524 1525
		break;

	case 4:
1526
		put_unaligned_le32(val, opt->val);
L
Linus Torvalds 已提交
1527 1528 1529 1530 1531 1532 1533 1534 1535 1536
		break;

	default:
		memcpy(opt->val, (void *) val, len);
		break;
	}

	*ptr += L2CAP_CONF_OPT_SIZE + len;
}

1537 1538 1539 1540 1541 1542 1543 1544 1545
static void l2cap_ack_timeout(unsigned long arg)
{
	struct sock *sk = (void *) arg;

	bh_lock_sock(sk);
	l2cap_send_ack(l2cap_pi(sk));
	bh_unlock_sock(sk);
}

1546 1547 1548 1549 1550
static inline void l2cap_ertm_init(struct sock *sk)
{
	l2cap_pi(sk)->expected_ack_seq = 0;
	l2cap_pi(sk)->unacked_frames = 0;
	l2cap_pi(sk)->buffer_seq = 0;
1551
	l2cap_pi(sk)->num_acked = 0;
1552
	l2cap_pi(sk)->frames_sent = 0;
1553 1554 1555 1556 1557

	setup_timer(&l2cap_pi(sk)->retrans_timer,
			l2cap_retrans_timeout, (unsigned long) sk);
	setup_timer(&l2cap_pi(sk)->monitor_timer,
			l2cap_monitor_timeout, (unsigned long) sk);
1558 1559
	setup_timer(&l2cap_pi(sk)->ack_timer,
			l2cap_ack_timeout, (unsigned long) sk);
1560 1561

	__skb_queue_head_init(SREJ_QUEUE(sk));
1562 1563 1564
	__skb_queue_head_init(BUSY_QUEUE(sk));

	INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work);
1565 1566

	sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
1567 1568
}

1569 1570 1571 1572 1573 1574 1575 1576 1577 1578 1579 1580 1581
static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
{
	switch (mode) {
	case L2CAP_MODE_STREAMING:
	case L2CAP_MODE_ERTM:
		if (l2cap_mode_supported(mode, remote_feat_mask))
			return mode;
		/* fall through */
	default:
		return L2CAP_MODE_BASIC;
	}
}

1582
int l2cap_build_conf_req(struct sock *sk, void *data)
L
Linus Torvalds 已提交
1583 1584 1585
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct l2cap_conf_req *req = data;
1586
	struct l2cap_conf_rfc rfc = { .mode = pi->mode };
L
Linus Torvalds 已提交
1587 1588 1589 1590
	void *ptr = req->data;

	BT_DBG("sk %p", sk);

1591 1592 1593 1594 1595 1596
	if (pi->num_conf_req || pi->num_conf_rsp)
		goto done;

	switch (pi->mode) {
	case L2CAP_MODE_STREAMING:
	case L2CAP_MODE_ERTM:
1597
		if (pi->conf_state & L2CAP_CONF_STATE2_DEVICE)
1598 1599
			break;

1600
		/* fall through */
1601 1602 1603 1604 1605 1606
	default:
		pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
		break;
	}

done:
1607 1608 1609
	if (pi->imtu != L2CAP_DEFAULT_MTU)
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);

1610 1611
	switch (pi->mode) {
	case L2CAP_MODE_BASIC:
1612 1613 1614 1615
		if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) &&
				!(pi->conn->feat_mask & L2CAP_FEAT_STREAMING))
			break;

1616 1617 1618 1619 1620 1621 1622
		rfc.mode            = L2CAP_MODE_BASIC;
		rfc.txwin_size      = 0;
		rfc.max_transmit    = 0;
		rfc.retrans_timeout = 0;
		rfc.monitor_timeout = 0;
		rfc.max_pdu_size    = 0;

1623 1624
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
							(unsigned long) &rfc);
1625 1626 1627 1628
		break;

	case L2CAP_MODE_ERTM:
		rfc.mode            = L2CAP_MODE_ERTM;
1629
		rfc.txwin_size      = pi->tx_win;
1630
		rfc.max_transmit    = pi->max_tx;
1631 1632
		rfc.retrans_timeout = 0;
		rfc.monitor_timeout = 0;
1633
		rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1634
		if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
1635
			rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1636

1637 1638 1639
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
							(unsigned long) &rfc);

1640 1641 1642 1643 1644 1645 1646 1647
		if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
			break;

		if (pi->fcs == L2CAP_FCS_NONE ||
				pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
			pi->fcs = L2CAP_FCS_NONE;
			l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
		}
1648 1649 1650 1651 1652 1653 1654 1655
		break;

	case L2CAP_MODE_STREAMING:
		rfc.mode            = L2CAP_MODE_STREAMING;
		rfc.txwin_size      = 0;
		rfc.max_transmit    = 0;
		rfc.retrans_timeout = 0;
		rfc.monitor_timeout = 0;
1656
		rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
1657
		if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
1658
			rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1659

1660 1661 1662
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
							(unsigned long) &rfc);

1663 1664 1665 1666 1667 1668 1669 1670
		if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
			break;

		if (pi->fcs == L2CAP_FCS_NONE ||
				pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
			pi->fcs = L2CAP_FCS_NONE;
			l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
		}
1671 1672
		break;
	}
L
Linus Torvalds 已提交
1673

1674 1675
	req->dcid  = cpu_to_le16(pi->dcid);
	req->flags = cpu_to_le16(0);
L
Linus Torvalds 已提交
1676 1677 1678 1679

	return ptr - data;
}

1680
static int l2cap_parse_conf_req(struct sock *sk, void *data)
L
Linus Torvalds 已提交
1681 1682
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
1683 1684 1685 1686 1687 1688
	struct l2cap_conf_rsp *rsp = data;
	void *ptr = rsp->data;
	void *req = pi->conf_req;
	int len = pi->conf_len;
	int type, hint, olen;
	unsigned long val;
1689
	struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
1690
	u16 mtu = L2CAP_DEFAULT_MTU;
1691
	u16 result = L2CAP_CONF_SUCCESS;
L
Linus Torvalds 已提交
1692

1693
	BT_DBG("sk %p", sk);
1694

1695 1696
	while (len >= L2CAP_CONF_OPT_SIZE) {
		len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
L
Linus Torvalds 已提交
1697

1698
		hint  = type & L2CAP_CONF_HINT;
1699
		type &= L2CAP_CONF_MASK;
1700 1701 1702

		switch (type) {
		case L2CAP_CONF_MTU:
1703
			mtu = val;
1704 1705 1706 1707 1708 1709 1710 1711 1712
			break;

		case L2CAP_CONF_FLUSH_TO:
			pi->flush_to = val;
			break;

		case L2CAP_CONF_QOS:
			break;

1713 1714 1715 1716 1717
		case L2CAP_CONF_RFC:
			if (olen == sizeof(rfc))
				memcpy(&rfc, (void *) val, olen);
			break;

1718 1719 1720 1721 1722 1723
		case L2CAP_CONF_FCS:
			if (val == L2CAP_FCS_NONE)
				pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;

			break;

1724 1725 1726 1727 1728 1729 1730 1731 1732 1733
		default:
			if (hint)
				break;

			result = L2CAP_CONF_UNKNOWN;
			*((u8 *) ptr++) = type;
			break;
		}
	}

1734
	if (pi->num_conf_rsp || pi->num_conf_req > 1)
1735 1736 1737 1738 1739
		goto done;

	switch (pi->mode) {
	case L2CAP_MODE_STREAMING:
	case L2CAP_MODE_ERTM:
1740 1741 1742 1743 1744 1745
		if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
			pi->mode = l2cap_select_mode(rfc.mode,
					pi->conn->feat_mask);
			break;
		}

1746
		if (pi->mode != rfc.mode)
1747
			return -ECONNREFUSED;
1748

1749 1750 1751 1752 1753 1754 1755 1756 1757 1758 1759 1760 1761 1762 1763 1764
		break;
	}

done:
	if (pi->mode != rfc.mode) {
		result = L2CAP_CONF_UNACCEPT;
		rfc.mode = pi->mode;

		if (pi->num_conf_rsp == 1)
			return -ECONNREFUSED;

		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
					sizeof(rfc), (unsigned long) &rfc);
	}


1765 1766 1767 1768
	if (result == L2CAP_CONF_SUCCESS) {
		/* Configure output options and let the other side know
		 * which ones we don't like. */

1769 1770 1771 1772 1773 1774 1775
		if (mtu < L2CAP_DEFAULT_MIN_MTU)
			result = L2CAP_CONF_UNACCEPT;
		else {
			pi->omtu = mtu;
			pi->conf_state |= L2CAP_CONF_MTU_DONE;
		}
		l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
1776

1777 1778 1779 1780 1781 1782 1783 1784 1785
		switch (rfc.mode) {
		case L2CAP_MODE_BASIC:
			pi->fcs = L2CAP_FCS_NONE;
			pi->conf_state |= L2CAP_CONF_MODE_DONE;
			break;

		case L2CAP_MODE_ERTM:
			pi->remote_tx_win = rfc.txwin_size;
			pi->remote_max_tx = rfc.max_transmit;
1786 1787 1788

			if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
				rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1789 1790

			pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1791

1792 1793 1794 1795
			rfc.retrans_timeout =
				le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
			rfc.monitor_timeout =
				le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
1796 1797

			pi->conf_state |= L2CAP_CONF_MODE_DONE;
1798 1799 1800 1801

			l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
					sizeof(rfc), (unsigned long) &rfc);

1802 1803 1804
			break;

		case L2CAP_MODE_STREAMING:
1805 1806
			if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
				rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
1807 1808

			pi->remote_mps = le16_to_cpu(rfc.max_pdu_size);
1809 1810

			pi->conf_state |= L2CAP_CONF_MODE_DONE;
1811 1812 1813 1814

			l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
					sizeof(rfc), (unsigned long) &rfc);

1815 1816 1817
			break;

		default:
1818 1819
			result = L2CAP_CONF_UNACCEPT;

1820
			memset(&rfc, 0, sizeof(rfc));
1821 1822
			rfc.mode = pi->mode;
		}
1823

1824 1825 1826
		if (result == L2CAP_CONF_SUCCESS)
			pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
	}
1827 1828 1829 1830 1831
	rsp->scid   = cpu_to_le16(pi->dcid);
	rsp->result = cpu_to_le16(result);
	rsp->flags  = cpu_to_le16(0x0000);

	return ptr - data;
L
Linus Torvalds 已提交
1832 1833
}

1834 1835 1836 1837 1838 1839 1840 1841 1842 1843 1844 1845 1846 1847 1848 1849 1850 1851
static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct l2cap_conf_req *req = data;
	void *ptr = req->data;
	int type, olen;
	unsigned long val;
	struct l2cap_conf_rfc rfc;

	BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);

	while (len >= L2CAP_CONF_OPT_SIZE) {
		len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);

		switch (type) {
		case L2CAP_CONF_MTU:
			if (val < L2CAP_DEFAULT_MIN_MTU) {
				*result = L2CAP_CONF_UNACCEPT;
1852
				pi->imtu = L2CAP_DEFAULT_MIN_MTU;
1853
			} else
1854 1855
				pi->imtu = val;
			l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
1856 1857 1858 1859 1860 1861 1862 1863 1864 1865 1866 1867 1868 1869 1870 1871 1872 1873 1874 1875 1876 1877 1878 1879
			break;

		case L2CAP_CONF_FLUSH_TO:
			pi->flush_to = val;
			l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
							2, pi->flush_to);
			break;

		case L2CAP_CONF_RFC:
			if (olen == sizeof(rfc))
				memcpy(&rfc, (void *)val, olen);

			if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
							rfc.mode != pi->mode)
				return -ECONNREFUSED;

			pi->fcs = 0;

			l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
					sizeof(rfc), (unsigned long) &rfc);
			break;
		}
	}

1880 1881 1882 1883 1884
	if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode)
		return -ECONNREFUSED;

	pi->mode = rfc.mode;

1885 1886 1887
	if (*result == L2CAP_CONF_SUCCESS) {
		switch (rfc.mode) {
		case L2CAP_MODE_ERTM:
1888 1889
			pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
			pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1890
			pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1891 1892
			break;
		case L2CAP_MODE_STREAMING:
1893
			pi->mps    = le16_to_cpu(rfc.max_pdu_size);
1894 1895 1896 1897 1898 1899 1900 1901 1902
		}
	}

	req->dcid   = cpu_to_le16(pi->dcid);
	req->flags  = cpu_to_le16(0x0000);

	return ptr - data;
}

1903
static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
L
Linus Torvalds 已提交
1904 1905 1906 1907
{
	struct l2cap_conf_rsp *rsp = data;
	void *ptr = rsp->data;

1908
	BT_DBG("sk %p", sk);
L
Linus Torvalds 已提交
1909

1910
	rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
1911
	rsp->result = cpu_to_le16(result);
1912
	rsp->flags  = cpu_to_le16(flags);
L
Linus Torvalds 已提交
1913 1914 1915 1916

	return ptr - data;
}

1917 1918 1919 1920 1921 1922 1923 1924 1925 1926 1927 1928 1929 1930 1931 1932 1933 1934 1935 1936 1937 1938 1939 1940 1941 1942
static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	int type, olen;
	unsigned long val;
	struct l2cap_conf_rfc rfc;

	BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len);

	if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING))
		return;

	while (len >= L2CAP_CONF_OPT_SIZE) {
		len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);

		switch (type) {
		case L2CAP_CONF_RFC:
			if (olen == sizeof(rfc))
				memcpy(&rfc, (void *)val, olen);
			goto done;
		}
	}

done:
	switch (rfc.mode) {
	case L2CAP_MODE_ERTM:
1943 1944
		pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
		pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1945 1946 1947 1948 1949 1950 1951
		pi->mps    = le16_to_cpu(rfc.max_pdu_size);
		break;
	case L2CAP_MODE_STREAMING:
		pi->mps    = le16_to_cpu(rfc.max_pdu_size);
	}
}

1952 1953 1954 1955 1956 1957 1958 1959 1960 1961
static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;

	if (rej->reason != 0x0000)
		return 0;

	if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
					cmd->ident == conn->info_ident) {
		del_timer(&conn->info_timer);
1962 1963

		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
1964
		conn->info_ident = 0;
1965

1966 1967 1968 1969 1970 1971
		l2cap_conn_start(conn);
	}

	return 0;
}

L
Linus Torvalds 已提交
1972 1973 1974 1975 1976
static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_chan_list *list = &conn->chan_list;
	struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
	struct l2cap_conn_rsp rsp;
1977
	struct sock *parent, *sk = NULL;
1978
	int result, status = L2CAP_CS_NO_INFO;
L
Linus Torvalds 已提交
1979 1980

	u16 dcid = 0, scid = __le16_to_cpu(req->scid);
1981
	__le16 psm = req->psm;
L
Linus Torvalds 已提交
1982 1983 1984 1985 1986 1987 1988 1989 1990 1991

	BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);

	/* Check if we have socket listening on psm */
	parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
	if (!parent) {
		result = L2CAP_CR_BAD_PSM;
		goto sendresp;
	}

1992 1993
	bh_lock_sock(parent);

1994 1995 1996
	/* Check if the ACL is secure enough (if not SDP) */
	if (psm != cpu_to_le16(0x0001) &&
				!hci_conn_check_link_mode(conn->hcon)) {
1997
		conn->disc_reason = 0x05;
1998 1999 2000 2001
		result = L2CAP_CR_SEC_BLOCK;
		goto response;
	}

L
Linus Torvalds 已提交
2002 2003 2004 2005
	result = L2CAP_CR_NO_MEM;

	/* Check for backlog size */
	if (sk_acceptq_is_full(parent)) {
2006
		BT_DBG("backlog full %d", parent->sk_ack_backlog);
L
Linus Torvalds 已提交
2007 2008 2009
		goto response;
	}

2010
	sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
L
Linus Torvalds 已提交
2011 2012 2013
	if (!sk)
		goto response;

2014
	write_lock_bh(&list->lock);
L
Linus Torvalds 已提交
2015 2016 2017

	/* Check if we already have channel with that dcid */
	if (__l2cap_get_chan_by_dcid(list, scid)) {
2018
		write_unlock_bh(&list->lock);
L
Linus Torvalds 已提交
2019 2020 2021 2022 2023 2024 2025 2026 2027 2028 2029 2030 2031 2032 2033 2034 2035 2036 2037 2038
		sock_set_flag(sk, SOCK_ZAPPED);
		l2cap_sock_kill(sk);
		goto response;
	}

	hci_conn_hold(conn->hcon);

	l2cap_sock_init(sk, parent);
	bacpy(&bt_sk(sk)->src, conn->src);
	bacpy(&bt_sk(sk)->dst, conn->dst);
	l2cap_pi(sk)->psm  = psm;
	l2cap_pi(sk)->dcid = scid;

	__l2cap_chan_add(conn, sk, parent);
	dcid = l2cap_pi(sk)->scid;

	l2cap_sock_set_timer(sk, sk->sk_sndtimeo);

	l2cap_pi(sk)->ident = cmd->ident;

2039
	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
2040
		if (l2cap_check_security(sk)) {
2041 2042 2043 2044 2045 2046 2047 2048 2049 2050
			if (bt_sk(sk)->defer_setup) {
				sk->sk_state = BT_CONNECT2;
				result = L2CAP_CR_PEND;
				status = L2CAP_CS_AUTHOR_PEND;
				parent->sk_data_ready(parent, 0);
			} else {
				sk->sk_state = BT_CONFIG;
				result = L2CAP_CR_SUCCESS;
				status = L2CAP_CS_NO_INFO;
			}
2051 2052 2053 2054 2055 2056 2057 2058 2059
		} else {
			sk->sk_state = BT_CONNECT2;
			result = L2CAP_CR_PEND;
			status = L2CAP_CS_AUTHEN_PEND;
		}
	} else {
		sk->sk_state = BT_CONNECT2;
		result = L2CAP_CR_PEND;
		status = L2CAP_CS_NO_INFO;
L
Linus Torvalds 已提交
2060 2061
	}

2062
	write_unlock_bh(&list->lock);
L
Linus Torvalds 已提交
2063 2064 2065 2066 2067

response:
	bh_unlock_sock(parent);

sendresp:
2068 2069 2070 2071
	rsp.scid   = cpu_to_le16(scid);
	rsp.dcid   = cpu_to_le16(dcid);
	rsp.result = cpu_to_le16(result);
	rsp.status = cpu_to_le16(status);
L
Linus Torvalds 已提交
2072
	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2073 2074 2075 2076 2077 2078 2079 2080 2081 2082 2083 2084 2085 2086 2087

	if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
		struct l2cap_info_req info;
		info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);

		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
		conn->info_ident = l2cap_get_ident(conn);

		mod_timer(&conn->info_timer, jiffies +
					msecs_to_jiffies(L2CAP_INFO_TIMEOUT));

		l2cap_send_cmd(conn, conn->info_ident,
					L2CAP_INFO_REQ, sizeof(info), &info);
	}

2088
	if (sk && !(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) &&
2089 2090 2091 2092 2093 2094 2095 2096
				result == L2CAP_CR_SUCCESS) {
		u8 buf[128];
		l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
					l2cap_build_conf_req(sk, buf), buf);
		l2cap_pi(sk)->num_conf_req++;
	}

L
Linus Torvalds 已提交
2097 2098 2099 2100 2101 2102 2103 2104 2105 2106 2107 2108 2109 2110 2111 2112 2113 2114
	return 0;
}

static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
	u16 scid, dcid, result, status;
	struct sock *sk;
	u8 req[128];

	scid   = __le16_to_cpu(rsp->scid);
	dcid   = __le16_to_cpu(rsp->dcid);
	result = __le16_to_cpu(rsp->result);
	status = __le16_to_cpu(rsp->status);

	BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);

	if (scid) {
2115 2116
		sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
		if (!sk)
2117
			return -EFAULT;
L
Linus Torvalds 已提交
2118
	} else {
2119 2120
		sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
		if (!sk)
2121
			return -EFAULT;
L
Linus Torvalds 已提交
2122 2123 2124 2125 2126 2127 2128
	}

	switch (result) {
	case L2CAP_CR_SUCCESS:
		sk->sk_state = BT_CONFIG;
		l2cap_pi(sk)->ident = 0;
		l2cap_pi(sk)->dcid = dcid;
2129 2130
		l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;

2131 2132 2133 2134 2135
		if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)
			break;

		l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;

L
Linus Torvalds 已提交
2136 2137
		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
					l2cap_build_conf_req(sk, req), req);
2138
		l2cap_pi(sk)->num_conf_req++;
L
Linus Torvalds 已提交
2139 2140 2141
		break;

	case L2CAP_CR_PEND:
2142
		l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
L
Linus Torvalds 已提交
2143 2144 2145
		break;

	default:
2146 2147 2148 2149 2150 2151 2152 2153
		/* don't delete l2cap channel if sk is owned by user */
		if (sock_owned_by_user(sk)) {
			sk->sk_state = BT_DISCONN;
			l2cap_sock_clear_timer(sk);
			l2cap_sock_set_timer(sk, HZ / 5);
			break;
		}

L
Linus Torvalds 已提交
2154 2155 2156 2157 2158 2159 2160 2161
		l2cap_chan_del(sk, ECONNREFUSED);
		break;
	}

	bh_unlock_sock(sk);
	return 0;
}

2162 2163 2164 2165 2166 2167 2168 2169 2170 2171 2172
static inline void set_default_fcs(struct l2cap_pinfo *pi)
{
	/* FCS is enabled only in ERTM or streaming mode, if one or both
	 * sides request it.
	 */
	if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING)
		pi->fcs = L2CAP_FCS_NONE;
	else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV))
		pi->fcs = L2CAP_FCS_CRC16;
}

2173
static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
L
Linus Torvalds 已提交
2174 2175 2176 2177 2178
{
	struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
	u16 dcid, flags;
	u8 rsp[64];
	struct sock *sk;
2179
	int len;
L
Linus Torvalds 已提交
2180 2181 2182 2183 2184 2185

	dcid  = __le16_to_cpu(req->dcid);
	flags = __le16_to_cpu(req->flags);

	BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);

2186 2187
	sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
	if (!sk)
L
Linus Torvalds 已提交
2188 2189
		return -ENOENT;

2190 2191 2192 2193 2194 2195
	if (sk->sk_state != BT_CONFIG) {
		struct l2cap_cmd_rej rej;

		rej.reason = cpu_to_le16(0x0002);
		l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
				sizeof(rej), &rej);
2196
		goto unlock;
2197
	}
2198

2199
	/* Reject if config buffer is too small. */
2200
	len = cmd_len - sizeof(*req);
2201 2202 2203 2204 2205 2206 2207 2208 2209 2210
	if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
		l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
				l2cap_build_conf_rsp(sk, rsp,
					L2CAP_CONF_REJECT, flags), rsp);
		goto unlock;
	}

	/* Store config. */
	memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len);
	l2cap_pi(sk)->conf_len += len;
L
Linus Torvalds 已提交
2211 2212 2213 2214

	if (flags & 0x0001) {
		/* Incomplete config. Send empty response. */
		l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2215 2216
				l2cap_build_conf_rsp(sk, rsp,
					L2CAP_CONF_SUCCESS, 0x0001), rsp);
L
Linus Torvalds 已提交
2217 2218 2219 2220
		goto unlock;
	}

	/* Complete config. */
2221
	len = l2cap_parse_conf_req(sk, rsp);
2222
	if (len < 0) {
2223
		l2cap_send_disconn_req(conn, sk, ECONNRESET);
L
Linus Torvalds 已提交
2224
		goto unlock;
2225
	}
L
Linus Torvalds 已提交
2226

2227
	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
2228
	l2cap_pi(sk)->num_conf_rsp++;
2229 2230 2231 2232

	/* Reset config buffer. */
	l2cap_pi(sk)->conf_len = 0;

2233 2234 2235
	if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
		goto unlock;

L
Linus Torvalds 已提交
2236
	if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2237
		set_default_fcs(l2cap_pi(sk));
2238

L
Linus Torvalds 已提交
2239
		sk->sk_state = BT_CONNECTED;
2240

2241 2242
		l2cap_pi(sk)->next_tx_seq = 0;
		l2cap_pi(sk)->expected_tx_seq = 0;
2243
		__skb_queue_head_init(TX_QUEUE(sk));
2244 2245 2246
		if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
			l2cap_ertm_init(sk);

L
Linus Torvalds 已提交
2247
		l2cap_chan_ready(sk);
2248 2249 2250 2251
		goto unlock;
	}

	if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
2252
		u8 buf[64];
2253
		l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
L
Linus Torvalds 已提交
2254
		l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2255
					l2cap_build_conf_req(sk, buf), buf);
2256
		l2cap_pi(sk)->num_conf_req++;
L
Linus Torvalds 已提交
2257 2258 2259 2260 2261 2262 2263 2264 2265 2266 2267 2268
	}

unlock:
	bh_unlock_sock(sk);
	return 0;
}

static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
	u16 scid, flags, result;
	struct sock *sk;
2269
	int len = cmd->len - sizeof(*rsp);
L
Linus Torvalds 已提交
2270 2271 2272 2273 2274

	scid   = __le16_to_cpu(rsp->scid);
	flags  = __le16_to_cpu(rsp->flags);
	result = __le16_to_cpu(rsp->result);

2275 2276
	BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
			scid, flags, result);
L
Linus Torvalds 已提交
2277

2278 2279
	sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
	if (!sk)
L
Linus Torvalds 已提交
2280 2281 2282 2283
		return 0;

	switch (result) {
	case L2CAP_CONF_SUCCESS:
2284
		l2cap_conf_rfc_get(sk, rsp->data, len);
L
Linus Torvalds 已提交
2285 2286 2287
		break;

	case L2CAP_CONF_UNACCEPT:
2288 2289 2290
		if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
			char req[64];

2291
			if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
2292
				l2cap_send_disconn_req(conn, sk, ECONNRESET);
2293 2294 2295
				goto done;
			}

2296 2297 2298 2299 2300
			/* throw out any old stored conf requests */
			result = L2CAP_CONF_SUCCESS;
			len = l2cap_parse_conf_rsp(sk, rsp->data,
							len, req, &result);
			if (len < 0) {
2301
				l2cap_send_disconn_req(conn, sk, ECONNRESET);
2302 2303 2304 2305 2306 2307 2308 2309 2310
				goto done;
			}

			l2cap_send_cmd(conn, l2cap_get_ident(conn),
						L2CAP_CONF_REQ, len, req);
			l2cap_pi(sk)->num_conf_req++;
			if (result != L2CAP_CONF_SUCCESS)
				goto done;
			break;
L
Linus Torvalds 已提交
2311 2312
		}

2313
	default:
2314
		sk->sk_err = ECONNRESET;
L
Linus Torvalds 已提交
2315
		l2cap_sock_set_timer(sk, HZ * 5);
2316
		l2cap_send_disconn_req(conn, sk, ECONNRESET);
L
Linus Torvalds 已提交
2317 2318 2319 2320 2321 2322 2323 2324 2325
		goto done;
	}

	if (flags & 0x01)
		goto done;

	l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;

	if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2326
		set_default_fcs(l2cap_pi(sk));
2327

L
Linus Torvalds 已提交
2328
		sk->sk_state = BT_CONNECTED;
2329
		l2cap_pi(sk)->next_tx_seq = 0;
2330 2331
		l2cap_pi(sk)->expected_tx_seq = 0;
		__skb_queue_head_init(TX_QUEUE(sk));
2332 2333 2334
		if (l2cap_pi(sk)->mode ==  L2CAP_MODE_ERTM)
			l2cap_ertm_init(sk);

L
Linus Torvalds 已提交
2335 2336 2337 2338 2339 2340 2341 2342 2343 2344 2345 2346 2347 2348 2349 2350 2351 2352 2353 2354
		l2cap_chan_ready(sk);
	}

done:
	bh_unlock_sock(sk);
	return 0;
}

static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
	struct l2cap_disconn_rsp rsp;
	u16 dcid, scid;
	struct sock *sk;

	scid = __le16_to_cpu(req->scid);
	dcid = __le16_to_cpu(req->dcid);

	BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);

2355 2356
	sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid);
	if (!sk)
L
Linus Torvalds 已提交
2357 2358
		return 0;

2359 2360
	rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
	rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
L
Linus Torvalds 已提交
2361 2362 2363 2364
	l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);

	sk->sk_shutdown = SHUTDOWN_MASK;

2365 2366 2367 2368 2369 2370 2371 2372 2373
	/* don't delete l2cap channel if sk is owned by user */
	if (sock_owned_by_user(sk)) {
		sk->sk_state = BT_DISCONN;
		l2cap_sock_clear_timer(sk);
		l2cap_sock_set_timer(sk, HZ / 5);
		bh_unlock_sock(sk);
		return 0;
	}

L
Linus Torvalds 已提交
2374 2375 2376 2377 2378 2379 2380 2381 2382 2383 2384 2385 2386 2387 2388 2389 2390 2391
	l2cap_chan_del(sk, ECONNRESET);
	bh_unlock_sock(sk);

	l2cap_sock_kill(sk);
	return 0;
}

static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
	u16 dcid, scid;
	struct sock *sk;

	scid = __le16_to_cpu(rsp->scid);
	dcid = __le16_to_cpu(rsp->dcid);

	BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);

2392 2393
	sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
	if (!sk)
L
Linus Torvalds 已提交
2394 2395
		return 0;

2396 2397 2398 2399 2400 2401 2402 2403 2404
	/* don't delete l2cap channel if sk is owned by user */
	if (sock_owned_by_user(sk)) {
		sk->sk_state = BT_DISCONN;
		l2cap_sock_clear_timer(sk);
		l2cap_sock_set_timer(sk, HZ / 5);
		bh_unlock_sock(sk);
		return 0;
	}

L
Linus Torvalds 已提交
2405 2406 2407 2408 2409 2410 2411 2412 2413 2414 2415 2416 2417 2418 2419 2420
	l2cap_chan_del(sk, 0);
	bh_unlock_sock(sk);

	l2cap_sock_kill(sk);
	return 0;
}

static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_info_req *req = (struct l2cap_info_req *) data;
	u16 type;

	type = __le16_to_cpu(req->type);

	BT_DBG("type 0x%4.4x", type);

2421 2422
	if (type == L2CAP_IT_FEAT_MASK) {
		u8 buf[8];
2423
		u32 feat_mask = l2cap_feat_mask;
2424 2425 2426
		struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
		rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
		rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2427
		if (!disable_ertm)
2428 2429
			feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
							 | L2CAP_FEAT_FCS;
2430
		put_unaligned_le32(feat_mask, rsp->data);
2431 2432
		l2cap_send_cmd(conn, cmd->ident,
					L2CAP_INFO_RSP, sizeof(buf), buf);
2433 2434 2435 2436 2437 2438 2439 2440
	} else if (type == L2CAP_IT_FIXED_CHAN) {
		u8 buf[12];
		struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
		rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
		rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
		memcpy(buf + 4, l2cap_fixed_chan, 8);
		l2cap_send_cmd(conn, cmd->ident,
					L2CAP_INFO_RSP, sizeof(buf), buf);
2441 2442 2443 2444 2445 2446 2447
	} else {
		struct l2cap_info_rsp rsp;
		rsp.type   = cpu_to_le16(type);
		rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
		l2cap_send_cmd(conn, cmd->ident,
					L2CAP_INFO_RSP, sizeof(rsp), &rsp);
	}
L
Linus Torvalds 已提交
2448 2449 2450 2451 2452 2453 2454 2455 2456 2457 2458 2459 2460 2461

	return 0;
}

static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
	u16 type, result;

	type   = __le16_to_cpu(rsp->type);
	result = __le16_to_cpu(rsp->result);

	BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);

2462 2463
	del_timer(&conn->info_timer);

2464 2465 2466 2467 2468 2469 2470 2471 2472
	if (result != L2CAP_IR_SUCCESS) {
		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
		conn->info_ident = 0;

		l2cap_conn_start(conn);

		return 0;
	}

2473
	if (type == L2CAP_IT_FEAT_MASK) {
2474
		conn->feat_mask = get_unaligned_le32(rsp->data);
2475

2476
		if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
2477 2478 2479 2480 2481 2482 2483 2484 2485 2486 2487 2488 2489 2490
			struct l2cap_info_req req;
			req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);

			conn->info_ident = l2cap_get_ident(conn);

			l2cap_send_cmd(conn, conn->info_ident,
					L2CAP_INFO_REQ, sizeof(req), &req);
		} else {
			conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
			conn->info_ident = 0;

			l2cap_conn_start(conn);
		}
	} else if (type == L2CAP_IT_FIXED_CHAN) {
2491
		conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2492
		conn->info_ident = 0;
2493 2494 2495

		l2cap_conn_start(conn);
	}
2496

L
Linus Torvalds 已提交
2497 2498 2499
	return 0;
}

2500
static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2501 2502 2503 2504 2505 2506 2507 2508 2509 2510 2511 2512 2513 2514 2515 2516 2517 2518 2519 2520 2521 2522 2523 2524 2525 2526 2527
							u16 to_multiplier)
{
	u16 max_latency;

	if (min > max || min < 6 || max > 3200)
		return -EINVAL;

	if (to_multiplier < 10 || to_multiplier > 3200)
		return -EINVAL;

	if (max >= to_multiplier * 8)
		return -EINVAL;

	max_latency = (to_multiplier * 8 / max) - 1;
	if (latency > 499 || latency > max_latency)
		return -EINVAL;

	return 0;
}

static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
					struct l2cap_cmd_hdr *cmd, u8 *data)
{
	struct hci_conn *hcon = conn->hcon;
	struct l2cap_conn_param_update_req *req;
	struct l2cap_conn_param_update_rsp rsp;
	u16 min, max, latency, to_multiplier, cmd_len;
2528
	int err;
2529 2530 2531 2532 2533 2534 2535 2536 2537

	if (!(hcon->link_mode & HCI_LM_MASTER))
		return -EINVAL;

	cmd_len = __le16_to_cpu(cmd->len);
	if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
		return -EPROTO;

	req = (struct l2cap_conn_param_update_req *) data;
2538 2539
	min		= __le16_to_cpu(req->min);
	max		= __le16_to_cpu(req->max);
2540 2541 2542 2543 2544 2545 2546
	latency		= __le16_to_cpu(req->latency);
	to_multiplier	= __le16_to_cpu(req->to_multiplier);

	BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
						min, max, latency, to_multiplier);

	memset(&rsp, 0, sizeof(rsp));
2547 2548 2549

	err = l2cap_check_conn_param(min, max, latency, to_multiplier);
	if (err)
2550 2551 2552 2553 2554 2555 2556
		rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
	else
		rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);

	l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
							sizeof(rsp), &rsp);

2557 2558 2559
	if (!err)
		hci_le_conn_update(hcon, min, max, latency, to_multiplier);

2560 2561 2562
	return 0;
}

2563 2564 2565 2566 2567 2568 2569 2570 2571 2572 2573 2574 2575 2576 2577 2578 2579 2580 2581 2582 2583 2584 2585 2586 2587 2588 2589 2590 2591 2592 2593 2594 2595 2596 2597 2598 2599 2600 2601 2602 2603 2604 2605 2606 2607 2608 2609 2610 2611 2612 2613 2614 2615 2616 2617 2618 2619 2620 2621 2622 2623 2624 2625 2626 2627 2628
static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
			struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
{
	int err = 0;

	switch (cmd->code) {
	case L2CAP_COMMAND_REJ:
		l2cap_command_rej(conn, cmd, data);
		break;

	case L2CAP_CONN_REQ:
		err = l2cap_connect_req(conn, cmd, data);
		break;

	case L2CAP_CONN_RSP:
		err = l2cap_connect_rsp(conn, cmd, data);
		break;

	case L2CAP_CONF_REQ:
		err = l2cap_config_req(conn, cmd, cmd_len, data);
		break;

	case L2CAP_CONF_RSP:
		err = l2cap_config_rsp(conn, cmd, data);
		break;

	case L2CAP_DISCONN_REQ:
		err = l2cap_disconnect_req(conn, cmd, data);
		break;

	case L2CAP_DISCONN_RSP:
		err = l2cap_disconnect_rsp(conn, cmd, data);
		break;

	case L2CAP_ECHO_REQ:
		l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
		break;

	case L2CAP_ECHO_RSP:
		break;

	case L2CAP_INFO_REQ:
		err = l2cap_information_req(conn, cmd, data);
		break;

	case L2CAP_INFO_RSP:
		err = l2cap_information_rsp(conn, cmd, data);
		break;

	default:
		BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
		err = -EINVAL;
		break;
	}

	return err;
}

static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
					struct l2cap_cmd_hdr *cmd, u8 *data)
{
	switch (cmd->code) {
	case L2CAP_COMMAND_REJ:
		return 0;

	case L2CAP_CONN_PARAM_UPDATE_REQ:
2629
		return l2cap_conn_param_update_req(conn, cmd, data);
2630 2631 2632 2633 2634 2635 2636 2637 2638 2639 2640 2641

	case L2CAP_CONN_PARAM_UPDATE_RSP:
		return 0;

	default:
		BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
		return -EINVAL;
	}
}

static inline void l2cap_sig_channel(struct l2cap_conn *conn,
							struct sk_buff *skb)
L
Linus Torvalds 已提交
2642 2643 2644 2645
{
	u8 *data = skb->data;
	int len = skb->len;
	struct l2cap_cmd_hdr cmd;
2646
	int err;
L
Linus Torvalds 已提交
2647 2648 2649 2650

	l2cap_raw_recv(conn, skb);

	while (len >= L2CAP_CMD_HDR_SIZE) {
2651
		u16 cmd_len;
L
Linus Torvalds 已提交
2652 2653 2654 2655
		memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
		data += L2CAP_CMD_HDR_SIZE;
		len  -= L2CAP_CMD_HDR_SIZE;

2656
		cmd_len = le16_to_cpu(cmd.len);
L
Linus Torvalds 已提交
2657

2658
		BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
L
Linus Torvalds 已提交
2659

2660
		if (cmd_len > len || !cmd.ident) {
L
Linus Torvalds 已提交
2661 2662 2663 2664
			BT_DBG("corrupted command");
			break;
		}

2665 2666 2667 2668
		if (conn->hcon->type == LE_LINK)
			err = l2cap_le_sig_cmd(conn, &cmd, data);
		else
			err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
L
Linus Torvalds 已提交
2669 2670 2671 2672 2673 2674

		if (err) {
			struct l2cap_cmd_rej rej;
			BT_DBG("error %d", err);

			/* FIXME: Map err to a valid reason */
2675
			rej.reason = cpu_to_le16(0);
L
Linus Torvalds 已提交
2676 2677 2678
			l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
		}

2679 2680
		data += cmd_len;
		len  -= cmd_len;
L
Linus Torvalds 已提交
2681 2682 2683 2684 2685
	}

	kfree_skb(skb);
}

2686 2687 2688 2689 2690 2691 2692 2693 2694 2695 2696
static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
{
	u16 our_fcs, rcv_fcs;
	int hdr_size = L2CAP_HDR_SIZE + 2;

	if (pi->fcs == L2CAP_FCS_CRC16) {
		skb_trim(skb, skb->len - 2);
		rcv_fcs = get_unaligned_le16(skb->data + skb->len);
		our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);

		if (our_fcs != rcv_fcs)
2697
			return -EBADMSG;
2698 2699 2700 2701
	}
	return 0;
}

2702 2703 2704 2705 2706 2707 2708 2709 2710 2711
static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	u16 control = 0;

	pi->frames_sent = 0;

	control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;

	if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
2712
		control |= L2CAP_SUPER_RCV_NOT_READY;
2713
		l2cap_send_sframe(pi, control);
2714
		pi->conn_state |= L2CAP_CONN_RNR_SENT;
2715 2716
	}

2717 2718
	if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
		l2cap_retransmit_frames(sk);
2719 2720 2721 2722 2723 2724 2725 2726 2727 2728

	l2cap_ertm_send(sk);

	if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
			pi->frames_sent == 0) {
		control |= L2CAP_SUPER_RCV_READY;
		l2cap_send_sframe(pi, control);
	}
}

2729
static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
2730 2731
{
	struct sk_buff *next_skb;
2732 2733
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	int tx_seq_offset, next_tx_seq_offset;
2734 2735 2736 2737 2738 2739 2740

	bt_cb(skb)->tx_seq = tx_seq;
	bt_cb(skb)->sar = sar;

	next_skb = skb_peek(SREJ_QUEUE(sk));
	if (!next_skb) {
		__skb_queue_tail(SREJ_QUEUE(sk), skb);
2741
		return 0;
2742 2743
	}

2744 2745 2746 2747
	tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
	if (tx_seq_offset < 0)
		tx_seq_offset += 64;

2748
	do {
2749 2750 2751
		if (bt_cb(next_skb)->tx_seq == tx_seq)
			return -EINVAL;

2752 2753 2754 2755 2756 2757
		next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
						pi->buffer_seq) % 64;
		if (next_tx_seq_offset < 0)
			next_tx_seq_offset += 64;

		if (next_tx_seq_offset > tx_seq_offset) {
2758
			__skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
2759
			return 0;
2760 2761 2762 2763 2764
		}

		if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
			break;

2765
	} while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
2766 2767

	__skb_queue_tail(SREJ_QUEUE(sk), skb);
2768 2769

	return 0;
2770 2771
}

2772 2773 2774 2775
static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct sk_buff *_skb;
2776
	int err;
2777 2778 2779 2780 2781 2782 2783 2784 2785 2786 2787 2788 2789 2790 2791 2792 2793 2794 2795 2796 2797 2798

	switch (control & L2CAP_CTRL_SAR) {
	case L2CAP_SDU_UNSEGMENTED:
		if (pi->conn_state & L2CAP_CONN_SAR_SDU)
			goto drop;

		err = sock_queue_rcv_skb(sk, skb);
		if (!err)
			return err;

		break;

	case L2CAP_SDU_START:
		if (pi->conn_state & L2CAP_CONN_SAR_SDU)
			goto drop;

		pi->sdu_len = get_unaligned_le16(skb->data);

		if (pi->sdu_len > pi->imtu)
			goto disconnect;

		pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
2799 2800 2801 2802 2803 2804 2805
		if (!pi->sdu)
			return -ENOMEM;

		/* pull sdu_len bytes only after alloc, because of Local Busy
		 * condition we have to be sure that this will be executed
		 * only once, i.e., when alloc does not fail */
		skb_pull(skb, 2);
2806 2807 2808 2809 2810 2811 2812 2813 2814 2815 2816 2817 2818 2819 2820 2821 2822 2823

		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);

		pi->conn_state |= L2CAP_CONN_SAR_SDU;
		pi->partial_sdu_len = skb->len;
		break;

	case L2CAP_SDU_CONTINUE:
		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
			goto disconnect;

		if (!pi->sdu)
			goto disconnect;

		pi->partial_sdu_len += skb->len;
		if (pi->partial_sdu_len > pi->sdu_len)
			goto drop;

2824 2825
		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);

2826 2827 2828 2829 2830 2831 2832 2833 2834
		break;

	case L2CAP_SDU_END:
		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
			goto disconnect;

		if (!pi->sdu)
			goto disconnect;

2835 2836
		if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
			pi->partial_sdu_len += skb->len;
2837

2838 2839
			if (pi->partial_sdu_len > pi->imtu)
				goto drop;
2840

2841 2842
			if (pi->partial_sdu_len != pi->sdu_len)
				goto drop;
2843 2844

			memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
2845
		}
2846 2847

		_skb = skb_clone(pi->sdu, GFP_ATOMIC);
2848 2849 2850 2851 2852
		if (!_skb) {
			pi->conn_state |= L2CAP_CONN_SAR_RETRY;
			return -ENOMEM;
		}

2853
		err = sock_queue_rcv_skb(sk, _skb);
2854
		if (err < 0) {
2855
			kfree_skb(_skb);
2856 2857 2858 2859 2860 2861
			pi->conn_state |= L2CAP_CONN_SAR_RETRY;
			return err;
		}

		pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
		pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
2862 2863 2864 2865 2866 2867

		kfree_skb(pi->sdu);
		break;
	}

	kfree_skb(skb);
2868
	return 0;
2869 2870 2871 2872 2873 2874

drop:
	kfree_skb(pi->sdu);
	pi->sdu = NULL;

disconnect:
2875
	l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
2876 2877 2878 2879
	kfree_skb(skb);
	return 0;
}

2880 2881 2882 2883 2884 2885 2886 2887 2888 2889 2890 2891 2892 2893 2894 2895 2896 2897 2898 2899 2900 2901 2902 2903 2904 2905 2906 2907 2908 2909 2910 2911 2912 2913 2914 2915 2916 2917 2918 2919
static int l2cap_try_push_rx_skb(struct sock *sk)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct sk_buff *skb;
	u16 control;
	int err;

	while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
		control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
		err = l2cap_ertm_reassembly_sdu(sk, skb, control);
		if (err < 0) {
			skb_queue_head(BUSY_QUEUE(sk), skb);
			return -EBUSY;
		}

		pi->buffer_seq = (pi->buffer_seq + 1) % 64;
	}

	if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
		goto done;

	control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
	control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
	l2cap_send_sframe(pi, control);
	l2cap_pi(sk)->retry_count = 1;

	del_timer(&pi->retrans_timer);
	__mod_monitor_timer();

	l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;

done:
	pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
	pi->conn_state &= ~L2CAP_CONN_RNR_SENT;

	BT_DBG("sk %p, Exit local busy", sk);

	return 0;
}

2920 2921 2922 2923 2924 2925 2926 2927 2928 2929 2930
static void l2cap_busy_work(struct work_struct *work)
{
	DECLARE_WAITQUEUE(wait, current);
	struct l2cap_pinfo *pi =
		container_of(work, struct l2cap_pinfo, busy_work);
	struct sock *sk = (struct sock *)pi;
	int n_tries = 0, timeo = HZ/5, err;
	struct sk_buff *skb;

	lock_sock(sk);

2931
	add_wait_queue(sk_sleep(sk), &wait);
2932 2933 2934 2935 2936
	while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
		set_current_state(TASK_INTERRUPTIBLE);

		if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
			err = -EBUSY;
2937
			l2cap_send_disconn_req(pi->conn, sk, EBUSY);
2938
			break;
2939 2940 2941 2942 2943 2944 2945
		}

		if (!timeo)
			timeo = HZ/5;

		if (signal_pending(current)) {
			err = sock_intr_errno(timeo);
2946
			break;
2947 2948 2949 2950 2951 2952 2953 2954
		}

		release_sock(sk);
		timeo = schedule_timeout(timeo);
		lock_sock(sk);

		err = sock_error(sk);
		if (err)
2955
			break;
2956

2957
		if (l2cap_try_push_rx_skb(sk) == 0)
2958 2959 2960 2961
			break;
	}

	set_current_state(TASK_RUNNING);
2962
	remove_wait_queue(sk_sleep(sk), &wait);
2963 2964 2965 2966 2967 2968 2969 2970 2971 2972 2973 2974

	release_sock(sk);
}

static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	int sctrl, err;

	if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
		bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
		__skb_queue_tail(BUSY_QUEUE(sk), skb);
2975 2976 2977
		return l2cap_try_push_rx_skb(sk);


2978 2979 2980 2981 2982 2983 2984 2985 2986
	}

	err = l2cap_ertm_reassembly_sdu(sk, skb, control);
	if (err >= 0) {
		pi->buffer_seq = (pi->buffer_seq + 1) % 64;
		return err;
	}

	/* Busy Condition */
2987 2988
	BT_DBG("sk %p, Enter local busy", sk);

2989 2990 2991 2992 2993 2994 2995 2996 2997 2998
	pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
	bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
	__skb_queue_tail(BUSY_QUEUE(sk), skb);

	sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
	sctrl |= L2CAP_SUPER_RCV_NOT_READY;
	l2cap_send_sframe(pi, sctrl);

	pi->conn_state |= L2CAP_CONN_RNR_SENT;

2999 3000
	del_timer(&pi->ack_timer);

3001 3002 3003 3004 3005
	queue_work(_busy_wq, &pi->busy_work);

	return err;
}

3006
static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
3007 3008 3009 3010 3011
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct sk_buff *_skb;
	int err = -EINVAL;

3012 3013 3014 3015 3016
	/*
	 * TODO: We have to notify the userland if some data is lost with the
	 * Streaming Mode.
	 */

3017 3018 3019 3020 3021 3022 3023 3024 3025 3026 3027 3028 3029 3030 3031 3032 3033 3034 3035 3036 3037 3038
	switch (control & L2CAP_CTRL_SAR) {
	case L2CAP_SDU_UNSEGMENTED:
		if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
			kfree_skb(pi->sdu);
			break;
		}

		err = sock_queue_rcv_skb(sk, skb);
		if (!err)
			return 0;

		break;

	case L2CAP_SDU_START:
		if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
			kfree_skb(pi->sdu);
			break;
		}

		pi->sdu_len = get_unaligned_le16(skb->data);
		skb_pull(skb, 2);

3039 3040 3041 3042 3043
		if (pi->sdu_len > pi->imtu) {
			err = -EMSGSIZE;
			break;
		}

3044 3045 3046 3047 3048 3049 3050 3051 3052 3053 3054 3055 3056 3057 3058 3059 3060 3061 3062 3063 3064 3065 3066 3067 3068 3069 3070 3071 3072 3073 3074 3075 3076 3077 3078 3079
		pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
		if (!pi->sdu) {
			err = -ENOMEM;
			break;
		}

		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);

		pi->conn_state |= L2CAP_CONN_SAR_SDU;
		pi->partial_sdu_len = skb->len;
		err = 0;
		break;

	case L2CAP_SDU_CONTINUE:
		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
			break;

		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);

		pi->partial_sdu_len += skb->len;
		if (pi->partial_sdu_len > pi->sdu_len)
			kfree_skb(pi->sdu);
		else
			err = 0;

		break;

	case L2CAP_SDU_END:
		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
			break;

		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);

		pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
		pi->partial_sdu_len += skb->len;

3080 3081 3082
		if (pi->partial_sdu_len > pi->imtu)
			goto drop;

3083 3084 3085 3086 3087 3088 3089 3090
		if (pi->partial_sdu_len == pi->sdu_len) {
			_skb = skb_clone(pi->sdu, GFP_ATOMIC);
			err = sock_queue_rcv_skb(sk, _skb);
			if (err < 0)
				kfree_skb(_skb);
		}
		err = 0;

3091 3092
drop:
		kfree_skb(pi->sdu);
3093 3094 3095 3096 3097 3098 3099
		break;
	}

	kfree_skb(skb);
	return err;
}

3100 3101 3102
static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
{
	struct sk_buff *skb;
3103
	u16 control;
3104

3105
	while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
3106 3107 3108 3109
		if (bt_cb(skb)->tx_seq != tx_seq)
			break;

		skb = skb_dequeue(SREJ_QUEUE(sk));
3110
		control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
3111
		l2cap_ertm_reassembly_sdu(sk, skb, control);
3112 3113
		l2cap_pi(sk)->buffer_seq_srej =
			(l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
3114
		tx_seq = (tx_seq + 1) % 64;
3115 3116 3117 3118 3119 3120 3121 3122 3123
	}
}

static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct srej_list *l, *tmp;
	u16 control;

3124
	list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
3125 3126 3127 3128 3129 3130 3131 3132 3133 3134 3135 3136 3137 3138 3139 3140 3141 3142 3143 3144 3145 3146 3147 3148 3149
		if (l->tx_seq == tx_seq) {
			list_del(&l->list);
			kfree(l);
			return;
		}
		control = L2CAP_SUPER_SELECT_REJECT;
		control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
		l2cap_send_sframe(pi, control);
		list_del(&l->list);
		list_add_tail(&l->list, SREJ_LIST(sk));
	}
}

static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	struct srej_list *new;
	u16 control;

	while (tx_seq != pi->expected_tx_seq) {
		control = L2CAP_SUPER_SELECT_REJECT;
		control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
		l2cap_send_sframe(pi, control);

		new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3150 3151
		new->tx_seq = pi->expected_tx_seq;
		pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3152 3153
		list_add_tail(&new->list, SREJ_LIST(sk));
	}
3154
	pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
3155 3156
}

3157 3158 3159 3160
static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	u8 tx_seq = __get_txseq(rx_control);
3161
	u8 req_seq = __get_reqseq(rx_control);
3162
	u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
3163
	int tx_seq_offset, expected_tx_seq_offset;
3164
	int num_to_ack = (pi->tx_win/6) + 1;
3165 3166
	int err = 0;

3167 3168
	BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
								rx_control);
3169

3170 3171
	if (L2CAP_CTRL_FINAL & rx_control &&
			l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3172 3173 3174 3175 3176 3177
		del_timer(&pi->monitor_timer);
		if (pi->unacked_frames > 0)
			__mod_retrans_timer();
		pi->conn_state &= ~L2CAP_CONN_WAIT_F;
	}

3178 3179 3180
	pi->expected_ack_seq = req_seq;
	l2cap_drop_acked_frames(sk);

3181 3182
	if (tx_seq == pi->expected_tx_seq)
		goto expected;
3183

3184 3185 3186 3187 3188 3189
	tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
	if (tx_seq_offset < 0)
		tx_seq_offset += 64;

	/* invalid tx_seq */
	if (tx_seq_offset >= pi->tx_win) {
3190
		l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
3191 3192 3193
		goto drop;
	}

3194 3195 3196
	if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
		goto drop;

3197 3198
	if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
		struct srej_list *first;
3199

3200 3201 3202 3203 3204 3205 3206 3207 3208 3209 3210 3211
		first = list_first_entry(SREJ_LIST(sk),
				struct srej_list, list);
		if (tx_seq == first->tx_seq) {
			l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
			l2cap_check_srej_gap(sk, tx_seq);

			list_del(&first->list);
			kfree(first);

			if (list_empty(SREJ_LIST(sk))) {
				pi->buffer_seq = pi->buffer_seq_srej;
				pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3212
				l2cap_send_ack(pi);
3213
				BT_DBG("sk %p, Exit SREJ_SENT", sk);
3214 3215 3216
			}
		} else {
			struct srej_list *l;
3217 3218 3219 3220

			/* duplicated tx_seq */
			if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
				goto drop;
3221 3222 3223 3224 3225 3226 3227 3228

			list_for_each_entry(l, SREJ_LIST(sk), list) {
				if (l->tx_seq == tx_seq) {
					l2cap_resend_srejframe(sk, tx_seq);
					return 0;
				}
			}
			l2cap_send_srejframe(sk, tx_seq);
3229 3230
		}
	} else {
3231 3232 3233 3234 3235 3236 3237 3238 3239
		expected_tx_seq_offset =
			(pi->expected_tx_seq - pi->buffer_seq) % 64;
		if (expected_tx_seq_offset < 0)
			expected_tx_seq_offset += 64;

		/* duplicated tx_seq */
		if (tx_seq_offset < expected_tx_seq_offset)
			goto drop;

3240
		pi->conn_state |= L2CAP_CONN_SREJ_SENT;
3241

3242 3243
		BT_DBG("sk %p, Enter SREJ", sk);

3244 3245 3246 3247
		INIT_LIST_HEAD(SREJ_LIST(sk));
		pi->buffer_seq_srej = pi->buffer_seq;

		__skb_queue_head_init(SREJ_QUEUE(sk));
3248
		__skb_queue_head_init(BUSY_QUEUE(sk));
3249 3250
		l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);

3251 3252
		pi->conn_state |= L2CAP_CONN_SEND_PBIT;

3253
		l2cap_send_srejframe(sk, tx_seq);
3254 3255

		del_timer(&pi->ack_timer);
3256
	}
3257 3258
	return 0;

3259 3260 3261 3262
expected:
	pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;

	if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
3263 3264 3265
		bt_cb(skb)->tx_seq = tx_seq;
		bt_cb(skb)->sar = sar;
		__skb_queue_tail(SREJ_QUEUE(sk), skb);
3266 3267 3268
		return 0;
	}

3269 3270 3271 3272
	err = l2cap_push_rx_skb(sk, skb, rx_control);
	if (err < 0)
		return 0;

3273 3274 3275
	if (rx_control & L2CAP_CTRL_FINAL) {
		if (pi->conn_state & L2CAP_CONN_REJ_ACT)
			pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3276 3277
		else
			l2cap_retransmit_frames(sk);
3278 3279
	}

3280 3281
	__mod_ack_timer();

3282 3283
	pi->num_acked = (pi->num_acked + 1) % num_to_ack;
	if (pi->num_acked == num_to_ack - 1)
3284 3285
		l2cap_send_ack(pi);

3286
	return 0;
3287 3288 3289 3290

drop:
	kfree_skb(skb);
	return 0;
3291 3292
}

3293
static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
3294 3295
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
3296

3297 3298 3299
	BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
						rx_control);

3300 3301
	pi->expected_ack_seq = __get_reqseq(rx_control);
	l2cap_drop_acked_frames(sk);
3302

3303
	if (rx_control & L2CAP_CTRL_POLL) {
3304
		pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3305 3306 3307 3308 3309 3310 3311 3312 3313 3314
		if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
			if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
					(pi->unacked_frames > 0))
				__mod_retrans_timer();

			pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
			l2cap_send_srejtail(sk);
		} else {
			l2cap_send_i_or_rr_or_rnr(sk);
		}
3315

3316 3317
	} else if (rx_control & L2CAP_CTRL_FINAL) {
		pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3318

3319 3320
		if (pi->conn_state & L2CAP_CONN_REJ_ACT)
			pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3321 3322
		else
			l2cap_retransmit_frames(sk);
3323

3324 3325 3326 3327
	} else {
		if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
				(pi->unacked_frames > 0))
			__mod_retrans_timer();
3328

3329
		pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3330
		if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
3331
			l2cap_send_ack(pi);
3332
		else
3333 3334 3335
			l2cap_ertm_send(sk);
	}
}
3336

3337 3338 3339 3340
static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	u8 tx_seq = __get_reqseq(rx_control);
3341

3342 3343
	BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);

3344 3345
	pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;

3346
	pi->expected_ack_seq = tx_seq;
3347 3348 3349 3350 3351
	l2cap_drop_acked_frames(sk);

	if (rx_control & L2CAP_CTRL_FINAL) {
		if (pi->conn_state & L2CAP_CONN_REJ_ACT)
			pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
3352 3353
		else
			l2cap_retransmit_frames(sk);
3354
	} else {
3355
		l2cap_retransmit_frames(sk);
3356

3357
		if (pi->conn_state & L2CAP_CONN_WAIT_F)
3358 3359 3360 3361 3362 3363 3364
			pi->conn_state |= L2CAP_CONN_REJ_ACT;
	}
}
static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	u8 tx_seq = __get_reqseq(rx_control);
3365

3366 3367
	BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);

3368
	pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3369

3370 3371 3372
	if (rx_control & L2CAP_CTRL_POLL) {
		pi->expected_ack_seq = tx_seq;
		l2cap_drop_acked_frames(sk);
3373 3374

		pi->conn_state |= L2CAP_CONN_SEND_FBIT;
3375 3376
		l2cap_retransmit_one_frame(sk, tx_seq);

3377
		l2cap_ertm_send(sk);
3378

3379 3380 3381
		if (pi->conn_state & L2CAP_CONN_WAIT_F) {
			pi->srej_save_reqseq = tx_seq;
			pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3382
		}
3383 3384 3385 3386 3387
	} else if (rx_control & L2CAP_CTRL_FINAL) {
		if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
				pi->srej_save_reqseq == tx_seq)
			pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
		else
3388
			l2cap_retransmit_one_frame(sk, tx_seq);
3389
	} else {
3390
		l2cap_retransmit_one_frame(sk, tx_seq);
3391 3392 3393
		if (pi->conn_state & L2CAP_CONN_WAIT_F) {
			pi->srej_save_reqseq = tx_seq;
			pi->conn_state |= L2CAP_CONN_SREJ_ACT;
3394
		}
3395 3396 3397 3398 3399 3400 3401 3402
	}
}

static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	u8 tx_seq = __get_reqseq(rx_control);

3403 3404
	BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);

3405 3406 3407 3408
	pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
	pi->expected_ack_seq = tx_seq;
	l2cap_drop_acked_frames(sk);

3409 3410 3411
	if (rx_control & L2CAP_CTRL_POLL)
		pi->conn_state |= L2CAP_CONN_SEND_FBIT;

3412 3413
	if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
		del_timer(&pi->retrans_timer);
3414 3415
		if (rx_control & L2CAP_CTRL_POLL)
			l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
3416
		return;
3417
	}
3418 3419 3420 3421 3422

	if (rx_control & L2CAP_CTRL_POLL)
		l2cap_send_srejtail(sk);
	else
		l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY);
3423 3424 3425 3426 3427 3428
}

static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
{
	BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);

3429 3430
	if (L2CAP_CTRL_FINAL & rx_control &&
			l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
3431 3432 3433 3434 3435 3436 3437 3438 3439
		del_timer(&l2cap_pi(sk)->monitor_timer);
		if (l2cap_pi(sk)->unacked_frames > 0)
			__mod_retrans_timer();
		l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F;
	}

	switch (rx_control & L2CAP_CTRL_SUPERVISE) {
	case L2CAP_SUPER_RCV_READY:
		l2cap_data_channel_rrframe(sk, rx_control);
3440 3441
		break;

3442 3443 3444
	case L2CAP_SUPER_REJECT:
		l2cap_data_channel_rejframe(sk, rx_control);
		break;
3445

3446 3447 3448 3449 3450 3451
	case L2CAP_SUPER_SELECT_REJECT:
		l2cap_data_channel_srejframe(sk, rx_control);
		break;

	case L2CAP_SUPER_RCV_NOT_READY:
		l2cap_data_channel_rnrframe(sk, rx_control);
3452 3453 3454
		break;
	}

3455
	kfree_skb(skb);
3456 3457 3458
	return 0;
}

3459 3460 3461 3462 3463 3464 3465 3466 3467 3468 3469 3470 3471 3472 3473 3474 3475 3476 3477 3478 3479 3480 3481 3482 3483 3484 3485 3486 3487 3488 3489 3490 3491 3492 3493 3494 3495 3496 3497 3498 3499 3500 3501 3502 3503 3504 3505 3506 3507 3508 3509 3510 3511 3512 3513 3514 3515 3516 3517 3518 3519 3520 3521 3522 3523 3524 3525 3526 3527 3528
static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
{
	struct l2cap_pinfo *pi = l2cap_pi(sk);
	u16 control;
	u8 req_seq;
	int len, next_tx_seq_offset, req_seq_offset;

	control = get_unaligned_le16(skb->data);
	skb_pull(skb, 2);
	len = skb->len;

	/*
	 * We can just drop the corrupted I-frame here.
	 * Receiver will miss it and start proper recovery
	 * procedures and ask retransmission.
	 */
	if (l2cap_check_fcs(pi, skb))
		goto drop;

	if (__is_sar_start(control) && __is_iframe(control))
		len -= 2;

	if (pi->fcs == L2CAP_FCS_CRC16)
		len -= 2;

	if (len > pi->mps) {
		l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
		goto drop;
	}

	req_seq = __get_reqseq(control);
	req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
	if (req_seq_offset < 0)
		req_seq_offset += 64;

	next_tx_seq_offset =
		(pi->next_tx_seq - pi->expected_ack_seq) % 64;
	if (next_tx_seq_offset < 0)
		next_tx_seq_offset += 64;

	/* check for invalid req-seq */
	if (req_seq_offset > next_tx_seq_offset) {
		l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
		goto drop;
	}

	if (__is_iframe(control)) {
		if (len < 0) {
			l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
			goto drop;
		}

		l2cap_data_channel_iframe(sk, control, skb);
	} else {
		if (len != 0) {
			BT_ERR("%d", len);
			l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
			goto drop;
		}

		l2cap_data_channel_sframe(sk, control, skb);
	}

	return 0;

drop:
	kfree_skb(skb);
	return 0;
}

L
Linus Torvalds 已提交
3529 3530 3531
static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
{
	struct sock *sk;
3532
	struct l2cap_pinfo *pi;
3533
	u16 control;
3534 3535
	u8 tx_seq;
	int len;
L
Linus Torvalds 已提交
3536 3537 3538 3539 3540 3541 3542

	sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
	if (!sk) {
		BT_DBG("unknown cid 0x%4.4x", cid);
		goto drop;
	}

3543 3544
	pi = l2cap_pi(sk);

L
Linus Torvalds 已提交
3545 3546 3547 3548 3549
	BT_DBG("sk %p, len %d", sk, skb->len);

	if (sk->sk_state != BT_CONNECTED)
		goto drop;

3550
	switch (pi->mode) {
3551 3552 3553 3554 3555
	case L2CAP_MODE_BASIC:
		/* If socket recv buffers overflows we drop data here
		 * which is *bad* because L2CAP has to be reliable.
		 * But we don't have any other choice. L2CAP doesn't
		 * provide flow control mechanism. */
L
Linus Torvalds 已提交
3556

3557
		if (pi->imtu < skb->len)
3558
			goto drop;
L
Linus Torvalds 已提交
3559

3560 3561 3562 3563 3564
		if (!sock_queue_rcv_skb(sk, skb))
			goto done;
		break;

	case L2CAP_MODE_ERTM:
3565 3566
		if (!sock_owned_by_user(sk)) {
			l2cap_ertm_data_rcv(sk, skb);
3567
		} else {
3568
			if (sk_add_backlog(sk, skb))
3569 3570
				goto drop;
		}
3571

3572
		goto done;
3573

3574 3575 3576 3577 3578
	case L2CAP_MODE_STREAMING:
		control = get_unaligned_le16(skb->data);
		skb_pull(skb, 2);
		len = skb->len;

3579 3580 3581
		if (l2cap_check_fcs(pi, skb))
			goto drop;

3582 3583 3584
		if (__is_sar_start(control))
			len -= 2;

3585 3586 3587
		if (pi->fcs == L2CAP_FCS_CRC16)
			len -= 2;

3588
		if (len > pi->mps || len < 0 || __is_sframe(control))
3589 3590 3591 3592 3593 3594 3595
			goto drop;

		tx_seq = __get_txseq(control);

		if (pi->expected_tx_seq == tx_seq)
			pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
		else
3596
			pi->expected_tx_seq = (tx_seq + 1) % 64;
3597

3598
		l2cap_streaming_reassembly_sdu(sk, skb, control);
3599 3600 3601

		goto done;

3602
	default:
3603
		BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode);
3604 3605
		break;
	}
L
Linus Torvalds 已提交
3606 3607 3608 3609 3610

drop:
	kfree_skb(skb);

done:
3611 3612 3613
	if (sk)
		bh_unlock_sock(sk);

L
Linus Torvalds 已提交
3614 3615 3616
	return 0;
}

3617
static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
L
Linus Torvalds 已提交
3618 3619 3620 3621 3622 3623 3624
{
	struct sock *sk;

	sk = l2cap_get_sock_by_psm(0, psm, conn->src);
	if (!sk)
		goto drop;

3625 3626
	bh_lock_sock(sk);

L
Linus Torvalds 已提交
3627 3628 3629 3630 3631 3632 3633 3634 3635 3636 3637 3638 3639 3640 3641
	BT_DBG("sk %p, len %d", sk, skb->len);

	if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
		goto drop;

	if (l2cap_pi(sk)->imtu < skb->len)
		goto drop;

	if (!sock_queue_rcv_skb(sk, skb))
		goto done;

drop:
	kfree_skb(skb);

done:
3642 3643
	if (sk)
		bh_unlock_sock(sk);
L
Linus Torvalds 已提交
3644 3645 3646 3647 3648 3649
	return 0;
}

static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
{
	struct l2cap_hdr *lh = (void *) skb->data;
3650 3651
	u16 cid, len;
	__le16 psm;
L
Linus Torvalds 已提交
3652 3653 3654 3655 3656

	skb_pull(skb, L2CAP_HDR_SIZE);
	cid = __le16_to_cpu(lh->cid);
	len = __le16_to_cpu(lh->len);

3657 3658 3659 3660 3661
	if (len != skb->len) {
		kfree_skb(skb);
		return;
	}

L
Linus Torvalds 已提交
3662 3663 3664
	BT_DBG("len %d, cid 0x%4.4x", len, cid);

	switch (cid) {
3665
	case L2CAP_CID_LE_SIGNALING:
3666
	case L2CAP_CID_SIGNALING:
L
Linus Torvalds 已提交
3667 3668 3669
		l2cap_sig_channel(conn, skb);
		break;

3670
	case L2CAP_CID_CONN_LESS:
3671
		psm = get_unaligned_le16(skb->data);
L
Linus Torvalds 已提交
3672 3673 3674 3675 3676 3677 3678 3679 3680 3681 3682 3683 3684 3685 3686 3687 3688 3689 3690
		skb_pull(skb, 2);
		l2cap_conless_channel(conn, psm, skb);
		break;

	default:
		l2cap_data_channel(conn, cid, skb);
		break;
	}
}

/* ---- L2CAP interface with lower layer (HCI) ---- */

static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
{
	int exact = 0, lm1 = 0, lm2 = 0;
	register struct sock *sk;
	struct hlist_node *node;

	if (type != ACL_LINK)
3691
		return -EINVAL;
L
Linus Torvalds 已提交
3692 3693 3694 3695 3696 3697 3698 3699 3700 3701

	BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));

	/* Find listening sockets and check their link_mode */
	read_lock(&l2cap_sk_list.lock);
	sk_for_each(sk, node, &l2cap_sk_list.head) {
		if (sk->sk_state != BT_LISTEN)
			continue;

		if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3702 3703 3704
			lm1 |= HCI_LM_ACCEPT;
			if (l2cap_pi(sk)->role_switch)
				lm1 |= HCI_LM_MASTER;
L
Linus Torvalds 已提交
3705
			exact++;
3706 3707 3708 3709 3710
		} else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
			lm2 |= HCI_LM_ACCEPT;
			if (l2cap_pi(sk)->role_switch)
				lm2 |= HCI_LM_MASTER;
		}
L
Linus Torvalds 已提交
3711 3712 3713 3714 3715 3716 3717 3718
	}
	read_unlock(&l2cap_sk_list.lock);

	return exact ? lm1 : lm2;
}

static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
{
3719 3720
	struct l2cap_conn *conn;

L
Linus Torvalds 已提交
3721 3722
	BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);

3723
	if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3724
		return -EINVAL;
L
Linus Torvalds 已提交
3725 3726 3727 3728 3729

	if (!status) {
		conn = l2cap_conn_add(hcon, status);
		if (conn)
			l2cap_conn_ready(conn);
3730
	} else
L
Linus Torvalds 已提交
3731 3732 3733 3734 3735
		l2cap_conn_del(hcon, bt_err(status));

	return 0;
}

3736 3737 3738 3739 3740 3741 3742 3743 3744 3745 3746 3747 3748
static int l2cap_disconn_ind(struct hci_conn *hcon)
{
	struct l2cap_conn *conn = hcon->l2cap_data;

	BT_DBG("hcon %p", hcon);

	if (hcon->type != ACL_LINK || !conn)
		return 0x13;

	return conn->disc_reason;
}

static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
L
Linus Torvalds 已提交
3749 3750 3751
{
	BT_DBG("hcon %p reason %d", hcon, reason);

3752
	if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
3753
		return -EINVAL;
L
Linus Torvalds 已提交
3754 3755

	l2cap_conn_del(hcon, bt_err(reason));
3756

L
Linus Torvalds 已提交
3757 3758 3759
	return 0;
}

3760 3761
static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
{
3762
	if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
3763 3764
		return;

3765 3766 3767 3768 3769 3770 3771 3772 3773 3774 3775 3776
	if (encrypt == 0x00) {
		if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) {
			l2cap_sock_clear_timer(sk);
			l2cap_sock_set_timer(sk, HZ * 5);
		} else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH)
			__l2cap_sock_close(sk, ECONNREFUSED);
	} else {
		if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM)
			l2cap_sock_clear_timer(sk);
	}
}

3777
static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
L
Linus Torvalds 已提交
3778 3779
{
	struct l2cap_chan_list *l;
3780
	struct l2cap_conn *conn = hcon->l2cap_data;
L
Linus Torvalds 已提交
3781 3782
	struct sock *sk;

3783
	if (!conn)
L
Linus Torvalds 已提交
3784
		return 0;
3785

L
Linus Torvalds 已提交
3786 3787 3788 3789 3790 3791 3792 3793 3794
	l = &conn->chan_list;

	BT_DBG("conn %p", conn);

	read_lock(&l->lock);

	for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
		bh_lock_sock(sk);

3795 3796 3797 3798 3799
		if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) {
			bh_unlock_sock(sk);
			continue;
		}

3800
		if (!status && (sk->sk_state == BT_CONNECTED ||
3801
						sk->sk_state == BT_CONFIG)) {
3802
			l2cap_check_encryption(sk, encrypt);
3803 3804 3805 3806
			bh_unlock_sock(sk);
			continue;
		}

3807 3808 3809 3810 3811
		if (sk->sk_state == BT_CONNECT) {
			if (!status) {
				struct l2cap_conn_req req;
				req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
				req.psm  = l2cap_pi(sk)->psm;
L
Linus Torvalds 已提交
3812

3813
				l2cap_pi(sk)->ident = l2cap_get_ident(conn);
3814
				l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
L
Linus Torvalds 已提交
3815

3816 3817 3818 3819 3820 3821 3822 3823 3824
				l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
					L2CAP_CONN_REQ, sizeof(req), &req);
			} else {
				l2cap_sock_clear_timer(sk);
				l2cap_sock_set_timer(sk, HZ / 10);
			}
		} else if (sk->sk_state == BT_CONNECT2) {
			struct l2cap_conn_rsp rsp;
			__u16 result;
L
Linus Torvalds 已提交
3825

3826 3827 3828 3829 3830 3831 3832 3833 3834 3835 3836 3837
			if (!status) {
				sk->sk_state = BT_CONFIG;
				result = L2CAP_CR_SUCCESS;
			} else {
				sk->sk_state = BT_DISCONN;
				l2cap_sock_set_timer(sk, HZ / 10);
				result = L2CAP_CR_SEC_BLOCK;
			}

			rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
			rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
			rsp.result = cpu_to_le16(result);
3838
			rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
3839 3840 3841
			l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
					L2CAP_CONN_RSP, sizeof(rsp), &rsp);
		}
L
Linus Torvalds 已提交
3842 3843 3844 3845 3846

		bh_unlock_sock(sk);
	}

	read_unlock(&l->lock);
3847

L
Linus Torvalds 已提交
3848 3849 3850 3851 3852 3853 3854
	return 0;
}

static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
{
	struct l2cap_conn *conn = hcon->l2cap_data;

3855 3856 3857 3858
	if (!conn)
		conn = l2cap_conn_add(hcon, 0);

	if (!conn)
L
Linus Torvalds 已提交
3859 3860 3861 3862
		goto drop;

	BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);

3863
	if (!(flags & ACL_CONT)) {
L
Linus Torvalds 已提交
3864
		struct l2cap_hdr *hdr;
3865 3866
		struct sock *sk;
		u16 cid;
L
Linus Torvalds 已提交
3867 3868 3869 3870 3871 3872 3873 3874 3875 3876
		int len;

		if (conn->rx_len) {
			BT_ERR("Unexpected start frame (len %d)", skb->len);
			kfree_skb(conn->rx_skb);
			conn->rx_skb = NULL;
			conn->rx_len = 0;
			l2cap_conn_unreliable(conn, ECOMM);
		}

3877 3878
		/* Start fragment always begin with Basic L2CAP header */
		if (skb->len < L2CAP_HDR_SIZE) {
L
Linus Torvalds 已提交
3879 3880 3881 3882 3883 3884 3885
			BT_ERR("Frame is too short (len %d)", skb->len);
			l2cap_conn_unreliable(conn, ECOMM);
			goto drop;
		}

		hdr = (struct l2cap_hdr *) skb->data;
		len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
3886
		cid = __le16_to_cpu(hdr->cid);
L
Linus Torvalds 已提交
3887 3888 3889 3890 3891 3892 3893 3894 3895 3896 3897 3898 3899 3900 3901 3902

		if (len == skb->len) {
			/* Complete frame received */
			l2cap_recv_frame(conn, skb);
			return 0;
		}

		BT_DBG("Start: total len %d, frag len %d", len, skb->len);

		if (skb->len > len) {
			BT_ERR("Frame is too long (len %d, expected len %d)",
				skb->len, len);
			l2cap_conn_unreliable(conn, ECOMM);
			goto drop;
		}

3903 3904 3905 3906 3907 3908 3909 3910 3911 3912 3913 3914 3915
		sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);

		if (sk && l2cap_pi(sk)->imtu < len - L2CAP_HDR_SIZE) {
			BT_ERR("Frame exceeding recv MTU (len %d, MTU %d)",
					len, l2cap_pi(sk)->imtu);
			bh_unlock_sock(sk);
			l2cap_conn_unreliable(conn, ECOMM);
			goto drop;
		}

		if (sk)
			bh_unlock_sock(sk);

L
Linus Torvalds 已提交
3916
		/* Allocate skb for the complete frame (with header) */
3917 3918
		conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
		if (!conn->rx_skb)
L
Linus Torvalds 已提交
3919 3920
			goto drop;

3921
		skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3922
								skb->len);
L
Linus Torvalds 已提交
3923 3924 3925 3926 3927 3928 3929 3930 3931 3932 3933 3934 3935 3936 3937 3938 3939 3940 3941 3942
		conn->rx_len = len - skb->len;
	} else {
		BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);

		if (!conn->rx_len) {
			BT_ERR("Unexpected continuation frame (len %d)", skb->len);
			l2cap_conn_unreliable(conn, ECOMM);
			goto drop;
		}

		if (skb->len > conn->rx_len) {
			BT_ERR("Fragment is too long (len %d, expected %d)",
					skb->len, conn->rx_len);
			kfree_skb(conn->rx_skb);
			conn->rx_skb = NULL;
			conn->rx_len = 0;
			l2cap_conn_unreliable(conn, ECOMM);
			goto drop;
		}

3943
		skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
3944
								skb->len);
L
Linus Torvalds 已提交
3945 3946 3947 3948 3949 3950 3951 3952 3953 3954 3955 3956 3957 3958
		conn->rx_len -= skb->len;

		if (!conn->rx_len) {
			/* Complete frame received */
			l2cap_recv_frame(conn, conn->rx_skb);
			conn->rx_skb = NULL;
		}
	}

drop:
	kfree_skb(skb);
	return 0;
}

3959
static int l2cap_debugfs_show(struct seq_file *f, void *p)
L
Linus Torvalds 已提交
3960 3961 3962 3963 3964 3965
{
	struct sock *sk;
	struct hlist_node *node;

	read_lock_bh(&l2cap_sk_list.lock);

3966 3967
	sk_for_each(sk, node, &l2cap_sk_list.head) {
		struct l2cap_pinfo *pi = l2cap_pi(sk);
3968

3969
		seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
3970 3971 3972 3973
					batostr(&bt_sk(sk)->src),
					batostr(&bt_sk(sk)->dst),
					sk->sk_state, __le16_to_cpu(pi->psm),
					pi->scid, pi->dcid,
3974 3975
					pi->imtu, pi->omtu, pi->sec_level,
					pi->mode);
3976
	}
L
Linus Torvalds 已提交
3977 3978 3979

	read_unlock_bh(&l2cap_sk_list.lock);

3980
	return 0;
L
Linus Torvalds 已提交
3981 3982
}

3983 3984 3985 3986 3987 3988 3989 3990 3991 3992 3993 3994 3995
static int l2cap_debugfs_open(struct inode *inode, struct file *file)
{
	return single_open(file, l2cap_debugfs_show, inode->i_private);
}

static const struct file_operations l2cap_debugfs_fops = {
	.open		= l2cap_debugfs_open,
	.read		= seq_read,
	.llseek		= seq_lseek,
	.release	= single_release,
};

static struct dentry *l2cap_debugfs;
L
Linus Torvalds 已提交
3996 3997 3998 3999 4000 4001 4002

static struct hci_proto l2cap_hci_proto = {
	.name		= "L2CAP",
	.id		= HCI_PROTO_L2CAP,
	.connect_ind	= l2cap_connect_ind,
	.connect_cfm	= l2cap_connect_cfm,
	.disconn_ind	= l2cap_disconn_ind,
4003
	.disconn_cfm	= l2cap_disconn_cfm,
4004
	.security_cfm	= l2cap_security_cfm,
L
Linus Torvalds 已提交
4005 4006 4007
	.recv_acldata	= l2cap_recv_acldata
};

4008
int __init l2cap_init(void)
L
Linus Torvalds 已提交
4009 4010
{
	int err;
4011

4012
	err = l2cap_init_sockets();
L
Linus Torvalds 已提交
4013 4014 4015
	if (err < 0)
		return err;

4016
	_busy_wq = create_singlethread_workqueue("l2cap");
4017
	if (!_busy_wq) {
4018
		err = -ENOMEM;
L
Linus Torvalds 已提交
4019 4020 4021 4022 4023 4024 4025 4026 4027 4028
		goto error;
	}

	err = hci_register_proto(&l2cap_hci_proto);
	if (err < 0) {
		BT_ERR("L2CAP protocol registration failed");
		bt_sock_unregister(BTPROTO_L2CAP);
		goto error;
	}

4029 4030 4031 4032 4033 4034
	if (bt_debugfs) {
		l2cap_debugfs = debugfs_create_file("l2cap", 0444,
					bt_debugfs, NULL, &l2cap_debugfs_fops);
		if (!l2cap_debugfs)
			BT_ERR("Failed to create L2CAP debug file");
	}
L
Linus Torvalds 已提交
4035 4036 4037 4038

	return 0;

error:
4039
	destroy_workqueue(_busy_wq);
4040
	l2cap_cleanup_sockets();
L
Linus Torvalds 已提交
4041 4042 4043
	return err;
}

4044
void l2cap_exit(void)
L
Linus Torvalds 已提交
4045
{
4046
	debugfs_remove(l2cap_debugfs);
L
Linus Torvalds 已提交
4047

4048 4049 4050
	flush_workqueue(_busy_wq);
	destroy_workqueue(_busy_wq);

L
Linus Torvalds 已提交
4051 4052 4053
	if (hci_unregister_proto(&l2cap_hci_proto) < 0)
		BT_ERR("L2CAP protocol unregistration failed");

4054
	l2cap_cleanup_sockets();
L
Linus Torvalds 已提交
4055 4056
}

4057 4058
module_param(disable_ertm, bool, 0644);
MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");