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