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