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