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