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