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