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