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