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