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