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