projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
Bluetooth: Fix L2CAP security check
[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / bluetooth / l2cap_core.c
CommitLineData
8e87d142 1/*
1da177e4
LT		 2 BlueZ - Bluetooth protocol stack for Linux
3 Copyright (C) 2000-2001 Qualcomm Incorporated
ce5706bd 4 Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
5d8868ff 5 Copyright (C) 2010 Google Inc.
1da177e4
LT		 6
7 Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
8
9 This program is free software; you can redistribute it and/or modify
10 it under the terms of the GNU General Public License version 2 as
11 published by the Free Software Foundation;
12
13 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
14 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
16 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
8e87d142
YH		 17 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
18 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
19 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
1da177e4
LT		 20 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
21
8e87d142
YH		 22 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
23 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
1da177e4
LT		 24 SOFTWARE IS DISCLAIMED.
25*/
26
bb58f747 27/* Bluetooth L2CAP core. */
1da177e4 28
1da177e4
LT		 29#include <linux/module.h>
30
31#include <linux/types.h>
4fc268d2 32#include <linux/capability.h>
1da177e4
LT		 33#include <linux/errno.h>
34#include <linux/kernel.h>
1da177e4
LT		 35#include <linux/sched.h>
36#include <linux/slab.h>
37#include <linux/poll.h>
38#include <linux/fcntl.h>
39#include <linux/init.h>
40#include <linux/interrupt.h>
41#include <linux/socket.h>
42#include <linux/skbuff.h>
1da177e4 43#include <linux/list.h>
be9d1227 44#include <linux/device.h>
aef7d97c
MH		 45#include <linux/debugfs.h>
46#include <linux/seq_file.h>
af05b30b 47#include <linux/uaccess.h>
fcc203c3 48#include <linux/crc16.h>
1da177e4
LT		 49#include <net/sock.h>
50
51#include <asm/system.h>
1da177e4
LT		 52#include <asm/unaligned.h>
53
54#include <net/bluetooth/bluetooth.h>
55#include <net/bluetooth/hci_core.h>
56#include <net/bluetooth/l2cap.h>
57
bb58f747 58int disable_ertm;
f0709e03 59
47ec1dcd 60static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
e1027a7c 61static u8 l2cap_fixed_chan[8] = { 0x02, };
1da177e4 62
1890d36b
GP		 63static struct workqueue_struct *_busy_wq;
64
23691d75
GP		 65LIST_HEAD(chan_list);
66DEFINE_RWLOCK(chan_list_lock);
1da177e4 67
1890d36b
GP		 68static void l2cap_busy_work(struct work_struct *work);
69
1da177e4
LT		 70static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
71 u8 code, u8 ident, u16 dlen, void *data);
710f9b0a 72static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
1da177e4 73
218bb9df
GP		 74static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
75
0139418c 76/* ---- L2CAP channels ---- */
baa7e1fa 77static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
0139418c 78{
48454079 79 struct l2cap_chan *c;
baa7e1fa
GP		 80
81 list_for_each_entry(c, &conn->chan_l, list) {
fe4128e0 82 if (c->dcid == cid)
baa7e1fa 83 return c;
0139418c 84 }
baa7e1fa
GP		 85 return NULL;
86
0139418c
MH		 87}
88
baa7e1fa 89static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
0139418c 90{
48454079 91 struct l2cap_chan *c;
baa7e1fa
GP		 92
93 list_for_each_entry(c, &conn->chan_l, list) {
fe4128e0 94 if (c->scid == cid)
baa7e1fa 95 return c;
0139418c 96 }
baa7e1fa 97 return NULL;
0139418c
MH		 98}
99
100/* Find channel with given SCID.
101 * Returns locked socket */
baa7e1fa 102static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
0139418c 103{
48454079 104 struct l2cap_chan *c;
baa7e1fa
GP		 105
106 read_lock(&conn->chan_lock);
107 c = __l2cap_get_chan_by_scid(conn, cid);
48454079
GP		 108 if (c)
109 bh_lock_sock(c->sk);
baa7e1fa 110 read_unlock(&conn->chan_lock);
48454079 111 return c;
0139418c
MH		 112}
113
baa7e1fa 114static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
0139418c 115{
48454079 116 struct l2cap_chan *c;
baa7e1fa
GP		 117
118 list_for_each_entry(c, &conn->chan_l, list) {
fc7f8a7e 119 if (c->ident == ident)
baa7e1fa 120 return c;
0139418c 121 }
baa7e1fa 122 return NULL;
0139418c
MH		 123}
124
baa7e1fa 125static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
0139418c 126{
48454079 127 struct l2cap_chan *c;
baa7e1fa
GP		 128
129 read_lock(&conn->chan_lock);
130 c = __l2cap_get_chan_by_ident(conn, ident);
48454079
GP		 131 if (c)
132 bh_lock_sock(c->sk);
baa7e1fa 133 read_unlock(&conn->chan_lock);
48454079 134 return c;
0139418c
MH		 135}
136
23691d75 137static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
9e4425ff 138{
23691d75 139 struct l2cap_chan *c;
9e4425ff 140
23691d75
GP		 141 list_for_each_entry(c, &chan_list, global_l) {
142 if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
9e4425ff
GP		 143 goto found;
144 }
145
23691d75 146 c = NULL;
9e4425ff 147found:
23691d75 148 return c;
9e4425ff
GP		 149}
150
151int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
152{
73b2ec18
GP		 153 int err;
154
23691d75 155 write_lock_bh(&chan_list_lock);
9e4425ff 156
23691d75 157 if (psm && __l2cap_global_chan_by_addr(psm, src)) {
73b2ec18
GP		 158 err = -EADDRINUSE;
159 goto done;
9e4425ff
GP		 160 }
161
73b2ec18
GP		 162 if (psm) {
163 chan->psm = psm;
164 chan->sport = psm;
165 err = 0;
166 } else {
167 u16 p;
168
169 err = -EINVAL;
170 for (p = 0x1001; p < 0x1100; p += 2)
23691d75 171 if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
73b2ec18
GP		 172 chan->psm = cpu_to_le16(p);
173 chan->sport = cpu_to_le16(p);
174 err = 0;
175 break;
176 }
177 }
9e4425ff 178
73b2ec18 179done:
23691d75 180 write_unlock_bh(&chan_list_lock);
73b2ec18 181 return err;
9e4425ff
GP		 182}
183
184int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid)
185{
23691d75 186 write_lock_bh(&chan_list_lock);
9e4425ff
GP		 187
188 chan->scid = scid;
189
23691d75 190 write_unlock_bh(&chan_list_lock);
9e4425ff
GP		 191
192 return 0;
193}
194
baa7e1fa 195static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
0139418c 196{
8db4dc46 197 u16 cid = L2CAP_CID_DYN_START;
0139418c 198
8db4dc46 199 for (; cid < L2CAP_CID_DYN_END; cid++) {
baa7e1fa 200 if (!__l2cap_get_chan_by_scid(conn, cid))
0139418c
MH		 201 return cid;
202 }
203
204 return 0;
205}
206
23691d75 207struct l2cap_chan *l2cap_chan_create(struct sock *sk)
48454079
GP		 208{
209 struct l2cap_chan *chan;
210
211 chan = kzalloc(sizeof(*chan), GFP_ATOMIC);
212 if (!chan)
213 return NULL;
214
215 chan->sk = sk;
216
23691d75
GP		 217 write_lock_bh(&chan_list_lock);
218 list_add(&chan->global_l, &chan_list);
219 write_unlock_bh(&chan_list_lock);
220
48454079
GP		 221 return chan;
222}
223
23691d75 224void l2cap_chan_destroy(struct l2cap_chan *chan)
6ff5abbf 225{
23691d75
GP		 226 write_lock_bh(&chan_list_lock);
227 list_del(&chan->global_l);
228 write_unlock_bh(&chan_list_lock);
229
6ff5abbf
GP		 230 kfree(chan);
231}
232
48454079 233static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
0139418c 234{
48454079 235 struct sock *sk = chan->sk;
0139418c 236
af05b30b 237 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
fe4128e0 238 chan->psm, chan->dcid);
0139418c 239
2950f21a
MH		 240 conn->disc_reason = 0x13;
241
8c1d787b 242 chan->conn = conn;
0139418c 243
bd3c9e25 244 if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
b62f328b
VT		 245 if (conn->hcon->type == LE_LINK) {
246 /* LE connection */
0c1bc5c6 247 chan->omtu = L2CAP_LE_DEFAULT_MTU;
fe4128e0
GP		 248 chan->scid = L2CAP_CID_LE_DATA;
249 chan->dcid = L2CAP_CID_LE_DATA;
b62f328b
VT		 250 } else {
251 /* Alloc CID for connection-oriented socket */
fe4128e0 252 chan->scid = l2cap_alloc_cid(conn);
0c1bc5c6 253 chan->omtu = L2CAP_DEFAULT_MTU;
b62f328b 254 }
0139418c
MH		 255 } else if (sk->sk_type == SOCK_DGRAM) {
256 /* Connectionless socket */
fe4128e0
GP		 257 chan->scid = L2CAP_CID_CONN_LESS;
258 chan->dcid = L2CAP_CID_CONN_LESS;
0c1bc5c6 259 chan->omtu = L2CAP_DEFAULT_MTU;
0139418c
MH		 260 } else {
261 /* Raw socket can send/recv signalling messages only */
fe4128e0
GP		 262 chan->scid = L2CAP_CID_SIGNALING;
263 chan->dcid = L2CAP_CID_SIGNALING;
0c1bc5c6 264 chan->omtu = L2CAP_DEFAULT_MTU;
0139418c
MH		 265 }
266
baa7e1fa
GP		 267 sock_hold(sk);
268
269 list_add(&chan->list, &conn->chan_l);
0139418c
MH		 270}
271
8e87d142 272/* Delete channel.
0139418c 273 * Must be called on the locked socket. */
48454079 274void l2cap_chan_del(struct l2cap_chan *chan, int err)
0139418c 275{
48454079 276 struct sock *sk = chan->sk;
8c1d787b 277 struct l2cap_conn *conn = chan->conn;
0139418c
MH		 278 struct sock *parent = bt_sk(sk)->parent;
279
280 l2cap_sock_clear_timer(sk);
281
49208c9c 282 BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
0139418c 283
8e87d142 284 if (conn) {
baa7e1fa
GP		 285 /* Delete from channel list */
286 write_lock_bh(&conn->chan_lock);
287 list_del(&chan->list);
288 write_unlock_bh(&conn->chan_lock);
289 __sock_put(sk);
290
8c1d787b 291 chan->conn = NULL;
0139418c
MH		 292 hci_conn_put(conn->hcon);
293 }
294
b1235d79 295 sk->sk_state = BT_CLOSED;
0139418c
MH		 296 sock_set_flag(sk, SOCK_ZAPPED);
297
298 if (err)
299 sk->sk_err = err;
300
301 if (parent) {
302 bt_accept_unlink(sk);
303 parent->sk_data_ready(parent, 0);
304 } else
305 sk->sk_state_change(sk);
c13ffa62 306
b4450035
GP		 307 if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE &&
308 chan->conf_state & L2CAP_CONF_INPUT_DONE))
6ff5abbf 309 return;
2ead70b8 310
58d35f87 311 skb_queue_purge(&chan->tx_q);
c13ffa62 312
0c1bc5c6 313 if (chan->mode == L2CAP_MODE_ERTM) {
c13ffa62
GP		 314 struct srej_list *l, *tmp;
315
e92c8e70
GP		 316 del_timer(&chan->retrans_timer);
317 del_timer(&chan->monitor_timer);
318 del_timer(&chan->ack_timer);
c13ffa62 319
f1c6775b
GP		 320 skb_queue_purge(&chan->srej_q);
321 skb_queue_purge(&chan->busy_q);
c13ffa62 322
39d5a3ee 323 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
c13ffa62
GP		 324 list_del(&l->list);
325 kfree(l);
326 }
327 }
0139418c
MH		 328}
329
4343478f 330static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
79d554a6 331{
4343478f
GP		 332 struct sock *sk = chan->sk;
333
8556edd3 334 if (sk->sk_type == SOCK_RAW) {
4343478f 335 switch (chan->sec_level) {
8556edd3
JH		 336 case BT_SECURITY_HIGH:
337 return HCI_AT_DEDICATED_BONDING_MITM;
338 case BT_SECURITY_MEDIUM:
339 return HCI_AT_DEDICATED_BONDING;
340 default:
341 return HCI_AT_NO_BONDING;
342 }
fe4128e0 343 } else if (chan->psm == cpu_to_le16(0x0001)) {
4343478f
GP		 344 if (chan->sec_level == BT_SECURITY_LOW)
345 chan->sec_level = BT_SECURITY_SDP;
0684e5f9 346
4343478f 347 if (chan->sec_level == BT_SECURITY_HIGH)
8556edd3 348 return HCI_AT_NO_BONDING_MITM;
00ae4af9 349 else
8556edd3 350 return HCI_AT_NO_BONDING;
00ae4af9 351 } else {
4343478f 352 switch (chan->sec_level) {
00ae4af9 353 case BT_SECURITY_HIGH:
8556edd3 354 return HCI_AT_GENERAL_BONDING_MITM;
00ae4af9 355 case BT_SECURITY_MEDIUM:
8556edd3 356 return HCI_AT_GENERAL_BONDING;
00ae4af9 357 default:
8556edd3 358 return HCI_AT_NO_BONDING;
00ae4af9 359 }
0684e5f9 360 }
8556edd3
JH		 361}
362
363/* Service level security */
4343478f 364static inline int l2cap_check_security(struct l2cap_chan *chan)
8556edd3 365{
8c1d787b 366 struct l2cap_conn *conn = chan->conn;
8556edd3
JH		 367 __u8 auth_type;
368
4343478f 369 auth_type = l2cap_get_auth_type(chan);
79d554a6 370
4343478f 371 return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
79d554a6
MH		 372}
373
68983259 374u8 l2cap_get_ident(struct l2cap_conn *conn)
4e8402a3
MH		 375{
376 u8 id;
377
378 /* Get next available identificator.
379 * 1 - 128 are used by kernel.
380 * 129 - 199 are reserved.
381 * 200 - 254 are used by utilities like l2ping, etc.
382 */
383
384 spin_lock_bh(&conn->lock);
385
386 if (++conn->tx_ident > 128)
387 conn->tx_ident = 1;
388
389 id = conn->tx_ident;
390
391 spin_unlock_bh(&conn->lock);
392
393 return id;
394}
395
68983259 396void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
4e8402a3
MH		 397{
398 struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
e702112f 399 u8 flags;
4e8402a3
MH		 400
401 BT_DBG("code 0x%2.2x", code);
402
403 if (!skb)
9a9c6a34 404 return;
4e8402a3 405
e702112f
AE		 406 if (lmp_no_flush_capable(conn->hcon->hdev))
407 flags = ACL_START_NO_FLUSH;
408 else
409 flags = ACL_START;
410
411 hci_send_acl(conn->hcon, skb, flags);
4e8402a3
MH		 412}
413
525cd185 414static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control)
1c2acffb
GP		 415{
416 struct sk_buff *skb;
417 struct l2cap_hdr *lh;
525cd185 418 struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
8c1d787b 419 struct l2cap_conn *conn = chan->conn;
c13ffa62 420 struct sock *sk = (struct sock *)pi;
fcc203c3 421 int count, hlen = L2CAP_HDR_SIZE + 2;
e702112f 422 u8 flags;
fcc203c3 423
c13ffa62
GP		 424 if (sk->sk_state != BT_CONNECTED)
425 return;
426
47d1ec61 427 if (chan->fcs == L2CAP_FCS_CRC16)
fcc203c3 428 hlen += 2;
1c2acffb 429
49208c9c 430 BT_DBG("chan %p, control 0x%2.2x", chan, control);
1c2acffb 431
fcc203c3 432 count = min_t(unsigned int, conn->mtu, hlen);
1c2acffb
GP		 433 control |= L2CAP_CTRL_FRAME_TYPE;
434
525cd185 435 if (chan->conn_state & L2CAP_CONN_SEND_FBIT) {
9e917af1 436 control |= L2CAP_CTRL_FINAL;
525cd185 437 chan->conn_state &= ~L2CAP_CONN_SEND_FBIT;
9e917af1
GP		 438 }
439
525cd185 440 if (chan->conn_state & L2CAP_CONN_SEND_PBIT) {
f0946ccf 441 control |= L2CAP_CTRL_POLL;
525cd185 442 chan->conn_state &= ~L2CAP_CONN_SEND_PBIT;
f0946ccf
GP		 443 }
444
1c2acffb
GP		 445 skb = bt_skb_alloc(count, GFP_ATOMIC);
446 if (!skb)
9a9c6a34 447 return;
1c2acffb
GP		 448
449 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fcc203c3 450 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
fe4128e0 451 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb
GP		 452 put_unaligned_le16(control, skb_put(skb, 2));
453
47d1ec61 454 if (chan->fcs == L2CAP_FCS_CRC16) {
fcc203c3
GP		 455 u16 fcs = crc16(0, (u8 *)lh, count - 2);
456 put_unaligned_le16(fcs, skb_put(skb, 2));
457 }
458
e702112f
AE		 459 if (lmp_no_flush_capable(conn->hcon->hdev))
460 flags = ACL_START_NO_FLUSH;
461 else
462 flags = ACL_START;
463
8c1d787b 464 hci_send_acl(chan->conn->hcon, skb, flags);
1c2acffb
GP		 465}
466
525cd185 467static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u16 control)
7e743090 468{
525cd185 469 if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
7e743090 470 control |= L2CAP_SUPER_RCV_NOT_READY;
525cd185 471 chan->conn_state |= L2CAP_CONN_RNR_SENT;
1890d36b 472 } else
7e743090
GP		 473 control |= L2CAP_SUPER_RCV_READY;
474
42e5c802 475 control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
2ab25cdd 476
525cd185 477 l2cap_send_sframe(chan, control);
7e743090
GP		 478}
479
b4450035 480static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
e501d055 481{
b4450035 482 return !(chan->conf_state & L2CAP_CONF_CONNECT_PEND);
e501d055
AE		 483}
484
fc7f8a7e 485static void l2cap_do_start(struct l2cap_chan *chan)
79d554a6 486{
8c1d787b 487 struct l2cap_conn *conn = chan->conn;
79d554a6
MH		 488
489 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
984947dc
MH		 490 if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
491 return;
492
4343478f
GP		 493 if (l2cap_check_security(chan) &&
494 __l2cap_no_conn_pending(chan)) {
b1235d79 495 struct l2cap_conn_req req;
fe4128e0
GP		 496 req.scid = cpu_to_le16(chan->scid);
497 req.psm = chan->psm;
79d554a6 498
fc7f8a7e 499 chan->ident = l2cap_get_ident(conn);
b4450035 500 chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
79d554a6 501
fc7f8a7e
GP		 502 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
503 sizeof(req), &req);
b1235d79 504 }
79d554a6
MH		 505 } else {
506 struct l2cap_info_req req;
507 req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
508
509 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
510 conn->info_ident = l2cap_get_ident(conn);
511
512 mod_timer(&conn->info_timer, jiffies +
513 msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
514
515 l2cap_send_cmd(conn, conn->info_ident,
516 L2CAP_INFO_REQ, sizeof(req), &req);
517 }
518}
519
cf6c2c0b
GP		 520static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
521{
522 u32 local_feat_mask = l2cap_feat_mask;
d1c4a17d 523 if (!disable_ertm)
cf6c2c0b
GP		 524 local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
525
526 switch (mode) {
527 case L2CAP_MODE_ERTM:
528 return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
529 case L2CAP_MODE_STREAMING:
530 return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
531 default:
532 return 0x00;
533 }
534}
535
e92c8e70 536void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
22121fc9 537{
e92c8e70 538 struct sock *sk;
22121fc9
GP		 539 struct l2cap_disconn_req req;
540
c13ffa62
GP		 541 if (!conn)
542 return;
543
e92c8e70
GP		 544 sk = chan->sk;
545
0c1bc5c6 546 if (chan->mode == L2CAP_MODE_ERTM) {
e92c8e70
GP		 547 del_timer(&chan->retrans_timer);
548 del_timer(&chan->monitor_timer);
549 del_timer(&chan->ack_timer);
c13ffa62
GP		 550 }
551
fe4128e0
GP		 552 req.dcid = cpu_to_le16(chan->dcid);
553 req.scid = cpu_to_le16(chan->scid);
22121fc9
GP		 554 l2cap_send_cmd(conn, l2cap_get_ident(conn),
555 L2CAP_DISCONN_REQ, sizeof(req), &req);
c13ffa62
GP		 556
557 sk->sk_state = BT_DISCONN;
9b108fc0 558 sk->sk_err = err;
22121fc9
GP		 559}
560
1da177e4 561/* ---- L2CAP connections ---- */
4e8402a3
MH		 562static void l2cap_conn_start(struct l2cap_conn *conn)
563{
820ffdb3 564 struct l2cap_chan *chan, *tmp;
4e8402a3
MH		 565
566 BT_DBG("conn %p", conn);
567
baa7e1fa 568 read_lock(&conn->chan_lock);
4e8402a3 569
820ffdb3 570 list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
48454079 571 struct sock *sk = chan->sk;
baa7e1fa 572
4e8402a3
MH		 573 bh_lock_sock(sk);
574
bd3c9e25
GP		 575 if (sk->sk_type != SOCK_SEQPACKET &&
576 sk->sk_type != SOCK_STREAM) {
79d554a6
MH		 577 bh_unlock_sock(sk);
578 continue;
579 }
580
581 if (sk->sk_state == BT_CONNECT) {
47731de7 582 struct l2cap_conn_req req;
79d554a6 583
4343478f 584 if (!l2cap_check_security(chan) ||
b4450035 585 !__l2cap_no_conn_pending(chan)) {
47731de7
GP		 586 bh_unlock_sock(sk);
587 continue;
588 }
79d554a6 589
0c1bc5c6 590 if (!l2cap_mode_supported(chan->mode,
47731de7 591 conn->feat_mask)
b4450035 592 && chan->conf_state &
47731de7 593 L2CAP_CONF_STATE2_DEVICE) {
820ffdb3
GP		 594 /* __l2cap_sock_close() calls list_del(chan)
595 * so release the lock */
596 read_unlock_bh(&conn->chan_lock);
597 __l2cap_sock_close(sk, ECONNRESET);
598 read_lock_bh(&conn->chan_lock);
47731de7
GP		 599 bh_unlock_sock(sk);
600 continue;
b1235d79 601 }
47731de7 602
fe4128e0
GP		 603 req.scid = cpu_to_le16(chan->scid);
604 req.psm = chan->psm;
47731de7 605
fc7f8a7e 606 chan->ident = l2cap_get_ident(conn);
b4450035 607 chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
47731de7 608
fc7f8a7e
GP		 609 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
610 sizeof(req), &req);
47731de7 611
79d554a6
MH		 612 } else if (sk->sk_state == BT_CONNECT2) {
613 struct l2cap_conn_rsp rsp;
e9aeb2dd 614 char buf[128];
fe4128e0
GP		 615 rsp.scid = cpu_to_le16(chan->dcid);
616 rsp.dcid = cpu_to_le16(chan->scid);
79d554a6 617
4343478f 618 if (l2cap_check_security(chan)) {
f66dc81f
MH		 619 if (bt_sk(sk)->defer_setup) {
620 struct sock *parent = bt_sk(sk)->parent;
621 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
622 rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
623 parent->sk_data_ready(parent, 0);
624
625 } else {
626 sk->sk_state = BT_CONFIG;
627 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
628 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
629 }
79d554a6
MH		 630 } else {
631 rsp.result = cpu_to_le16(L2CAP_CR_PEND);
632 rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
633 }
634
fc7f8a7e
GP		 635 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
636 sizeof(rsp), &rsp);
e9aeb2dd 637
b4450035 638 if (chan->conf_state & L2CAP_CONF_REQ_SENT ||
e9aeb2dd
GP		 639 rsp.result != L2CAP_CR_SUCCESS) {
640 bh_unlock_sock(sk);
641 continue;
642 }
643
b4450035 644 chan->conf_state |= L2CAP_CONF_REQ_SENT;
e9aeb2dd 645 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 646 l2cap_build_conf_req(chan, buf), buf);
647 chan->num_conf_req++;
4e8402a3
MH		 648 }
649
650 bh_unlock_sock(sk);
651 }
652
baa7e1fa 653 read_unlock(&conn->chan_lock);
4e8402a3
MH		 654}
655
b62f328b
VT		 656/* Find socket with cid and source bdaddr.
657 * Returns closest match, locked.
658 */
23691d75 659static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src)
b62f328b 660{
23691d75 661 struct l2cap_chan *c, *c1 = NULL;
b62f328b 662
23691d75 663 read_lock(&chan_list_lock);
b62f328b 664
23691d75
GP		 665 list_for_each_entry(c, &chan_list, global_l) {
666 struct sock *sk = c->sk;
fe4128e0 667
b62f328b
VT		 668 if (state && sk->sk_state != state)
669 continue;
670
23691d75 671 if (c->scid == cid) {
b62f328b 672 /* Exact match. */
23691d75
GP		 673 if (!bacmp(&bt_sk(sk)->src, src)) {
674 read_unlock(&chan_list_lock);
675 return c;
676 }
b62f328b
VT		 677
678 /* Closest match */
679 if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
23691d75 680 c1 = c;
b62f328b
VT		 681 }
682 }
280f294f 683
23691d75 684 read_unlock(&chan_list_lock);
b62f328b 685
23691d75 686 return c1;
b62f328b
VT		 687}
688
689static void l2cap_le_conn_ready(struct l2cap_conn *conn)
690{
c916fbe4 691 struct sock *parent, *sk;
23691d75 692 struct l2cap_chan *chan, *pchan;
b62f328b
VT		 693
694 BT_DBG("");
695
696 /* Check if we have socket listening on cid */
23691d75 697 pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
b62f328b 698 conn->src);
23691d75 699 if (!pchan)
b62f328b
VT		 700 return;
701
23691d75
GP		 702 parent = pchan->sk;
703
62f3a2cf
GP		 704 bh_lock_sock(parent);
705
b62f328b
VT		 706 /* Check for backlog size */
707 if (sk_acceptq_is_full(parent)) {
708 BT_DBG("backlog full %d", parent->sk_ack_backlog);
709 goto clean;
710 }
711
712 sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
713 if (!sk)
714 goto clean;
715
23691d75 716 chan = l2cap_chan_create(sk);
48454079
GP		 717 if (!chan) {
718 l2cap_sock_kill(sk);
719 goto clean;
720 }
721
5d41ce1d
GP		 722 l2cap_pi(sk)->chan = chan;
723
baa7e1fa 724 write_lock_bh(&conn->chan_lock);
b62f328b
VT		 725
726 hci_conn_hold(conn->hcon);
727
728 l2cap_sock_init(sk, parent);
baa7e1fa 729
b62f328b
VT		 730 bacpy(&bt_sk(sk)->src, conn->src);
731 bacpy(&bt_sk(sk)->dst, conn->dst);
732
d1010240
GP		 733 bt_accept_enqueue(parent, sk);
734
48454079
GP		 735 __l2cap_chan_add(conn, chan);
736
b62f328b
VT		 737 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
738
739 sk->sk_state = BT_CONNECTED;
740 parent->sk_data_ready(parent, 0);
741
baa7e1fa 742 write_unlock_bh(&conn->chan_lock);
b62f328b
VT		 743
744clean:
745 bh_unlock_sock(parent);
746}
747
4e8402a3
MH		 748static void l2cap_conn_ready(struct l2cap_conn *conn)
749{
48454079 750 struct l2cap_chan *chan;
4e8402a3 751
79d554a6 752 BT_DBG("conn %p", conn);
4e8402a3 753
b62f328b
VT		 754 if (!conn->hcon->out && conn->hcon->type == LE_LINK)
755 l2cap_le_conn_ready(conn);
756
baa7e1fa 757 read_lock(&conn->chan_lock);
4e8402a3 758
baa7e1fa 759 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 760 struct sock *sk = chan->sk;
baa7e1fa 761
79d554a6 762 bh_lock_sock(sk);
4e8402a3 763
acd7d370
VT		 764 if (conn->hcon->type == LE_LINK) {
765 l2cap_sock_clear_timer(sk);
766 sk->sk_state = BT_CONNECTED;
767 sk->sk_state_change(sk);
768 }
769
bd3c9e25
GP		 770 if (sk->sk_type != SOCK_SEQPACKET &&
771 sk->sk_type != SOCK_STREAM) {
79d554a6
MH		 772 l2cap_sock_clear_timer(sk);
773 sk->sk_state = BT_CONNECTED;
774 sk->sk_state_change(sk);
775 } else if (sk->sk_state == BT_CONNECT)
fc7f8a7e 776 l2cap_do_start(chan);
4e8402a3 777
79d554a6 778 bh_unlock_sock(sk);
4e8402a3 779 }
79d554a6 780
baa7e1fa 781 read_unlock(&conn->chan_lock);
4e8402a3
MH		 782}
783
784/* Notify sockets that we cannot guaranty reliability anymore */
785static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
786{
48454079 787 struct l2cap_chan *chan;
4e8402a3
MH		 788
789 BT_DBG("conn %p", conn);
790
baa7e1fa 791 read_lock(&conn->chan_lock);
4e8402a3 792
baa7e1fa 793 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 794 struct sock *sk = chan->sk;
baa7e1fa 795
4343478f 796 if (chan->force_reliable)
4e8402a3
MH		 797 sk->sk_err = err;
798 }
799
baa7e1fa 800 read_unlock(&conn->chan_lock);
4e8402a3
MH		 801}
802
803static void l2cap_info_timeout(unsigned long arg)
804{
805 struct l2cap_conn *conn = (void *) arg;
806
984947dc 807 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 808 conn->info_ident = 0;
984947dc 809
4e8402a3
MH		 810 l2cap_conn_start(conn);
811}
812
1da177e4
LT		 813static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
814{
0139418c 815 struct l2cap_conn *conn = hcon->l2cap_data;
1da177e4 816
0139418c 817 if (conn || status)
1da177e4
LT		 818 return conn;
819
0139418c
MH		 820 conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
821 if (!conn)
1da177e4 822 return NULL;
1da177e4
LT		 823
824 hcon->l2cap_data = conn;
825 conn->hcon = hcon;
826
0139418c
MH		 827 BT_DBG("hcon %p conn %p", hcon, conn);
828
acd7d370
VT		 829 if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
830 conn->mtu = hcon->hdev->le_mtu;
831 else
832 conn->mtu = hcon->hdev->acl_mtu;
833
1da177e4
LT		 834 conn->src = &hcon->hdev->bdaddr;
835 conn->dst = &hcon->dst;
836
4e8402a3
MH		 837 conn->feat_mask = 0;
838
1da177e4 839 spin_lock_init(&conn->lock);
baa7e1fa
GP		 840 rwlock_init(&conn->chan_lock);
841
842 INIT_LIST_HEAD(&conn->chan_l);
1da177e4 843
b62f328b
VT		 844 if (hcon->type != LE_LINK)
845 setup_timer(&conn->info_timer, l2cap_info_timeout,
45054dc1
DY		 846 (unsigned long) conn);
847
2950f21a
MH		 848 conn->disc_reason = 0x13;
849
1da177e4
LT		 850 return conn;
851}
852
0139418c 853static void l2cap_conn_del(struct hci_conn *hcon, int err)
1da177e4 854{
0139418c 855 struct l2cap_conn *conn = hcon->l2cap_data;
baa7e1fa 856 struct l2cap_chan *chan, *l;
1da177e4
LT		 857 struct sock *sk;
858
0139418c
MH		 859 if (!conn)
860 return;
1da177e4
LT		 861
862 BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
863
7585b97a 864 kfree_skb(conn->rx_skb);
1da177e4
LT		 865
866 /* Kill channels */
baa7e1fa 867 list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
48454079 868 sk = chan->sk;
1da177e4 869 bh_lock_sock(sk);
48454079 870 l2cap_chan_del(chan, err);
1da177e4
LT		 871 bh_unlock_sock(sk);
872 l2cap_sock_kill(sk);
873 }
874
8e8440f5
DY		 875 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
876 del_timer_sync(&conn->info_timer);
3ab22731 877
1da177e4
LT		 878 hcon->l2cap_data = NULL;
879 kfree(conn);
1da177e4
LT		 880}
881
48454079 882static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
1da177e4 883{
baa7e1fa 884 write_lock_bh(&conn->chan_lock);
48454079 885 __l2cap_chan_add(conn, chan);
baa7e1fa 886 write_unlock_bh(&conn->chan_lock);
1da177e4
LT		 887}
888
1da177e4 889/* ---- Socket interface ---- */
1da177e4
LT		 890
891/* Find socket with psm and source bdaddr.
892 * Returns closest match.
893 */
23691d75 894static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src)
1da177e4 895{
23691d75 896 struct l2cap_chan *c, *c1 = NULL;
1da177e4 897
23691d75 898 read_lock(&chan_list_lock);
e0f0cb56 899
23691d75
GP		 900 list_for_each_entry(c, &chan_list, global_l) {
901 struct sock *sk = c->sk;
fe4128e0 902
1da177e4
LT		 903 if (state && sk->sk_state != state)
904 continue;
905
23691d75 906 if (c->psm == psm) {
1da177e4 907 /* Exact match. */
23691d75 908 if (!bacmp(&bt_sk(sk)->src, src)) {
a7567b20 909 read_unlock(&chan_list_lock);
23691d75
GP		 910 return c;
911 }
1da177e4
LT		 912
913 /* Closest match */
914 if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
23691d75 915 c1 = c;
1da177e4
LT		 916 }
917 }
1da177e4 918
23691d75 919 read_unlock(&chan_list_lock);
e0f0cb56 920
23691d75 921 return c1;
1da177e4
LT		 922}
923
77a74c7e 924int l2cap_chan_connect(struct l2cap_chan *chan)
1da177e4 925{
5d41ce1d 926 struct sock *sk = chan->sk;
1da177e4
LT		 927 bdaddr_t *src = &bt_sk(sk)->src;
928 bdaddr_t *dst = &bt_sk(sk)->dst;
929 struct l2cap_conn *conn;
930 struct hci_conn *hcon;
931 struct hci_dev *hdev;
09ab6f4c 932 __u8 auth_type;
44d0e48e 933 int err;
1da177e4 934
f29972de 935 BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
fe4128e0 936 chan->psm);
1da177e4 937
af05b30b
GP		 938 hdev = hci_get_route(dst, src);
939 if (!hdev)
1da177e4
LT		 940 return -EHOSTUNREACH;
941
942 hci_dev_lock_bh(hdev);
943
4343478f 944 auth_type = l2cap_get_auth_type(chan);
09ab6f4c 945
fe4128e0 946 if (chan->dcid == L2CAP_CID_LE_DATA)
acd7d370 947 hcon = hci_connect(hdev, LE_LINK, dst,
4343478f 948 chan->sec_level, auth_type);
acd7d370
VT		 949 else
950 hcon = hci_connect(hdev, ACL_LINK, dst,
4343478f 951 chan->sec_level, auth_type);
acd7d370 952
30e76272
VT		 953 if (IS_ERR(hcon)) {
954 err = PTR_ERR(hcon);
1da177e4 955 goto done;
30e76272 956 }
1da177e4
LT		 957
958 conn = l2cap_conn_add(hcon, 0);
959 if (!conn) {
960 hci_conn_put(hcon);
30e76272 961 err = -ENOMEM;
1da177e4
LT		 962 goto done;
963 }
964
1da177e4
LT		 965 /* Update source addr of the socket */
966 bacpy(src, conn->src);
967
48454079
GP		 968 l2cap_chan_add(conn, chan);
969
1da177e4
LT		 970 sk->sk_state = BT_CONNECT;
971 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
972
973 if (hcon->state == BT_CONNECTED) {
bd3c9e25
GP		 974 if (sk->sk_type != SOCK_SEQPACKET &&
975 sk->sk_type != SOCK_STREAM) {
1da177e4 976 l2cap_sock_clear_timer(sk);
4343478f 977 if (l2cap_check_security(chan))
d00ef24f 978 sk->sk_state = BT_CONNECTED;
79d554a6 979 } else
fc7f8a7e 980 l2cap_do_start(chan);
1da177e4
LT		 981 }
982
30e76272
VT		 983 err = 0;
984
1da177e4
LT		 985done:
986 hci_dev_unlock_bh(hdev);
987 hci_dev_put(hdev);
988 return err;
989}
990
dcba0dba 991int __l2cap_wait_ack(struct sock *sk)
6161c038 992{
8c1d787b 993 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
6161c038
GP		 994 DECLARE_WAITQUEUE(wait, current);
995 int err = 0;
996 int timeo = HZ/5;
997
2b0b05dd 998 add_wait_queue(sk_sleep(sk), &wait);
8c1d787b 999 while ((chan->unacked_frames > 0 && chan->conn)) {
6161c038
GP		 1000 set_current_state(TASK_INTERRUPTIBLE);
1001
1002 if (!timeo)
1003 timeo = HZ/5;
1004
1005 if (signal_pending(current)) {
1006 err = sock_intr_errno(timeo);
1007 break;
1008 }
1009
1010 release_sock(sk);
1011 timeo = schedule_timeout(timeo);
1012 lock_sock(sk);
1013
1014 err = sock_error(sk);
1015 if (err)
1016 break;
1017 }
1018 set_current_state(TASK_RUNNING);
2b0b05dd 1019 remove_wait_queue(sk_sleep(sk), &wait);
6161c038
GP		 1020 return err;
1021}
1022
e90bac06
GP		 1023static void l2cap_monitor_timeout(unsigned long arg)
1024{
525cd185
GP		 1025 struct l2cap_chan *chan = (void *) arg;
1026 struct sock *sk = chan->sk;
e90bac06 1027
525cd185 1028 BT_DBG("chan %p", chan);
0e98958d 1029
e686219a 1030 bh_lock_sock(sk);
2c03a7a4 1031 if (chan->retry_count >= chan->remote_max_tx) {
8c1d787b 1032 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
b13f5860 1033 bh_unlock_sock(sk);
e90bac06
GP		 1034 return;
1035 }
1036
6a026610 1037 chan->retry_count++;
e90bac06
GP		 1038 __mod_monitor_timer();
1039
525cd185 1040 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
e686219a 1041 bh_unlock_sock(sk);
e90bac06
GP		 1042}
1043
1044static void l2cap_retrans_timeout(unsigned long arg)
1045{
525cd185
GP		 1046 struct l2cap_chan *chan = (void *) arg;
1047 struct sock *sk = chan->sk;
e90bac06 1048
49208c9c 1049 BT_DBG("chan %p", chan);
0e98958d 1050
e686219a 1051 bh_lock_sock(sk);
6a026610 1052 chan->retry_count = 1;
e90bac06
GP		 1053 __mod_monitor_timer();
1054
525cd185 1055 chan->conn_state |= L2CAP_CONN_WAIT_F;
e90bac06 1056
525cd185 1057 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
e686219a 1058 bh_unlock_sock(sk);
e90bac06
GP		 1059}
1060
42e5c802 1061static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
1da177e4 1062{
1c2acffb 1063 struct sk_buff *skb;
1da177e4 1064
58d35f87 1065 while ((skb = skb_peek(&chan->tx_q)) &&
6a026610 1066 chan->unacked_frames) {
42e5c802 1067 if (bt_cb(skb)->tx_seq == chan->expected_ack_seq)
1c2acffb 1068 break;
1da177e4 1069
58d35f87 1070 skb = skb_dequeue(&chan->tx_q);
1c2acffb 1071 kfree_skb(skb);
1da177e4 1072
6a026610 1073 chan->unacked_frames--;
1c2acffb 1074 }
1da177e4 1075
6a026610 1076 if (!chan->unacked_frames)
e92c8e70 1077 del_timer(&chan->retrans_timer);
1c2acffb 1078}
1da177e4 1079
4343478f 1080void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
1c2acffb 1081{
8c1d787b 1082 struct hci_conn *hcon = chan->conn->hcon;
e702112f 1083 u16 flags;
1c2acffb 1084
4343478f 1085 BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len);
1da177e4 1086
4343478f 1087 if (!chan->flushable && lmp_no_flush_capable(hcon->hdev))
e702112f
AE		 1088 flags = ACL_START_NO_FLUSH;
1089 else
1090 flags = ACL_START;
1091
1092 hci_send_acl(hcon, skb, flags);
1c2acffb
GP		 1093}
1094
42e5c802 1095void l2cap_streaming_send(struct l2cap_chan *chan)
6840ed07 1096{
ccbb84af 1097 struct sk_buff *skb;
fcc203c3 1098 u16 control, fcs;
6840ed07 1099
58d35f87 1100 while ((skb = skb_dequeue(&chan->tx_q))) {
ccbb84af 1101 control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE);
42e5c802 1102 control |= chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
ccbb84af 1103 put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE);
6840ed07 1104
47d1ec61 1105 if (chan->fcs == L2CAP_FCS_CRC16) {
ccbb84af
GP		 1106 fcs = crc16(0, (u8 *)skb->data, skb->len - 2);
1107 put_unaligned_le16(fcs, skb->data + skb->len - 2);
fcc203c3
GP		 1108 }
1109
4343478f 1110 l2cap_do_send(chan, skb);
6840ed07 1111
42e5c802 1112 chan->next_tx_seq = (chan->next_tx_seq + 1) % 64;
6840ed07 1113 }
6840ed07
GP		 1114}
1115
525cd185 1116static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq)
8f17154f 1117{
8f17154f
GP		 1118 struct sk_buff *skb, *tx_skb;
1119 u16 control, fcs;
8f17154f 1120
58d35f87 1121 skb = skb_peek(&chan->tx_q);
f11d676d
GP		 1122 if (!skb)
1123 return;
8f17154f 1124
f11d676d
GP		 1125 do {
1126 if (bt_cb(skb)->tx_seq == tx_seq)
8f17154f 1127 break;
8f17154f 1128
58d35f87 1129 if (skb_queue_is_last(&chan->tx_q, skb))
f11d676d 1130 return;
8f17154f 1131
58d35f87 1132 } while ((skb = skb_queue_next(&chan->tx_q, skb)));
8f17154f 1133
2c03a7a4
GP		 1134 if (chan->remote_max_tx &&
1135 bt_cb(skb)->retries == chan->remote_max_tx) {
8c1d787b 1136 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
f11d676d
GP		 1137 return;
1138 }
1139
1140 tx_skb = skb_clone(skb, GFP_ATOMIC);
1141 bt_cb(skb)->retries++;
1142 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
a429b519 1143 control &= L2CAP_CTRL_SAR;
3cb123d1 1144
525cd185 1145 if (chan->conn_state & L2CAP_CONN_SEND_FBIT) {
3cb123d1 1146 control |= L2CAP_CTRL_FINAL;
525cd185 1147 chan->conn_state &= ~L2CAP_CONN_SEND_FBIT;
3cb123d1 1148 }
95ffa978 1149
42e5c802 1150 control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
f11d676d 1151 | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
3cb123d1 1152
f11d676d
GP		 1153 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1154
47d1ec61 1155 if (chan->fcs == L2CAP_FCS_CRC16) {
f11d676d
GP		 1156 fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
1157 put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
1158 }
1159
4343478f 1160 l2cap_do_send(chan, tx_skb);
8f17154f
GP		 1161}
1162
525cd185 1163int l2cap_ertm_send(struct l2cap_chan *chan)
1c2acffb
GP		 1164{
1165 struct sk_buff *skb, *tx_skb;
525cd185 1166 struct sock *sk = chan->sk;
fcc203c3 1167 u16 control, fcs;
9a9c6a34 1168 int nsent = 0;
1c2acffb 1169
c13ffa62
GP		 1170 if (sk->sk_state != BT_CONNECTED)
1171 return -ENOTCONN;
e90bac06 1172
58d35f87 1173 while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
1c2acffb 1174
2c03a7a4
GP		 1175 if (chan->remote_max_tx &&
1176 bt_cb(skb)->retries == chan->remote_max_tx) {
8c1d787b 1177 l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
e90bac06
GP		 1178 break;
1179 }
1180
e420aba3
AE		 1181 tx_skb = skb_clone(skb, GFP_ATOMIC);
1182
e90bac06
GP		 1183 bt_cb(skb)->retries++;
1184
1c2acffb 1185 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
95ffa978
GP		 1186 control &= L2CAP_CTRL_SAR;
1187
525cd185 1188 if (chan->conn_state & L2CAP_CONN_SEND_FBIT) {
d5392c8f 1189 control |= L2CAP_CTRL_FINAL;
525cd185 1190 chan->conn_state &= ~L2CAP_CONN_SEND_FBIT;
d5392c8f 1191 }
42e5c802
GP		 1192 control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
1193 | (chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
1c2acffb
GP		 1194 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1195
e90bac06 1196
47d1ec61 1197 if (chan->fcs == L2CAP_FCS_CRC16) {
fcc203c3
GP		 1198 fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
1199 put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
1200 }
1201
4343478f 1202 l2cap_do_send(chan, tx_skb);
9a9c6a34 1203
e90bac06 1204 __mod_retrans_timer();
1c2acffb 1205
42e5c802
GP		 1206 bt_cb(skb)->tx_seq = chan->next_tx_seq;
1207 chan->next_tx_seq = (chan->next_tx_seq + 1) % 64;
1c2acffb 1208
23e9fde2 1209 if (bt_cb(skb)->retries == 1)
6a026610 1210 chan->unacked_frames++;
23e9fde2 1211
6a026610 1212 chan->frames_sent++;
1c2acffb 1213
58d35f87
GP		 1214 if (skb_queue_is_last(&chan->tx_q, skb))
1215 chan->tx_send_head = NULL;
1c2acffb 1216 else
58d35f87 1217 chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
9e917af1
GP		 1218
1219 nsent++;
1c2acffb
GP		 1220 }
1221
9e917af1
GP		 1222 return nsent;
1223}
1224
525cd185 1225static int l2cap_retransmit_frames(struct l2cap_chan *chan)
dfc909be 1226{
dfc909be
GP		 1227 int ret;
1228
58d35f87
GP		 1229 if (!skb_queue_empty(&chan->tx_q))
1230 chan->tx_send_head = chan->tx_q.next;
dfc909be 1231
42e5c802 1232 chan->next_tx_seq = chan->expected_ack_seq;
525cd185 1233 ret = l2cap_ertm_send(chan);
dfc909be
GP		 1234 return ret;
1235}
1236
525cd185 1237static void l2cap_send_ack(struct l2cap_chan *chan)
9e917af1 1238{
9e917af1
GP		 1239 u16 control = 0;
1240
42e5c802 1241 control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
9e917af1 1242
525cd185 1243 if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
9e917af1 1244 control |= L2CAP_SUPER_RCV_NOT_READY;
525cd185
GP		 1245 chan->conn_state |= L2CAP_CONN_RNR_SENT;
1246 l2cap_send_sframe(chan, control);
9a9c6a34 1247 return;
9e917af1 1248 }
dfc909be 1249
525cd185 1250 if (l2cap_ertm_send(chan) > 0)
dfc909be
GP		 1251 return;
1252
1253 control |= L2CAP_SUPER_RCV_READY;
525cd185 1254 l2cap_send_sframe(chan, control);
1c2acffb
GP		 1255}
1256
525cd185 1257static void l2cap_send_srejtail(struct l2cap_chan *chan)
99b0d4b7
GP		 1258{
1259 struct srej_list *tail;
1260 u16 control;
1261
1262 control = L2CAP_SUPER_SELECT_REJECT;
1263 control |= L2CAP_CTRL_FINAL;
1264
39d5a3ee 1265 tail = list_entry((&chan->srej_l)->prev, struct srej_list, list);
99b0d4b7
GP		 1266 control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1267
525cd185 1268 l2cap_send_sframe(chan, control);
99b0d4b7
GP		 1269}
1270
1c2acffb
GP		 1271static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
1272{
8c1d787b 1273 struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn;
1c2acffb
GP		 1274 struct sk_buff **frag;
1275 int err, sent = 0;
1da177e4 1276
59203a21 1277 if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
1c2acffb 1278 return -EFAULT;
1da177e4
LT		 1279
1280 sent += count;
1281 len -= count;
1282
1283 /* Continuation fragments (no L2CAP header) */
1284 frag = &skb_shinfo(skb)->frag_list;
1285 while (len) {
1286 count = min_t(unsigned int, conn->mtu, len);
1287
1288 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1289 if (!*frag)
0175d629 1290 return err;
1c2acffb
GP		 1291 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1292 return -EFAULT;
1da177e4
LT		 1293
1294 sent += count;
1295 len -= count;
1296
1297 frag = &(*frag)->next;
1298 }
1da177e4
LT		 1299
1300 return sent;
1c2acffb 1301}
1da177e4 1302
fe4128e0 1303struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
1c2acffb 1304{
fe4128e0 1305 struct sock *sk = chan->sk;
8c1d787b 1306 struct l2cap_conn *conn = chan->conn;
1c2acffb
GP		 1307 struct sk_buff *skb;
1308 int err, count, hlen = L2CAP_HDR_SIZE + 2;
1309 struct l2cap_hdr *lh;
1310
1311 BT_DBG("sk %p len %d", sk, (int)len);
1312
1313 count = min_t(unsigned int, (conn->mtu - hlen), len);
1314 skb = bt_skb_send_alloc(sk, count + hlen,
1315 msg->msg_flags & MSG_DONTWAIT, &err);
1316 if (!skb)
0175d629 1317 return ERR_PTR(err);
1c2acffb
GP		 1318
1319 /* Create L2CAP header */
1320 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 1321 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb 1322 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
fe4128e0 1323 put_unaligned_le16(chan->psm, skb_put(skb, 2));
1c2acffb
GP		 1324
1325 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1326 if (unlikely(err < 0)) {
1327 kfree_skb(skb);
1328 return ERR_PTR(err);
1329 }
1330 return skb;
1331}
1332
fe4128e0 1333struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
1c2acffb 1334{
fe4128e0 1335 struct sock *sk = chan->sk;
8c1d787b 1336 struct l2cap_conn *conn = chan->conn;
1c2acffb
GP		 1337 struct sk_buff *skb;
1338 int err, count, hlen = L2CAP_HDR_SIZE;
1339 struct l2cap_hdr *lh;
1340
1341 BT_DBG("sk %p len %d", sk, (int)len);
1342
1343 count = min_t(unsigned int, (conn->mtu - hlen), len);
1344 skb = bt_skb_send_alloc(sk, count + hlen,
1345 msg->msg_flags & MSG_DONTWAIT, &err);
1346 if (!skb)
0175d629 1347 return ERR_PTR(err);
1c2acffb
GP		 1348
1349 /* Create L2CAP header */
1350 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 1351 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb
GP		 1352 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1353
1354 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1355 if (unlikely(err < 0)) {
1356 kfree_skb(skb);
1357 return ERR_PTR(err);
1358 }
1359 return skb;
1360}
1361
47d1ec61 1362struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
1c2acffb 1363{
47d1ec61 1364 struct sock *sk = chan->sk;
8c1d787b 1365 struct l2cap_conn *conn = chan->conn;
1c2acffb
GP		 1366 struct sk_buff *skb;
1367 int err, count, hlen = L2CAP_HDR_SIZE + 2;
1368 struct l2cap_hdr *lh;
1369
1370 BT_DBG("sk %p len %d", sk, (int)len);
1371
0ee0d208
GP		 1372 if (!conn)
1373 return ERR_PTR(-ENOTCONN);
1374
c74e560c
GP		 1375 if (sdulen)
1376 hlen += 2;
1377
47d1ec61 1378 if (chan->fcs == L2CAP_FCS_CRC16)
fcc203c3
GP		 1379 hlen += 2;
1380
1c2acffb
GP		 1381 count = min_t(unsigned int, (conn->mtu - hlen), len);
1382 skb = bt_skb_send_alloc(sk, count + hlen,
1383 msg->msg_flags & MSG_DONTWAIT, &err);
1384 if (!skb)
0175d629 1385 return ERR_PTR(err);
1c2acffb
GP		 1386
1387 /* Create L2CAP header */
1388 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
fe4128e0 1389 lh->cid = cpu_to_le16(chan->dcid);
1c2acffb
GP		 1390 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1391 put_unaligned_le16(control, skb_put(skb, 2));
c74e560c
GP		 1392 if (sdulen)
1393 put_unaligned_le16(sdulen, skb_put(skb, 2));
1c2acffb
GP		 1394
1395 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1396 if (unlikely(err < 0)) {
1397 kfree_skb(skb);
1398 return ERR_PTR(err);
1399 }
e90bac06 1400
47d1ec61 1401 if (chan->fcs == L2CAP_FCS_CRC16)
fcc203c3
GP		 1402 put_unaligned_le16(0, skb_put(skb, 2));
1403
e90bac06 1404 bt_cb(skb)->retries = 0;
1c2acffb 1405 return skb;
1da177e4
LT		 1406}
1407
2c03a7a4 1408int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
c74e560c 1409{
c74e560c
GP		 1410 struct sk_buff *skb;
1411 struct sk_buff_head sar_queue;
1412 u16 control;
1413 size_t size = 0;
1414
ff12fd64 1415 skb_queue_head_init(&sar_queue);
c74e560c 1416 control = L2CAP_SDU_START;
47d1ec61 1417 skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len);
c74e560c
GP		 1418 if (IS_ERR(skb))
1419 return PTR_ERR(skb);
1420
1421 __skb_queue_tail(&sar_queue, skb);
2c03a7a4
GP		 1422 len -= chan->remote_mps;
1423 size += chan->remote_mps;
c74e560c
GP		 1424
1425 while (len > 0) {
1426 size_t buflen;
1427
2c03a7a4 1428 if (len > chan->remote_mps) {
44651b85 1429 control = L2CAP_SDU_CONTINUE;
2c03a7a4 1430 buflen = chan->remote_mps;
c74e560c 1431 } else {
44651b85 1432 control = L2CAP_SDU_END;
c74e560c
GP		 1433 buflen = len;
1434 }
1435
47d1ec61 1436 skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0);
c74e560c
GP		 1437 if (IS_ERR(skb)) {
1438 skb_queue_purge(&sar_queue);
1439 return PTR_ERR(skb);
1440 }
1441
1442 __skb_queue_tail(&sar_queue, skb);
1443 len -= buflen;
1444 size += buflen;
c74e560c 1445 }
58d35f87
GP		 1446 skb_queue_splice_tail(&sar_queue, &chan->tx_q);
1447 if (chan->tx_send_head == NULL)
1448 chan->tx_send_head = sar_queue.next;
c74e560c
GP		 1449
1450 return size;
1451}
1452
1da177e4
LT		 1453static void l2cap_chan_ready(struct sock *sk)
1454{
1455 struct sock *parent = bt_sk(sk)->parent;
b4450035 1456 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
1da177e4
LT		 1457
1458 BT_DBG("sk %p, parent %p", sk, parent);
1459
b4450035 1460 chan->conf_state = 0;
1da177e4
LT		 1461 l2cap_sock_clear_timer(sk);
1462
1463 if (!parent) {
1464 /* Outgoing channel.
1465 * Wake up socket sleeping on connect.
1466 */
1467 sk->sk_state = BT_CONNECTED;
1468 sk->sk_state_change(sk);
1469 } else {
1470 /* Incoming channel.
1471 * Wake up socket sleeping on accept.
1472 */
1473 parent->sk_data_ready(parent, 0);
1474 }
1475}
1476
1477/* Copy frame to all raw sockets on that connection */
1478static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
1479{
1da177e4 1480 struct sk_buff *nskb;
48454079 1481 struct l2cap_chan *chan;
1da177e4
LT		 1482
1483 BT_DBG("conn %p", conn);
1484
baa7e1fa
GP		 1485 read_lock(&conn->chan_lock);
1486 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 1487 struct sock *sk = chan->sk;
1da177e4
LT		 1488 if (sk->sk_type != SOCK_RAW)
1489 continue;
1490
1491 /* Don't send frame to the socket it came from */
1492 if (skb->sk == sk)
1493 continue;
af05b30b
GP		 1494 nskb = skb_clone(skb, GFP_ATOMIC);
1495 if (!nskb)
1da177e4
LT		 1496 continue;
1497
1498 if (sock_queue_rcv_skb(sk, nskb))
1499 kfree_skb(nskb);
1500 }
baa7e1fa 1501 read_unlock(&conn->chan_lock);
1da177e4
LT		 1502}
1503
1504/* ---- L2CAP signalling commands ---- */
1505static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
1506 u8 code, u8 ident, u16 dlen, void *data)
1507{
1508 struct sk_buff *skb, **frag;
1509 struct l2cap_cmd_hdr *cmd;
1510 struct l2cap_hdr *lh;
1511 int len, count;
1512
af05b30b
GP		 1513 BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d",
1514 conn, code, ident, dlen);
1da177e4
LT		 1515
1516 len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
1517 count = min_t(unsigned int, conn->mtu, len);
1518
1519 skb = bt_skb_alloc(count, GFP_ATOMIC);
1520 if (!skb)
1521 return NULL;
1522
1523 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
aca3192c 1524 lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
3300d9a9
CT		 1525
1526 if (conn->hcon->type == LE_LINK)
1527 lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
1528 else
1529 lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
1da177e4
LT		 1530
1531 cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
1532 cmd->code = code;
1533 cmd->ident = ident;
aca3192c 1534 cmd->len = cpu_to_le16(dlen);
1da177e4
LT		 1535
1536 if (dlen) {
1537 count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE;
1538 memcpy(skb_put(skb, count), data, count);
1539 data += count;
1540 }
1541
1542 len -= skb->len;
1543
1544 /* Continuation fragments (no L2CAP header) */
1545 frag = &skb_shinfo(skb)->frag_list;
1546 while (len) {
1547 count = min_t(unsigned int, conn->mtu, len);
1548
1549 *frag = bt_skb_alloc(count, GFP_ATOMIC);
1550 if (!*frag)
1551 goto fail;
1552
1553 memcpy(skb_put(*frag, count), data, count);
1554
1555 len -= count;
1556 data += count;
1557
1558 frag = &(*frag)->next;
1559 }
1560
1561 return skb;
1562
1563fail:
1564 kfree_skb(skb);
1565 return NULL;
1566}
1567
1568static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
1569{
1570 struct l2cap_conf_opt *opt = *ptr;
1571 int len;
1572
1573 len = L2CAP_CONF_OPT_SIZE + opt->len;
1574 *ptr += len;
1575
1576 *type = opt->type;
1577 *olen = opt->len;
1578
1579 switch (opt->len) {
1580 case 1:
1581 *val = *((u8 *) opt->val);
1582 break;
1583
1584 case 2:
bfaaeb3e 1585 *val = get_unaligned_le16(opt->val);
1da177e4
LT		 1586 break;
1587
1588 case 4:
bfaaeb3e 1589 *val = get_unaligned_le32(opt->val);
1da177e4
LT		 1590 break;
1591
1592 default:
1593 *val = (unsigned long) opt->val;
1594 break;
1595 }
1596
1597 BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val);
1598 return len;
1599}
1600
1da177e4
LT		 1601static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1602{
1603 struct l2cap_conf_opt *opt = *ptr;
1604
1605 BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val);
1606
1607 opt->type = type;
1608 opt->len = len;
1609
1610 switch (len) {
1611 case 1:
1612 *((u8 *) opt->val) = val;
1613 break;
1614
1615 case 2:
4f8b691c 1616 put_unaligned_le16(val, opt->val);
1da177e4
LT		 1617 break;
1618
1619 case 4:
4f8b691c 1620 put_unaligned_le32(val, opt->val);
1da177e4
LT		 1621 break;
1622
1623 default:
1624 memcpy(opt->val, (void *) val, len);
1625 break;
1626 }
1627
1628 *ptr += L2CAP_CONF_OPT_SIZE + len;
1629}
1630
c1b4f43b
GP		 1631static void l2cap_ack_timeout(unsigned long arg)
1632{
525cd185 1633 struct l2cap_chan *chan = (void *) arg;
c1b4f43b 1634
525cd185
GP		 1635 bh_lock_sock(chan->sk);
1636 l2cap_send_ack(chan);
1637 bh_unlock_sock(chan->sk);
c1b4f43b
GP		 1638}
1639
525cd185 1640static inline void l2cap_ertm_init(struct l2cap_chan *chan)
0565c1c2 1641{
525cd185
GP		 1642 struct sock *sk = chan->sk;
1643
42e5c802 1644 chan->expected_ack_seq = 0;
6a026610 1645 chan->unacked_frames = 0;
42e5c802 1646 chan->buffer_seq = 0;
6a026610
GP		 1647 chan->num_acked = 0;
1648 chan->frames_sent = 0;
0565c1c2 1649
e92c8e70
GP		 1650 setup_timer(&chan->retrans_timer, l2cap_retrans_timeout,
1651 (unsigned long) chan);
1652 setup_timer(&chan->monitor_timer, l2cap_monitor_timeout,
1653 (unsigned long) chan);
1654 setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan);
0565c1c2 1655
f1c6775b
GP		 1656 skb_queue_head_init(&chan->srej_q);
1657 skb_queue_head_init(&chan->busy_q);
1890d36b 1658
39d5a3ee
GP		 1659 INIT_LIST_HEAD(&chan->srej_l);
1660
311bb895 1661 INIT_WORK(&chan->busy_work, l2cap_busy_work);
218bb9df
GP		 1662
1663 sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
0565c1c2
GP		 1664}
1665
f2fcfcd6
GP		 1666static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1667{
1668 switch (mode) {
1669 case L2CAP_MODE_STREAMING:
1670 case L2CAP_MODE_ERTM:
1671 if (l2cap_mode_supported(mode, remote_feat_mask))
1672 return mode;
1673 /* fall through */
1674 default:
1675 return L2CAP_MODE_BASIC;
1676 }
1677}
1678
710f9b0a 1679static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 1680{
1da177e4 1681 struct l2cap_conf_req *req = data;
0c1bc5c6 1682 struct l2cap_conf_rfc rfc = { .mode = chan->mode };
1da177e4
LT		 1683 void *ptr = req->data;
1684
49208c9c 1685 BT_DBG("chan %p", chan);
1da177e4 1686
73ffa904 1687 if (chan->num_conf_req || chan->num_conf_rsp)
f2fcfcd6
GP		 1688 goto done;
1689
0c1bc5c6 1690 switch (chan->mode) {
f2fcfcd6
GP		 1691 case L2CAP_MODE_STREAMING:
1692 case L2CAP_MODE_ERTM:
b4450035 1693 if (chan->conf_state & L2CAP_CONF_STATE2_DEVICE)
85eb53c6 1694 break;
85eb53c6 1695
2ba13ed6 1696 /* fall through */
f2fcfcd6 1697 default:
8c1d787b 1698 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
f2fcfcd6
GP		 1699 break;
1700 }
1701
1702done:
0c1bc5c6
GP		 1703 if (chan->imtu != L2CAP_DEFAULT_MTU)
1704 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
7990681c 1705
0c1bc5c6 1706 switch (chan->mode) {
65c7c491 1707 case L2CAP_MODE_BASIC:
8c1d787b
GP		 1708 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
1709 !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
63406504
GP		 1710 break;
1711
62547752
GP		 1712 rfc.mode = L2CAP_MODE_BASIC;
1713 rfc.txwin_size = 0;
1714 rfc.max_transmit = 0;
1715 rfc.retrans_timeout = 0;
1716 rfc.monitor_timeout = 0;
1717 rfc.max_pdu_size = 0;
1718
63406504
GP		 1719 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1720 (unsigned long) &rfc);
65c7c491
MH		 1721 break;
1722
1723 case L2CAP_MODE_ERTM:
1724 rfc.mode = L2CAP_MODE_ERTM;
47d1ec61
GP		 1725 rfc.txwin_size = chan->tx_win;
1726 rfc.max_transmit = chan->max_tx;
f2fcfcd6
GP		 1727 rfc.retrans_timeout = 0;
1728 rfc.monitor_timeout = 0;
c74e560c 1729 rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
8c1d787b
GP		 1730 if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10)
1731 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
f2fcfcd6 1732
63406504
GP		 1733 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1734 (unsigned long) &rfc);
1735
8c1d787b 1736 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
fcc203c3
GP		 1737 break;
1738
47d1ec61 1739 if (chan->fcs == L2CAP_FCS_NONE ||
b4450035 1740 chan->conf_state & L2CAP_CONF_NO_FCS_RECV) {
47d1ec61
GP		 1741 chan->fcs = L2CAP_FCS_NONE;
1742 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
fcc203c3 1743 }
f2fcfcd6
GP		 1744 break;
1745
1746 case L2CAP_MODE_STREAMING:
1747 rfc.mode = L2CAP_MODE_STREAMING;
1748 rfc.txwin_size = 0;
1749 rfc.max_transmit = 0;
1750 rfc.retrans_timeout = 0;
1751 rfc.monitor_timeout = 0;
c74e560c 1752 rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
8c1d787b
GP		 1753 if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10)
1754 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
65c7c491 1755
63406504
GP		 1756 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1757 (unsigned long) &rfc);
1758
8c1d787b 1759 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
fcc203c3
GP		 1760 break;
1761
47d1ec61 1762 if (chan->fcs == L2CAP_FCS_NONE ||
b4450035 1763 chan->conf_state & L2CAP_CONF_NO_FCS_RECV) {
47d1ec61
GP		 1764 chan->fcs = L2CAP_FCS_NONE;
1765 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
fcc203c3 1766 }
65c7c491
MH		 1767 break;
1768 }
1da177e4 1769
fe4128e0 1770 req->dcid = cpu_to_le16(chan->dcid);
aca3192c 1771 req->flags = cpu_to_le16(0);
1da177e4
LT		 1772
1773 return ptr - data;
1774}
1775
73ffa904 1776static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
1da177e4 1777{
5dee9e7c
MH		 1778 struct l2cap_conf_rsp *rsp = data;
1779 void *ptr = rsp->data;
73ffa904
GP		 1780 void *req = chan->conf_req;
1781 int len = chan->conf_len;
5dee9e7c
MH		 1782 int type, hint, olen;
1783 unsigned long val;
6464f35f 1784 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
861d6882 1785 u16 mtu = L2CAP_DEFAULT_MTU;
5dee9e7c 1786 u16 result = L2CAP_CONF_SUCCESS;
1da177e4 1787
73ffa904 1788 BT_DBG("chan %p", chan);
820ae1b8 1789
5dee9e7c
MH		 1790 while (len >= L2CAP_CONF_OPT_SIZE) {
1791 len -= l2cap_get_conf_opt(&req, &type, &olen, &val);
1da177e4 1792
589d2746 1793 hint = type & L2CAP_CONF_HINT;
47ec1dcd 1794 type &= L2CAP_CONF_MASK;
5dee9e7c
MH		 1795
1796 switch (type) {
1797 case L2CAP_CONF_MTU:
861d6882 1798 mtu = val;
5dee9e7c
MH		 1799 break;
1800
1801 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 1802 chan->flush_to = val;
5dee9e7c
MH		 1803 break;
1804
1805 case L2CAP_CONF_QOS:
1806 break;
1807
6464f35f
MH		 1808 case L2CAP_CONF_RFC:
1809 if (olen == sizeof(rfc))
1810 memcpy(&rfc, (void *) val, olen);
1811 break;
1812
fcc203c3
GP		 1813 case L2CAP_CONF_FCS:
1814 if (val == L2CAP_FCS_NONE)
b4450035 1815 chan->conf_state |= L2CAP_CONF_NO_FCS_RECV;
fcc203c3
GP		 1816
1817 break;
1818
5dee9e7c
MH		 1819 default:
1820 if (hint)
1821 break;
1822
1823 result = L2CAP_CONF_UNKNOWN;
1824 *((u8 *) ptr++) = type;
1825 break;
1826 }
1827 }
1828
73ffa904 1829 if (chan->num_conf_rsp || chan->num_conf_req > 1)
f2fcfcd6
GP		 1830 goto done;
1831
0c1bc5c6 1832 switch (chan->mode) {
f2fcfcd6
GP		 1833 case L2CAP_MODE_STREAMING:
1834 case L2CAP_MODE_ERTM:
b4450035 1835 if (!(chan->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
0c1bc5c6 1836 chan->mode = l2cap_select_mode(rfc.mode,
8c1d787b 1837 chan->conn->feat_mask);
85eb53c6
GP		 1838 break;
1839 }
1840
0c1bc5c6 1841 if (chan->mode != rfc.mode)
f2fcfcd6 1842 return -ECONNREFUSED;
742e519b 1843
f2fcfcd6 1844 break;
f2fcfcd6
GP		 1845 }
1846
1847done:
0c1bc5c6 1848 if (chan->mode != rfc.mode) {
f2fcfcd6 1849 result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 1850 rfc.mode = chan->mode;
f2fcfcd6 1851
73ffa904 1852 if (chan->num_conf_rsp == 1)
f2fcfcd6
GP		 1853 return -ECONNREFUSED;
1854
1855 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1856 sizeof(rfc), (unsigned long) &rfc);
1857 }
1858
1859
5dee9e7c
MH		 1860 if (result == L2CAP_CONF_SUCCESS) {
1861 /* Configure output options and let the other side know
1862 * which ones we don't like. */
1863
f2fcfcd6
GP		 1864 if (mtu < L2CAP_DEFAULT_MIN_MTU)
1865 result = L2CAP_CONF_UNACCEPT;
1866 else {
0c1bc5c6 1867 chan->omtu = mtu;
b4450035 1868 chan->conf_state |= L2CAP_CONF_MTU_DONE;
f2fcfcd6 1869 }
0c1bc5c6 1870 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
6464f35f 1871
f2fcfcd6
GP		 1872 switch (rfc.mode) {
1873 case L2CAP_MODE_BASIC:
47d1ec61 1874 chan->fcs = L2CAP_FCS_NONE;
b4450035 1875 chan->conf_state |= L2CAP_CONF_MODE_DONE;
f2fcfcd6
GP		 1876 break;
1877
1878 case L2CAP_MODE_ERTM:
2c03a7a4
GP		 1879 chan->remote_tx_win = rfc.txwin_size;
1880 chan->remote_max_tx = rfc.max_transmit;
86b1b263 1881
8c1d787b
GP		 1882 if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10)
1883 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
1c762159 1884
2c03a7a4 1885 chan->remote_mps = le16_to_cpu(rfc.max_pdu_size);
f2fcfcd6 1886
10467e9e
GP		 1887 rfc.retrans_timeout =
1888 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
1889 rfc.monitor_timeout =
1890 le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
f2fcfcd6 1891
b4450035 1892 chan->conf_state |= L2CAP_CONF_MODE_DONE;
68ae6639
GP		 1893
1894 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1895 sizeof(rfc), (unsigned long) &rfc);
1896
f2fcfcd6
GP		 1897 break;
1898
1899 case L2CAP_MODE_STREAMING:
8c1d787b
GP		 1900 if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10)
1901 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
1c762159 1902
2c03a7a4 1903 chan->remote_mps = le16_to_cpu(rfc.max_pdu_size);
f2fcfcd6 1904
b4450035 1905 chan->conf_state |= L2CAP_CONF_MODE_DONE;
68ae6639
GP		 1906
1907 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1908 sizeof(rfc), (unsigned long) &rfc);
1909
f2fcfcd6
GP		 1910 break;
1911
1912 default:
5dee9e7c 1913 result = L2CAP_CONF_UNACCEPT;
5dee9e7c 1914
6464f35f 1915 memset(&rfc, 0, sizeof(rfc));
0c1bc5c6 1916 rfc.mode = chan->mode;
f2fcfcd6 1917 }
6464f35f 1918
f2fcfcd6 1919 if (result == L2CAP_CONF_SUCCESS)
b4450035 1920 chan->conf_state |= L2CAP_CONF_OUTPUT_DONE;
f2fcfcd6 1921 }
fe4128e0 1922 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c
MH		 1923 rsp->result = cpu_to_le16(result);
1924 rsp->flags = cpu_to_le16(0x0000);
1925
1926 return ptr - data;
1da177e4
LT		 1927}
1928
b4450035 1929static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
f2fcfcd6 1930{
f2fcfcd6
GP		 1931 struct l2cap_conf_req *req = data;
1932 void *ptr = req->data;
1933 int type, olen;
1934 unsigned long val;
1935 struct l2cap_conf_rfc rfc;
1936
fe4128e0 1937 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
f2fcfcd6
GP		 1938
1939 while (len >= L2CAP_CONF_OPT_SIZE) {
1940 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
1941
1942 switch (type) {
1943 case L2CAP_CONF_MTU:
1944 if (val < L2CAP_DEFAULT_MIN_MTU) {
1945 *result = L2CAP_CONF_UNACCEPT;
0c1bc5c6 1946 chan->imtu = L2CAP_DEFAULT_MIN_MTU;
f2fcfcd6 1947 } else
0c1bc5c6
GP		 1948 chan->imtu = val;
1949 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu);
f2fcfcd6
GP		 1950 break;
1951
1952 case L2CAP_CONF_FLUSH_TO:
0c1bc5c6 1953 chan->flush_to = val;
f2fcfcd6 1954 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
0c1bc5c6 1955 2, chan->flush_to);
f2fcfcd6
GP		 1956 break;
1957
1958 case L2CAP_CONF_RFC:
1959 if (olen == sizeof(rfc))
1960 memcpy(&rfc, (void *)val, olen);
1961
b4450035 1962 if ((chan->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
0c1bc5c6 1963 rfc.mode != chan->mode)
f2fcfcd6
GP		 1964 return -ECONNREFUSED;
1965
47d1ec61 1966 chan->fcs = 0;
f2fcfcd6
GP		 1967
1968 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
1969 sizeof(rfc), (unsigned long) &rfc);
1970 break;
1971 }
1972 }
1973
0c1bc5c6 1974 if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode)
6c2ea7a8
GP		 1975 return -ECONNREFUSED;
1976
0c1bc5c6 1977 chan->mode = rfc.mode;
6c2ea7a8 1978
f2fcfcd6
GP		 1979 if (*result == L2CAP_CONF_SUCCESS) {
1980 switch (rfc.mode) {
1981 case L2CAP_MODE_ERTM:
47d1ec61
GP		 1982 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
1983 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
1984 chan->mps = le16_to_cpu(rfc.max_pdu_size);
f2fcfcd6
GP		 1985 break;
1986 case L2CAP_MODE_STREAMING:
47d1ec61 1987 chan->mps = le16_to_cpu(rfc.max_pdu_size);
f2fcfcd6
GP		 1988 }
1989 }
1990
fe4128e0 1991 req->dcid = cpu_to_le16(chan->dcid);
f2fcfcd6
GP		 1992 req->flags = cpu_to_le16(0x0000);
1993
1994 return ptr - data;
1995}
1996
fe4128e0 1997static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
1da177e4
LT		 1998{
1999 struct l2cap_conf_rsp *rsp = data;
2000 void *ptr = rsp->data;
1da177e4 2001
fe4128e0 2002 BT_DBG("chan %p", chan);
1da177e4 2003
fe4128e0 2004 rsp->scid = cpu_to_le16(chan->dcid);
5dee9e7c 2005 rsp->result = cpu_to_le16(result);
aca3192c 2006 rsp->flags = cpu_to_le16(flags);
1da177e4
LT		 2007
2008 return ptr - data;
2009}
2010
8c1d787b 2011void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
710f9b0a
GP		 2012{
2013 struct l2cap_conn_rsp rsp;
8c1d787b 2014 struct l2cap_conn *conn = chan->conn;
710f9b0a
GP		 2015 u8 buf[128];
2016
fe4128e0
GP		 2017 rsp.scid = cpu_to_le16(chan->dcid);
2018 rsp.dcid = cpu_to_le16(chan->scid);
710f9b0a
GP		 2019 rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
2020 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
2021 l2cap_send_cmd(conn, chan->ident,
2022 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
2023
b4450035 2024 if (chan->conf_state & L2CAP_CONF_REQ_SENT)
710f9b0a
GP		 2025 return;
2026
b4450035 2027 chan->conf_state |= L2CAP_CONF_REQ_SENT;
710f9b0a
GP		 2028 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
2029 l2cap_build_conf_req(chan, buf), buf);
2030 chan->num_conf_req++;
2031}
2032
47d1ec61 2033static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
7b1c0049 2034{
7b1c0049
GP		 2035 int type, olen;
2036 unsigned long val;
2037 struct l2cap_conf_rfc rfc;
2038
47d1ec61 2039 BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len);
7b1c0049 2040
0c1bc5c6 2041 if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING))
7b1c0049
GP		 2042 return;
2043
2044 while (len >= L2CAP_CONF_OPT_SIZE) {
2045 len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
2046
2047 switch (type) {
2048 case L2CAP_CONF_RFC:
2049 if (olen == sizeof(rfc))
2050 memcpy(&rfc, (void *)val, olen);
2051 goto done;
2052 }
2053 }
2054
2055done:
2056 switch (rfc.mode) {
2057 case L2CAP_MODE_ERTM:
47d1ec61
GP		 2058 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2059 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2060 chan->mps = le16_to_cpu(rfc.max_pdu_size);
7b1c0049
GP		 2061 break;
2062 case L2CAP_MODE_STREAMING:
47d1ec61 2063 chan->mps = le16_to_cpu(rfc.max_pdu_size);
7b1c0049
GP		 2064 }
2065}
2066
4e8402a3
MH		 2067static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2068{
2069 struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
2070
2071 if (rej->reason != 0x0000)
2072 return 0;
2073
2074 if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
2075 cmd->ident == conn->info_ident) {
4e8402a3 2076 del_timer(&conn->info_timer);
984947dc
MH		 2077
2078 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 2079 conn->info_ident = 0;
984947dc 2080
4e8402a3
MH		 2081 l2cap_conn_start(conn);
2082 }
2083
2084 return 0;
2085}
2086
1da177e4
LT		 2087static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2088{
1da177e4
LT		 2089 struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
2090 struct l2cap_conn_rsp rsp;
23691d75 2091 struct l2cap_chan *chan = NULL, *pchan;
d793fe8c 2092 struct sock *parent, *sk = NULL;
e7c29cb1 2093 int result, status = L2CAP_CS_NO_INFO;
1da177e4
LT		 2094
2095 u16 dcid = 0, scid = __le16_to_cpu(req->scid);
e7c29cb1 2096 __le16 psm = req->psm;
1da177e4
LT		 2097
2098 BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
2099
2100 /* Check if we have socket listening on psm */
23691d75
GP		 2101 pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
2102 if (!pchan) {
1da177e4
LT		 2103 result = L2CAP_CR_BAD_PSM;
2104 goto sendresp;
2105 }
2106
23691d75
GP		 2107 parent = pchan->sk;
2108
e0f0cb56
GP		 2109 bh_lock_sock(parent);
2110
e7c29cb1
MH		 2111 /* Check if the ACL is secure enough (if not SDP) */
2112 if (psm != cpu_to_le16(0x0001) &&
2113 !hci_conn_check_link_mode(conn->hcon)) {
2950f21a 2114 conn->disc_reason = 0x05;
e7c29cb1
MH		 2115 result = L2CAP_CR_SEC_BLOCK;
2116 goto response;
2117 }
2118
1da177e4
LT		 2119 result = L2CAP_CR_NO_MEM;
2120
2121 /* Check for backlog size */
2122 if (sk_acceptq_is_full(parent)) {
8e87d142 2123 BT_DBG("backlog full %d", parent->sk_ack_backlog);
1da177e4
LT		 2124 goto response;
2125 }
2126
3b1e0a65 2127 sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
1da177e4
LT		 2128 if (!sk)
2129 goto response;
2130
23691d75 2131 chan = l2cap_chan_create(sk);
48454079
GP		 2132 if (!chan) {
2133 l2cap_sock_kill(sk);
2134 goto response;
2135 }
2136
5d41ce1d
GP		 2137 l2cap_pi(sk)->chan = chan;
2138
baa7e1fa 2139 write_lock_bh(&conn->chan_lock);
1da177e4
LT		 2140
2141 /* Check if we already have channel with that dcid */
baa7e1fa
GP		 2142 if (__l2cap_get_chan_by_dcid(conn, scid)) {
2143 write_unlock_bh(&conn->chan_lock);
1da177e4
LT		 2144 sock_set_flag(sk, SOCK_ZAPPED);
2145 l2cap_sock_kill(sk);
2146 goto response;
2147 }
2148
2149 hci_conn_hold(conn->hcon);
2150
2151 l2cap_sock_init(sk, parent);
2152 bacpy(&bt_sk(sk)->src, conn->src);
2153 bacpy(&bt_sk(sk)->dst, conn->dst);
fe4128e0
GP		 2154 chan->psm = psm;
2155 chan->dcid = scid;
1da177e4 2156
d1010240
GP		 2157 bt_accept_enqueue(parent, sk);
2158
48454079
GP		 2159 __l2cap_chan_add(conn, chan);
2160
fe4128e0 2161 dcid = chan->scid;
1da177e4
LT		 2162
2163 l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
2164
fc7f8a7e 2165 chan->ident = cmd->ident;
1da177e4 2166
984947dc 2167 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
4343478f 2168 if (l2cap_check_security(chan)) {
f66dc81f
MH		 2169 if (bt_sk(sk)->defer_setup) {
2170 sk->sk_state = BT_CONNECT2;
2171 result = L2CAP_CR_PEND;
2172 status = L2CAP_CS_AUTHOR_PEND;
2173 parent->sk_data_ready(parent, 0);
2174 } else {
2175 sk->sk_state = BT_CONFIG;
2176 result = L2CAP_CR_SUCCESS;
2177 status = L2CAP_CS_NO_INFO;
2178 }
79d554a6
MH		 2179 } else {
2180 sk->sk_state = BT_CONNECT2;
2181 result = L2CAP_CR_PEND;
2182 status = L2CAP_CS_AUTHEN_PEND;
2183 }
2184 } else {
2185 sk->sk_state = BT_CONNECT2;
2186 result = L2CAP_CR_PEND;
2187 status = L2CAP_CS_NO_INFO;
1da177e4
LT		 2188 }
2189
baa7e1fa 2190 write_unlock_bh(&conn->chan_lock);
1da177e4
LT		 2191
2192response:
2193 bh_unlock_sock(parent);
2194
2195sendresp:
aca3192c
YH		 2196 rsp.scid = cpu_to_le16(scid);
2197 rsp.dcid = cpu_to_le16(dcid);
2198 rsp.result = cpu_to_le16(result);
2199 rsp.status = cpu_to_le16(status);
1da177e4 2200 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
79d554a6
MH		 2201
2202 if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
2203 struct l2cap_info_req info;
2204 info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2205
2206 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
2207 conn->info_ident = l2cap_get_ident(conn);
2208
2209 mod_timer(&conn->info_timer, jiffies +
2210 msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
2211
2212 l2cap_send_cmd(conn, conn->info_ident,
2213 L2CAP_INFO_REQ, sizeof(info), &info);
2214 }
2215
b4450035 2216 if (chan && !(chan->conf_state & L2CAP_CONF_REQ_SENT) &&
e9aeb2dd
GP		 2217 result == L2CAP_CR_SUCCESS) {
2218 u8 buf[128];
b4450035 2219 chan->conf_state |= L2CAP_CONF_REQ_SENT;
e9aeb2dd 2220 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2221 l2cap_build_conf_req(chan, buf), buf);
2222 chan->num_conf_req++;
e9aeb2dd
GP		 2223 }
2224
1da177e4
LT		 2225 return 0;
2226}
2227
2228static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2229{
2230 struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
2231 u16 scid, dcid, result, status;
48454079 2232 struct l2cap_chan *chan;
1da177e4
LT		 2233 struct sock *sk;
2234 u8 req[128];
2235
2236 scid = __le16_to_cpu(rsp->scid);
2237 dcid = __le16_to_cpu(rsp->dcid);
2238 result = __le16_to_cpu(rsp->result);
2239 status = __le16_to_cpu(rsp->status);
2240
2241 BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status);
2242
2243 if (scid) {
baa7e1fa 2244 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 2245 if (!chan)
57d3b22b 2246 return -EFAULT;
1da177e4 2247 } else {
baa7e1fa 2248 chan = l2cap_get_chan_by_ident(conn, cmd->ident);
48454079 2249 if (!chan)
57d3b22b 2250 return -EFAULT;
1da177e4
LT		 2251 }
2252
48454079
GP		 2253 sk = chan->sk;
2254
1da177e4
LT		 2255 switch (result) {
2256 case L2CAP_CR_SUCCESS:
2257 sk->sk_state = BT_CONFIG;
fc7f8a7e 2258 chan->ident = 0;
fe4128e0 2259 chan->dcid = dcid;
b4450035 2260 chan->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
6a8d3010 2261
b4450035 2262 if (chan->conf_state & L2CAP_CONF_REQ_SENT)
e9aeb2dd
GP		 2263 break;
2264
b4450035 2265 chan->conf_state |= L2CAP_CONF_REQ_SENT;
e9aeb2dd 2266
1da177e4 2267 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2268 l2cap_build_conf_req(chan, req), req);
2269 chan->num_conf_req++;
1da177e4
LT		 2270 break;
2271
2272 case L2CAP_CR_PEND:
b4450035 2273 chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
1da177e4
LT		 2274 break;
2275
2276 default:
a49184c2
AE		 2277 /* don't delete l2cap channel if sk is owned by user */
2278 if (sock_owned_by_user(sk)) {
2279 sk->sk_state = BT_DISCONN;
2280 l2cap_sock_clear_timer(sk);
2281 l2cap_sock_set_timer(sk, HZ / 5);
2282 break;
2283 }
2284
48454079 2285 l2cap_chan_del(chan, ECONNREFUSED);
1da177e4
LT		 2286 break;
2287 }
2288
2289 bh_unlock_sock(sk);
2290 return 0;
2291}
2292
47d1ec61 2293static inline void set_default_fcs(struct l2cap_chan *chan)
8c462b60 2294{
47d1ec61
GP		 2295 struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
2296
8c462b60
MM		 2297 /* FCS is enabled only in ERTM or streaming mode, if one or both
2298 * sides request it.
2299 */
0c1bc5c6 2300 if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
47d1ec61 2301 chan->fcs = L2CAP_FCS_NONE;
b4450035 2302 else if (!(pi->chan->conf_state & L2CAP_CONF_NO_FCS_RECV))
47d1ec61 2303 chan->fcs = L2CAP_FCS_CRC16;
8c462b60
MM		 2304}
2305
88219a0f 2306static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
1da177e4
LT		 2307{
2308 struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
2309 u16 dcid, flags;
2310 u8 rsp[64];
48454079 2311 struct l2cap_chan *chan;
1da177e4 2312 struct sock *sk;
5dee9e7c 2313 int len;
1da177e4
LT		 2314
2315 dcid = __le16_to_cpu(req->dcid);
2316 flags = __le16_to_cpu(req->flags);
2317
2318 BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags);
2319
baa7e1fa 2320 chan = l2cap_get_chan_by_scid(conn, dcid);
48454079 2321 if (!chan)
1da177e4
LT		 2322 return -ENOENT;
2323
48454079
GP		 2324 sk = chan->sk;
2325
df6bd743
GP		 2326 if (sk->sk_state != BT_CONFIG) {
2327 struct l2cap_cmd_rej rej;
2328
2329 rej.reason = cpu_to_le16(0x0002);
2330 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
2331 sizeof(rej), &rej);
354f60a9 2332 goto unlock;
df6bd743 2333 }
354f60a9 2334
5dee9e7c 2335 /* Reject if config buffer is too small. */
88219a0f 2336 len = cmd_len - sizeof(*req);
73ffa904 2337 if (chan->conf_len + len > sizeof(chan->conf_req)) {
5dee9e7c 2338 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
fe4128e0 2339 l2cap_build_conf_rsp(chan, rsp,
5dee9e7c
MH		 2340 L2CAP_CONF_REJECT, flags), rsp);
2341 goto unlock;
2342 }
2343
2344 /* Store config. */
73ffa904
GP		 2345 memcpy(chan->conf_req + chan->conf_len, req->data, len);
2346 chan->conf_len += len;
1da177e4
LT		 2347
2348 if (flags & 0x0001) {
2349 /* Incomplete config. Send empty response. */
2350 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
fe4128e0 2351 l2cap_build_conf_rsp(chan, rsp,
5dee9e7c 2352 L2CAP_CONF_SUCCESS, 0x0001), rsp);
1da177e4
LT		 2353 goto unlock;
2354 }
2355
2356 /* Complete config. */
73ffa904 2357 len = l2cap_parse_conf_req(chan, rsp);
f2fcfcd6 2358 if (len < 0) {
e92c8e70 2359 l2cap_send_disconn_req(conn, chan, ECONNRESET);
1da177e4 2360 goto unlock;
f2fcfcd6 2361 }
1da177e4 2362
5dee9e7c 2363 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
73ffa904 2364 chan->num_conf_rsp++;
5dee9e7c 2365
5dee9e7c 2366 /* Reset config buffer. */
73ffa904 2367 chan->conf_len = 0;
5dee9e7c 2368
b4450035 2369 if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE))
876d9484
MH		 2370 goto unlock;
2371
b4450035 2372 if (chan->conf_state & L2CAP_CONF_INPUT_DONE) {
47d1ec61 2373 set_default_fcs(chan);
fcc203c3 2374
1da177e4 2375 sk->sk_state = BT_CONNECTED;
e90bac06 2376
42e5c802
GP		 2377 chan->next_tx_seq = 0;
2378 chan->expected_tx_seq = 0;
58d35f87 2379 skb_queue_head_init(&chan->tx_q);
0c1bc5c6 2380 if (chan->mode == L2CAP_MODE_ERTM)
525cd185 2381 l2cap_ertm_init(chan);
0565c1c2 2382
1da177e4 2383 l2cap_chan_ready(sk);
876d9484
MH		 2384 goto unlock;
2385 }
2386
b4450035 2387 if (!(chan->conf_state & L2CAP_CONF_REQ_SENT)) {
79d554a6 2388 u8 buf[64];
b4450035 2389 chan->conf_state |= L2CAP_CONF_REQ_SENT;
1da177e4 2390 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
73ffa904
GP		 2391 l2cap_build_conf_req(chan, buf), buf);
2392 chan->num_conf_req++;
1da177e4
LT		 2393 }
2394
2395unlock:
2396 bh_unlock_sock(sk);
2397 return 0;
2398}
2399
2400static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2401{
2402 struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
2403 u16 scid, flags, result;
48454079 2404 struct l2cap_chan *chan;
1da177e4 2405 struct sock *sk;
7b1c0049 2406 int len = cmd->len - sizeof(*rsp);
1da177e4
LT		 2407
2408 scid = __le16_to_cpu(rsp->scid);
2409 flags = __le16_to_cpu(rsp->flags);
2410 result = __le16_to_cpu(rsp->result);
2411
af05b30b
GP		 2412 BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x",
2413 scid, flags, result);
1da177e4 2414
baa7e1fa 2415 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 2416 if (!chan)
1da177e4
LT		 2417 return 0;
2418
48454079
GP		 2419 sk = chan->sk;
2420
1da177e4
LT		 2421 switch (result) {
2422 case L2CAP_CONF_SUCCESS:
47d1ec61 2423 l2cap_conf_rfc_get(chan, rsp->data, len);
1da177e4
LT		 2424 break;
2425
2426 case L2CAP_CONF_UNACCEPT:
73ffa904 2427 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
f2fcfcd6
GP		 2428 char req[64];
2429
c2c77ec8 2430 if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
e92c8e70 2431 l2cap_send_disconn_req(conn, chan, ECONNRESET);
c2c77ec8
AE		 2432 goto done;
2433 }
2434
f2fcfcd6
GP		 2435 /* throw out any old stored conf requests */
2436 result = L2CAP_CONF_SUCCESS;
b4450035
GP		 2437 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2438 req, &result);
f2fcfcd6 2439 if (len < 0) {
e92c8e70 2440 l2cap_send_disconn_req(conn, chan, ECONNRESET);
f2fcfcd6
GP		 2441 goto done;
2442 }
2443
2444 l2cap_send_cmd(conn, l2cap_get_ident(conn),
2445 L2CAP_CONF_REQ, len, req);
73ffa904 2446 chan->num_conf_req++;
f2fcfcd6
GP		 2447 if (result != L2CAP_CONF_SUCCESS)
2448 goto done;
2449 break;
1da177e4
LT		 2450 }
2451
8e87d142 2452 default:
b1235d79 2453 sk->sk_err = ECONNRESET;
1da177e4 2454 l2cap_sock_set_timer(sk, HZ * 5);
e92c8e70 2455 l2cap_send_disconn_req(conn, chan, ECONNRESET);
1da177e4
LT		 2456 goto done;
2457 }
2458
2459 if (flags & 0x01)
2460 goto done;
2461
b4450035 2462 chan->conf_state |= L2CAP_CONF_INPUT_DONE;
1da177e4 2463
b4450035 2464 if (chan->conf_state & L2CAP_CONF_OUTPUT_DONE) {
47d1ec61 2465 set_default_fcs(chan);
fcc203c3 2466
1da177e4 2467 sk->sk_state = BT_CONNECTED;
42e5c802
GP		 2468 chan->next_tx_seq = 0;
2469 chan->expected_tx_seq = 0;
58d35f87 2470 skb_queue_head_init(&chan->tx_q);
0c1bc5c6 2471 if (chan->mode == L2CAP_MODE_ERTM)
525cd185 2472 l2cap_ertm_init(chan);
0565c1c2 2473
1da177e4
LT		 2474 l2cap_chan_ready(sk);
2475 }
2476
2477done:
2478 bh_unlock_sock(sk);
2479 return 0;
2480}
2481
2482static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2483{
2484 struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
2485 struct l2cap_disconn_rsp rsp;
2486 u16 dcid, scid;
48454079 2487 struct l2cap_chan *chan;
1da177e4
LT		 2488 struct sock *sk;
2489
2490 scid = __le16_to_cpu(req->scid);
2491 dcid = __le16_to_cpu(req->dcid);
2492
2493 BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
2494
baa7e1fa 2495 chan = l2cap_get_chan_by_scid(conn, dcid);
48454079 2496 if (!chan)
1da177e4
LT		 2497 return 0;
2498
48454079
GP		 2499 sk = chan->sk;
2500
fe4128e0
GP		 2501 rsp.dcid = cpu_to_le16(chan->scid);
2502 rsp.scid = cpu_to_le16(chan->dcid);
1da177e4
LT		 2503 l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
2504
2505 sk->sk_shutdown = SHUTDOWN_MASK;
2506
a49184c2
AE		 2507 /* don't delete l2cap channel if sk is owned by user */
2508 if (sock_owned_by_user(sk)) {
2509 sk->sk_state = BT_DISCONN;
2510 l2cap_sock_clear_timer(sk);
2511 l2cap_sock_set_timer(sk, HZ / 5);
2512 bh_unlock_sock(sk);
2513 return 0;
2514 }
2515
48454079 2516 l2cap_chan_del(chan, ECONNRESET);
1da177e4
LT		 2517 bh_unlock_sock(sk);
2518
2519 l2cap_sock_kill(sk);
2520 return 0;
2521}
2522
2523static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2524{
2525 struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
2526 u16 dcid, scid;
48454079 2527 struct l2cap_chan *chan;
1da177e4
LT		 2528 struct sock *sk;
2529
2530 scid = __le16_to_cpu(rsp->scid);
2531 dcid = __le16_to_cpu(rsp->dcid);
2532
2533 BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
2534
baa7e1fa 2535 chan = l2cap_get_chan_by_scid(conn, scid);
48454079 2536 if (!chan)
1da177e4
LT		 2537 return 0;
2538
48454079
GP		 2539 sk = chan->sk;
2540
a49184c2
AE		 2541 /* don't delete l2cap channel if sk is owned by user */
2542 if (sock_owned_by_user(sk)) {
2543 sk->sk_state = BT_DISCONN;
2544 l2cap_sock_clear_timer(sk);
2545 l2cap_sock_set_timer(sk, HZ / 5);
2546 bh_unlock_sock(sk);
2547 return 0;
2548 }
2549
48454079 2550 l2cap_chan_del(chan, 0);
1da177e4
LT		 2551 bh_unlock_sock(sk);
2552
2553 l2cap_sock_kill(sk);
2554 return 0;
2555}
2556
2557static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2558{
2559 struct l2cap_info_req *req = (struct l2cap_info_req *) data;
1da177e4
LT		 2560 u16 type;
2561
2562 type = __le16_to_cpu(req->type);
2563
2564 BT_DBG("type 0x%4.4x", type);
2565
f0709e03
MH		 2566 if (type == L2CAP_IT_FEAT_MASK) {
2567 u8 buf[8];
44dd46de 2568 u32 feat_mask = l2cap_feat_mask;
f0709e03
MH		 2569 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2570 rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
2571 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
d1c4a17d 2572 if (!disable_ertm)
fcc203c3
GP		 2573 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2574 | L2CAP_FEAT_FCS;
1b7bf4ed 2575 put_unaligned_le32(feat_mask, rsp->data);
f0709e03
MH		 2576 l2cap_send_cmd(conn, cmd->ident,
2577 L2CAP_INFO_RSP, sizeof(buf), buf);
e1027a7c
MH		 2578 } else if (type == L2CAP_IT_FIXED_CHAN) {
2579 u8 buf[12];
2580 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
2581 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2582 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2583 memcpy(buf + 4, l2cap_fixed_chan, 8);
2584 l2cap_send_cmd(conn, cmd->ident,
2585 L2CAP_INFO_RSP, sizeof(buf), buf);
f0709e03
MH		 2586 } else {
2587 struct l2cap_info_rsp rsp;
2588 rsp.type = cpu_to_le16(type);
2589 rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
2590 l2cap_send_cmd(conn, cmd->ident,
2591 L2CAP_INFO_RSP, sizeof(rsp), &rsp);
2592 }
1da177e4
LT		 2593
2594 return 0;
2595}
2596
2597static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
2598{
2599 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
2600 u16 type, result;
2601
2602 type = __le16_to_cpu(rsp->type);
2603 result = __le16_to_cpu(rsp->result);
2604
2605 BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
2606
e90165be
AE		 2607 /* L2CAP Info req/rsp are unbound to channels, add extra checks */
2608 if (cmd->ident != conn->info_ident ||
2609 conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
2610 return 0;
2611
4e8402a3
MH		 2612 del_timer(&conn->info_timer);
2613
adb08ede
VT		 2614 if (result != L2CAP_IR_SUCCESS) {
2615 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2616 conn->info_ident = 0;
2617
2618 l2cap_conn_start(conn);
2619
2620 return 0;
2621 }
2622
984947dc 2623 if (type == L2CAP_IT_FEAT_MASK) {
83985319 2624 conn->feat_mask = get_unaligned_le32(rsp->data);
4e8402a3 2625
47ec1dcd 2626 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
e1027a7c
MH		 2627 struct l2cap_info_req req;
2628 req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2629
2630 conn->info_ident = l2cap_get_ident(conn);
2631
2632 l2cap_send_cmd(conn, conn->info_ident,
2633 L2CAP_INFO_REQ, sizeof(req), &req);
2634 } else {
2635 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
2636 conn->info_ident = 0;
2637
2638 l2cap_conn_start(conn);
2639 }
2640 } else if (type == L2CAP_IT_FIXED_CHAN) {
984947dc 2641 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
e1027a7c 2642 conn->info_ident = 0;
984947dc
MH		 2643
2644 l2cap_conn_start(conn);
2645 }
4e8402a3 2646
1da177e4
LT		 2647 return 0;
2648}
2649
e2174ca4 2650static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
de73115a
CT		 2651 u16 to_multiplier)
2652{
2653 u16 max_latency;
2654
2655 if (min > max || min < 6 || max > 3200)
2656 return -EINVAL;
2657
2658 if (to_multiplier < 10 || to_multiplier > 3200)
2659 return -EINVAL;
2660
2661 if (max >= to_multiplier * 8)
2662 return -EINVAL;
2663
2664 max_latency = (to_multiplier * 8 / max) - 1;
2665 if (latency > 499 || latency > max_latency)
2666 return -EINVAL;
2667
2668 return 0;
2669}
2670
2671static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
2672 struct l2cap_cmd_hdr *cmd, u8 *data)
2673{
2674 struct hci_conn *hcon = conn->hcon;
2675 struct l2cap_conn_param_update_req *req;
2676 struct l2cap_conn_param_update_rsp rsp;
2677 u16 min, max, latency, to_multiplier, cmd_len;
2ce603eb 2678 int err;
de73115a
CT		 2679
2680 if (!(hcon->link_mode & HCI_LM_MASTER))
2681 return -EINVAL;
2682
2683 cmd_len = __le16_to_cpu(cmd->len);
2684 if (cmd_len != sizeof(struct l2cap_conn_param_update_req))
2685 return -EPROTO;
2686
2687 req = (struct l2cap_conn_param_update_req *) data;
e2174ca4
GP		 2688 min = __le16_to_cpu(req->min);
2689 max = __le16_to_cpu(req->max);
de73115a
CT		 2690 latency = __le16_to_cpu(req->latency);
2691 to_multiplier = __le16_to_cpu(req->to_multiplier);
2692
2693 BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
2694 min, max, latency, to_multiplier);
2695
2696 memset(&rsp, 0, sizeof(rsp));
2ce603eb
CT		 2697
2698 err = l2cap_check_conn_param(min, max, latency, to_multiplier);
2699 if (err)
de73115a
CT		 2700 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
2701 else
2702 rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
2703
2704 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
2705 sizeof(rsp), &rsp);
2706
2ce603eb
CT		 2707 if (!err)
2708 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
2709
de73115a
CT		 2710 return 0;
2711}
2712
3300d9a9
CT		 2713static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2714 struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
2715{
2716 int err = 0;
2717
2718 switch (cmd->code) {
2719 case L2CAP_COMMAND_REJ:
2720 l2cap_command_rej(conn, cmd, data);
2721 break;
2722
2723 case L2CAP_CONN_REQ:
2724 err = l2cap_connect_req(conn, cmd, data);
2725 break;
2726
2727 case L2CAP_CONN_RSP:
2728 err = l2cap_connect_rsp(conn, cmd, data);
2729 break;
2730
2731 case L2CAP_CONF_REQ:
2732 err = l2cap_config_req(conn, cmd, cmd_len, data);
2733 break;
2734
2735 case L2CAP_CONF_RSP:
2736 err = l2cap_config_rsp(conn, cmd, data);
2737 break;
2738
2739 case L2CAP_DISCONN_REQ:
2740 err = l2cap_disconnect_req(conn, cmd, data);
2741 break;
2742
2743 case L2CAP_DISCONN_RSP:
2744 err = l2cap_disconnect_rsp(conn, cmd, data);
2745 break;
2746
2747 case L2CAP_ECHO_REQ:
2748 l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data);
2749 break;
2750
2751 case L2CAP_ECHO_RSP:
2752 break;
2753
2754 case L2CAP_INFO_REQ:
2755 err = l2cap_information_req(conn, cmd, data);
2756 break;
2757
2758 case L2CAP_INFO_RSP:
2759 err = l2cap_information_rsp(conn, cmd, data);
2760 break;
2761
2762 default:
2763 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
2764 err = -EINVAL;
2765 break;
2766 }
2767
2768 return err;
2769}
2770
2771static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
2772 struct l2cap_cmd_hdr *cmd, u8 *data)
2773{
2774 switch (cmd->code) {
2775 case L2CAP_COMMAND_REJ:
2776 return 0;
2777
2778 case L2CAP_CONN_PARAM_UPDATE_REQ:
de73115a 2779 return l2cap_conn_param_update_req(conn, cmd, data);
3300d9a9
CT		 2780
2781 case L2CAP_CONN_PARAM_UPDATE_RSP:
2782 return 0;
2783
2784 default:
2785 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
2786 return -EINVAL;
2787 }
2788}
2789
2790static inline void l2cap_sig_channel(struct l2cap_conn *conn,
2791 struct sk_buff *skb)
1da177e4
LT		 2792{
2793 u8 *data = skb->data;
2794 int len = skb->len;
2795 struct l2cap_cmd_hdr cmd;
3300d9a9 2796 int err;
1da177e4
LT		 2797
2798 l2cap_raw_recv(conn, skb);
2799
2800 while (len >= L2CAP_CMD_HDR_SIZE) {
88219a0f 2801 u16 cmd_len;
1da177e4
LT		 2802 memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE);
2803 data += L2CAP_CMD_HDR_SIZE;
2804 len -= L2CAP_CMD_HDR_SIZE;
2805
88219a0f 2806 cmd_len = le16_to_cpu(cmd.len);
1da177e4 2807
88219a0f 2808 BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
1da177e4 2809
88219a0f 2810 if (cmd_len > len || !cmd.ident) {
1da177e4
LT		 2811 BT_DBG("corrupted command");
2812 break;
2813 }
2814
3300d9a9
CT		 2815 if (conn->hcon->type == LE_LINK)
2816 err = l2cap_le_sig_cmd(conn, &cmd, data);
2817 else
2818 err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
1da177e4
LT		 2819
2820 if (err) {
2821 struct l2cap_cmd_rej rej;
2c6d1a2e
GP		 2822
2823 BT_ERR("Wrong link type (%d)", err);
1da177e4
LT		 2824
2825 /* FIXME: Map err to a valid reason */
aca3192c 2826 rej.reason = cpu_to_le16(0);
1da177e4
LT		 2827 l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
2828 }
2829
88219a0f
AV		 2830 data += cmd_len;
2831 len -= cmd_len;
1da177e4
LT		 2832 }
2833
2834 kfree_skb(skb);
2835}
2836
47d1ec61 2837static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
fcc203c3
GP		 2838{
2839 u16 our_fcs, rcv_fcs;
2840 int hdr_size = L2CAP_HDR_SIZE + 2;
2841
47d1ec61 2842 if (chan->fcs == L2CAP_FCS_CRC16) {
fcc203c3
GP		 2843 skb_trim(skb, skb->len - 2);
2844 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
2845 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
2846
2847 if (our_fcs != rcv_fcs)
7a560e5c 2848 return -EBADMSG;
fcc203c3
GP		 2849 }
2850 return 0;
2851}
2852
525cd185 2853static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
d5392c8f 2854{
d5392c8f
GP		 2855 u16 control = 0;
2856
6a026610 2857 chan->frames_sent = 0;
d5392c8f 2858
42e5c802 2859 control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
d5392c8f 2860
525cd185 2861 if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
64988868 2862 control |= L2CAP_SUPER_RCV_NOT_READY;
525cd185
GP		 2863 l2cap_send_sframe(chan, control);
2864 chan->conn_state |= L2CAP_CONN_RNR_SENT;
d5392c8f
GP		 2865 }
2866
525cd185
GP		 2867 if (chan->conn_state & L2CAP_CONN_REMOTE_BUSY)
2868 l2cap_retransmit_frames(chan);
d5392c8f 2869
525cd185 2870 l2cap_ertm_send(chan);
d5392c8f 2871
525cd185 2872 if (!(chan->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
6a026610 2873 chan->frames_sent == 0) {
d5392c8f 2874 control |= L2CAP_SUPER_RCV_READY;
525cd185 2875 l2cap_send_sframe(chan, control);
d5392c8f
GP		 2876 }
2877}
2878
42e5c802 2879static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar)
8f17154f
GP		 2880{
2881 struct sk_buff *next_skb;
bfbacc11 2882 int tx_seq_offset, next_tx_seq_offset;
8f17154f
GP		 2883
2884 bt_cb(skb)->tx_seq = tx_seq;
2885 bt_cb(skb)->sar = sar;
2886
f1c6775b 2887 next_skb = skb_peek(&chan->srej_q);
8f17154f 2888 if (!next_skb) {
f1c6775b 2889 __skb_queue_tail(&chan->srej_q, skb);
9b53350d 2890 return 0;
8f17154f
GP		 2891 }
2892
42e5c802 2893 tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
bfbacc11
JPRV		 2894 if (tx_seq_offset < 0)
2895 tx_seq_offset += 64;
2896
8f17154f 2897 do {
9b53350d
JPRV		 2898 if (bt_cb(next_skb)->tx_seq == tx_seq)
2899 return -EINVAL;
2900
bfbacc11 2901 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
42e5c802 2902 chan->buffer_seq) % 64;
bfbacc11
JPRV		 2903 if (next_tx_seq_offset < 0)
2904 next_tx_seq_offset += 64;
2905
2906 if (next_tx_seq_offset > tx_seq_offset) {
f1c6775b 2907 __skb_queue_before(&chan->srej_q, next_skb, skb);
9b53350d 2908 return 0;
8f17154f
GP		 2909 }
2910
f1c6775b 2911 if (skb_queue_is_last(&chan->srej_q, next_skb))
8f17154f
GP		 2912 break;
2913
f1c6775b 2914 } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb)));
8f17154f 2915
f1c6775b 2916 __skb_queue_tail(&chan->srej_q, skb);
9b53350d
JPRV		 2917
2918 return 0;
8f17154f
GP		 2919}
2920
525cd185 2921static int l2cap_ertm_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
18778a63 2922{
18778a63 2923 struct sk_buff *_skb;
1890d36b 2924 int err;
18778a63
GP		 2925
2926 switch (control & L2CAP_CTRL_SAR) {
2927 case L2CAP_SDU_UNSEGMENTED:
525cd185 2928 if (chan->conn_state & L2CAP_CONN_SAR_SDU)
18778a63
GP		 2929 goto drop;
2930
525cd185 2931 err = sock_queue_rcv_skb(chan->sk, skb);
18778a63
GP		 2932 if (!err)
2933 return err;
2934
2935 break;
2936
2937 case L2CAP_SDU_START:
525cd185 2938 if (chan->conn_state & L2CAP_CONN_SAR_SDU)
18778a63
GP		 2939 goto drop;
2940
6f61fd47 2941 chan->sdu_len = get_unaligned_le16(skb->data);
18778a63 2942
0c1bc5c6 2943 if (chan->sdu_len > chan->imtu)
18778a63
GP		 2944 goto disconnect;
2945
6f61fd47
GP		 2946 chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC);
2947 if (!chan->sdu)
1890d36b
GP		 2948 return -ENOMEM;
2949
2950 /* pull sdu_len bytes only after alloc, because of Local Busy
2951 * condition we have to be sure that this will be executed
2952 * only once, i.e., when alloc does not fail */
2953 skb_pull(skb, 2);
18778a63 2954
6f61fd47 2955 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
18778a63 2956
525cd185 2957 chan->conn_state |= L2CAP_CONN_SAR_SDU;
6f61fd47 2958 chan->partial_sdu_len = skb->len;
18778a63
GP		 2959 break;
2960
2961 case L2CAP_SDU_CONTINUE:
525cd185 2962 if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
18778a63
GP		 2963 goto disconnect;
2964
6f61fd47 2965 if (!chan->sdu)
18778a63
GP		 2966 goto disconnect;
2967
6f61fd47
GP		 2968 chan->partial_sdu_len += skb->len;
2969 if (chan->partial_sdu_len > chan->sdu_len)
18778a63
GP		 2970 goto drop;
2971
6f61fd47 2972 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
4178ba46 2973
18778a63
GP		 2974 break;
2975
2976 case L2CAP_SDU_END:
525cd185 2977 if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
18778a63
GP		 2978 goto disconnect;
2979
6f61fd47 2980 if (!chan->sdu)
18778a63
GP		 2981 goto disconnect;
2982
525cd185 2983 if (!(chan->conn_state & L2CAP_CONN_SAR_RETRY)) {
6f61fd47 2984 chan->partial_sdu_len += skb->len;
18778a63 2985
0c1bc5c6 2986 if (chan->partial_sdu_len > chan->imtu)
1890d36b 2987 goto drop;
18778a63 2988
6f61fd47 2989 if (chan->partial_sdu_len != chan->sdu_len)
1890d36b 2990 goto drop;
4178ba46 2991
6f61fd47 2992 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
1890d36b 2993 }
18778a63 2994
6f61fd47 2995 _skb = skb_clone(chan->sdu, GFP_ATOMIC);
1890d36b 2996 if (!_skb) {
525cd185 2997 chan->conn_state |= L2CAP_CONN_SAR_RETRY;
1890d36b
GP		 2998 return -ENOMEM;
2999 }
3000
525cd185 3001 err = sock_queue_rcv_skb(chan->sk, _skb);
1890d36b 3002 if (err < 0) {
18778a63 3003 kfree_skb(_skb);
525cd185 3004 chan->conn_state |= L2CAP_CONN_SAR_RETRY;
1890d36b
GP		 3005 return err;
3006 }
3007
525cd185
GP		 3008 chan->conn_state &= ~L2CAP_CONN_SAR_RETRY;
3009 chan->conn_state &= ~L2CAP_CONN_SAR_SDU;
18778a63 3010
6f61fd47 3011 kfree_skb(chan->sdu);
18778a63
GP		 3012 break;
3013 }
3014
3015 kfree_skb(skb);
1890d36b 3016 return 0;
18778a63
GP		 3017
3018drop:
6f61fd47
GP		 3019 kfree_skb(chan->sdu);
3020 chan->sdu = NULL;
18778a63
GP		 3021
3022disconnect:
8c1d787b 3023 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
18778a63
GP		 3024 kfree_skb(skb);
3025 return 0;
3026}
3027
525cd185 3028static int l2cap_try_push_rx_skb(struct l2cap_chan *chan)
712132eb 3029{
712132eb
GP		 3030 struct sk_buff *skb;
3031 u16 control;
3032 int err;
3033
f1c6775b 3034 while ((skb = skb_dequeue(&chan->busy_q))) {
712132eb 3035 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
525cd185 3036 err = l2cap_ertm_reassembly_sdu(chan, skb, control);
712132eb 3037 if (err < 0) {
f1c6775b 3038 skb_queue_head(&chan->busy_q, skb);
712132eb
GP		 3039 return -EBUSY;
3040 }
3041
42e5c802 3042 chan->buffer_seq = (chan->buffer_seq + 1) % 64;
712132eb
GP		 3043 }
3044
525cd185 3045 if (!(chan->conn_state & L2CAP_CONN_RNR_SENT))
712132eb
GP		 3046 goto done;
3047
42e5c802 3048 control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
712132eb 3049 control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
525cd185 3050 l2cap_send_sframe(chan, control);
6a026610 3051 chan->retry_count = 1;
712132eb 3052
e92c8e70 3053 del_timer(&chan->retrans_timer);
712132eb
GP		 3054 __mod_monitor_timer();
3055
525cd185 3056 chan->conn_state |= L2CAP_CONN_WAIT_F;
712132eb
GP		 3057
3058done:
525cd185
GP		 3059 chan->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
3060 chan->conn_state &= ~L2CAP_CONN_RNR_SENT;
712132eb 3061
49208c9c 3062 BT_DBG("chan %p, Exit local busy", chan);
712132eb
GP		 3063
3064 return 0;
3065}
3066
1890d36b
GP		 3067static void l2cap_busy_work(struct work_struct *work)
3068{
3069 DECLARE_WAITQUEUE(wait, current);
311bb895
GP		 3070 struct l2cap_chan *chan =
3071 container_of(work, struct l2cap_chan, busy_work);
3072 struct sock *sk = chan->sk;
1890d36b
GP		 3073 int n_tries = 0, timeo = HZ/5, err;
3074 struct sk_buff *skb;
1890d36b
GP		 3075
3076 lock_sock(sk);
3077
2b0b05dd 3078 add_wait_queue(sk_sleep(sk), &wait);
311bb895 3079 while ((skb = skb_peek(&chan->busy_q))) {
1890d36b
GP		 3080 set_current_state(TASK_INTERRUPTIBLE);
3081
3082 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
3083 err = -EBUSY;
8c1d787b 3084 l2cap_send_disconn_req(chan->conn, chan, EBUSY);
712132eb 3085 break;
1890d36b
GP		 3086 }
3087
3088 if (!timeo)
3089 timeo = HZ/5;
3090
3091 if (signal_pending(current)) {
3092 err = sock_intr_errno(timeo);
712132eb 3093 break;
1890d36b
GP		 3094 }
3095
3096 release_sock(sk);
3097 timeo = schedule_timeout(timeo);
3098 lock_sock(sk);
3099
3100 err = sock_error(sk);
3101 if (err)
712132eb 3102 break;
1890d36b 3103
311bb895 3104 if (l2cap_try_push_rx_skb(chan) == 0)
1890d36b
GP		 3105 break;
3106 }
3107
1890d36b 3108 set_current_state(TASK_RUNNING);
2b0b05dd 3109 remove_wait_queue(sk_sleep(sk), &wait);
1890d36b
GP		 3110
3111 release_sock(sk);
3112}
3113
525cd185 3114static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
1890d36b 3115{
1890d36b
GP		 3116 int sctrl, err;
3117
525cd185 3118 if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
1890d36b 3119 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
f1c6775b 3120 __skb_queue_tail(&chan->busy_q, skb);
525cd185 3121 return l2cap_try_push_rx_skb(chan);
712132eb
GP		 3122
3123
1890d36b
GP		 3124 }
3125
525cd185 3126 err = l2cap_ertm_reassembly_sdu(chan, skb, control);
1890d36b 3127 if (err >= 0) {
42e5c802 3128 chan->buffer_seq = (chan->buffer_seq + 1) % 64;
1890d36b
GP		 3129 return err;
3130 }
3131
3132 /* Busy Condition */
311bb895 3133 BT_DBG("chan %p, Enter local busy", chan);
0e98958d 3134
525cd185 3135 chan->conn_state |= L2CAP_CONN_LOCAL_BUSY;
1890d36b 3136 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
f1c6775b 3137 __skb_queue_tail(&chan->busy_q, skb);
1890d36b 3138
42e5c802 3139 sctrl = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
1890d36b 3140 sctrl |= L2CAP_SUPER_RCV_NOT_READY;
525cd185 3141 l2cap_send_sframe(chan, sctrl);
1890d36b 3142
525cd185 3143 chan->conn_state |= L2CAP_CONN_RNR_SENT;
1890d36b 3144
e92c8e70 3145 del_timer(&chan->ack_timer);
7fe9b298 3146
311bb895 3147 queue_work(_busy_wq, &chan->busy_work);
1890d36b
GP		 3148
3149 return err;
3150}
3151
525cd185 3152static int l2cap_streaming_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
c74e560c 3153{
c74e560c
GP		 3154 struct sk_buff *_skb;
3155 int err = -EINVAL;
3156
18778a63
GP		 3157 /*
3158 * TODO: We have to notify the userland if some data is lost with the
3159 * Streaming Mode.
3160 */
3161
c74e560c
GP		 3162 switch (control & L2CAP_CTRL_SAR) {
3163 case L2CAP_SDU_UNSEGMENTED:
525cd185 3164 if (chan->conn_state & L2CAP_CONN_SAR_SDU) {
6f61fd47 3165 kfree_skb(chan->sdu);
c74e560c
GP		 3166 break;
3167 }
3168
525cd185 3169 err = sock_queue_rcv_skb(chan->sk, skb);
c74e560c
GP		 3170 if (!err)
3171 return 0;
3172
3173 break;
3174
3175 case L2CAP_SDU_START:
525cd185 3176 if (chan->conn_state & L2CAP_CONN_SAR_SDU) {
6f61fd47 3177 kfree_skb(chan->sdu);
c74e560c
GP		 3178 break;
3179 }
3180
6f61fd47 3181 chan->sdu_len = get_unaligned_le16(skb->data);
c74e560c
GP		 3182 skb_pull(skb, 2);
3183
0c1bc5c6 3184 if (chan->sdu_len > chan->imtu) {
052897ca
GP		 3185 err = -EMSGSIZE;
3186 break;
3187 }
3188
6f61fd47
GP		 3189 chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC);
3190 if (!chan->sdu) {
c74e560c
GP		 3191 err = -ENOMEM;
3192 break;
3193 }
3194
6f61fd47 3195 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
c74e560c 3196
525cd185 3197 chan->conn_state |= L2CAP_CONN_SAR_SDU;
6f61fd47 3198 chan->partial_sdu_len = skb->len;
c74e560c
GP		 3199 err = 0;
3200 break;
3201
3202 case L2CAP_SDU_CONTINUE:
525cd185 3203 if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
c74e560c
GP		 3204 break;
3205
6f61fd47 3206 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
c74e560c 3207
6f61fd47
GP		 3208 chan->partial_sdu_len += skb->len;
3209 if (chan->partial_sdu_len > chan->sdu_len)
3210 kfree_skb(chan->sdu);
c74e560c
GP		 3211 else
3212 err = 0;
3213
3214 break;
3215
3216 case L2CAP_SDU_END:
525cd185 3217 if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
c74e560c
GP		 3218 break;
3219
6f61fd47 3220 memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
c74e560c 3221
525cd185 3222 chan->conn_state &= ~L2CAP_CONN_SAR_SDU;
6f61fd47 3223 chan->partial_sdu_len += skb->len;
c74e560c 3224
0c1bc5c6 3225 if (chan->partial_sdu_len > chan->imtu)
36f2fd58
GP		 3226 goto drop;
3227
6f61fd47
GP		 3228 if (chan->partial_sdu_len == chan->sdu_len) {
3229 _skb = skb_clone(chan->sdu, GFP_ATOMIC);
525cd185 3230 err = sock_queue_rcv_skb(chan->sk, _skb);
c74e560c
GP		 3231 if (err < 0)
3232 kfree_skb(_skb);
3233 }
c74e560c
GP		 3234 err = 0;
3235
36f2fd58 3236drop:
6f61fd47 3237 kfree_skb(chan->sdu);
c74e560c
GP		 3238 break;
3239 }
3240
3241 kfree_skb(skb);
3242 return err;
3243}
3244
525cd185 3245static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
8f17154f
GP		 3246{
3247 struct sk_buff *skb;
afefdbc4 3248 u16 control;
8f17154f 3249
f1c6775b 3250 while ((skb = skb_peek(&chan->srej_q))) {
8f17154f
GP		 3251 if (bt_cb(skb)->tx_seq != tx_seq)
3252 break;
3253
f1c6775b 3254 skb = skb_dequeue(&chan->srej_q);
afefdbc4 3255 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
525cd185 3256 l2cap_ertm_reassembly_sdu(chan, skb, control);
42e5c802
GP		 3257 chan->buffer_seq_srej =
3258 (chan->buffer_seq_srej + 1) % 64;
8ff50ec0 3259 tx_seq = (tx_seq + 1) % 64;
8f17154f
GP		 3260 }
3261}
3262
525cd185 3263static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq)
8f17154f 3264{
8f17154f
GP		 3265 struct srej_list *l, *tmp;
3266 u16 control;
3267
39d5a3ee 3268 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
8f17154f
GP		 3269 if (l->tx_seq == tx_seq) {
3270 list_del(&l->list);
3271 kfree(l);
3272 return;
3273 }
3274 control = L2CAP_SUPER_SELECT_REJECT;
3275 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
525cd185 3276 l2cap_send_sframe(chan, control);
8f17154f 3277 list_del(&l->list);
39d5a3ee 3278 list_add_tail(&l->list, &chan->srej_l);
8f17154f
GP		 3279 }
3280}
3281
525cd185 3282static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq)
8f17154f 3283{
8f17154f
GP		 3284 struct srej_list *new;
3285 u16 control;
3286
42e5c802 3287 while (tx_seq != chan->expected_tx_seq) {
8f17154f 3288 control = L2CAP_SUPER_SELECT_REJECT;
42e5c802 3289 control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
525cd185 3290 l2cap_send_sframe(chan, control);
8f17154f
GP		 3291
3292 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
42e5c802
GP		 3293 new->tx_seq = chan->expected_tx_seq;
3294 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
39d5a3ee 3295 list_add_tail(&new->list, &chan->srej_l);
8f17154f 3296 }
42e5c802 3297 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
8f17154f
GP		 3298}
3299
525cd185 3300static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
1c2acffb 3301{
1c2acffb 3302 u8 tx_seq = __get_txseq(rx_control);
9f121a5a 3303 u8 req_seq = __get_reqseq(rx_control);
8f17154f 3304 u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
f6337c77 3305 int tx_seq_offset, expected_tx_seq_offset;
47d1ec61 3306 int num_to_ack = (chan->tx_win/6) + 1;
1c2acffb
GP		 3307 int err = 0;
3308
525cd185
GP		 3309 BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len,
3310 tx_seq, rx_control);
1c2acffb 3311
9b16dc65 3312 if (L2CAP_CTRL_FINAL & rx_control &&
525cd185 3313 chan->conn_state & L2CAP_CONN_WAIT_F) {
e92c8e70 3314 del_timer(&chan->monitor_timer);
6a026610 3315 if (chan->unacked_frames > 0)
1d8f5d16 3316 __mod_retrans_timer();
525cd185 3317 chan->conn_state &= ~L2CAP_CONN_WAIT_F;
1d8f5d16
GP		 3318 }
3319
42e5c802
GP		 3320 chan->expected_ack_seq = req_seq;
3321 l2cap_drop_acked_frames(chan);
9f121a5a 3322
42e5c802 3323 if (tx_seq == chan->expected_tx_seq)
8f17154f 3324 goto expected;
1c2acffb 3325
42e5c802 3326 tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
9b53350d
JPRV		 3327 if (tx_seq_offset < 0)
3328 tx_seq_offset += 64;
3329
3330 /* invalid tx_seq */
47d1ec61 3331 if (tx_seq_offset >= chan->tx_win) {
8c1d787b 3332 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
9b53350d
JPRV		 3333 goto drop;
3334 }
3335
525cd185 3336 if (chan->conn_state == L2CAP_CONN_LOCAL_BUSY)
1890d36b
GP		 3337 goto drop;
3338
525cd185 3339 if (chan->conn_state & L2CAP_CONN_SREJ_SENT) {
8f17154f 3340 struct srej_list *first;
30afb5b2 3341
39d5a3ee 3342 first = list_first_entry(&chan->srej_l,
8f17154f
GP		 3343 struct srej_list, list);
3344 if (tx_seq == first->tx_seq) {
42e5c802 3345 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
525cd185 3346 l2cap_check_srej_gap(chan, tx_seq);
8f17154f
GP		 3347
3348 list_del(&first->list);
3349 kfree(first);
3350
39d5a3ee 3351 if (list_empty(&chan->srej_l)) {
42e5c802 3352 chan->buffer_seq = chan->buffer_seq_srej;
525cd185
GP		 3353 chan->conn_state &= ~L2CAP_CONN_SREJ_SENT;
3354 l2cap_send_ack(chan);
49208c9c 3355 BT_DBG("chan %p, Exit SREJ_SENT", chan);
8f17154f
GP		 3356 }
3357 } else {
3358 struct srej_list *l;
9b53350d
JPRV		 3359
3360 /* duplicated tx_seq */
42e5c802 3361 if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
9b53350d 3362 goto drop;
8f17154f 3363
39d5a3ee 3364 list_for_each_entry(l, &chan->srej_l, list) {
8f17154f 3365 if (l->tx_seq == tx_seq) {
525cd185 3366 l2cap_resend_srejframe(chan, tx_seq);
8f17154f
GP		 3367 return 0;
3368 }
3369 }
525cd185 3370 l2cap_send_srejframe(chan, tx_seq);
30afb5b2
GP		 3371 }
3372 } else {
9b53350d 3373 expected_tx_seq_offset =
42e5c802 3374 (chan->expected_tx_seq - chan->buffer_seq) % 64;
9b53350d
JPRV		 3375 if (expected_tx_seq_offset < 0)
3376 expected_tx_seq_offset += 64;
3377
3378 /* duplicated tx_seq */
3379 if (tx_seq_offset < expected_tx_seq_offset)
3380 goto drop;
3381
525cd185 3382 chan->conn_state |= L2CAP_CONN_SREJ_SENT;
1c2acffb 3383
49208c9c 3384 BT_DBG("chan %p, Enter SREJ", chan);
0e98958d 3385
39d5a3ee 3386 INIT_LIST_HEAD(&chan->srej_l);
42e5c802 3387 chan->buffer_seq_srej = chan->buffer_seq;
8f17154f 3388
f1c6775b
GP		 3389 __skb_queue_head_init(&chan->srej_q);
3390 __skb_queue_head_init(&chan->busy_q);
42e5c802 3391 l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
8f17154f 3392
525cd185 3393 chan->conn_state |= L2CAP_CONN_SEND_PBIT;
ef54fd93 3394
525cd185 3395 l2cap_send_srejframe(chan, tx_seq);
7fe9b298 3396
e92c8e70 3397 del_timer(&chan->ack_timer);
1c2acffb 3398 }
30afb5b2
GP		 3399 return 0;
3400
8f17154f 3401expected:
42e5c802 3402 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
8f17154f 3403
525cd185 3404 if (chan->conn_state & L2CAP_CONN_SREJ_SENT) {
3b1a9f3f
GP		 3405 bt_cb(skb)->tx_seq = tx_seq;
3406 bt_cb(skb)->sar = sar;
f1c6775b 3407 __skb_queue_tail(&chan->srej_q, skb);
8f17154f
GP		 3408 return 0;
3409 }
3410
525cd185 3411 err = l2cap_push_rx_skb(chan, skb, rx_control);
2ece3684
GP		 3412 if (err < 0)
3413 return 0;
3414
4ec10d97 3415 if (rx_control & L2CAP_CTRL_FINAL) {
525cd185
GP		 3416 if (chan->conn_state & L2CAP_CONN_REJ_ACT)
3417 chan->conn_state &= ~L2CAP_CONN_REJ_ACT;
dfc909be 3418 else
525cd185 3419 l2cap_retransmit_frames(chan);
4ec10d97
GP		 3420 }
3421
c1b4f43b
GP		 3422 __mod_ack_timer();
3423
6a026610
GP		 3424 chan->num_acked = (chan->num_acked + 1) % num_to_ack;
3425 if (chan->num_acked == num_to_ack - 1)
525cd185 3426 l2cap_send_ack(chan);
9e917af1 3427
8f17154f 3428 return 0;
9b53350d
JPRV		 3429
3430drop:
3431 kfree_skb(skb);
3432 return 0;
1c2acffb
GP		 3433}
3434
525cd185 3435static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control)
1c2acffb 3436{
49208c9c 3437 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control),
0e98958d
GP		 3438 rx_control);
3439
42e5c802
GP		 3440 chan->expected_ack_seq = __get_reqseq(rx_control);
3441 l2cap_drop_acked_frames(chan);
1c2acffb 3442
e072745f 3443 if (rx_control & L2CAP_CTRL_POLL) {
525cd185
GP		 3444 chan->conn_state |= L2CAP_CONN_SEND_FBIT;
3445 if (chan->conn_state & L2CAP_CONN_SREJ_SENT) {
3446 if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
6a026610 3447 (chan->unacked_frames > 0))
05fbd89d
GP		 3448 __mod_retrans_timer();
3449
525cd185
GP		 3450 chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3451 l2cap_send_srejtail(chan);
05fbd89d 3452 } else {
525cd185 3453 l2cap_send_i_or_rr_or_rnr(chan);
05fbd89d 3454 }
1d8f5d16 3455
e072745f 3456 } else if (rx_control & L2CAP_CTRL_FINAL) {
525cd185 3457 chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
4ec10d97 3458
525cd185
GP		 3459 if (chan->conn_state & L2CAP_CONN_REJ_ACT)
3460 chan->conn_state &= ~L2CAP_CONN_REJ_ACT;
dfc909be 3461 else
525cd185 3462 l2cap_retransmit_frames(chan);
2246b2f1 3463
e072745f 3464 } else {
525cd185 3465 if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
6a026610 3466 (chan->unacked_frames > 0))
e072745f 3467 __mod_retrans_timer();
1c2acffb 3468
525cd185
GP		 3469 chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
3470 if (chan->conn_state & L2CAP_CONN_SREJ_SENT)
3471 l2cap_send_ack(chan);
894718a6 3472 else
525cd185 3473 l2cap_ertm_send(chan);
e072745f
GP		 3474 }
3475}
2246b2f1 3476
525cd185 3477static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control)
e072745f 3478{
e072745f 3479 u8 tx_seq = __get_reqseq(rx_control);
30afb5b2 3480
525cd185 3481 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
0e98958d 3482
525cd185 3483 chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
e072745f 3484
42e5c802
GP		 3485 chan->expected_ack_seq = tx_seq;
3486 l2cap_drop_acked_frames(chan);
e072745f
GP		 3487
3488 if (rx_control & L2CAP_CTRL_FINAL) {
525cd185
GP		 3489 if (chan->conn_state & L2CAP_CONN_REJ_ACT)
3490 chan->conn_state &= ~L2CAP_CONN_REJ_ACT;
dfc909be 3491 else
525cd185 3492 l2cap_retransmit_frames(chan);
e072745f 3493 } else {
525cd185 3494 l2cap_retransmit_frames(chan);
30afb5b2 3495
525cd185
GP		 3496 if (chan->conn_state & L2CAP_CONN_WAIT_F)
3497 chan->conn_state |= L2CAP_CONN_REJ_ACT;
e072745f
GP		 3498 }
3499}
525cd185 3500static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control)
e072745f 3501{
e072745f 3502 u8 tx_seq = __get_reqseq(rx_control);
30afb5b2 3503
525cd185 3504 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
0e98958d 3505
525cd185 3506 chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
2246b2f1 3507
e072745f 3508 if (rx_control & L2CAP_CTRL_POLL) {
42e5c802
GP		 3509 chan->expected_ack_seq = tx_seq;
3510 l2cap_drop_acked_frames(chan);
3cb123d1 3511
525cd185
GP		 3512 chan->conn_state |= L2CAP_CONN_SEND_FBIT;
3513 l2cap_retransmit_one_frame(chan, tx_seq);
dfc909be 3514
525cd185 3515 l2cap_ertm_send(chan);
dfc909be 3516
525cd185 3517 if (chan->conn_state & L2CAP_CONN_WAIT_F) {
6a026610 3518 chan->srej_save_reqseq = tx_seq;
525cd185 3519 chan->conn_state |= L2CAP_CONN_SREJ_ACT;
ef54fd93 3520 }
e072745f 3521 } else if (rx_control & L2CAP_CTRL_FINAL) {
525cd185 3522 if ((chan->conn_state & L2CAP_CONN_SREJ_ACT) &&
6a026610 3523 chan->srej_save_reqseq == tx_seq)
525cd185 3524 chan->conn_state &= ~L2CAP_CONN_SREJ_ACT;
e072745f 3525 else
525cd185 3526 l2cap_retransmit_one_frame(chan, tx_seq);
e072745f 3527 } else {
525cd185
GP		 3528 l2cap_retransmit_one_frame(chan, tx_seq);
3529 if (chan->conn_state & L2CAP_CONN_WAIT_F) {
6a026610 3530 chan->srej_save_reqseq = tx_seq;
525cd185 3531 chan->conn_state |= L2CAP_CONN_SREJ_ACT;
ef54fd93 3532 }
e072745f
GP		 3533 }
3534}
3535
525cd185 3536static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control)
e072745f 3537{
e072745f
GP		 3538 u8 tx_seq = __get_reqseq(rx_control);
3539
525cd185 3540 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
0e98958d 3541
525cd185 3542 chan->conn_state |= L2CAP_CONN_REMOTE_BUSY;
42e5c802
GP		 3543 chan->expected_ack_seq = tx_seq;
3544 l2cap_drop_acked_frames(chan);
e072745f 3545
3cb123d1 3546 if (rx_control & L2CAP_CTRL_POLL)
525cd185 3547 chan->conn_state |= L2CAP_CONN_SEND_FBIT;
3cb123d1 3548
525cd185 3549 if (!(chan->conn_state & L2CAP_CONN_SREJ_SENT)) {
e92c8e70 3550 del_timer(&chan->retrans_timer);
a2e12a2a 3551 if (rx_control & L2CAP_CTRL_POLL)
525cd185 3552 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
99b0d4b7 3553 return;
e072745f 3554 }
99b0d4b7
GP		 3555
3556 if (rx_control & L2CAP_CTRL_POLL)
525cd185 3557 l2cap_send_srejtail(chan);
99b0d4b7 3558 else
525cd185 3559 l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY);
e072745f
GP		 3560}
3561
525cd185 3562static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
e072745f 3563{
525cd185 3564 BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len);
e072745f 3565
9b16dc65 3566 if (L2CAP_CTRL_FINAL & rx_control &&
525cd185 3567 chan->conn_state & L2CAP_CONN_WAIT_F) {
e92c8e70 3568 del_timer(&chan->monitor_timer);
6a026610 3569 if (chan->unacked_frames > 0)
e072745f 3570 __mod_retrans_timer();
525cd185 3571 chan->conn_state &= ~L2CAP_CONN_WAIT_F;
e072745f
GP		 3572 }
3573
3574 switch (rx_control & L2CAP_CTRL_SUPERVISE) {
3575 case L2CAP_SUPER_RCV_READY:
525cd185 3576 l2cap_data_channel_rrframe(chan, rx_control);
8f17154f
GP		 3577 break;
3578
e072745f 3579 case L2CAP_SUPER_REJECT:
525cd185 3580 l2cap_data_channel_rejframe(chan, rx_control);
e072745f 3581 break;
2246b2f1 3582
e072745f 3583 case L2CAP_SUPER_SELECT_REJECT:
525cd185 3584 l2cap_data_channel_srejframe(chan, rx_control);
e072745f
GP		 3585 break;
3586
3587 case L2CAP_SUPER_RCV_NOT_READY:
525cd185 3588 l2cap_data_channel_rnrframe(chan, rx_control);
1c2acffb
GP		 3589 break;
3590 }
3591
faaebd19 3592 kfree_skb(skb);
1c2acffb
GP		 3593 return 0;
3594}
3595
218bb9df
GP		 3596static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3597{
525cd185 3598 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
218bb9df
GP		 3599 u16 control;
3600 u8 req_seq;
3601 int len, next_tx_seq_offset, req_seq_offset;
3602
3603 control = get_unaligned_le16(skb->data);
3604 skb_pull(skb, 2);
3605 len = skb->len;
3606
3607 /*
3608 * We can just drop the corrupted I-frame here.
3609 * Receiver will miss it and start proper recovery
3610 * procedures and ask retransmission.
3611 */
47d1ec61 3612 if (l2cap_check_fcs(chan, skb))
218bb9df
GP		 3613 goto drop;
3614
3615 if (__is_sar_start(control) && __is_iframe(control))
3616 len -= 2;
3617
47d1ec61 3618 if (chan->fcs == L2CAP_FCS_CRC16)
218bb9df
GP		 3619 len -= 2;
3620
47d1ec61 3621 if (len > chan->mps) {
8c1d787b 3622 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 3623 goto drop;
3624 }
3625
3626 req_seq = __get_reqseq(control);
42e5c802 3627 req_seq_offset = (req_seq - chan->expected_ack_seq) % 64;
218bb9df
GP		 3628 if (req_seq_offset < 0)
3629 req_seq_offset += 64;
3630
3631 next_tx_seq_offset =
42e5c802 3632 (chan->next_tx_seq - chan->expected_ack_seq) % 64;
218bb9df
GP		 3633 if (next_tx_seq_offset < 0)
3634 next_tx_seq_offset += 64;
3635
3636 /* check for invalid req-seq */
3637 if (req_seq_offset > next_tx_seq_offset) {
8c1d787b 3638 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 3639 goto drop;
3640 }
3641
3642 if (__is_iframe(control)) {
3643 if (len < 0) {
8c1d787b 3644 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 3645 goto drop;
3646 }
3647
525cd185 3648 l2cap_data_channel_iframe(chan, control, skb);
218bb9df
GP		 3649 } else {
3650 if (len != 0) {
3651 BT_ERR("%d", len);
8c1d787b 3652 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
218bb9df
GP		 3653 goto drop;
3654 }
3655
525cd185 3656 l2cap_data_channel_sframe(chan, control, skb);
218bb9df
GP		 3657 }
3658
3659 return 0;
3660
3661drop:
3662 kfree_skb(skb);
3663 return 0;
3664}
3665
1da177e4
LT		 3666static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
3667{
48454079 3668 struct l2cap_chan *chan;
bf734843 3669 struct sock *sk = NULL;
6840ed07 3670 struct l2cap_pinfo *pi;
51893f88 3671 u16 control;
218bb9df
GP		 3672 u8 tx_seq;
3673 int len;
1da177e4 3674
baa7e1fa 3675 chan = l2cap_get_chan_by_scid(conn, cid);
48454079 3676 if (!chan) {
1da177e4
LT		 3677 BT_DBG("unknown cid 0x%4.4x", cid);
3678 goto drop;
3679 }
3680
48454079 3681 sk = chan->sk;
6840ed07
GP		 3682 pi = l2cap_pi(sk);
3683
49208c9c 3684 BT_DBG("chan %p, len %d", chan, skb->len);
1da177e4
LT		 3685
3686 if (sk->sk_state != BT_CONNECTED)
3687 goto drop;
3688
0c1bc5c6 3689 switch (chan->mode) {
1c2acffb
GP		 3690 case L2CAP_MODE_BASIC:
3691 /* If socket recv buffers overflows we drop data here
3692 * which is *bad* because L2CAP has to be reliable.
3693 * But we don't have any other choice. L2CAP doesn't
3694 * provide flow control mechanism. */
1da177e4 3695
0c1bc5c6 3696 if (chan->imtu < skb->len)
1c2acffb 3697 goto drop;
1da177e4 3698
1c2acffb
GP		 3699 if (!sock_queue_rcv_skb(sk, skb))
3700 goto done;
3701 break;
3702
3703 case L2CAP_MODE_ERTM:
218bb9df
GP		 3704 if (!sock_owned_by_user(sk)) {
3705 l2cap_ertm_data_rcv(sk, skb);
277ffbe3 3706 } else {
218bb9df 3707 if (sk_add_backlog(sk, skb))
277ffbe3 3708 goto drop;
277ffbe3 3709 }
1c2acffb 3710
fcafde2e 3711 goto done;
1c2acffb 3712
6840ed07
GP		 3713 case L2CAP_MODE_STREAMING:
3714 control = get_unaligned_le16(skb->data);
3715 skb_pull(skb, 2);
3716 len = skb->len;
3717
47d1ec61 3718 if (l2cap_check_fcs(chan, skb))
26000089
GP		 3719 goto drop;
3720
6840ed07
GP		 3721 if (__is_sar_start(control))
3722 len -= 2;
3723
47d1ec61 3724 if (chan->fcs == L2CAP_FCS_CRC16)
fcc203c3
GP		 3725 len -= 2;
3726
47d1ec61 3727 if (len > chan->mps || len < 0 || __is_sframe(control))
6840ed07
GP		 3728 goto drop;
3729
3730 tx_seq = __get_txseq(control);
3731
42e5c802
GP		 3732 if (chan->expected_tx_seq == tx_seq)
3733 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
6840ed07 3734 else
42e5c802 3735 chan->expected_tx_seq = (tx_seq + 1) % 64;
6840ed07 3736
525cd185 3737 l2cap_streaming_reassembly_sdu(chan, skb, control);
6840ed07
GP		 3738
3739 goto done;
3740
1c2acffb 3741 default:
0c1bc5c6 3742 BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode);
1c2acffb
GP		 3743 break;
3744 }
1da177e4
LT		 3745
3746drop:
3747 kfree_skb(skb);
3748
3749done:
0139418c
MH		 3750 if (sk)
3751 bh_unlock_sock(sk);
3752
1da177e4
LT		 3753 return 0;
3754}
3755
8e036fc3 3756static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
1da177e4 3757{
6dcae1ea 3758 struct sock *sk = NULL;
23691d75 3759 struct l2cap_chan *chan;
1da177e4 3760
23691d75
GP		 3761 chan = l2cap_global_chan_by_psm(0, psm, conn->src);
3762 if (!chan)
1da177e4
LT		 3763 goto drop;
3764
23691d75
GP		 3765 sk = chan->sk;
3766
e0f0cb56
GP		 3767 bh_lock_sock(sk);
3768
1da177e4
LT		 3769 BT_DBG("sk %p, len %d", sk, skb->len);
3770
3771 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3772 goto drop;
3773
0c1bc5c6 3774 if (l2cap_pi(sk)->chan->imtu < skb->len)
1da177e4
LT		 3775 goto drop;
3776
3777 if (!sock_queue_rcv_skb(sk, skb))
3778 goto done;
3779
3780drop:
3781 kfree_skb(skb);
3782
3783done:
af05b30b
GP		 3784 if (sk)
3785 bh_unlock_sock(sk);
1da177e4
LT		 3786 return 0;
3787}
3788
9f69bda6
GP		 3789static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
3790{
6dcae1ea 3791 struct sock *sk = NULL;
23691d75 3792 struct l2cap_chan *chan;
9f69bda6 3793
23691d75
GP		 3794 chan = l2cap_global_chan_by_scid(0, cid, conn->src);
3795 if (!chan)
9f69bda6
GP		 3796 goto drop;
3797
23691d75
GP		 3798 sk = chan->sk;
3799
9f69bda6
GP		 3800 bh_lock_sock(sk);
3801
3802 BT_DBG("sk %p, len %d", sk, skb->len);
3803
3804 if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
3805 goto drop;
3806
0c1bc5c6 3807 if (l2cap_pi(sk)->chan->imtu < skb->len)
9f69bda6
GP		 3808 goto drop;
3809
3810 if (!sock_queue_rcv_skb(sk, skb))
3811 goto done;
3812
3813drop:
3814 kfree_skb(skb);
3815
3816done:
3817 if (sk)
3818 bh_unlock_sock(sk);
3819 return 0;
3820}
3821
1da177e4
LT		 3822static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
3823{
3824 struct l2cap_hdr *lh = (void *) skb->data;
8e036fc3
AV		 3825 u16 cid, len;
3826 __le16 psm;
1da177e4
LT		 3827
3828 skb_pull(skb, L2CAP_HDR_SIZE);
3829 cid = __le16_to_cpu(lh->cid);
3830 len = __le16_to_cpu(lh->len);
3831
1c2acffb
GP		 3832 if (len != skb->len) {
3833 kfree_skb(skb);
3834 return;
3835 }
3836
1da177e4
LT		 3837 BT_DBG("len %d, cid 0x%4.4x", len, cid);
3838
3839 switch (cid) {
3300d9a9 3840 case L2CAP_CID_LE_SIGNALING:
8db4dc46 3841 case L2CAP_CID_SIGNALING:
1da177e4
LT		 3842 l2cap_sig_channel(conn, skb);
3843 break;
3844
8db4dc46 3845 case L2CAP_CID_CONN_LESS:
1b7bf4ed 3846 psm = get_unaligned_le16(skb->data);
1da177e4
LT		 3847 skb_pull(skb, 2);
3848 l2cap_conless_channel(conn, psm, skb);
3849 break;
3850
9f69bda6
GP		 3851 case L2CAP_CID_LE_DATA:
3852 l2cap_att_channel(conn, cid, skb);
3853 break;
3854
1da177e4
LT		 3855 default:
3856 l2cap_data_channel(conn, cid, skb);
3857 break;
3858 }
3859}
3860
3861/* ---- L2CAP interface with lower layer (HCI) ---- */
3862
3863static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3864{
3865 int exact = 0, lm1 = 0, lm2 = 0;
23691d75 3866 struct l2cap_chan *c;
1da177e4
LT		 3867
3868 if (type != ACL_LINK)
963cf687 3869 return -EINVAL;
1da177e4
LT		 3870
3871 BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
3872
3873 /* Find listening sockets and check their link_mode */
23691d75
GP		 3874 read_lock(&chan_list_lock);
3875 list_for_each_entry(c, &chan_list, global_l) {
3876 struct sock *sk = c->sk;
4343478f 3877
1da177e4
LT		 3878 if (sk->sk_state != BT_LISTEN)
3879 continue;
3880
3881 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
2af6b9d5 3882 lm1 |= HCI_LM_ACCEPT;
23691d75 3883 if (c->role_switch)
2af6b9d5 3884 lm1 |= HCI_LM_MASTER;
1da177e4 3885 exact++;
2af6b9d5
MH		 3886 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3887 lm2 |= HCI_LM_ACCEPT;
23691d75 3888 if (c->role_switch)
2af6b9d5
MH		 3889 lm2 |= HCI_LM_MASTER;
3890 }
1da177e4 3891 }
23691d75 3892 read_unlock(&chan_list_lock);
1da177e4
LT		 3893
3894 return exact ? lm1 : lm2;
3895}
3896
3897static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
3898{
0139418c
MH		 3899 struct l2cap_conn *conn;
3900
1da177e4
LT		 3901 BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
3902
acd7d370 3903 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
963cf687 3904 return -EINVAL;
1da177e4
LT		 3905
3906 if (!status) {
1da177e4
LT		 3907 conn = l2cap_conn_add(hcon, status);
3908 if (conn)
3909 l2cap_conn_ready(conn);
0139418c 3910 } else
1da177e4
LT		 3911 l2cap_conn_del(hcon, bt_err(status));
3912
3913 return 0;
3914}
3915
2950f21a
MH		 3916static int l2cap_disconn_ind(struct hci_conn *hcon)
3917{
3918 struct l2cap_conn *conn = hcon->l2cap_data;
3919
3920 BT_DBG("hcon %p", hcon);
3921
3922 if (hcon->type != ACL_LINK || !conn)
3923 return 0x13;
3924
3925 return conn->disc_reason;
3926}
3927
3928static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
1da177e4
LT		 3929{
3930 BT_DBG("hcon %p reason %d", hcon, reason);
3931
acd7d370 3932 if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
963cf687 3933 return -EINVAL;
1da177e4
LT		 3934
3935 l2cap_conn_del(hcon, bt_err(reason));
0139418c 3936
1da177e4
LT		 3937 return 0;
3938}
3939
4343478f 3940static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
f62e4323 3941{
4343478f
GP		 3942 struct sock *sk = chan->sk;
3943
bd3c9e25 3944 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
255c7601
MH		 3945 return;
3946
f62e4323 3947 if (encrypt == 0x00) {
4343478f 3948 if (chan->sec_level == BT_SECURITY_MEDIUM) {
f62e4323
MH		 3949 l2cap_sock_clear_timer(sk);
3950 l2cap_sock_set_timer(sk, HZ * 5);
4343478f 3951 } else if (chan->sec_level == BT_SECURITY_HIGH)
f62e4323
MH		 3952 __l2cap_sock_close(sk, ECONNREFUSED);
3953 } else {
4343478f 3954 if (chan->sec_level == BT_SECURITY_MEDIUM)
f62e4323
MH		 3955 l2cap_sock_clear_timer(sk);
3956 }
3957}
3958
8c1b2355 3959static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
1da177e4 3960{
0139418c 3961 struct l2cap_conn *conn = hcon->l2cap_data;
48454079 3962 struct l2cap_chan *chan;
1da177e4 3963
0139418c 3964 if (!conn)
1da177e4 3965 return 0;
0139418c 3966
1da177e4
LT		 3967 BT_DBG("conn %p", conn);
3968
baa7e1fa 3969 read_lock(&conn->chan_lock);
1da177e4 3970
baa7e1fa 3971 list_for_each_entry(chan, &conn->chan_l, list) {
48454079 3972 struct sock *sk = chan->sk;
baa7e1fa 3973
1da177e4
LT		 3974 bh_lock_sock(sk);
3975
b4450035 3976 if (chan->conf_state & L2CAP_CONF_CONNECT_PEND) {
6a8d3010
MH		 3977 bh_unlock_sock(sk);
3978 continue;
3979 }
3980
f62e4323 3981 if (!status && (sk->sk_state == BT_CONNECTED ||
8c1b2355 3982 sk->sk_state == BT_CONFIG)) {
4343478f 3983 l2cap_check_encryption(chan, encrypt);
9719f8af
MH		 3984 bh_unlock_sock(sk);
3985 continue;
3986 }
3987
b1235d79
MH		 3988 if (sk->sk_state == BT_CONNECT) {
3989 if (!status) {
3990 struct l2cap_conn_req req;
fe4128e0
GP		 3991 req.scid = cpu_to_le16(chan->scid);
3992 req.psm = chan->psm;
1da177e4 3993
fc7f8a7e 3994 chan->ident = l2cap_get_ident(conn);
b4450035 3995 chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
1da177e4 3996
fc7f8a7e 3997 l2cap_send_cmd(conn, chan->ident,
b1235d79
MH		 3998 L2CAP_CONN_REQ, sizeof(req), &req);
3999 } else {
4000 l2cap_sock_clear_timer(sk);
4001 l2cap_sock_set_timer(sk, HZ / 10);
4002 }
4003 } else if (sk->sk_state == BT_CONNECT2) {
4004 struct l2cap_conn_rsp rsp;
4005 __u16 result;
1da177e4 4006
b1235d79
MH		 4007 if (!status) {
4008 sk->sk_state = BT_CONFIG;
4009 result = L2CAP_CR_SUCCESS;
4010 } else {
4011 sk->sk_state = BT_DISCONN;
4012 l2cap_sock_set_timer(sk, HZ / 10);
4013 result = L2CAP_CR_SEC_BLOCK;
4014 }
4015
fe4128e0
GP		 4016 rsp.scid = cpu_to_le16(chan->dcid);
4017 rsp.dcid = cpu_to_le16(chan->scid);
b1235d79 4018 rsp.result = cpu_to_le16(result);
e7c29cb1 4019 rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
fc7f8a7e
GP		 4020 l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
4021 sizeof(rsp), &rsp);
b1235d79 4022 }
1da177e4
LT		 4023
4024 bh_unlock_sock(sk);
4025 }
4026
baa7e1fa 4027 read_unlock(&conn->chan_lock);
b1235d79 4028
1da177e4
LT		 4029 return 0;
4030}
4031
4032static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
4033{
4034 struct l2cap_conn *conn = hcon->l2cap_data;
4035
5a08ecce
AE		 4036 if (!conn)
4037 conn = l2cap_conn_add(hcon, 0);
4038
4039 if (!conn)
1da177e4
LT		 4040 goto drop;
4041
4042 BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
4043
e702112f 4044 if (!(flags & ACL_CONT)) {
1da177e4 4045 struct l2cap_hdr *hdr;
48454079 4046 struct l2cap_chan *chan;
89794813 4047 u16 cid;
1da177e4
LT		 4048 int len;
4049
4050 if (conn->rx_len) {
4051 BT_ERR("Unexpected start frame (len %d)", skb->len);
4052 kfree_skb(conn->rx_skb);
4053 conn->rx_skb = NULL;
4054 conn->rx_len = 0;
4055 l2cap_conn_unreliable(conn, ECOMM);
4056 }
4057
aae7fe22
AE		 4058 /* Start fragment always begin with Basic L2CAP header */
4059 if (skb->len < L2CAP_HDR_SIZE) {
1da177e4
LT		 4060 BT_ERR("Frame is too short (len %d)", skb->len);
4061 l2cap_conn_unreliable(conn, ECOMM);
4062 goto drop;
4063 }
4064
4065 hdr = (struct l2cap_hdr *) skb->data;
4066 len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE;
89794813 4067 cid = __le16_to_cpu(hdr->cid);
1da177e4
LT		 4068
4069 if (len == skb->len) {
4070 /* Complete frame received */
4071 l2cap_recv_frame(conn, skb);
4072 return 0;
4073 }
4074
4075 BT_DBG("Start: total len %d, frag len %d", len, skb->len);
4076
4077 if (skb->len > len) {
4078 BT_ERR("Frame is too long (len %d, expected len %d)",
4079 skb->len, len);
4080 l2cap_conn_unreliable(conn, ECOMM);
4081 goto drop;
4082 }
4083
baa7e1fa 4084 chan = l2cap_get_chan_by_scid(conn, cid);
89794813 4085
48454079
GP		 4086 if (chan && chan->sk) {
4087 struct sock *sk = chan->sk;
89794813 4088
0c1bc5c6 4089 if (chan->imtu < len - L2CAP_HDR_SIZE) {
48454079
GP		 4090 BT_ERR("Frame exceeding recv MTU (len %d, "
4091 "MTU %d)", len,
0c1bc5c6 4092 chan->imtu);
48454079
GP		 4093 bh_unlock_sock(sk);
4094 l2cap_conn_unreliable(conn, ECOMM);
4095 goto drop;
4096 }
89794813 4097 bh_unlock_sock(sk);
48454079 4098 }
89794813 4099
1da177e4 4100 /* Allocate skb for the complete frame (with header) */
af05b30b
GP		 4101 conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
4102 if (!conn->rx_skb)
1da177e4
LT		 4103 goto drop;
4104
d626f62b 4105 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
e1027a7c 4106 skb->len);
1da177e4
LT		 4107 conn->rx_len = len - skb->len;
4108 } else {
4109 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
4110
4111 if (!conn->rx_len) {
4112 BT_ERR("Unexpected continuation frame (len %d)", skb->len);
4113 l2cap_conn_unreliable(conn, ECOMM);
4114 goto drop;
4115 }
4116
4117 if (skb->len > conn->rx_len) {
4118 BT_ERR("Fragment is too long (len %d, expected %d)",
4119 skb->len, conn->rx_len);
4120 kfree_skb(conn->rx_skb);
4121 conn->rx_skb = NULL;
4122 conn->rx_len = 0;
4123 l2cap_conn_unreliable(conn, ECOMM);
4124 goto drop;
4125 }
4126
d626f62b 4127 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
e1027a7c 4128 skb->len);
1da177e4
LT		 4129 conn->rx_len -= skb->len;
4130
4131 if (!conn->rx_len) {
4132 /* Complete frame received */
4133 l2cap_recv_frame(conn, conn->rx_skb);
4134 conn->rx_skb = NULL;
4135 }
4136 }
4137
4138drop:
4139 kfree_skb(skb);
4140 return 0;
4141}
4142
aef7d97c 4143static int l2cap_debugfs_show(struct seq_file *f, void *p)
1da177e4 4144{
23691d75 4145 struct l2cap_chan *c;
1da177e4 4146
23691d75 4147 read_lock_bh(&chan_list_lock);
1da177e4 4148
23691d75
GP		 4149 list_for_each_entry(c, &chan_list, global_l) {
4150 struct sock *sk = c->sk;
101545f6 4151
903d343e 4152 seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
aef7d97c
MH		 4153 batostr(&bt_sk(sk)->src),
4154 batostr(&bt_sk(sk)->dst),
23691d75
GP		 4155 sk->sk_state, __le16_to_cpu(c->psm),
4156 c->scid, c->dcid, c->imtu, c->omtu,
4157 c->sec_level, c->mode);
be9d1227 4158 }
1da177e4 4159
23691d75 4160 read_unlock_bh(&chan_list_lock);
1da177e4 4161
aef7d97c 4162 return 0;
1da177e4
LT		 4163}
4164
aef7d97c
MH		 4165static int l2cap_debugfs_open(struct inode *inode, struct file *file)
4166{
4167 return single_open(file, l2cap_debugfs_show, inode->i_private);
4168}
4169
4170static const struct file_operations l2cap_debugfs_fops = {
4171 .open = l2cap_debugfs_open,
4172 .read = seq_read,
4173 .llseek = seq_lseek,
4174 .release = single_release,
4175};
4176
4177static struct dentry *l2cap_debugfs;
1da177e4 4178
1da177e4
LT		 4179static struct hci_proto l2cap_hci_proto = {
4180 .name = "L2CAP",
4181 .id = HCI_PROTO_L2CAP,
4182 .connect_ind = l2cap_connect_ind,
4183 .connect_cfm = l2cap_connect_cfm,
4184 .disconn_ind = l2cap_disconn_ind,
2950f21a 4185 .disconn_cfm = l2cap_disconn_cfm,
8c1b2355 4186 .security_cfm = l2cap_security_cfm,
1da177e4
LT		 4187 .recv_acldata = l2cap_recv_acldata
4188};
4189
64274518 4190int __init l2cap_init(void)
1da177e4
LT		 4191{
4192 int err;
be9d1227 4193
bb58f747 4194 err = l2cap_init_sockets();
1da177e4
LT		 4195 if (err < 0)
4196 return err;
4197
1890d36b 4198 _busy_wq = create_singlethread_workqueue("l2cap");
b78d7b4f 4199 if (!_busy_wq) {
bb58f747 4200 err = -ENOMEM;
1da177e4
LT		 4201 goto error;
4202 }
4203
4204 err = hci_register_proto(&l2cap_hci_proto);
4205 if (err < 0) {
4206 BT_ERR("L2CAP protocol registration failed");
4207 bt_sock_unregister(BTPROTO_L2CAP);
4208 goto error;
4209 }
4210
aef7d97c
MH		 4211 if (bt_debugfs) {
4212 l2cap_debugfs = debugfs_create_file("l2cap", 0444,
4213 bt_debugfs, NULL, &l2cap_debugfs_fops);
4214 if (!l2cap_debugfs)
4215 BT_ERR("Failed to create L2CAP debug file");
4216 }
1da177e4 4217
1da177e4
LT		 4218 return 0;
4219
4220error:
b78d7b4f 4221 destroy_workqueue(_busy_wq);
bb58f747 4222 l2cap_cleanup_sockets();
1da177e4
LT		 4223 return err;
4224}
4225
64274518 4226void l2cap_exit(void)
1da177e4 4227{
aef7d97c 4228 debugfs_remove(l2cap_debugfs);
1da177e4 4229
1890d36b
GP		 4230 flush_workqueue(_busy_wq);
4231 destroy_workqueue(_busy_wq);
4232
1da177e4
LT		 4233 if (hci_unregister_proto(&l2cap_hci_proto) < 0)
4234 BT_ERR("L2CAP protocol unregistration failed");
4235
bb58f747 4236 l2cap_cleanup_sockets();
1da177e4
LT		 4237}
4238
d1c4a17d
GP		 4239module_param(disable_ertm, bool, 0644);
4240MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
kernel source for telekom puls (alcatel/mediatek)