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