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