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