aboutsummaryrefslogtreecommitdiffstats
path: root/net/bluetooth/l2cap_core.c
diff options
context:
space:
mode:
Diffstat (limited to 'net/bluetooth/l2cap_core.c')
-rw-r--r--net/bluetooth/l2cap_core.c947
1 files changed, 706 insertions, 241 deletions
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 5ea94a1eecf2..e8a6837996cf 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -57,9 +57,10 @@
57#include <net/bluetooth/smp.h> 57#include <net/bluetooth/smp.h>
58 58
59int disable_ertm; 59int disable_ertm;
60int enable_hs;
60 61
61static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN; 62static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
62static u8 l2cap_fixed_chan[8] = { 0x02, }; 63static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
63 64
64static LIST_HEAD(chan_list); 65static LIST_HEAD(chan_list);
65static DEFINE_RWLOCK(chan_list_lock); 66static DEFINE_RWLOCK(chan_list_lock);
@@ -219,7 +220,7 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
219 220
220static void l2cap_set_timer(struct l2cap_chan *chan, struct timer_list *timer, long timeout) 221static void l2cap_set_timer(struct l2cap_chan *chan, struct timer_list *timer, long timeout)
221{ 222{
222 BT_DBG("chan %p state %d timeout %ld", chan->sk, chan->state, timeout); 223 BT_DBG("chan %p state %d timeout %ld", chan, chan->state, timeout);
223 224
224 if (!mod_timer(timer, jiffies + msecs_to_jiffies(timeout))) 225 if (!mod_timer(timer, jiffies + msecs_to_jiffies(timeout)))
225 chan_hold(chan); 226 chan_hold(chan);
@@ -293,6 +294,8 @@ struct l2cap_chan *l2cap_chan_create(struct sock *sk)
293 294
294 atomic_set(&chan->refcnt, 1); 295 atomic_set(&chan->refcnt, 1);
295 296
297 BT_DBG("sk %p chan %p", sk, chan);
298
296 return chan; 299 return chan;
297} 300}
298 301
@@ -310,7 +313,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
310 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, 313 BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
311 chan->psm, chan->dcid); 314 chan->psm, chan->dcid);
312 315
313 conn->disc_reason = 0x13; 316 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
314 317
315 chan->conn = conn; 318 chan->conn = conn;
316 319
@@ -337,6 +340,13 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
337 chan->omtu = L2CAP_DEFAULT_MTU; 340 chan->omtu = L2CAP_DEFAULT_MTU;
338 } 341 }
339 342
343 chan->local_id = L2CAP_BESTEFFORT_ID;
344 chan->local_stype = L2CAP_SERV_BESTEFFORT;
345 chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE;
346 chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME;
347 chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT;
348 chan->local_flush_to = L2CAP_DEFAULT_FLUSH_TO;
349
340 chan_hold(chan); 350 chan_hold(chan);
341 351
342 list_add(&chan->list, &conn->chan_l); 352 list_add(&chan->list, &conn->chan_l);
@@ -556,34 +566,58 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
556 flags = ACL_START; 566 flags = ACL_START;
557 567
558 bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON; 568 bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
569 skb->priority = HCI_PRIO_MAX;
570
571 hci_send_acl(conn->hchan, skb, flags);
572}
573
574static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
575{
576 struct hci_conn *hcon = chan->conn->hcon;
577 u16 flags;
578
579 BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
580 skb->priority);
581
582 if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
583 lmp_no_flush_capable(hcon->hdev))
584 flags = ACL_START_NO_FLUSH;
585 else
586 flags = ACL_START;
559 587
560 hci_send_acl(conn->hcon, skb, flags); 588 bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
589 hci_send_acl(chan->conn->hchan, skb, flags);
561} 590}
562 591
563static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) 592static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control)
564{ 593{
565 struct sk_buff *skb; 594 struct sk_buff *skb;
566 struct l2cap_hdr *lh; 595 struct l2cap_hdr *lh;
567 struct l2cap_conn *conn = chan->conn; 596 struct l2cap_conn *conn = chan->conn;
568 int count, hlen = L2CAP_HDR_SIZE + 2; 597 int count, hlen;
569 u8 flags;
570 598
571 if (chan->state != BT_CONNECTED) 599 if (chan->state != BT_CONNECTED)
572 return; 600 return;
573 601
602 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
603 hlen = L2CAP_EXT_HDR_SIZE;
604 else
605 hlen = L2CAP_ENH_HDR_SIZE;
606
574 if (chan->fcs == L2CAP_FCS_CRC16) 607 if (chan->fcs == L2CAP_FCS_CRC16)
575 hlen += 2; 608 hlen += L2CAP_FCS_SIZE;
576 609
577 BT_DBG("chan %p, control 0x%2.2x", chan, control); 610 BT_DBG("chan %p, control 0x%8.8x", chan, control);
578 611
579 count = min_t(unsigned int, conn->mtu, hlen); 612 count = min_t(unsigned int, conn->mtu, hlen);
580 control |= L2CAP_CTRL_FRAME_TYPE; 613
614 control |= __set_sframe(chan);
581 615
582 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) 616 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
583 control |= L2CAP_CTRL_FINAL; 617 control |= __set_ctrl_final(chan);
584 618
585 if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state)) 619 if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state))
586 control |= L2CAP_CTRL_POLL; 620 control |= __set_ctrl_poll(chan);
587 621
588 skb = bt_skb_alloc(count, GFP_ATOMIC); 622 skb = bt_skb_alloc(count, GFP_ATOMIC);
589 if (!skb) 623 if (!skb)
@@ -592,32 +626,27 @@ static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control)
592 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 626 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
593 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); 627 lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
594 lh->cid = cpu_to_le16(chan->dcid); 628 lh->cid = cpu_to_le16(chan->dcid);
595 put_unaligned_le16(control, skb_put(skb, 2)); 629
630 __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
596 631
597 if (chan->fcs == L2CAP_FCS_CRC16) { 632 if (chan->fcs == L2CAP_FCS_CRC16) {
598 u16 fcs = crc16(0, (u8 *)lh, count - 2); 633 u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE);
599 put_unaligned_le16(fcs, skb_put(skb, 2)); 634 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
600 } 635 }
601 636
602 if (lmp_no_flush_capable(conn->hcon->hdev)) 637 skb->priority = HCI_PRIO_MAX;
603 flags = ACL_START_NO_FLUSH; 638 l2cap_do_send(chan, skb);
604 else
605 flags = ACL_START;
606
607 bt_cb(skb)->force_active = chan->force_active;
608
609 hci_send_acl(chan->conn->hcon, skb, flags);
610} 639}
611 640
612static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u16 control) 641static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control)
613{ 642{
614 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 643 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
615 control |= L2CAP_SUPER_RCV_NOT_READY; 644 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
616 set_bit(CONN_RNR_SENT, &chan->conn_state); 645 set_bit(CONN_RNR_SENT, &chan->conn_state);
617 } else 646 } else
618 control |= L2CAP_SUPER_RCV_READY; 647 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
619 648
620 control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 649 control |= __set_reqseq(chan, chan->buffer_seq);
621 650
622 l2cap_send_sframe(chan, control); 651 l2cap_send_sframe(chan, control);
623} 652}
@@ -947,7 +976,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
947 list_for_each_entry(chan, &conn->chan_l, list) { 976 list_for_each_entry(chan, &conn->chan_l, list) {
948 struct sock *sk = chan->sk; 977 struct sock *sk = chan->sk;
949 978
950 if (chan->force_reliable) 979 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
951 sk->sk_err = err; 980 sk->sk_err = err;
952 } 981 }
953 982
@@ -986,6 +1015,8 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
986 chan->ops->close(chan->data); 1015 chan->ops->close(chan->data);
987 } 1016 }
988 1017
1018 hci_chan_del(conn->hchan);
1019
989 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) 1020 if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
990 del_timer_sync(&conn->info_timer); 1021 del_timer_sync(&conn->info_timer);
991 1022
@@ -1008,18 +1039,26 @@ static void security_timeout(unsigned long arg)
1008static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) 1039static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
1009{ 1040{
1010 struct l2cap_conn *conn = hcon->l2cap_data; 1041 struct l2cap_conn *conn = hcon->l2cap_data;
1042 struct hci_chan *hchan;
1011 1043
1012 if (conn || status) 1044 if (conn || status)
1013 return conn; 1045 return conn;
1014 1046
1047 hchan = hci_chan_create(hcon);
1048 if (!hchan)
1049 return NULL;
1050
1015 conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); 1051 conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
1016 if (!conn) 1052 if (!conn) {
1053 hci_chan_del(hchan);
1017 return NULL; 1054 return NULL;
1055 }
1018 1056
1019 hcon->l2cap_data = conn; 1057 hcon->l2cap_data = conn;
1020 conn->hcon = hcon; 1058 conn->hcon = hcon;
1059 conn->hchan = hchan;
1021 1060
1022 BT_DBG("hcon %p conn %p", hcon, conn); 1061 BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
1023 1062
1024 if (hcon->hdev->le_mtu && hcon->type == LE_LINK) 1063 if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
1025 conn->mtu = hcon->hdev->le_mtu; 1064 conn->mtu = hcon->hdev->le_mtu;
@@ -1043,7 +1082,7 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
1043 setup_timer(&conn->info_timer, l2cap_info_timeout, 1082 setup_timer(&conn->info_timer, l2cap_info_timeout,
1044 (unsigned long) conn); 1083 (unsigned long) conn);
1045 1084
1046 conn->disc_reason = 0x13; 1085 conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM;
1047 1086
1048 return conn; 1087 return conn;
1049} 1088}
@@ -1245,47 +1284,35 @@ static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
1245 __clear_retrans_timer(chan); 1284 __clear_retrans_timer(chan);
1246} 1285}
1247 1286
1248static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
1249{
1250 struct hci_conn *hcon = chan->conn->hcon;
1251 u16 flags;
1252
1253 BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len);
1254
1255 if (!chan->flushable && lmp_no_flush_capable(hcon->hdev))
1256 flags = ACL_START_NO_FLUSH;
1257 else
1258 flags = ACL_START;
1259
1260 bt_cb(skb)->force_active = chan->force_active;
1261 hci_send_acl(hcon, skb, flags);
1262}
1263
1264static void l2cap_streaming_send(struct l2cap_chan *chan) 1287static void l2cap_streaming_send(struct l2cap_chan *chan)
1265{ 1288{
1266 struct sk_buff *skb; 1289 struct sk_buff *skb;
1267 u16 control, fcs; 1290 u32 control;
1291 u16 fcs;
1268 1292
1269 while ((skb = skb_dequeue(&chan->tx_q))) { 1293 while ((skb = skb_dequeue(&chan->tx_q))) {
1270 control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE); 1294 control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
1271 control |= chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT; 1295 control |= __set_txseq(chan, chan->next_tx_seq);
1272 put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); 1296 __put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
1273 1297
1274 if (chan->fcs == L2CAP_FCS_CRC16) { 1298 if (chan->fcs == L2CAP_FCS_CRC16) {
1275 fcs = crc16(0, (u8 *)skb->data, skb->len - 2); 1299 fcs = crc16(0, (u8 *)skb->data,
1276 put_unaligned_le16(fcs, skb->data + skb->len - 2); 1300 skb->len - L2CAP_FCS_SIZE);
1301 put_unaligned_le16(fcs,
1302 skb->data + skb->len - L2CAP_FCS_SIZE);
1277 } 1303 }
1278 1304
1279 l2cap_do_send(chan, skb); 1305 l2cap_do_send(chan, skb);
1280 1306
1281 chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 1307 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1282 } 1308 }
1283} 1309}
1284 1310
1285static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq) 1311static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq)
1286{ 1312{
1287 struct sk_buff *skb, *tx_skb; 1313 struct sk_buff *skb, *tx_skb;
1288 u16 control, fcs; 1314 u16 fcs;
1315 u32 control;
1289 1316
1290 skb = skb_peek(&chan->tx_q); 1317 skb = skb_peek(&chan->tx_q);
1291 if (!skb) 1318 if (!skb)
@@ -1308,20 +1335,23 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq)
1308 1335
1309 tx_skb = skb_clone(skb, GFP_ATOMIC); 1336 tx_skb = skb_clone(skb, GFP_ATOMIC);
1310 bt_cb(skb)->retries++; 1337 bt_cb(skb)->retries++;
1311 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 1338
1312 control &= L2CAP_CTRL_SAR; 1339 control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
1340 control &= __get_sar_mask(chan);
1313 1341
1314 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) 1342 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1315 control |= L2CAP_CTRL_FINAL; 1343 control |= __set_ctrl_final(chan);
1316 1344
1317 control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 1345 control |= __set_reqseq(chan, chan->buffer_seq);
1318 | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 1346 control |= __set_txseq(chan, tx_seq);
1319 1347
1320 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 1348 __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
1321 1349
1322 if (chan->fcs == L2CAP_FCS_CRC16) { 1350 if (chan->fcs == L2CAP_FCS_CRC16) {
1323 fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2); 1351 fcs = crc16(0, (u8 *)tx_skb->data,
1324 put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2); 1352 tx_skb->len - L2CAP_FCS_SIZE);
1353 put_unaligned_le16(fcs,
1354 tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE);
1325 } 1355 }
1326 1356
1327 l2cap_do_send(chan, tx_skb); 1357 l2cap_do_send(chan, tx_skb);
@@ -1330,7 +1360,8 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq)
1330static int l2cap_ertm_send(struct l2cap_chan *chan) 1360static int l2cap_ertm_send(struct l2cap_chan *chan)
1331{ 1361{
1332 struct sk_buff *skb, *tx_skb; 1362 struct sk_buff *skb, *tx_skb;
1333 u16 control, fcs; 1363 u16 fcs;
1364 u32 control;
1334 int nsent = 0; 1365 int nsent = 0;
1335 1366
1336 if (chan->state != BT_CONNECTED) 1367 if (chan->state != BT_CONNECTED)
@@ -1348,20 +1379,22 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
1348 1379
1349 bt_cb(skb)->retries++; 1380 bt_cb(skb)->retries++;
1350 1381
1351 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 1382 control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
1352 control &= L2CAP_CTRL_SAR; 1383 control &= __get_sar_mask(chan);
1353 1384
1354 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) 1385 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
1355 control |= L2CAP_CTRL_FINAL; 1386 control |= __set_ctrl_final(chan);
1356 1387
1357 control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 1388 control |= __set_reqseq(chan, chan->buffer_seq);
1358 | (chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 1389 control |= __set_txseq(chan, chan->next_tx_seq);
1359 put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
1360 1390
1391 __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
1361 1392
1362 if (chan->fcs == L2CAP_FCS_CRC16) { 1393 if (chan->fcs == L2CAP_FCS_CRC16) {
1363 fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2); 1394 fcs = crc16(0, (u8 *)skb->data,
1364 put_unaligned_le16(fcs, skb->data + tx_skb->len - 2); 1395 tx_skb->len - L2CAP_FCS_SIZE);
1396 put_unaligned_le16(fcs, skb->data +
1397 tx_skb->len - L2CAP_FCS_SIZE);
1365 } 1398 }
1366 1399
1367 l2cap_do_send(chan, tx_skb); 1400 l2cap_do_send(chan, tx_skb);
@@ -1369,7 +1402,8 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
1369 __set_retrans_timer(chan); 1402 __set_retrans_timer(chan);
1370 1403
1371 bt_cb(skb)->tx_seq = chan->next_tx_seq; 1404 bt_cb(skb)->tx_seq = chan->next_tx_seq;
1372 chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 1405
1406 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
1373 1407
1374 if (bt_cb(skb)->retries == 1) 1408 if (bt_cb(skb)->retries == 1)
1375 chan->unacked_frames++; 1409 chan->unacked_frames++;
@@ -1401,12 +1435,12 @@ static int l2cap_retransmit_frames(struct l2cap_chan *chan)
1401 1435
1402static void l2cap_send_ack(struct l2cap_chan *chan) 1436static void l2cap_send_ack(struct l2cap_chan *chan)
1403{ 1437{
1404 u16 control = 0; 1438 u32 control = 0;
1405 1439
1406 control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 1440 control |= __set_reqseq(chan, chan->buffer_seq);
1407 1441
1408 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 1442 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
1409 control |= L2CAP_SUPER_RCV_NOT_READY; 1443 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
1410 set_bit(CONN_RNR_SENT, &chan->conn_state); 1444 set_bit(CONN_RNR_SENT, &chan->conn_state);
1411 l2cap_send_sframe(chan, control); 1445 l2cap_send_sframe(chan, control);
1412 return; 1446 return;
@@ -1415,20 +1449,20 @@ static void l2cap_send_ack(struct l2cap_chan *chan)
1415 if (l2cap_ertm_send(chan) > 0) 1449 if (l2cap_ertm_send(chan) > 0)
1416 return; 1450 return;
1417 1451
1418 control |= L2CAP_SUPER_RCV_READY; 1452 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
1419 l2cap_send_sframe(chan, control); 1453 l2cap_send_sframe(chan, control);
1420} 1454}
1421 1455
1422static void l2cap_send_srejtail(struct l2cap_chan *chan) 1456static void l2cap_send_srejtail(struct l2cap_chan *chan)
1423{ 1457{
1424 struct srej_list *tail; 1458 struct srej_list *tail;
1425 u16 control; 1459 u32 control;
1426 1460
1427 control = L2CAP_SUPER_SELECT_REJECT; 1461 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
1428 control |= L2CAP_CTRL_FINAL; 1462 control |= __set_ctrl_final(chan);
1429 1463
1430 tail = list_entry((&chan->srej_l)->prev, struct srej_list, list); 1464 tail = list_entry((&chan->srej_l)->prev, struct srej_list, list);
1431 control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 1465 control |= __set_reqseq(chan, tail->tx_seq);
1432 1466
1433 l2cap_send_sframe(chan, control); 1467 l2cap_send_sframe(chan, control);
1434} 1468}
@@ -1456,6 +1490,8 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
1456 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) 1490 if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1457 return -EFAULT; 1491 return -EFAULT;
1458 1492
1493 (*frag)->priority = skb->priority;
1494
1459 sent += count; 1495 sent += count;
1460 len -= count; 1496 len -= count;
1461 1497
@@ -1465,15 +1501,17 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
1465 return sent; 1501 return sent;
1466} 1502}
1467 1503
1468static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 1504static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
1505 struct msghdr *msg, size_t len,
1506 u32 priority)
1469{ 1507{
1470 struct sock *sk = chan->sk; 1508 struct sock *sk = chan->sk;
1471 struct l2cap_conn *conn = chan->conn; 1509 struct l2cap_conn *conn = chan->conn;
1472 struct sk_buff *skb; 1510 struct sk_buff *skb;
1473 int err, count, hlen = L2CAP_HDR_SIZE + 2; 1511 int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
1474 struct l2cap_hdr *lh; 1512 struct l2cap_hdr *lh;
1475 1513
1476 BT_DBG("sk %p len %d", sk, (int)len); 1514 BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
1477 1515
1478 count = min_t(unsigned int, (conn->mtu - hlen), len); 1516 count = min_t(unsigned int, (conn->mtu - hlen), len);
1479 skb = bt_skb_send_alloc(sk, count + hlen, 1517 skb = bt_skb_send_alloc(sk, count + hlen,
@@ -1481,6 +1519,8 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct
1481 if (!skb) 1519 if (!skb)
1482 return ERR_PTR(err); 1520 return ERR_PTR(err);
1483 1521
1522 skb->priority = priority;
1523
1484 /* Create L2CAP header */ 1524 /* Create L2CAP header */
1485 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1525 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1486 lh->cid = cpu_to_le16(chan->dcid); 1526 lh->cid = cpu_to_le16(chan->dcid);
@@ -1495,7 +1535,9 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct
1495 return skb; 1535 return skb;
1496} 1536}
1497 1537
1498static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 1538static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
1539 struct msghdr *msg, size_t len,
1540 u32 priority)
1499{ 1541{
1500 struct sock *sk = chan->sk; 1542 struct sock *sk = chan->sk;
1501 struct l2cap_conn *conn = chan->conn; 1543 struct l2cap_conn *conn = chan->conn;
@@ -1511,6 +1553,8 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct ms
1511 if (!skb) 1553 if (!skb)
1512 return ERR_PTR(err); 1554 return ERR_PTR(err);
1513 1555
1556 skb->priority = priority;
1557
1514 /* Create L2CAP header */ 1558 /* Create L2CAP header */
1515 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1559 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1516 lh->cid = cpu_to_le16(chan->dcid); 1560 lh->cid = cpu_to_le16(chan->dcid);
@@ -1526,12 +1570,12 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct ms
1526 1570
1527static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, 1571static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
1528 struct msghdr *msg, size_t len, 1572 struct msghdr *msg, size_t len,
1529 u16 control, u16 sdulen) 1573 u32 control, u16 sdulen)
1530{ 1574{
1531 struct sock *sk = chan->sk; 1575 struct sock *sk = chan->sk;
1532 struct l2cap_conn *conn = chan->conn; 1576 struct l2cap_conn *conn = chan->conn;
1533 struct sk_buff *skb; 1577 struct sk_buff *skb;
1534 int err, count, hlen = L2CAP_HDR_SIZE + 2; 1578 int err, count, hlen;
1535 struct l2cap_hdr *lh; 1579 struct l2cap_hdr *lh;
1536 1580
1537 BT_DBG("sk %p len %d", sk, (int)len); 1581 BT_DBG("sk %p len %d", sk, (int)len);
@@ -1539,11 +1583,16 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
1539 if (!conn) 1583 if (!conn)
1540 return ERR_PTR(-ENOTCONN); 1584 return ERR_PTR(-ENOTCONN);
1541 1585
1586 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
1587 hlen = L2CAP_EXT_HDR_SIZE;
1588 else
1589 hlen = L2CAP_ENH_HDR_SIZE;
1590
1542 if (sdulen) 1591 if (sdulen)
1543 hlen += 2; 1592 hlen += L2CAP_SDULEN_SIZE;
1544 1593
1545 if (chan->fcs == L2CAP_FCS_CRC16) 1594 if (chan->fcs == L2CAP_FCS_CRC16)
1546 hlen += 2; 1595 hlen += L2CAP_FCS_SIZE;
1547 1596
1548 count = min_t(unsigned int, (conn->mtu - hlen), len); 1597 count = min_t(unsigned int, (conn->mtu - hlen), len);
1549 skb = bt_skb_send_alloc(sk, count + hlen, 1598 skb = bt_skb_send_alloc(sk, count + hlen,
@@ -1555,9 +1604,11 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
1555 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1604 lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
1556 lh->cid = cpu_to_le16(chan->dcid); 1605 lh->cid = cpu_to_le16(chan->dcid);
1557 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 1606 lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
1558 put_unaligned_le16(control, skb_put(skb, 2)); 1607
1608 __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
1609
1559 if (sdulen) 1610 if (sdulen)
1560 put_unaligned_le16(sdulen, skb_put(skb, 2)); 1611 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
1561 1612
1562 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 1613 err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
1563 if (unlikely(err < 0)) { 1614 if (unlikely(err < 0)) {
@@ -1566,7 +1617,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
1566 } 1617 }
1567 1618
1568 if (chan->fcs == L2CAP_FCS_CRC16) 1619 if (chan->fcs == L2CAP_FCS_CRC16)
1569 put_unaligned_le16(0, skb_put(skb, 2)); 1620 put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE));
1570 1621
1571 bt_cb(skb)->retries = 0; 1622 bt_cb(skb)->retries = 0;
1572 return skb; 1623 return skb;
@@ -1576,11 +1627,11 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si
1576{ 1627{
1577 struct sk_buff *skb; 1628 struct sk_buff *skb;
1578 struct sk_buff_head sar_queue; 1629 struct sk_buff_head sar_queue;
1579 u16 control; 1630 u32 control;
1580 size_t size = 0; 1631 size_t size = 0;
1581 1632
1582 skb_queue_head_init(&sar_queue); 1633 skb_queue_head_init(&sar_queue);
1583 control = L2CAP_SDU_START; 1634 control = __set_ctrl_sar(chan, L2CAP_SAR_START);
1584 skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len); 1635 skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len);
1585 if (IS_ERR(skb)) 1636 if (IS_ERR(skb))
1586 return PTR_ERR(skb); 1637 return PTR_ERR(skb);
@@ -1593,10 +1644,10 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si
1593 size_t buflen; 1644 size_t buflen;
1594 1645
1595 if (len > chan->remote_mps) { 1646 if (len > chan->remote_mps) {
1596 control = L2CAP_SDU_CONTINUE; 1647 control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE);
1597 buflen = chan->remote_mps; 1648 buflen = chan->remote_mps;
1598 } else { 1649 } else {
1599 control = L2CAP_SDU_END; 1650 control = __set_ctrl_sar(chan, L2CAP_SAR_END);
1600 buflen = len; 1651 buflen = len;
1601 } 1652 }
1602 1653
@@ -1617,15 +1668,16 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si
1617 return size; 1668 return size;
1618} 1669}
1619 1670
1620int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 1671int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
1672 u32 priority)
1621{ 1673{
1622 struct sk_buff *skb; 1674 struct sk_buff *skb;
1623 u16 control; 1675 u32 control;
1624 int err; 1676 int err;
1625 1677
1626 /* Connectionless channel */ 1678 /* Connectionless channel */
1627 if (chan->chan_type == L2CAP_CHAN_CONN_LESS) { 1679 if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
1628 skb = l2cap_create_connless_pdu(chan, msg, len); 1680 skb = l2cap_create_connless_pdu(chan, msg, len, priority);
1629 if (IS_ERR(skb)) 1681 if (IS_ERR(skb))
1630 return PTR_ERR(skb); 1682 return PTR_ERR(skb);
1631 1683
@@ -1640,7 +1692,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
1640 return -EMSGSIZE; 1692 return -EMSGSIZE;
1641 1693
1642 /* Create a basic PDU */ 1694 /* Create a basic PDU */
1643 skb = l2cap_create_basic_pdu(chan, msg, len); 1695 skb = l2cap_create_basic_pdu(chan, msg, len, priority);
1644 if (IS_ERR(skb)) 1696 if (IS_ERR(skb))
1645 return PTR_ERR(skb); 1697 return PTR_ERR(skb);
1646 1698
@@ -1652,7 +1704,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
1652 case L2CAP_MODE_STREAMING: 1704 case L2CAP_MODE_STREAMING:
1653 /* Entire SDU fits into one PDU */ 1705 /* Entire SDU fits into one PDU */
1654 if (len <= chan->remote_mps) { 1706 if (len <= chan->remote_mps) {
1655 control = L2CAP_SDU_UNSEGMENTED; 1707 control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED);
1656 skb = l2cap_create_iframe_pdu(chan, msg, len, control, 1708 skb = l2cap_create_iframe_pdu(chan, msg, len, control,
1657 0); 1709 0);
1658 if (IS_ERR(skb)) 1710 if (IS_ERR(skb))
@@ -1850,6 +1902,37 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
1850 *ptr += L2CAP_CONF_OPT_SIZE + len; 1902 *ptr += L2CAP_CONF_OPT_SIZE + len;
1851} 1903}
1852 1904
1905static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
1906{
1907 struct l2cap_conf_efs efs;
1908
1909 switch(chan->mode) {
1910 case L2CAP_MODE_ERTM:
1911 efs.id = chan->local_id;
1912 efs.stype = chan->local_stype;
1913 efs.msdu = cpu_to_le16(chan->local_msdu);
1914 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
1915 efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
1916 efs.flush_to = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
1917 break;
1918
1919 case L2CAP_MODE_STREAMING:
1920 efs.id = 1;
1921 efs.stype = L2CAP_SERV_BESTEFFORT;
1922 efs.msdu = cpu_to_le16(chan->local_msdu);
1923 efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
1924 efs.acc_lat = 0;
1925 efs.flush_to = 0;
1926 break;
1927
1928 default:
1929 return;
1930 }
1931
1932 l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
1933 (unsigned long) &efs);
1934}
1935
1853static void l2cap_ack_timeout(unsigned long arg) 1936static void l2cap_ack_timeout(unsigned long arg)
1854{ 1937{
1855 struct l2cap_chan *chan = (void *) arg; 1938 struct l2cap_chan *chan = (void *) arg;
@@ -1896,11 +1979,36 @@ static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
1896 } 1979 }
1897} 1980}
1898 1981
1982static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
1983{
1984 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
1985}
1986
1987static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
1988{
1989 return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
1990}
1991
1992static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
1993{
1994 if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
1995 __l2cap_ews_supported(chan)) {
1996 /* use extended control field */
1997 set_bit(FLAG_EXT_CTRL, &chan->flags);
1998 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
1999 } else {
2000 chan->tx_win = min_t(u16, chan->tx_win,
2001 L2CAP_DEFAULT_TX_WINDOW);
2002 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
2003 }
2004}
2005
1899static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) 2006static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
1900{ 2007{
1901 struct l2cap_conf_req *req = data; 2008 struct l2cap_conf_req *req = data;
1902 struct l2cap_conf_rfc rfc = { .mode = chan->mode }; 2009 struct l2cap_conf_rfc rfc = { .mode = chan->mode };
1903 void *ptr = req->data; 2010 void *ptr = req->data;
2011 u16 size;
1904 2012
1905 BT_DBG("chan %p", chan); 2013 BT_DBG("chan %p", chan);
1906 2014
@@ -1913,6 +2021,9 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
1913 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) 2021 if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
1914 break; 2022 break;
1915 2023
2024 if (__l2cap_efs_supported(chan))
2025 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2026
1916 /* fall through */ 2027 /* fall through */
1917 default: 2028 default:
1918 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); 2029 chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
@@ -1942,17 +2053,27 @@ done:
1942 2053
1943 case L2CAP_MODE_ERTM: 2054 case L2CAP_MODE_ERTM:
1944 rfc.mode = L2CAP_MODE_ERTM; 2055 rfc.mode = L2CAP_MODE_ERTM;
1945 rfc.txwin_size = chan->tx_win;
1946 rfc.max_transmit = chan->max_tx; 2056 rfc.max_transmit = chan->max_tx;
1947 rfc.retrans_timeout = 0; 2057 rfc.retrans_timeout = 0;
1948 rfc.monitor_timeout = 0; 2058 rfc.monitor_timeout = 0;
1949 rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 2059
1950 if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) 2060 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
1951 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 2061 L2CAP_EXT_HDR_SIZE -
2062 L2CAP_SDULEN_SIZE -
2063 L2CAP_FCS_SIZE);
2064 rfc.max_pdu_size = cpu_to_le16(size);
2065
2066 l2cap_txwin_setup(chan);
2067
2068 rfc.txwin_size = min_t(u16, chan->tx_win,
2069 L2CAP_DEFAULT_TX_WINDOW);
1952 2070
1953 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 2071 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1954 (unsigned long) &rfc); 2072 (unsigned long) &rfc);
1955 2073
2074 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2075 l2cap_add_opt_efs(&ptr, chan);
2076
1956 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) 2077 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
1957 break; 2078 break;
1958 2079
@@ -1961,6 +2082,10 @@ done:
1961 chan->fcs = L2CAP_FCS_NONE; 2082 chan->fcs = L2CAP_FCS_NONE;
1962 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); 2083 l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
1963 } 2084 }
2085
2086 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
2087 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2088 chan->tx_win);
1964 break; 2089 break;
1965 2090
1966 case L2CAP_MODE_STREAMING: 2091 case L2CAP_MODE_STREAMING:
@@ -1969,13 +2094,19 @@ done:
1969 rfc.max_transmit = 0; 2094 rfc.max_transmit = 0;
1970 rfc.retrans_timeout = 0; 2095 rfc.retrans_timeout = 0;
1971 rfc.monitor_timeout = 0; 2096 rfc.monitor_timeout = 0;
1972 rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 2097
1973 if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) 2098 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
1974 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 2099 L2CAP_EXT_HDR_SIZE -
2100 L2CAP_SDULEN_SIZE -
2101 L2CAP_FCS_SIZE);
2102 rfc.max_pdu_size = cpu_to_le16(size);
1975 2103
1976 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 2104 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
1977 (unsigned long) &rfc); 2105 (unsigned long) &rfc);
1978 2106
2107 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
2108 l2cap_add_opt_efs(&ptr, chan);
2109
1979 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) 2110 if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
1980 break; 2111 break;
1981 2112
@@ -2002,8 +2133,11 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
2002 int type, hint, olen; 2133 int type, hint, olen;
2003 unsigned long val; 2134 unsigned long val;
2004 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; 2135 struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
2136 struct l2cap_conf_efs efs;
2137 u8 remote_efs = 0;
2005 u16 mtu = L2CAP_DEFAULT_MTU; 2138 u16 mtu = L2CAP_DEFAULT_MTU;
2006 u16 result = L2CAP_CONF_SUCCESS; 2139 u16 result = L2CAP_CONF_SUCCESS;
2140 u16 size;
2007 2141
2008 BT_DBG("chan %p", chan); 2142 BT_DBG("chan %p", chan);
2009 2143
@@ -2033,7 +2167,22 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
2033 case L2CAP_CONF_FCS: 2167 case L2CAP_CONF_FCS:
2034 if (val == L2CAP_FCS_NONE) 2168 if (val == L2CAP_FCS_NONE)
2035 set_bit(CONF_NO_FCS_RECV, &chan->conf_state); 2169 set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
2170 break;
2171
2172 case L2CAP_CONF_EFS:
2173 remote_efs = 1;
2174 if (olen == sizeof(efs))
2175 memcpy(&efs, (void *) val, olen);
2176 break;
2177
2178 case L2CAP_CONF_EWS:
2179 if (!enable_hs)
2180 return -ECONNREFUSED;
2036 2181
2182 set_bit(FLAG_EXT_CTRL, &chan->flags);
2183 set_bit(CONF_EWS_RECV, &chan->conf_state);
2184 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
2185 chan->remote_tx_win = val;
2037 break; 2186 break;
2038 2187
2039 default: 2188 default:
@@ -2058,6 +2207,13 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
2058 break; 2207 break;
2059 } 2208 }
2060 2209
2210 if (remote_efs) {
2211 if (__l2cap_efs_supported(chan))
2212 set_bit(FLAG_EFS_ENABLE, &chan->flags);
2213 else
2214 return -ECONNREFUSED;
2215 }
2216
2061 if (chan->mode != rfc.mode) 2217 if (chan->mode != rfc.mode)
2062 return -ECONNREFUSED; 2218 return -ECONNREFUSED;
2063 2219
@@ -2076,7 +2232,6 @@ done:
2076 sizeof(rfc), (unsigned long) &rfc); 2232 sizeof(rfc), (unsigned long) &rfc);
2077 } 2233 }
2078 2234
2079
2080 if (result == L2CAP_CONF_SUCCESS) { 2235 if (result == L2CAP_CONF_SUCCESS) {
2081 /* Configure output options and let the other side know 2236 /* Configure output options and let the other side know
2082 * which ones we don't like. */ 2237 * which ones we don't like. */
@@ -2089,6 +2244,26 @@ done:
2089 } 2244 }
2090 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu); 2245 l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
2091 2246
2247 if (remote_efs) {
2248 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2249 efs.stype != L2CAP_SERV_NOTRAFIC &&
2250 efs.stype != chan->local_stype) {
2251
2252 result = L2CAP_CONF_UNACCEPT;
2253
2254 if (chan->num_conf_req >= 1)
2255 return -ECONNREFUSED;
2256
2257 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2258 sizeof(efs),
2259 (unsigned long) &efs);
2260 } else {
2261 /* Send PENDING Conf Rsp */
2262 result = L2CAP_CONF_PENDING;
2263 set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2264 }
2265 }
2266
2092 switch (rfc.mode) { 2267 switch (rfc.mode) {
2093 case L2CAP_MODE_BASIC: 2268 case L2CAP_MODE_BASIC:
2094 chan->fcs = L2CAP_FCS_NONE; 2269 chan->fcs = L2CAP_FCS_NONE;
@@ -2096,13 +2271,20 @@ done:
2096 break; 2271 break;
2097 2272
2098 case L2CAP_MODE_ERTM: 2273 case L2CAP_MODE_ERTM:
2099 chan->remote_tx_win = rfc.txwin_size; 2274 if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
2100 chan->remote_max_tx = rfc.max_transmit; 2275 chan->remote_tx_win = rfc.txwin_size;
2276 else
2277 rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
2101 2278
2102 if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) 2279 chan->remote_max_tx = rfc.max_transmit;
2103 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
2104 2280
2105 chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 2281 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2282 chan->conn->mtu -
2283 L2CAP_EXT_HDR_SIZE -
2284 L2CAP_SDULEN_SIZE -
2285 L2CAP_FCS_SIZE);
2286 rfc.max_pdu_size = cpu_to_le16(size);
2287 chan->remote_mps = size;
2106 2288
2107 rfc.retrans_timeout = 2289 rfc.retrans_timeout =
2108 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); 2290 le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
@@ -2114,13 +2296,29 @@ done:
2114 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 2296 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2115 sizeof(rfc), (unsigned long) &rfc); 2297 sizeof(rfc), (unsigned long) &rfc);
2116 2298
2299 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2300 chan->remote_id = efs.id;
2301 chan->remote_stype = efs.stype;
2302 chan->remote_msdu = le16_to_cpu(efs.msdu);
2303 chan->remote_flush_to =
2304 le32_to_cpu(efs.flush_to);
2305 chan->remote_acc_lat =
2306 le32_to_cpu(efs.acc_lat);
2307 chan->remote_sdu_itime =
2308 le32_to_cpu(efs.sdu_itime);
2309 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2310 sizeof(efs), (unsigned long) &efs);
2311 }
2117 break; 2312 break;
2118 2313
2119 case L2CAP_MODE_STREAMING: 2314 case L2CAP_MODE_STREAMING:
2120 if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) 2315 size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
2121 rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 2316 chan->conn->mtu -
2122 2317 L2CAP_EXT_HDR_SIZE -
2123 chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 2318 L2CAP_SDULEN_SIZE -
2319 L2CAP_FCS_SIZE);
2320 rfc.max_pdu_size = cpu_to_le16(size);
2321 chan->remote_mps = size;
2124 2322
2125 set_bit(CONF_MODE_DONE, &chan->conf_state); 2323 set_bit(CONF_MODE_DONE, &chan->conf_state);
2126 2324
@@ -2153,6 +2351,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
2153 int type, olen; 2351 int type, olen;
2154 unsigned long val; 2352 unsigned long val;
2155 struct l2cap_conf_rfc rfc; 2353 struct l2cap_conf_rfc rfc;
2354 struct l2cap_conf_efs efs;
2156 2355
2157 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); 2356 BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
2158 2357
@@ -2188,6 +2387,26 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
2188 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 2387 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
2189 sizeof(rfc), (unsigned long) &rfc); 2388 sizeof(rfc), (unsigned long) &rfc);
2190 break; 2389 break;
2390
2391 case L2CAP_CONF_EWS:
2392 chan->tx_win = min_t(u16, val,
2393 L2CAP_DEFAULT_EXT_WINDOW);
2394 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
2395 chan->tx_win);
2396 break;
2397
2398 case L2CAP_CONF_EFS:
2399 if (olen == sizeof(efs))
2400 memcpy(&efs, (void *)val, olen);
2401
2402 if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
2403 efs.stype != L2CAP_SERV_NOTRAFIC &&
2404 efs.stype != chan->local_stype)
2405 return -ECONNREFUSED;
2406
2407 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
2408 sizeof(efs), (unsigned long) &efs);
2409 break;
2191 } 2410 }
2192 } 2411 }
2193 2412
@@ -2196,13 +2415,23 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
2196 2415
2197 chan->mode = rfc.mode; 2416 chan->mode = rfc.mode;
2198 2417
2199 if (*result == L2CAP_CONF_SUCCESS) { 2418 if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
2200 switch (rfc.mode) { 2419 switch (rfc.mode) {
2201 case L2CAP_MODE_ERTM: 2420 case L2CAP_MODE_ERTM:
2202 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 2421 chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
2203 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 2422 chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout);
2204 chan->mps = le16_to_cpu(rfc.max_pdu_size); 2423 chan->mps = le16_to_cpu(rfc.max_pdu_size);
2424
2425 if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
2426 chan->local_msdu = le16_to_cpu(efs.msdu);
2427 chan->local_sdu_itime =
2428 le32_to_cpu(efs.sdu_itime);
2429 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
2430 chan->local_flush_to =
2431 le32_to_cpu(efs.flush_to);
2432 }
2205 break; 2433 break;
2434
2206 case L2CAP_MODE_STREAMING: 2435 case L2CAP_MODE_STREAMING:
2207 chan->mps = le16_to_cpu(rfc.max_pdu_size); 2436 chan->mps = le16_to_cpu(rfc.max_pdu_size);
2208 } 2437 }
@@ -2330,7 +2559,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
2330 /* Check if the ACL is secure enough (if not SDP) */ 2559 /* Check if the ACL is secure enough (if not SDP) */
2331 if (psm != cpu_to_le16(0x0001) && 2560 if (psm != cpu_to_le16(0x0001) &&
2332 !hci_conn_check_link_mode(conn->hcon)) { 2561 !hci_conn_check_link_mode(conn->hcon)) {
2333 conn->disc_reason = 0x05; 2562 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
2334 result = L2CAP_CR_SEC_BLOCK; 2563 result = L2CAP_CR_SEC_BLOCK;
2335 goto response; 2564 goto response;
2336 } 2565 }
@@ -2602,6 +2831,21 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
2602 chan->num_conf_req++; 2831 chan->num_conf_req++;
2603 } 2832 }
2604 2833
2834 /* Got Conf Rsp PENDING from remote side and asume we sent
2835 Conf Rsp PENDING in the code above */
2836 if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
2837 test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2838
2839 /* check compatibility */
2840
2841 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2842 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2843
2844 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2845 l2cap_build_conf_rsp(chan, rsp,
2846 L2CAP_CONF_SUCCESS, 0x0000), rsp);
2847 }
2848
2605unlock: 2849unlock:
2606 bh_unlock_sock(sk); 2850 bh_unlock_sock(sk);
2607 return 0; 2851 return 0;
@@ -2631,8 +2875,33 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
2631 switch (result) { 2875 switch (result) {
2632 case L2CAP_CONF_SUCCESS: 2876 case L2CAP_CONF_SUCCESS:
2633 l2cap_conf_rfc_get(chan, rsp->data, len); 2877 l2cap_conf_rfc_get(chan, rsp->data, len);
2878 clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2634 break; 2879 break;
2635 2880
2881 case L2CAP_CONF_PENDING:
2882 set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
2883
2884 if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
2885 char buf[64];
2886
2887 len = l2cap_parse_conf_rsp(chan, rsp->data, len,
2888 buf, &result);
2889 if (len < 0) {
2890 l2cap_send_disconn_req(conn, chan, ECONNRESET);
2891 goto done;
2892 }
2893
2894 /* check compatibility */
2895
2896 clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
2897 set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
2898
2899 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
2900 l2cap_build_conf_rsp(chan, buf,
2901 L2CAP_CONF_SUCCESS, 0x0000), buf);
2902 }
2903 goto done;
2904
2636 case L2CAP_CONF_UNACCEPT: 2905 case L2CAP_CONF_UNACCEPT:
2637 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { 2906 if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
2638 char req[64]; 2907 char req[64];
@@ -2782,15 +3051,25 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
2782 if (!disable_ertm) 3051 if (!disable_ertm)
2783 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING 3052 feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
2784 | L2CAP_FEAT_FCS; 3053 | L2CAP_FEAT_FCS;
3054 if (enable_hs)
3055 feat_mask |= L2CAP_FEAT_EXT_FLOW
3056 | L2CAP_FEAT_EXT_WINDOW;
3057
2785 put_unaligned_le32(feat_mask, rsp->data); 3058 put_unaligned_le32(feat_mask, rsp->data);
2786 l2cap_send_cmd(conn, cmd->ident, 3059 l2cap_send_cmd(conn, cmd->ident,
2787 L2CAP_INFO_RSP, sizeof(buf), buf); 3060 L2CAP_INFO_RSP, sizeof(buf), buf);
2788 } else if (type == L2CAP_IT_FIXED_CHAN) { 3061 } else if (type == L2CAP_IT_FIXED_CHAN) {
2789 u8 buf[12]; 3062 u8 buf[12];
2790 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 3063 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
3064
3065 if (enable_hs)
3066 l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
3067 else
3068 l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
3069
2791 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 3070 rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
2792 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 3071 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
2793 memcpy(buf + 4, l2cap_fixed_chan, 8); 3072 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
2794 l2cap_send_cmd(conn, cmd->ident, 3073 l2cap_send_cmd(conn, cmd->ident,
2795 L2CAP_INFO_RSP, sizeof(buf), buf); 3074 L2CAP_INFO_RSP, sizeof(buf), buf);
2796 } else { 3075 } else {
@@ -2857,6 +3136,165 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
2857 return 0; 3136 return 0;
2858} 3137}
2859 3138
3139static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
3140 struct l2cap_cmd_hdr *cmd, u16 cmd_len,
3141 void *data)
3142{
3143 struct l2cap_create_chan_req *req = data;
3144 struct l2cap_create_chan_rsp rsp;
3145 u16 psm, scid;
3146
3147 if (cmd_len != sizeof(*req))
3148 return -EPROTO;
3149
3150 if (!enable_hs)
3151 return -EINVAL;
3152
3153 psm = le16_to_cpu(req->psm);
3154 scid = le16_to_cpu(req->scid);
3155
3156 BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id);
3157
3158 /* Placeholder: Always reject */
3159 rsp.dcid = 0;
3160 rsp.scid = cpu_to_le16(scid);
3161 rsp.result = L2CAP_CR_NO_MEM;
3162 rsp.status = L2CAP_CS_NO_INFO;
3163
3164 l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
3165 sizeof(rsp), &rsp);
3166
3167 return 0;
3168}
3169
3170static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
3171 struct l2cap_cmd_hdr *cmd, void *data)
3172{
3173 BT_DBG("conn %p", conn);
3174
3175 return l2cap_connect_rsp(conn, cmd, data);
3176}
3177
3178static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
3179 u16 icid, u16 result)
3180{
3181 struct l2cap_move_chan_rsp rsp;
3182
3183 BT_DBG("icid %d, result %d", icid, result);
3184
3185 rsp.icid = cpu_to_le16(icid);
3186 rsp.result = cpu_to_le16(result);
3187
3188 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
3189}
3190
3191static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
3192 struct l2cap_chan *chan, u16 icid, u16 result)
3193{
3194 struct l2cap_move_chan_cfm cfm;
3195 u8 ident;
3196
3197 BT_DBG("icid %d, result %d", icid, result);
3198
3199 ident = l2cap_get_ident(conn);
3200 if (chan)
3201 chan->ident = ident;
3202
3203 cfm.icid = cpu_to_le16(icid);
3204 cfm.result = cpu_to_le16(result);
3205
3206 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
3207}
3208
3209static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
3210 u16 icid)
3211{
3212 struct l2cap_move_chan_cfm_rsp rsp;
3213
3214 BT_DBG("icid %d", icid);
3215
3216 rsp.icid = cpu_to_le16(icid);
3217 l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
3218}
3219
3220static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
3221 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3222{
3223 struct l2cap_move_chan_req *req = data;
3224 u16 icid = 0;
3225 u16 result = L2CAP_MR_NOT_ALLOWED;
3226
3227 if (cmd_len != sizeof(*req))
3228 return -EPROTO;
3229
3230 icid = le16_to_cpu(req->icid);
3231
3232 BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id);
3233
3234 if (!enable_hs)
3235 return -EINVAL;
3236
3237 /* Placeholder: Always refuse */
3238 l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
3239
3240 return 0;
3241}
3242
3243static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
3244 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3245{
3246 struct l2cap_move_chan_rsp *rsp = data;
3247 u16 icid, result;
3248
3249 if (cmd_len != sizeof(*rsp))
3250 return -EPROTO;
3251
3252 icid = le16_to_cpu(rsp->icid);
3253 result = le16_to_cpu(rsp->result);
3254
3255 BT_DBG("icid %d, result %d", icid, result);
3256
3257 /* Placeholder: Always unconfirmed */
3258 l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
3259
3260 return 0;
3261}
3262
3263static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
3264 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3265{
3266 struct l2cap_move_chan_cfm *cfm = data;
3267 u16 icid, result;
3268
3269 if (cmd_len != sizeof(*cfm))
3270 return -EPROTO;
3271
3272 icid = le16_to_cpu(cfm->icid);
3273 result = le16_to_cpu(cfm->result);
3274
3275 BT_DBG("icid %d, result %d", icid, result);
3276
3277 l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
3278
3279 return 0;
3280}
3281
3282static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
3283 struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
3284{
3285 struct l2cap_move_chan_cfm_rsp *rsp = data;
3286 u16 icid;
3287
3288 if (cmd_len != sizeof(*rsp))
3289 return -EPROTO;
3290
3291 icid = le16_to_cpu(rsp->icid);
3292
3293 BT_DBG("icid %d", icid);
3294
3295 return 0;
3296}
3297
2860static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, 3298static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
2861 u16 to_multiplier) 3299 u16 to_multiplier)
2862{ 3300{
@@ -2969,6 +3407,30 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
2969 err = l2cap_information_rsp(conn, cmd, data); 3407 err = l2cap_information_rsp(conn, cmd, data);
2970 break; 3408 break;
2971 3409
3410 case L2CAP_CREATE_CHAN_REQ:
3411 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
3412 break;
3413
3414 case L2CAP_CREATE_CHAN_RSP:
3415 err = l2cap_create_channel_rsp(conn, cmd, data);
3416 break;
3417
3418 case L2CAP_MOVE_CHAN_REQ:
3419 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
3420 break;
3421
3422 case L2CAP_MOVE_CHAN_RSP:
3423 err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
3424 break;
3425
3426 case L2CAP_MOVE_CHAN_CFM:
3427 err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
3428 break;
3429
3430 case L2CAP_MOVE_CHAN_CFM_RSP:
3431 err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
3432 break;
3433
2972 default: 3434 default:
2973 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); 3435 BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
2974 err = -EINVAL; 3436 err = -EINVAL;
@@ -3047,10 +3509,15 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
3047static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) 3509static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
3048{ 3510{
3049 u16 our_fcs, rcv_fcs; 3511 u16 our_fcs, rcv_fcs;
3050 int hdr_size = L2CAP_HDR_SIZE + 2; 3512 int hdr_size;
3513
3514 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
3515 hdr_size = L2CAP_EXT_HDR_SIZE;
3516 else
3517 hdr_size = L2CAP_ENH_HDR_SIZE;
3051 3518
3052 if (chan->fcs == L2CAP_FCS_CRC16) { 3519 if (chan->fcs == L2CAP_FCS_CRC16) {
3053 skb_trim(skb, skb->len - 2); 3520 skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
3054 rcv_fcs = get_unaligned_le16(skb->data + skb->len); 3521 rcv_fcs = get_unaligned_le16(skb->data + skb->len);
3055 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); 3522 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
3056 3523
@@ -3062,14 +3529,14 @@ static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb)
3062 3529
3063static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) 3530static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3064{ 3531{
3065 u16 control = 0; 3532 u32 control = 0;
3066 3533
3067 chan->frames_sent = 0; 3534 chan->frames_sent = 0;
3068 3535
3069 control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3536 control |= __set_reqseq(chan, chan->buffer_seq);
3070 3537
3071 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 3538 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
3072 control |= L2CAP_SUPER_RCV_NOT_READY; 3539 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3073 l2cap_send_sframe(chan, control); 3540 l2cap_send_sframe(chan, control);
3074 set_bit(CONN_RNR_SENT, &chan->conn_state); 3541 set_bit(CONN_RNR_SENT, &chan->conn_state);
3075 } 3542 }
@@ -3081,12 +3548,12 @@ static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
3081 3548
3082 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && 3549 if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
3083 chan->frames_sent == 0) { 3550 chan->frames_sent == 0) {
3084 control |= L2CAP_SUPER_RCV_READY; 3551 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3085 l2cap_send_sframe(chan, control); 3552 l2cap_send_sframe(chan, control);
3086 } 3553 }
3087} 3554}
3088 3555
3089static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar) 3556static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
3090{ 3557{
3091 struct sk_buff *next_skb; 3558 struct sk_buff *next_skb;
3092 int tx_seq_offset, next_tx_seq_offset; 3559 int tx_seq_offset, next_tx_seq_offset;
@@ -3100,18 +3567,14 @@ static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb,
3100 return 0; 3567 return 0;
3101 } 3568 }
3102 3569
3103 tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 3570 tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3104 if (tx_seq_offset < 0)
3105 tx_seq_offset += 64;
3106 3571
3107 do { 3572 do {
3108 if (bt_cb(next_skb)->tx_seq == tx_seq) 3573 if (bt_cb(next_skb)->tx_seq == tx_seq)
3109 return -EINVAL; 3574 return -EINVAL;
3110 3575
3111 next_tx_seq_offset = (bt_cb(next_skb)->tx_seq - 3576 next_tx_seq_offset = __seq_offset(chan,
3112 chan->buffer_seq) % 64; 3577 bt_cb(next_skb)->tx_seq, chan->buffer_seq);
3113 if (next_tx_seq_offset < 0)
3114 next_tx_seq_offset += 64;
3115 3578
3116 if (next_tx_seq_offset > tx_seq_offset) { 3579 if (next_tx_seq_offset > tx_seq_offset) {
3117 __skb_queue_before(&chan->srej_q, next_skb, skb); 3580 __skb_queue_before(&chan->srej_q, next_skb, skb);
@@ -3147,24 +3610,24 @@ static void append_skb_frag(struct sk_buff *skb,
3147 skb->truesize += new_frag->truesize; 3610 skb->truesize += new_frag->truesize;
3148} 3611}
3149 3612
3150static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 3613static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
3151{ 3614{
3152 int err = -EINVAL; 3615 int err = -EINVAL;
3153 3616
3154 switch (control & L2CAP_CTRL_SAR) { 3617 switch (__get_ctrl_sar(chan, control)) {
3155 case L2CAP_SDU_UNSEGMENTED: 3618 case L2CAP_SAR_UNSEGMENTED:
3156 if (chan->sdu) 3619 if (chan->sdu)
3157 break; 3620 break;
3158 3621
3159 err = chan->ops->recv(chan->data, skb); 3622 err = chan->ops->recv(chan->data, skb);
3160 break; 3623 break;
3161 3624
3162 case L2CAP_SDU_START: 3625 case L2CAP_SAR_START:
3163 if (chan->sdu) 3626 if (chan->sdu)
3164 break; 3627 break;
3165 3628
3166 chan->sdu_len = get_unaligned_le16(skb->data); 3629 chan->sdu_len = get_unaligned_le16(skb->data);
3167 skb_pull(skb, 2); 3630 skb_pull(skb, L2CAP_SDULEN_SIZE);
3168 3631
3169 if (chan->sdu_len > chan->imtu) { 3632 if (chan->sdu_len > chan->imtu) {
3170 err = -EMSGSIZE; 3633 err = -EMSGSIZE;
@@ -3181,7 +3644,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1
3181 err = 0; 3644 err = 0;
3182 break; 3645 break;
3183 3646
3184 case L2CAP_SDU_CONTINUE: 3647 case L2CAP_SAR_CONTINUE:
3185 if (!chan->sdu) 3648 if (!chan->sdu)
3186 break; 3649 break;
3187 3650
@@ -3195,7 +3658,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1
3195 err = 0; 3658 err = 0;
3196 break; 3659 break;
3197 3660
3198 case L2CAP_SDU_END: 3661 case L2CAP_SAR_END:
3199 if (!chan->sdu) 3662 if (!chan->sdu)
3200 break; 3663 break;
3201 3664
@@ -3230,14 +3693,14 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1
3230 3693
3231static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan) 3694static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
3232{ 3695{
3233 u16 control; 3696 u32 control;
3234 3697
3235 BT_DBG("chan %p, Enter local busy", chan); 3698 BT_DBG("chan %p, Enter local busy", chan);
3236 3699
3237 set_bit(CONN_LOCAL_BUSY, &chan->conn_state); 3700 set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
3238 3701
3239 control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3702 control = __set_reqseq(chan, chan->buffer_seq);
3240 control |= L2CAP_SUPER_RCV_NOT_READY; 3703 control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
3241 l2cap_send_sframe(chan, control); 3704 l2cap_send_sframe(chan, control);
3242 3705
3243 set_bit(CONN_RNR_SENT, &chan->conn_state); 3706 set_bit(CONN_RNR_SENT, &chan->conn_state);
@@ -3247,13 +3710,14 @@ static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
3247 3710
3248static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan) 3711static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
3249{ 3712{
3250 u16 control; 3713 u32 control;
3251 3714
3252 if (!test_bit(CONN_RNR_SENT, &chan->conn_state)) 3715 if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
3253 goto done; 3716 goto done;
3254 3717
3255 control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3718 control = __set_reqseq(chan, chan->buffer_seq);
3256 control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL; 3719 control |= __set_ctrl_poll(chan);
3720 control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
3257 l2cap_send_sframe(chan, control); 3721 l2cap_send_sframe(chan, control);
3258 chan->retry_count = 1; 3722 chan->retry_count = 1;
3259 3723
@@ -3279,10 +3743,10 @@ void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
3279 } 3743 }
3280} 3744}
3281 3745
3282static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq) 3746static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
3283{ 3747{
3284 struct sk_buff *skb; 3748 struct sk_buff *skb;
3285 u16 control; 3749 u32 control;
3286 3750
3287 while ((skb = skb_peek(&chan->srej_q)) && 3751 while ((skb = skb_peek(&chan->srej_q)) &&
3288 !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { 3752 !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
@@ -3292,7 +3756,7 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
3292 break; 3756 break;
3293 3757
3294 skb = skb_dequeue(&chan->srej_q); 3758 skb = skb_dequeue(&chan->srej_q);
3295 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 3759 control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
3296 err = l2cap_reassemble_sdu(chan, skb, control); 3760 err = l2cap_reassemble_sdu(chan, skb, control);
3297 3761
3298 if (err < 0) { 3762 if (err < 0) {
@@ -3300,16 +3764,15 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
3300 break; 3764 break;
3301 } 3765 }
3302 3766
3303 chan->buffer_seq_srej = 3767 chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
3304 (chan->buffer_seq_srej + 1) % 64; 3768 tx_seq = __next_seq(chan, tx_seq);
3305 tx_seq = (tx_seq + 1) % 64;
3306 } 3769 }
3307} 3770}
3308 3771
3309static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq) 3772static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3310{ 3773{
3311 struct srej_list *l, *tmp; 3774 struct srej_list *l, *tmp;
3312 u16 control; 3775 u32 control;
3313 3776
3314 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { 3777 list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
3315 if (l->tx_seq == tx_seq) { 3778 if (l->tx_seq == tx_seq) {
@@ -3317,45 +3780,48 @@ static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq)
3317 kfree(l); 3780 kfree(l);
3318 return; 3781 return;
3319 } 3782 }
3320 control = L2CAP_SUPER_SELECT_REJECT; 3783 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3321 control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3784 control |= __set_reqseq(chan, l->tx_seq);
3322 l2cap_send_sframe(chan, control); 3785 l2cap_send_sframe(chan, control);
3323 list_del(&l->list); 3786 list_del(&l->list);
3324 list_add_tail(&l->list, &chan->srej_l); 3787 list_add_tail(&l->list, &chan->srej_l);
3325 } 3788 }
3326} 3789}
3327 3790
3328static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq) 3791static void l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
3329{ 3792{
3330 struct srej_list *new; 3793 struct srej_list *new;
3331 u16 control; 3794 u32 control;
3332 3795
3333 while (tx_seq != chan->expected_tx_seq) { 3796 while (tx_seq != chan->expected_tx_seq) {
3334 control = L2CAP_SUPER_SELECT_REJECT; 3797 control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
3335 control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3798 control |= __set_reqseq(chan, chan->expected_tx_seq);
3336 l2cap_send_sframe(chan, control); 3799 l2cap_send_sframe(chan, control);
3337 3800
3338 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); 3801 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
3339 new->tx_seq = chan->expected_tx_seq; 3802 new->tx_seq = chan->expected_tx_seq;
3340 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 3803
3804 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3805
3341 list_add_tail(&new->list, &chan->srej_l); 3806 list_add_tail(&new->list, &chan->srej_l);
3342 } 3807 }
3343 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 3808
3809 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3344} 3810}
3345 3811
3346static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 3812static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
3347{ 3813{
3348 u8 tx_seq = __get_txseq(rx_control); 3814 u16 tx_seq = __get_txseq(chan, rx_control);
3349 u8 req_seq = __get_reqseq(rx_control); 3815 u16 req_seq = __get_reqseq(chan, rx_control);
3350 u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT; 3816 u8 sar = __get_ctrl_sar(chan, rx_control);
3351 int tx_seq_offset, expected_tx_seq_offset; 3817 int tx_seq_offset, expected_tx_seq_offset;
3352 int num_to_ack = (chan->tx_win/6) + 1; 3818 int num_to_ack = (chan->tx_win/6) + 1;
3353 int err = 0; 3819 int err = 0;
3354 3820
3355 BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len, 3821 BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
3356 tx_seq, rx_control); 3822 tx_seq, rx_control);
3357 3823
3358 if (L2CAP_CTRL_FINAL & rx_control && 3824 if (__is_ctrl_final(chan, rx_control) &&
3359 test_bit(CONN_WAIT_F, &chan->conn_state)) { 3825 test_bit(CONN_WAIT_F, &chan->conn_state)) {
3360 __clear_monitor_timer(chan); 3826 __clear_monitor_timer(chan);
3361 if (chan->unacked_frames > 0) 3827 if (chan->unacked_frames > 0)
@@ -3366,9 +3832,7 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
3366 chan->expected_ack_seq = req_seq; 3832 chan->expected_ack_seq = req_seq;
3367 l2cap_drop_acked_frames(chan); 3833 l2cap_drop_acked_frames(chan);
3368 3834
3369 tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 3835 tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
3370 if (tx_seq_offset < 0)
3371 tx_seq_offset += 64;
3372 3836
3373 /* invalid tx_seq */ 3837 /* invalid tx_seq */
3374 if (tx_seq_offset >= chan->tx_win) { 3838 if (tx_seq_offset >= chan->tx_win) {
@@ -3416,10 +3880,8 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
3416 l2cap_send_srejframe(chan, tx_seq); 3880 l2cap_send_srejframe(chan, tx_seq);
3417 } 3881 }
3418 } else { 3882 } else {
3419 expected_tx_seq_offset = 3883 expected_tx_seq_offset = __seq_offset(chan,
3420 (chan->expected_tx_seq - chan->buffer_seq) % 64; 3884 chan->expected_tx_seq, chan->buffer_seq);
3421 if (expected_tx_seq_offset < 0)
3422 expected_tx_seq_offset += 64;
3423 3885
3424 /* duplicated tx_seq */ 3886 /* duplicated tx_seq */
3425 if (tx_seq_offset < expected_tx_seq_offset) 3887 if (tx_seq_offset < expected_tx_seq_offset)
@@ -3444,7 +3906,7 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
3444 return 0; 3906 return 0;
3445 3907
3446expected: 3908expected:
3447 chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 3909 chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
3448 3910
3449 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { 3911 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3450 bt_cb(skb)->tx_seq = tx_seq; 3912 bt_cb(skb)->tx_seq = tx_seq;
@@ -3454,13 +3916,14 @@ expected:
3454 } 3916 }
3455 3917
3456 err = l2cap_reassemble_sdu(chan, skb, rx_control); 3918 err = l2cap_reassemble_sdu(chan, skb, rx_control);
3457 chan->buffer_seq = (chan->buffer_seq + 1) % 64; 3919 chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
3920
3458 if (err < 0) { 3921 if (err < 0) {
3459 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 3922 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3460 return err; 3923 return err;
3461 } 3924 }
3462 3925
3463 if (rx_control & L2CAP_CTRL_FINAL) { 3926 if (__is_ctrl_final(chan, rx_control)) {
3464 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) 3927 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3465 l2cap_retransmit_frames(chan); 3928 l2cap_retransmit_frames(chan);
3466 } 3929 }
@@ -3478,15 +3941,15 @@ drop:
3478 return 0; 3941 return 0;
3479} 3942}
3480 3943
3481static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control) 3944static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
3482{ 3945{
3483 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control), 3946 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
3484 rx_control); 3947 __get_reqseq(chan, rx_control), rx_control);
3485 3948
3486 chan->expected_ack_seq = __get_reqseq(rx_control); 3949 chan->expected_ack_seq = __get_reqseq(chan, rx_control);
3487 l2cap_drop_acked_frames(chan); 3950 l2cap_drop_acked_frames(chan);
3488 3951
3489 if (rx_control & L2CAP_CTRL_POLL) { 3952 if (__is_ctrl_poll(chan, rx_control)) {
3490 set_bit(CONN_SEND_FBIT, &chan->conn_state); 3953 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3491 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { 3954 if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3492 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) && 3955 if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
@@ -3499,7 +3962,7 @@ static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_co
3499 l2cap_send_i_or_rr_or_rnr(chan); 3962 l2cap_send_i_or_rr_or_rnr(chan);
3500 } 3963 }
3501 3964
3502 } else if (rx_control & L2CAP_CTRL_FINAL) { 3965 } else if (__is_ctrl_final(chan, rx_control)) {
3503 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 3966 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3504 3967
3505 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) 3968 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
@@ -3518,18 +3981,18 @@ static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_co
3518 } 3981 }
3519} 3982}
3520 3983
3521static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control) 3984static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
3522{ 3985{
3523 u8 tx_seq = __get_reqseq(rx_control); 3986 u16 tx_seq = __get_reqseq(chan, rx_control);
3524 3987
3525 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 3988 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3526 3989
3527 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 3990 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3528 3991
3529 chan->expected_ack_seq = tx_seq; 3992 chan->expected_ack_seq = tx_seq;
3530 l2cap_drop_acked_frames(chan); 3993 l2cap_drop_acked_frames(chan);
3531 3994
3532 if (rx_control & L2CAP_CTRL_FINAL) { 3995 if (__is_ctrl_final(chan, rx_control)) {
3533 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) 3996 if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
3534 l2cap_retransmit_frames(chan); 3997 l2cap_retransmit_frames(chan);
3535 } else { 3998 } else {
@@ -3539,15 +4002,15 @@ static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_c
3539 set_bit(CONN_REJ_ACT, &chan->conn_state); 4002 set_bit(CONN_REJ_ACT, &chan->conn_state);
3540 } 4003 }
3541} 4004}
3542static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control) 4005static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
3543{ 4006{
3544 u8 tx_seq = __get_reqseq(rx_control); 4007 u16 tx_seq = __get_reqseq(chan, rx_control);
3545 4008
3546 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 4009 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3547 4010
3548 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); 4011 clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3549 4012
3550 if (rx_control & L2CAP_CTRL_POLL) { 4013 if (__is_ctrl_poll(chan, rx_control)) {
3551 chan->expected_ack_seq = tx_seq; 4014 chan->expected_ack_seq = tx_seq;
3552 l2cap_drop_acked_frames(chan); 4015 l2cap_drop_acked_frames(chan);
3553 4016
@@ -3560,7 +4023,7 @@ static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_
3560 chan->srej_save_reqseq = tx_seq; 4023 chan->srej_save_reqseq = tx_seq;
3561 set_bit(CONN_SREJ_ACT, &chan->conn_state); 4024 set_bit(CONN_SREJ_ACT, &chan->conn_state);
3562 } 4025 }
3563 } else if (rx_control & L2CAP_CTRL_FINAL) { 4026 } else if (__is_ctrl_final(chan, rx_control)) {
3564 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) && 4027 if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
3565 chan->srej_save_reqseq == tx_seq) 4028 chan->srej_save_reqseq == tx_seq)
3566 clear_bit(CONN_SREJ_ACT, &chan->conn_state); 4029 clear_bit(CONN_SREJ_ACT, &chan->conn_state);
@@ -3575,37 +4038,39 @@ static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_
3575 } 4038 }
3576} 4039}
3577 4040
3578static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control) 4041static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
3579{ 4042{
3580 u8 tx_seq = __get_reqseq(rx_control); 4043 u16 tx_seq = __get_reqseq(chan, rx_control);
3581 4044
3582 BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 4045 BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
3583 4046
3584 set_bit(CONN_REMOTE_BUSY, &chan->conn_state); 4047 set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
3585 chan->expected_ack_seq = tx_seq; 4048 chan->expected_ack_seq = tx_seq;
3586 l2cap_drop_acked_frames(chan); 4049 l2cap_drop_acked_frames(chan);
3587 4050
3588 if (rx_control & L2CAP_CTRL_POLL) 4051 if (__is_ctrl_poll(chan, rx_control))
3589 set_bit(CONN_SEND_FBIT, &chan->conn_state); 4052 set_bit(CONN_SEND_FBIT, &chan->conn_state);
3590 4053
3591 if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) { 4054 if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
3592 __clear_retrans_timer(chan); 4055 __clear_retrans_timer(chan);
3593 if (rx_control & L2CAP_CTRL_POLL) 4056 if (__is_ctrl_poll(chan, rx_control))
3594 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL); 4057 l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
3595 return; 4058 return;
3596 } 4059 }
3597 4060
3598 if (rx_control & L2CAP_CTRL_POLL) 4061 if (__is_ctrl_poll(chan, rx_control)) {
3599 l2cap_send_srejtail(chan); 4062 l2cap_send_srejtail(chan);
3600 else 4063 } else {
3601 l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY); 4064 rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
4065 l2cap_send_sframe(chan, rx_control);
4066 }
3602} 4067}
3603 4068
3604static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 4069static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
3605{ 4070{
3606 BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len); 4071 BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
3607 4072
3608 if (L2CAP_CTRL_FINAL & rx_control && 4073 if (__is_ctrl_final(chan, rx_control) &&
3609 test_bit(CONN_WAIT_F, &chan->conn_state)) { 4074 test_bit(CONN_WAIT_F, &chan->conn_state)) {
3610 __clear_monitor_timer(chan); 4075 __clear_monitor_timer(chan);
3611 if (chan->unacked_frames > 0) 4076 if (chan->unacked_frames > 0)
@@ -3613,20 +4078,20 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_cont
3613 clear_bit(CONN_WAIT_F, &chan->conn_state); 4078 clear_bit(CONN_WAIT_F, &chan->conn_state);
3614 } 4079 }
3615 4080
3616 switch (rx_control & L2CAP_CTRL_SUPERVISE) { 4081 switch (__get_ctrl_super(chan, rx_control)) {
3617 case L2CAP_SUPER_RCV_READY: 4082 case L2CAP_SUPER_RR:
3618 l2cap_data_channel_rrframe(chan, rx_control); 4083 l2cap_data_channel_rrframe(chan, rx_control);
3619 break; 4084 break;
3620 4085
3621 case L2CAP_SUPER_REJECT: 4086 case L2CAP_SUPER_REJ:
3622 l2cap_data_channel_rejframe(chan, rx_control); 4087 l2cap_data_channel_rejframe(chan, rx_control);
3623 break; 4088 break;
3624 4089
3625 case L2CAP_SUPER_SELECT_REJECT: 4090 case L2CAP_SUPER_SREJ:
3626 l2cap_data_channel_srejframe(chan, rx_control); 4091 l2cap_data_channel_srejframe(chan, rx_control);
3627 break; 4092 break;
3628 4093
3629 case L2CAP_SUPER_RCV_NOT_READY: 4094 case L2CAP_SUPER_RNR:
3630 l2cap_data_channel_rnrframe(chan, rx_control); 4095 l2cap_data_channel_rnrframe(chan, rx_control);
3631 break; 4096 break;
3632 } 4097 }
@@ -3638,12 +4103,12 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_cont
3638static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) 4103static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3639{ 4104{
3640 struct l2cap_chan *chan = l2cap_pi(sk)->chan; 4105 struct l2cap_chan *chan = l2cap_pi(sk)->chan;
3641 u16 control; 4106 u32 control;
3642 u8 req_seq; 4107 u16 req_seq;
3643 int len, next_tx_seq_offset, req_seq_offset; 4108 int len, next_tx_seq_offset, req_seq_offset;
3644 4109
3645 control = get_unaligned_le16(skb->data); 4110 control = __get_control(chan, skb->data);
3646 skb_pull(skb, 2); 4111 skb_pull(skb, __ctrl_size(chan));
3647 len = skb->len; 4112 len = skb->len;
3648 4113
3649 /* 4114 /*
@@ -3654,26 +4119,23 @@ static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3654 if (l2cap_check_fcs(chan, skb)) 4119 if (l2cap_check_fcs(chan, skb))
3655 goto drop; 4120 goto drop;
3656 4121
3657 if (__is_sar_start(control) && __is_iframe(control)) 4122 if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
3658 len -= 2; 4123 len -= L2CAP_SDULEN_SIZE;
3659 4124
3660 if (chan->fcs == L2CAP_FCS_CRC16) 4125 if (chan->fcs == L2CAP_FCS_CRC16)
3661 len -= 2; 4126 len -= L2CAP_FCS_SIZE;
3662 4127
3663 if (len > chan->mps) { 4128 if (len > chan->mps) {
3664 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 4129 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3665 goto drop; 4130 goto drop;
3666 } 4131 }
3667 4132
3668 req_seq = __get_reqseq(control); 4133 req_seq = __get_reqseq(chan, control);
3669 req_seq_offset = (req_seq - chan->expected_ack_seq) % 64;
3670 if (req_seq_offset < 0)
3671 req_seq_offset += 64;
3672 4134
3673 next_tx_seq_offset = 4135 req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
3674 (chan->next_tx_seq - chan->expected_ack_seq) % 64; 4136
3675 if (next_tx_seq_offset < 0) 4137 next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
3676 next_tx_seq_offset += 64; 4138 chan->expected_ack_seq);
3677 4139
3678 /* check for invalid req-seq */ 4140 /* check for invalid req-seq */
3679 if (req_seq_offset > next_tx_seq_offset) { 4141 if (req_seq_offset > next_tx_seq_offset) {
@@ -3681,7 +4143,7 @@ static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
3681 goto drop; 4143 goto drop;
3682 } 4144 }
3683 4145
3684 if (__is_iframe(control)) { 4146 if (!__is_sframe(chan, control)) {
3685 if (len < 0) { 4147 if (len < 0) {
3686 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 4148 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
3687 goto drop; 4149 goto drop;
@@ -3709,8 +4171,8 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
3709{ 4171{
3710 struct l2cap_chan *chan; 4172 struct l2cap_chan *chan;
3711 struct sock *sk = NULL; 4173 struct sock *sk = NULL;
3712 u16 control; 4174 u32 control;
3713 u8 tx_seq; 4175 u16 tx_seq;
3714 int len; 4176 int len;
3715 4177
3716 chan = l2cap_get_chan_by_scid(conn, cid); 4178 chan = l2cap_get_chan_by_scid(conn, cid);
@@ -3751,23 +4213,23 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
3751 goto done; 4213 goto done;
3752 4214
3753 case L2CAP_MODE_STREAMING: 4215 case L2CAP_MODE_STREAMING:
3754 control = get_unaligned_le16(skb->data); 4216 control = __get_control(chan, skb->data);
3755 skb_pull(skb, 2); 4217 skb_pull(skb, __ctrl_size(chan));
3756 len = skb->len; 4218 len = skb->len;
3757 4219
3758 if (l2cap_check_fcs(chan, skb)) 4220 if (l2cap_check_fcs(chan, skb))
3759 goto drop; 4221 goto drop;
3760 4222
3761 if (__is_sar_start(control)) 4223 if (__is_sar_start(chan, control))
3762 len -= 2; 4224 len -= L2CAP_SDULEN_SIZE;
3763 4225
3764 if (chan->fcs == L2CAP_FCS_CRC16) 4226 if (chan->fcs == L2CAP_FCS_CRC16)
3765 len -= 2; 4227 len -= L2CAP_FCS_SIZE;
3766 4228
3767 if (len > chan->mps || len < 0 || __is_sframe(control)) 4229 if (len > chan->mps || len < 0 || __is_sframe(chan, control))
3768 goto drop; 4230 goto drop;
3769 4231
3770 tx_seq = __get_txseq(control); 4232 tx_seq = __get_txseq(chan, control);
3771 4233
3772 if (chan->expected_tx_seq != tx_seq) { 4234 if (chan->expected_tx_seq != tx_seq) {
3773 /* Frame(s) missing - must discard partial SDU */ 4235 /* Frame(s) missing - must discard partial SDU */
@@ -3779,7 +4241,7 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
3779 /* TODO: Notify userland of missing data */ 4241 /* TODO: Notify userland of missing data */
3780 } 4242 }
3781 4243
3782 chan->expected_tx_seq = (tx_seq + 1) % 64; 4244 chan->expected_tx_seq = __next_seq(chan, tx_seq);
3783 4245
3784 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE) 4246 if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
3785 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 4247 l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
@@ -3933,12 +4395,12 @@ static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
3933 4395
3934 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { 4396 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
3935 lm1 |= HCI_LM_ACCEPT; 4397 lm1 |= HCI_LM_ACCEPT;
3936 if (c->role_switch) 4398 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
3937 lm1 |= HCI_LM_MASTER; 4399 lm1 |= HCI_LM_MASTER;
3938 exact++; 4400 exact++;
3939 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { 4401 } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
3940 lm2 |= HCI_LM_ACCEPT; 4402 lm2 |= HCI_LM_ACCEPT;
3941 if (c->role_switch) 4403 if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
3942 lm2 |= HCI_LM_MASTER; 4404 lm2 |= HCI_LM_MASTER;
3943 } 4405 }
3944 } 4406 }
@@ -3973,7 +4435,7 @@ static int l2cap_disconn_ind(struct hci_conn *hcon)
3973 BT_DBG("hcon %p", hcon); 4435 BT_DBG("hcon %p", hcon);
3974 4436
3975 if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn) 4437 if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
3976 return 0x13; 4438 return HCI_ERROR_REMOTE_USER_TERM;
3977 4439
3978 return conn->disc_reason; 4440 return conn->disc_reason;
3979} 4441}
@@ -4306,3 +4768,6 @@ void l2cap_exit(void)
4306 4768
4307module_param(disable_ertm, bool, 0644); 4769module_param(disable_ertm, bool, 0644);
4308MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); 4770MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
4771
4772module_param(enable_hs, bool, 0644);
4773MODULE_PARM_DESC(enable_hs, "Enable High Speed");
generated by cgit v1.2.2 (git 2.25.0) at 2025-01-23 11:36:37 -0500