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