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