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