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