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