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