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