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