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