diff options
Diffstat (limited to 'net/bluetooth/l2cap_core.c')
-rw-r--r-- | net/bluetooth/l2cap_core.c | 1038 |
1 files changed, 767 insertions, 271 deletions
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index 17b5b1cd965..9bc22e4c4c6 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -56,10 +56,10 @@ #include <net/bluetooth/l2cap.h> #include <net/bluetooth/smp.h> -int disable_ertm; +bool disable_ertm; static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN; -static u8 l2cap_fixed_chan[8] = { 0x02, }; +static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, }; static LIST_HEAD(chan_list); static DEFINE_RWLOCK(chan_list_lock); @@ -96,7 +96,6 @@ static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 return c; } return NULL; - } static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) @@ -153,12 +152,9 @@ static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src) list_for_each_entry(c, &chan_list, global_l) { if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src)) - goto found; + return c; } - - c = NULL; -found: - return c; + return NULL; } int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm) @@ -219,7 +215,7 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn) static void l2cap_set_timer(struct l2cap_chan *chan, struct timer_list *timer, long timeout) { - BT_DBG("chan %p state %d timeout %ld", chan->sk, chan->state, timeout); + BT_DBG("chan %p state %d timeout %ld", chan, chan->state, timeout); if (!mod_timer(timer, jiffies + msecs_to_jiffies(timeout))) chan_hold(chan); @@ -233,8 +229,37 @@ static void l2cap_clear_timer(struct l2cap_chan *chan, struct timer_list *timer) chan_put(chan); } +static char *state_to_string(int state) +{ + switch(state) { + case BT_CONNECTED: + return "BT_CONNECTED"; + case BT_OPEN: + return "BT_OPEN"; + case BT_BOUND: + return "BT_BOUND"; + case BT_LISTEN: + return "BT_LISTEN"; + case BT_CONNECT: + return "BT_CONNECT"; + case BT_CONNECT2: + return "BT_CONNECT2"; + case BT_CONFIG: + return "BT_CONFIG"; + case BT_DISCONN: + return "BT_DISCONN"; + case BT_CLOSED: + return "BT_CLOSED"; + } + + return "invalid state"; +} + static void l2cap_state_change(struct l2cap_chan *chan, int state) { + BT_DBG("%p %s -> %s", chan, state_to_string(chan->state), + state_to_string(state)); + chan->state = state; chan->ops->state_change(chan->data, state); } @@ -293,6 +318,8 @@ struct l2cap_chan *l2cap_chan_create(struct sock *sk) atomic_set(&chan->refcnt, 1); + BT_DBG("sk %p chan %p", sk, chan); + return chan; } @@ -310,7 +337,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, chan->psm, chan->dcid); - conn->disc_reason = 0x13; + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; chan->conn = conn; @@ -337,6 +364,13 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) chan->omtu = L2CAP_DEFAULT_MTU; } + chan->local_id = L2CAP_BESTEFFORT_ID; + chan->local_stype = L2CAP_SERV_BESTEFFORT; + chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE; + chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME; + chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT; + chan->local_flush_to = L2CAP_DEFAULT_FLUSH_TO; + chan_hold(chan); list_add(&chan->list, &conn->chan_l); @@ -508,7 +542,7 @@ static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) } /* Service level security */ -static inline int l2cap_check_security(struct l2cap_chan *chan) +int l2cap_chan_check_security(struct l2cap_chan *chan) { struct l2cap_conn *conn = chan->conn; __u8 auth_type; @@ -556,34 +590,58 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, flags = ACL_START; bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON; + skb->priority = HCI_PRIO_MAX; + + hci_send_acl(conn->hchan, skb, flags); +} + +static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct hci_conn *hcon = chan->conn->hcon; + u16 flags; + + BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len, + skb->priority); - hci_send_acl(conn->hcon, skb, flags); + if (!test_bit(FLAG_FLUSHABLE, &chan->flags) && + lmp_no_flush_capable(hcon->hdev)) + flags = ACL_START_NO_FLUSH; + else + flags = ACL_START; + + bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags); + hci_send_acl(chan->conn->hchan, skb, flags); } -static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) +static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control) { struct sk_buff *skb; struct l2cap_hdr *lh; struct l2cap_conn *conn = chan->conn; - int count, hlen = L2CAP_HDR_SIZE + 2; - u8 flags; + int count, hlen; if (chan->state != BT_CONNECTED) return; + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hlen = L2CAP_EXT_HDR_SIZE; + else + hlen = L2CAP_ENH_HDR_SIZE; + if (chan->fcs == L2CAP_FCS_CRC16) - hlen += 2; + hlen += L2CAP_FCS_SIZE; - BT_DBG("chan %p, control 0x%2.2x", chan, control); + BT_DBG("chan %p, control 0x%8.8x", chan, control); count = min_t(unsigned int, conn->mtu, hlen); - control |= L2CAP_CTRL_FRAME_TYPE; + + control |= __set_sframe(chan); if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) - control |= L2CAP_CTRL_FINAL; + control |= __set_ctrl_final(chan); if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state)) - control |= L2CAP_CTRL_POLL; + control |= __set_ctrl_poll(chan); skb = bt_skb_alloc(count, GFP_ATOMIC); if (!skb) @@ -592,32 +650,27 @@ static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); lh->cid = cpu_to_le16(chan->dcid); - put_unaligned_le16(control, skb_put(skb, 2)); + + __put_control(chan, control, skb_put(skb, __ctrl_size(chan))); if (chan->fcs == L2CAP_FCS_CRC16) { - u16 fcs = crc16(0, (u8 *)lh, count - 2); - put_unaligned_le16(fcs, skb_put(skb, 2)); + u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); } - if (lmp_no_flush_capable(conn->hcon->hdev)) - flags = ACL_START_NO_FLUSH; - else - flags = ACL_START; - - bt_cb(skb)->force_active = chan->force_active; - - hci_send_acl(chan->conn->hcon, skb, flags); + skb->priority = HCI_PRIO_MAX; + l2cap_do_send(chan, skb); } -static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u16 control) +static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control) { if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - control |= L2CAP_SUPER_RCV_NOT_READY; + control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); set_bit(CONN_RNR_SENT, &chan->conn_state); } else - control |= L2CAP_SUPER_RCV_READY; + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); - control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; + control |= __set_reqseq(chan, chan->buffer_seq); l2cap_send_sframe(chan, control); } @@ -635,7 +688,7 @@ static void l2cap_do_start(struct l2cap_chan *chan) if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) return; - if (l2cap_check_security(chan) && + if (l2cap_chan_check_security(chan) && __l2cap_no_conn_pending(chan)) { struct l2cap_conn_req req; req.scid = cpu_to_le16(chan->scid); @@ -725,7 +778,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn) if (chan->state == BT_CONNECT) { struct l2cap_conn_req req; - if (!l2cap_check_security(chan) || + if (!l2cap_chan_check_security(chan) || !__l2cap_no_conn_pending(chan)) { bh_unlock_sock(sk); continue; @@ -758,7 +811,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn) rsp.scid = cpu_to_le16(chan->dcid); rsp.dcid = cpu_to_le16(chan->scid); - if (l2cap_check_security(chan)) { + if (l2cap_chan_check_security(chan)) { if (bt_sk(sk)->defer_setup) { struct sock *parent = bt_sk(sk)->parent; rsp.result = cpu_to_le16(L2CAP_CR_PEND); @@ -947,7 +1000,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) list_for_each_entry(chan, &conn->chan_l, list) { struct sock *sk = chan->sk; - if (chan->force_reliable) + if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) sk->sk_err = err; } @@ -986,6 +1039,8 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err) chan->ops->close(chan->data); } + hci_chan_del(conn->hchan); + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) del_timer_sync(&conn->info_timer); @@ -1008,18 +1063,26 @@ static void security_timeout(unsigned long arg) static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) { struct l2cap_conn *conn = hcon->l2cap_data; + struct hci_chan *hchan; if (conn || status) return conn; + hchan = hci_chan_create(hcon); + if (!hchan) + return NULL; + conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); - if (!conn) + if (!conn) { + hci_chan_del(hchan); return NULL; + } hcon->l2cap_data = conn; conn->hcon = hcon; + conn->hchan = hchan; - BT_DBG("hcon %p conn %p", hcon, conn); + BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan); if (hcon->hdev->le_mtu && hcon->type == LE_LINK) conn->mtu = hcon->hdev->le_mtu; @@ -1043,7 +1106,7 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) setup_timer(&conn->info_timer, l2cap_info_timeout, (unsigned long) conn); - conn->disc_reason = 0x13; + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; return conn; } @@ -1142,7 +1205,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan) if (hcon->state == BT_CONNECTED) { if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { __clear_chan_timer(chan); - if (l2cap_check_security(chan)) + if (l2cap_chan_check_security(chan)) l2cap_state_change(chan, BT_CONNECTED); } else l2cap_do_start(chan); @@ -1245,60 +1308,46 @@ static void l2cap_drop_acked_frames(struct l2cap_chan *chan) __clear_retrans_timer(chan); } -static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) -{ - struct hci_conn *hcon = chan->conn->hcon; - u16 flags; - - BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len); - - if (!chan->flushable && lmp_no_flush_capable(hcon->hdev)) - flags = ACL_START_NO_FLUSH; - else - flags = ACL_START; - - bt_cb(skb)->force_active = chan->force_active; - hci_send_acl(hcon, skb, flags); -} - static void l2cap_streaming_send(struct l2cap_chan *chan) { struct sk_buff *skb; - u16 control, fcs; + u32 control; + u16 fcs; while ((skb = skb_dequeue(&chan->tx_q))) { - control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE); - control |= chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT; - put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); + control = __get_control(chan, skb->data + L2CAP_HDR_SIZE); + control |= __set_txseq(chan, chan->next_tx_seq); + __put_control(chan, control, skb->data + L2CAP_HDR_SIZE); if (chan->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)skb->data, skb->len - 2); - put_unaligned_le16(fcs, skb->data + skb->len - 2); + fcs = crc16(0, (u8 *)skb->data, + skb->len - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, + skb->data + skb->len - L2CAP_FCS_SIZE); } l2cap_do_send(chan, skb); - chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); } } -static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq) +static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq) { struct sk_buff *skb, *tx_skb; - u16 control, fcs; + u16 fcs; + u32 control; skb = skb_peek(&chan->tx_q); if (!skb) return; - do { - if (bt_cb(skb)->tx_seq == tx_seq) - break; - + while (bt_cb(skb)->tx_seq != tx_seq) { if (skb_queue_is_last(&chan->tx_q, skb)) return; - } while ((skb = skb_queue_next(&chan->tx_q, skb))); + skb = skb_queue_next(&chan->tx_q, skb); + } if (chan->remote_max_tx && bt_cb(skb)->retries == chan->remote_max_tx) { @@ -1308,20 +1357,23 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq) tx_skb = skb_clone(skb, GFP_ATOMIC); bt_cb(skb)->retries++; - control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); - control &= L2CAP_CTRL_SAR; + + control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE); + control &= __get_sar_mask(chan); if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) - control |= L2CAP_CTRL_FINAL; + control |= __set_ctrl_final(chan); - control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) - | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); + control |= __set_reqseq(chan, chan->buffer_seq); + control |= __set_txseq(chan, tx_seq); - put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); + __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE); if (chan->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2); - put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2); + fcs = crc16(0, (u8 *)tx_skb->data, + tx_skb->len - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, + tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE); } l2cap_do_send(chan, tx_skb); @@ -1330,7 +1382,8 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq) static int l2cap_ertm_send(struct l2cap_chan *chan) { struct sk_buff *skb, *tx_skb; - u16 control, fcs; + u16 fcs; + u32 control; int nsent = 0; if (chan->state != BT_CONNECTED) @@ -1348,20 +1401,22 @@ static int l2cap_ertm_send(struct l2cap_chan *chan) bt_cb(skb)->retries++; - control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); - control &= L2CAP_CTRL_SAR; + control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE); + control &= __get_sar_mask(chan); if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) - control |= L2CAP_CTRL_FINAL; + control |= __set_ctrl_final(chan); - control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) - | (chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); - put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); + control |= __set_reqseq(chan, chan->buffer_seq); + control |= __set_txseq(chan, chan->next_tx_seq); + __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE); if (chan->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2); - put_unaligned_le16(fcs, skb->data + tx_skb->len - 2); + fcs = crc16(0, (u8 *)skb->data, + tx_skb->len - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, skb->data + + tx_skb->len - L2CAP_FCS_SIZE); } l2cap_do_send(chan, tx_skb); @@ -1369,7 +1424,8 @@ static int l2cap_ertm_send(struct l2cap_chan *chan) __set_retrans_timer(chan); bt_cb(skb)->tx_seq = chan->next_tx_seq; - chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; + + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); if (bt_cb(skb)->retries == 1) chan->unacked_frames++; @@ -1401,12 +1457,12 @@ static int l2cap_retransmit_frames(struct l2cap_chan *chan) static void l2cap_send_ack(struct l2cap_chan *chan) { - u16 control = 0; + u32 control = 0; - control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; + control |= __set_reqseq(chan, chan->buffer_seq); if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - control |= L2CAP_SUPER_RCV_NOT_READY; + control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); set_bit(CONN_RNR_SENT, &chan->conn_state); l2cap_send_sframe(chan, control); return; @@ -1415,20 +1471,20 @@ static void l2cap_send_ack(struct l2cap_chan *chan) if (l2cap_ertm_send(chan) > 0) return; - control |= L2CAP_SUPER_RCV_READY; + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); l2cap_send_sframe(chan, control); } static void l2cap_send_srejtail(struct l2cap_chan *chan) { struct srej_list *tail; - u16 control; + u32 control; - control = L2CAP_SUPER_SELECT_REJECT; - control |= L2CAP_CTRL_FINAL; + control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); + control |= __set_ctrl_final(chan); tail = list_entry((&chan->srej_l)->prev, struct srej_list, list); - control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; + control |= __set_reqseq(chan, tail->tx_seq); l2cap_send_sframe(chan, control); } @@ -1456,6 +1512,8 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) return -EFAULT; + (*frag)->priority = skb->priority; + sent += count; len -= count; @@ -1465,15 +1523,17 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in return sent; } -static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) +static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u32 priority) { struct sock *sk = chan->sk; struct l2cap_conn *conn = chan->conn; struct sk_buff *skb; - int err, count, hlen = L2CAP_HDR_SIZE + 2; + int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE; struct l2cap_hdr *lh; - BT_DBG("sk %p len %d", sk, (int)len); + BT_DBG("sk %p len %d priority %u", sk, (int)len, priority); count = min_t(unsigned int, (conn->mtu - hlen), len); skb = bt_skb_send_alloc(sk, count + hlen, @@ -1481,6 +1541,8 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct if (!skb) return ERR_PTR(err); + skb->priority = priority; + /* Create L2CAP header */ lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); lh->cid = cpu_to_le16(chan->dcid); @@ -1495,7 +1557,9 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct return skb; } -static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) +static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u32 priority) { struct sock *sk = chan->sk; struct l2cap_conn *conn = chan->conn; @@ -1511,6 +1575,8 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct ms if (!skb) return ERR_PTR(err); + skb->priority = priority; + /* Create L2CAP header */ lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); lh->cid = cpu_to_le16(chan->dcid); @@ -1526,12 +1592,12 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct ms static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len, - u16 control, u16 sdulen) + u32 control, u16 sdulen) { struct sock *sk = chan->sk; struct l2cap_conn *conn = chan->conn; struct sk_buff *skb; - int err, count, hlen = L2CAP_HDR_SIZE + 2; + int err, count, hlen; struct l2cap_hdr *lh; BT_DBG("sk %p len %d", sk, (int)len); @@ -1539,11 +1605,16 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, if (!conn) return ERR_PTR(-ENOTCONN); + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hlen = L2CAP_EXT_HDR_SIZE; + else + hlen = L2CAP_ENH_HDR_SIZE; + if (sdulen) - hlen += 2; + hlen += L2CAP_SDULEN_SIZE; if (chan->fcs == L2CAP_FCS_CRC16) - hlen += 2; + hlen += L2CAP_FCS_SIZE; count = min_t(unsigned int, (conn->mtu - hlen), len); skb = bt_skb_send_alloc(sk, count + hlen, @@ -1555,9 +1626,11 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); lh->cid = cpu_to_le16(chan->dcid); lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); - put_unaligned_le16(control, skb_put(skb, 2)); + + __put_control(chan, control, skb_put(skb, __ctrl_size(chan))); + if (sdulen) - put_unaligned_le16(sdulen, skb_put(skb, 2)); + put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); if (unlikely(err < 0)) { @@ -1566,7 +1639,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, } if (chan->fcs == L2CAP_FCS_CRC16) - put_unaligned_le16(0, skb_put(skb, 2)); + put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE)); bt_cb(skb)->retries = 0; return skb; @@ -1576,11 +1649,11 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si { struct sk_buff *skb; struct sk_buff_head sar_queue; - u16 control; + u32 control; size_t size = 0; skb_queue_head_init(&sar_queue); - control = L2CAP_SDU_START; + control = __set_ctrl_sar(chan, L2CAP_SAR_START); skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len); if (IS_ERR(skb)) return PTR_ERR(skb); @@ -1593,10 +1666,10 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si size_t buflen; if (len > chan->remote_mps) { - control = L2CAP_SDU_CONTINUE; + control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE); buflen = chan->remote_mps; } else { - control = L2CAP_SDU_END; + control = __set_ctrl_sar(chan, L2CAP_SAR_END); buflen = len; } @@ -1617,15 +1690,16 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si return size; } -int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) +int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len, + u32 priority) { struct sk_buff *skb; - u16 control; + u32 control; int err; /* Connectionless channel */ if (chan->chan_type == L2CAP_CHAN_CONN_LESS) { - skb = l2cap_create_connless_pdu(chan, msg, len); + skb = l2cap_create_connless_pdu(chan, msg, len, priority); if (IS_ERR(skb)) return PTR_ERR(skb); @@ -1640,7 +1714,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) return -EMSGSIZE; /* Create a basic PDU */ - skb = l2cap_create_basic_pdu(chan, msg, len); + skb = l2cap_create_basic_pdu(chan, msg, len, priority); if (IS_ERR(skb)) return PTR_ERR(skb); @@ -1652,7 +1726,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) case L2CAP_MODE_STREAMING: /* Entire SDU fits into one PDU */ if (len <= chan->remote_mps) { - control = L2CAP_SDU_UNSEGMENTED; + control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED); skb = l2cap_create_iframe_pdu(chan, msg, len, control, 0); if (IS_ERR(skb)) @@ -1850,6 +1924,37 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) *ptr += L2CAP_CONF_OPT_SIZE + len; } +static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan) +{ + struct l2cap_conf_efs efs; + + switch (chan->mode) { + case L2CAP_MODE_ERTM: + efs.id = chan->local_id; + efs.stype = chan->local_stype; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT); + efs.flush_to = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO); + break; + + case L2CAP_MODE_STREAMING: + efs.id = 1; + efs.stype = L2CAP_SERV_BESTEFFORT; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = 0; + efs.flush_to = 0; + break; + + default: + return; + } + + l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs), + (unsigned long) &efs); +} + static void l2cap_ack_timeout(unsigned long arg) { struct l2cap_chan *chan = (void *) arg; @@ -1896,11 +2001,36 @@ static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) } } +static inline bool __l2cap_ews_supported(struct l2cap_chan *chan) +{ + return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW; +} + +static inline bool __l2cap_efs_supported(struct l2cap_chan *chan) +{ + return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW; +} + +static inline void l2cap_txwin_setup(struct l2cap_chan *chan) +{ + if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW && + __l2cap_ews_supported(chan)) { + /* use extended control field */ + set_bit(FLAG_EXT_CTRL, &chan->flags); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + } else { + chan->tx_win = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); + chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; + } +} + static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) { struct l2cap_conf_req *req = data; struct l2cap_conf_rfc rfc = { .mode = chan->mode }; void *ptr = req->data; + u16 size; BT_DBG("chan %p", chan); @@ -1913,6 +2043,9 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) break; + if (__l2cap_efs_supported(chan)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + /* fall through */ default: chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); @@ -1942,17 +2075,27 @@ done: case L2CAP_MODE_ERTM: rfc.mode = L2CAP_MODE_ERTM; - rfc.txwin_size = chan->tx_win; rfc.max_transmit = chan->max_tx; rfc.retrans_timeout = 0; rfc.monitor_timeout = 0; - rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); - if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + + l2cap_txwin_setup(chan); + + rfc.txwin_size = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), (unsigned long) &rfc); + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); + if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) break; @@ -1961,6 +2104,10 @@ done: chan->fcs = L2CAP_FCS_NONE; l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); } + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); break; case L2CAP_MODE_STREAMING: @@ -1969,13 +2116,19 @@ done: rfc.max_transmit = 0; rfc.retrans_timeout = 0; rfc.monitor_timeout = 0; - rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); - if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), (unsigned long) &rfc); + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); + if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) break; @@ -2002,8 +2155,11 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) int type, hint, olen; unsigned long val; struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; + u8 remote_efs = 0; u16 mtu = L2CAP_DEFAULT_MTU; u16 result = L2CAP_CONF_SUCCESS; + u16 size; BT_DBG("chan %p", chan); @@ -2033,7 +2189,22 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) case L2CAP_CONF_FCS: if (val == L2CAP_FCS_NONE) set_bit(CONF_NO_FCS_RECV, &chan->conf_state); + break; + case L2CAP_CONF_EFS: + remote_efs = 1; + if (olen == sizeof(efs)) + memcpy(&efs, (void *) val, olen); + break; + + case L2CAP_CONF_EWS: + if (!enable_hs) + return -ECONNREFUSED; + + set_bit(FLAG_EXT_CTRL, &chan->flags); + set_bit(CONF_EWS_RECV, &chan->conf_state); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + chan->remote_tx_win = val; break; default: @@ -2058,6 +2229,13 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) break; } + if (remote_efs) { + if (__l2cap_efs_supported(chan)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + else + return -ECONNREFUSED; + } + if (chan->mode != rfc.mode) return -ECONNREFUSED; @@ -2076,7 +2254,6 @@ done: sizeof(rfc), (unsigned long) &rfc); } - if (result == L2CAP_CONF_SUCCESS) { /* Configure output options and let the other side know * which ones we don't like. */ @@ -2089,6 +2266,26 @@ done: } l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu); + if (remote_efs) { + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) { + + result = L2CAP_CONF_UNACCEPT; + + if (chan->num_conf_req >= 1) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), + (unsigned long) &efs); + } else { + /* Send PENDING Conf Rsp */ + result = L2CAP_CONF_PENDING; + set_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + } + } + switch (rfc.mode) { case L2CAP_MODE_BASIC: chan->fcs = L2CAP_FCS_NONE; @@ -2096,13 +2293,20 @@ done: break; case L2CAP_MODE_ERTM: - chan->remote_tx_win = rfc.txwin_size; - chan->remote_max_tx = rfc.max_transmit; + if (!test_bit(CONF_EWS_RECV, &chan->conf_state)) + chan->remote_tx_win = rfc.txwin_size; + else + rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW; - if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); + chan->remote_max_tx = rfc.max_transmit; - chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; rfc.retrans_timeout = le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); @@ -2114,13 +2318,29 @@ done: l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), (unsigned long) &rfc); + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->remote_id = efs.id; + chan->remote_stype = efs.stype; + chan->remote_msdu = le16_to_cpu(efs.msdu); + chan->remote_flush_to = + le32_to_cpu(efs.flush_to); + chan->remote_acc_lat = + le32_to_cpu(efs.acc_lat); + chan->remote_sdu_itime = + le32_to_cpu(efs.sdu_itime); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), (unsigned long) &efs); + } break; case L2CAP_MODE_STREAMING: - if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); - - chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; set_bit(CONF_MODE_DONE, &chan->conf_state); @@ -2153,6 +2373,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi int type, olen; unsigned long val; struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); @@ -2188,6 +2409,26 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), (unsigned long) &rfc); break; + + case L2CAP_CONF_EWS: + chan->tx_win = min_t(u16, val, + L2CAP_DEFAULT_EXT_WINDOW); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); + break; + + case L2CAP_CONF_EFS: + if (olen == sizeof(efs)) + memcpy(&efs, (void *)val, olen); + + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), (unsigned long) &efs); + break; } } @@ -2196,13 +2437,23 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi chan->mode = rfc.mode; - if (*result == L2CAP_CONF_SUCCESS) { + if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) { switch (rfc.mode) { case L2CAP_MODE_ERTM: chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); chan->mps = le16_to_cpu(rfc.max_pdu_size); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->local_msdu = le16_to_cpu(efs.msdu); + chan->local_sdu_itime = + le32_to_cpu(efs.sdu_itime); + chan->local_acc_lat = le32_to_cpu(efs.acc_lat); + chan->local_flush_to = + le32_to_cpu(efs.flush_to); + } break; + case L2CAP_MODE_STREAMING: chan->mps = le16_to_cpu(rfc.max_pdu_size); } @@ -2340,7 +2591,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd /* Check if the ACL is secure enough (if not SDP) */ if (psm != cpu_to_le16(0x0001) && !hci_conn_check_link_mode(conn->hcon)) { - conn->disc_reason = 0x05; + conn->disc_reason = HCI_ERROR_AUTH_FAILURE; result = L2CAP_CR_SEC_BLOCK; goto response; } @@ -2387,7 +2638,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd chan->ident = cmd->ident; if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { - if (l2cap_check_security(chan)) { + if (l2cap_chan_check_security(chan)) { if (bt_sk(sk)->defer_setup) { l2cap_state_change(chan, BT_CONNECT2); result = L2CAP_CR_PEND; @@ -2612,6 +2863,21 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr chan->num_conf_req++; } + /* Got Conf Rsp PENDING from remote side and asume we sent + Conf Rsp PENDING in the code above */ + if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) && + test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + + /* check compatibility */ + + clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_SUCCESS, 0x0000), rsp); + } + unlock: bh_unlock_sock(sk); return 0; @@ -2641,8 +2907,33 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr switch (result) { case L2CAP_CONF_SUCCESS: l2cap_conf_rfc_get(chan, rsp->data, len); + clear_bit(CONF_REM_CONF_PEND, &chan->conf_state); break; + case L2CAP_CONF_PENDING: + set_bit(CONF_REM_CONF_PEND, &chan->conf_state); + + if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + char buf[64]; + + len = l2cap_parse_conf_rsp(chan, rsp->data, len, + buf, &result); + if (len < 0) { + l2cap_send_disconn_req(conn, chan, ECONNRESET); + goto done; + } + + /* check compatibility */ + + clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, buf, + L2CAP_CONF_SUCCESS, 0x0000), buf); + } + goto done; + case L2CAP_CONF_UNACCEPT: if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { char req[64]; @@ -2760,7 +3051,7 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd /* don't delete l2cap channel if sk is owned by user */ if (sock_owned_by_user(sk)) { - l2cap_state_change(chan,BT_DISCONN); + l2cap_state_change(chan, BT_DISCONN); __clear_chan_timer(chan); __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); bh_unlock_sock(sk); @@ -2792,15 +3083,25 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm if (!disable_ertm) feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING | L2CAP_FEAT_FCS; + if (enable_hs) + feat_mask |= L2CAP_FEAT_EXT_FLOW + | L2CAP_FEAT_EXT_WINDOW; + put_unaligned_le32(feat_mask, rsp->data); l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), buf); } else if (type == L2CAP_IT_FIXED_CHAN) { u8 buf[12]; struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; + + if (enable_hs) + l2cap_fixed_chan[0] |= L2CAP_FC_A2MP; + else + l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP; + rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); - memcpy(buf + 4, l2cap_fixed_chan, 8); + memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan)); l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), buf); } else { @@ -2867,6 +3168,165 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm return 0; } +static inline int l2cap_create_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + void *data) +{ + struct l2cap_create_chan_req *req = data; + struct l2cap_create_chan_rsp rsp; + u16 psm, scid; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + if (!enable_hs) + return -EINVAL; + + psm = le16_to_cpu(req->psm); + scid = le16_to_cpu(req->scid); + + BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id); + + /* Placeholder: Always reject */ + rsp.dcid = 0; + rsp.scid = cpu_to_le16(scid); + rsp.result = L2CAP_CR_NO_MEM; + rsp.status = L2CAP_CS_NO_INFO; + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP, + sizeof(rsp), &rsp); + + return 0; +} + +static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, void *data) +{ + BT_DBG("conn %p", conn); + + return l2cap_connect_rsp(conn, cmd, data); +} + +static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident, + u16 icid, u16 result) +{ + struct l2cap_move_chan_rsp rsp; + + BT_DBG("icid %d, result %d", icid, result); + + rsp.icid = cpu_to_le16(icid); + rsp.result = cpu_to_le16(result); + + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp); +} + +static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn, + struct l2cap_chan *chan, u16 icid, u16 result) +{ + struct l2cap_move_chan_cfm cfm; + u8 ident; + + BT_DBG("icid %d, result %d", icid, result); + + ident = l2cap_get_ident(conn); + if (chan) + chan->ident = ident; + + cfm.icid = cpu_to_le16(icid); + cfm.result = cpu_to_le16(result); + + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm); +} + +static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident, + u16 icid) +{ + struct l2cap_move_chan_cfm_rsp rsp; + + BT_DBG("icid %d", icid); + + rsp.icid = cpu_to_le16(icid); + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp); +} + +static inline int l2cap_move_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_req *req = data; + u16 icid = 0; + u16 result = L2CAP_MR_NOT_ALLOWED; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + icid = le16_to_cpu(req->icid); + + BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id); + + if (!enable_hs) + return -EINVAL; + + /* Placeholder: Always refuse */ + l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result); + + return 0; +} + +static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_rsp *rsp = data; + u16 icid, result; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + result = le16_to_cpu(rsp->result); + + BT_DBG("icid %d, result %d", icid, result); + + /* Placeholder: Always unconfirmed */ + l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED); + + return 0; +} + +static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm *cfm = data; + u16 icid, result; + + if (cmd_len != sizeof(*cfm)) + return -EPROTO; + + icid = le16_to_cpu(cfm->icid); + result = le16_to_cpu(cfm->result); + + BT_DBG("icid %d, result %d", icid, result); + + l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); + + return 0; +} + +static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm_rsp *rsp = data; + u16 icid; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + + BT_DBG("icid %d", icid); + + return 0; +} + static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, u16 to_multiplier) { @@ -2979,6 +3439,30 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, err = l2cap_information_rsp(conn, cmd, data); break; + case L2CAP_CREATE_CHAN_REQ: + err = l2cap_create_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_CREATE_CHAN_RSP: + err = l2cap_create_channel_rsp(conn, cmd, data); + break; + + case L2CAP_MOVE_CHAN_REQ: + err = l2cap_move_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_RSP: + err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM: + err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM_RSP: + err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data); + break; + default: BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); err = -EINVAL; @@ -3057,10 +3541,15 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) { u16 our_fcs, rcv_fcs; - int hdr_size = L2CAP_HDR_SIZE + 2; + int hdr_size; + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hdr_size = L2CAP_EXT_HDR_SIZE; + else + hdr_size = L2CAP_ENH_HDR_SIZE; if (chan->fcs == L2CAP_FCS_CRC16) { - skb_trim(skb, skb->len - 2); + skb_trim(skb, skb->len - L2CAP_FCS_SIZE); rcv_fcs = get_unaligned_le16(skb->data + skb->len); our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); @@ -3072,14 +3561,14 @@ static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) { - u16 control = 0; + u32 control = 0; chan->frames_sent = 0; - control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; + control |= __set_reqseq(chan, chan->buffer_seq); if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - control |= L2CAP_SUPER_RCV_NOT_READY; + control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); l2cap_send_sframe(chan, control); set_bit(CONN_RNR_SENT, &chan->conn_state); } @@ -3091,12 +3580,12 @@ static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && chan->frames_sent == 0) { - control |= L2CAP_SUPER_RCV_READY; + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); l2cap_send_sframe(chan, control); } } -static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar) +static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar) { struct sk_buff *next_skb; int tx_seq_offset, next_tx_seq_offset; @@ -3105,23 +3594,15 @@ static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, bt_cb(skb)->sar = sar; next_skb = skb_peek(&chan->srej_q); - if (!next_skb) { - __skb_queue_tail(&chan->srej_q, skb); - return 0; - } - tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; - if (tx_seq_offset < 0) - tx_seq_offset += 64; + tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq); - do { + while (next_skb) { if (bt_cb(next_skb)->tx_seq == tx_seq) return -EINVAL; - next_tx_seq_offset = (bt_cb(next_skb)->tx_seq - - chan->buffer_seq) % 64; - if (next_tx_seq_offset < 0) - next_tx_seq_offset += 64; + next_tx_seq_offset = __seq_offset(chan, + bt_cb(next_skb)->tx_seq, chan->buffer_seq); if (next_tx_seq_offset > tx_seq_offset) { __skb_queue_before(&chan->srej_q, next_skb, skb); @@ -3129,9 +3610,10 @@ static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, } if (skb_queue_is_last(&chan->srej_q, next_skb)) - break; - - } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb))); + next_skb = NULL; + else + next_skb = skb_queue_next(&chan->srej_q, next_skb); + } __skb_queue_tail(&chan->srej_q, skb); @@ -3157,24 +3639,24 @@ static void append_skb_frag(struct sk_buff *skb, skb->truesize += new_frag->truesize; } -static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) +static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control) { int err = -EINVAL; - switch (control & L2CAP_CTRL_SAR) { - case L2CAP_SDU_UNSEGMENTED: + switch (__get_ctrl_sar(chan, control)) { + case L2CAP_SAR_UNSEGMENTED: if (chan->sdu) break; err = chan->ops->recv(chan->data, skb); break; - case L2CAP_SDU_START: + case L2CAP_SAR_START: if (chan->sdu) break; chan->sdu_len = get_unaligned_le16(skb->data); - skb_pull(skb, 2); + skb_pull(skb, L2CAP_SDULEN_SIZE); if (chan->sdu_len > chan->imtu) { err = -EMSGSIZE; @@ -3191,7 +3673,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1 err = 0; break; - case L2CAP_SDU_CONTINUE: + case L2CAP_SAR_CONTINUE: if (!chan->sdu) break; @@ -3205,7 +3687,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1 err = 0; break; - case L2CAP_SDU_END: + case L2CAP_SAR_END: if (!chan->sdu) break; @@ -3240,14 +3722,14 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan) { - u16 control; + u32 control; BT_DBG("chan %p, Enter local busy", chan); set_bit(CONN_LOCAL_BUSY, &chan->conn_state); - control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; - control |= L2CAP_SUPER_RCV_NOT_READY; + control = __set_reqseq(chan, chan->buffer_seq); + control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); l2cap_send_sframe(chan, control); set_bit(CONN_RNR_SENT, &chan->conn_state); @@ -3257,13 +3739,14 @@ static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan) static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan) { - u16 control; + u32 control; if (!test_bit(CONN_RNR_SENT, &chan->conn_state)) goto done; - control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; - control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL; + control = __set_reqseq(chan, chan->buffer_seq); + control |= __set_ctrl_poll(chan); + control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); l2cap_send_sframe(chan, control); chan->retry_count = 1; @@ -3289,10 +3772,10 @@ void l2cap_chan_busy(struct l2cap_chan *chan, int busy) } } -static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq) +static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq) { struct sk_buff *skb; - u16 control; + u32 control; while ((skb = skb_peek(&chan->srej_q)) && !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { @@ -3302,7 +3785,7 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq) break; skb = skb_dequeue(&chan->srej_q); - control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; + control = __set_ctrl_sar(chan, bt_cb(skb)->sar); err = l2cap_reassemble_sdu(chan, skb, control); if (err < 0) { @@ -3310,16 +3793,15 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq) break; } - chan->buffer_seq_srej = - (chan->buffer_seq_srej + 1) % 64; - tx_seq = (tx_seq + 1) % 64; + chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej); + tx_seq = __next_seq(chan, tx_seq); } } -static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq) +static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq) { struct srej_list *l, *tmp; - u16 control; + u32 control; list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { if (l->tx_seq == tx_seq) { @@ -3327,45 +3809,53 @@ static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq) kfree(l); return; } - control = L2CAP_SUPER_SELECT_REJECT; - control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; + control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); + control |= __set_reqseq(chan, l->tx_seq); l2cap_send_sframe(chan, control); list_del(&l->list); list_add_tail(&l->list, &chan->srej_l); } } -static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq) +static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq) { struct srej_list *new; - u16 control; + u32 control; while (tx_seq != chan->expected_tx_seq) { - control = L2CAP_SUPER_SELECT_REJECT; - control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; + control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); + control |= __set_reqseq(chan, chan->expected_tx_seq); l2cap_send_sframe(chan, control); new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); + if (!new) + return -ENOMEM; + new->tx_seq = chan->expected_tx_seq; - chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; + + chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); + list_add_tail(&new->list, &chan->srej_l); } - chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; + + chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); + + return 0; } -static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) +static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb) { - u8 tx_seq = __get_txseq(rx_control); - u8 req_seq = __get_reqseq(rx_control); - u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT; + u16 tx_seq = __get_txseq(chan, rx_control); + u16 req_seq = __get_reqseq(chan, rx_control); + u8 sar = __get_ctrl_sar(chan, rx_control); int tx_seq_offset, expected_tx_seq_offset; int num_to_ack = (chan->tx_win/6) + 1; int err = 0; - BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len, + BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len, tx_seq, rx_control); - if (L2CAP_CTRL_FINAL & rx_control && + if (__is_ctrl_final(chan, rx_control) && test_bit(CONN_WAIT_F, &chan->conn_state)) { __clear_monitor_timer(chan); if (chan->unacked_frames > 0) @@ -3376,9 +3866,7 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont chan->expected_ack_seq = req_seq; l2cap_drop_acked_frames(chan); - tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; - if (tx_seq_offset < 0) - tx_seq_offset += 64; + tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq); /* invalid tx_seq */ if (tx_seq_offset >= chan->tx_win) { @@ -3423,13 +3911,16 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont return 0; } } - l2cap_send_srejframe(chan, tx_seq); + + err = l2cap_send_srejframe(chan, tx_seq); + if (err < 0) { + l2cap_send_disconn_req(chan->conn, chan, -err); + return err; + } } } else { - expected_tx_seq_offset = - (chan->expected_tx_seq - chan->buffer_seq) % 64; - if (expected_tx_seq_offset < 0) - expected_tx_seq_offset += 64; + expected_tx_seq_offset = __seq_offset(chan, + chan->expected_tx_seq, chan->buffer_seq); /* duplicated tx_seq */ if (tx_seq_offset < expected_tx_seq_offset) @@ -3447,14 +3938,18 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont set_bit(CONN_SEND_PBIT, &chan->conn_state); - l2cap_send_srejframe(chan, tx_seq); + err = l2cap_send_srejframe(chan, tx_seq); + if (err < 0) { + l2cap_send_disconn_req(chan->conn, chan, -err); + return err; + } __clear_ack_timer(chan); } return 0; expected: - chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; + chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { bt_cb(skb)->tx_seq = tx_seq; @@ -3464,22 +3959,24 @@ expected: } err = l2cap_reassemble_sdu(chan, skb, rx_control); - chan->buffer_seq = (chan->buffer_seq + 1) % 64; + chan->buffer_seq = __next_seq(chan, chan->buffer_seq); + if (err < 0) { l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); return err; } - if (rx_control & L2CAP_CTRL_FINAL) { + if (__is_ctrl_final(chan, rx_control)) { if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) l2cap_retransmit_frames(chan); } - __set_ack_timer(chan); chan->num_acked = (chan->num_acked + 1) % num_to_ack; if (chan->num_acked == num_to_ack - 1) l2cap_send_ack(chan); + else + __set_ack_timer(chan); return 0; @@ -3488,15 +3985,15 @@ drop: return 0; } -static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control) +static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control) { - BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control), - rx_control); + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, + __get_reqseq(chan, rx_control), rx_control); - chan->expected_ack_seq = __get_reqseq(rx_control); + chan->expected_ack_seq = __get_reqseq(chan, rx_control); l2cap_drop_acked_frames(chan); - if (rx_control & L2CAP_CTRL_POLL) { + if (__is_ctrl_poll(chan, rx_control)) { set_bit(CONN_SEND_FBIT, &chan->conn_state); if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) && @@ -3509,7 +4006,7 @@ static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_co l2cap_send_i_or_rr_or_rnr(chan); } - } else if (rx_control & L2CAP_CTRL_FINAL) { + } else if (__is_ctrl_final(chan, rx_control)) { clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) @@ -3528,18 +4025,18 @@ static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_co } } -static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control) +static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control) { - u8 tx_seq = __get_reqseq(rx_control); + u16 tx_seq = __get_reqseq(chan, rx_control); - BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); chan->expected_ack_seq = tx_seq; l2cap_drop_acked_frames(chan); - if (rx_control & L2CAP_CTRL_FINAL) { + if (__is_ctrl_final(chan, rx_control)) { if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) l2cap_retransmit_frames(chan); } else { @@ -3549,15 +4046,15 @@ static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_c set_bit(CONN_REJ_ACT, &chan->conn_state); } } -static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control) +static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control) { - u8 tx_seq = __get_reqseq(rx_control); + u16 tx_seq = __get_reqseq(chan, rx_control); - BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - if (rx_control & L2CAP_CTRL_POLL) { + if (__is_ctrl_poll(chan, rx_control)) { chan->expected_ack_seq = tx_seq; l2cap_drop_acked_frames(chan); @@ -3570,7 +4067,7 @@ static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_ chan->srej_save_reqseq = tx_seq; set_bit(CONN_SREJ_ACT, &chan->conn_state); } - } else if (rx_control & L2CAP_CTRL_FINAL) { + } else if (__is_ctrl_final(chan, rx_control)) { if (test_bit(CONN_SREJ_ACT, &chan->conn_state) && chan->srej_save_reqseq == tx_seq) clear_bit(CONN_SREJ_ACT, &chan->conn_state); @@ -3585,37 +4082,39 @@ static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_ } } -static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control) +static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control) { - u8 tx_seq = __get_reqseq(rx_control); + u16 tx_seq = __get_reqseq(chan, rx_control); - BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); + BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); set_bit(CONN_REMOTE_BUSY, &chan->conn_state); chan->expected_ack_seq = tx_seq; l2cap_drop_acked_frames(chan); - if (rx_control & L2CAP_CTRL_POLL) + if (__is_ctrl_poll(chan, rx_control)) set_bit(CONN_SEND_FBIT, &chan->conn_state); if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) { __clear_retrans_timer(chan); - if (rx_control & L2CAP_CTRL_POLL) + if (__is_ctrl_poll(chan, rx_control)) l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL); return; } - if (rx_control & L2CAP_CTRL_POLL) + if (__is_ctrl_poll(chan, rx_control)) { l2cap_send_srejtail(chan); - else - l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY); + } else { + rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR); + l2cap_send_sframe(chan, rx_control); + } } -static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) +static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb) { - BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len); + BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len); - if (L2CAP_CTRL_FINAL & rx_control && + if (__is_ctrl_final(chan, rx_control) && test_bit(CONN_WAIT_F, &chan->conn_state)) { __clear_monitor_timer(chan); if (chan->unacked_frames > 0) @@ -3623,20 +4122,20 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_cont clear_bit(CONN_WAIT_F, &chan->conn_state); } - switch (rx_control & L2CAP_CTRL_SUPERVISE) { - case L2CAP_SUPER_RCV_READY: + switch (__get_ctrl_super(chan, rx_control)) { + case L2CAP_SUPER_RR: l2cap_data_channel_rrframe(chan, rx_control); break; - case L2CAP_SUPER_REJECT: + case L2CAP_SUPER_REJ: l2cap_data_channel_rejframe(chan, rx_control); break; - case L2CAP_SUPER_SELECT_REJECT: + case L2CAP_SUPER_SREJ: l2cap_data_channel_srejframe(chan, rx_control); break; - case L2CAP_SUPER_RCV_NOT_READY: + case L2CAP_SUPER_RNR: l2cap_data_channel_rnrframe(chan, rx_control); break; } @@ -3648,12 +4147,12 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_cont static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) { struct l2cap_chan *chan = l2cap_pi(sk)->chan; - u16 control; - u8 req_seq; + u32 control; + u16 req_seq; int len, next_tx_seq_offset, req_seq_offset; - control = get_unaligned_le16(skb->data); - skb_pull(skb, 2); + control = __get_control(chan, skb->data); + skb_pull(skb, __ctrl_size(chan)); len = skb->len; /* @@ -3664,26 +4163,23 @@ static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) if (l2cap_check_fcs(chan, skb)) goto drop; - if (__is_sar_start(control) && __is_iframe(control)) - len -= 2; + if (__is_sar_start(chan, control) && !__is_sframe(chan, control)) + len -= L2CAP_SDULEN_SIZE; if (chan->fcs == L2CAP_FCS_CRC16) - len -= 2; + len -= L2CAP_FCS_SIZE; if (len > chan->mps) { l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); goto drop; } - req_seq = __get_reqseq(control); - req_seq_offset = (req_seq - chan->expected_ack_seq) % 64; - if (req_seq_offset < 0) - req_seq_offset += 64; + req_seq = __get_reqseq(chan, control); - next_tx_seq_offset = - (chan->next_tx_seq - chan->expected_ack_seq) % 64; - if (next_tx_seq_offset < 0) - next_tx_seq_offset += 64; + req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq); + + next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq, + chan->expected_ack_seq); /* check for invalid req-seq */ if (req_seq_offset > next_tx_seq_offset) { @@ -3691,7 +4187,7 @@ static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) goto drop; } - if (__is_iframe(control)) { + if (!__is_sframe(chan, control)) { if (len < 0) { l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); goto drop; @@ -3719,8 +4215,8 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk { struct l2cap_chan *chan; struct sock *sk = NULL; - u16 control; - u8 tx_seq; + u32 control; + u16 tx_seq; int len; chan = l2cap_get_chan_by_scid(conn, cid); @@ -3761,23 +4257,23 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk goto done; case L2CAP_MODE_STREAMING: - control = get_unaligned_le16(skb->data); - skb_pull(skb, 2); + control = __get_control(chan, skb->data); + skb_pull(skb, __ctrl_size(chan)); len = skb->len; if (l2cap_check_fcs(chan, skb)) goto drop; - if (__is_sar_start(control)) - len -= 2; + if (__is_sar_start(chan, control)) + len -= L2CAP_SDULEN_SIZE; if (chan->fcs == L2CAP_FCS_CRC16) - len -= 2; + len -= L2CAP_FCS_SIZE; - if (len > chan->mps || len < 0 || __is_sframe(control)) + if (len > chan->mps || len < 0 || __is_sframe(chan, control)) goto drop; - tx_seq = __get_txseq(control); + tx_seq = __get_txseq(chan, control); if (chan->expected_tx_seq != tx_seq) { /* Frame(s) missing - must discard partial SDU */ @@ -3789,7 +4285,7 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk /* TODO: Notify userland of missing data */ } - chan->expected_tx_seq = (tx_seq + 1) % 64; + chan->expected_tx_seq = __next_seq(chan, tx_seq); if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE) l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); @@ -3943,12 +4439,12 @@ static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { lm1 |= HCI_LM_ACCEPT; - if (c->role_switch) + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) lm1 |= HCI_LM_MASTER; exact++; } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { lm2 |= HCI_LM_ACCEPT; - if (c->role_switch) + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) lm2 |= HCI_LM_MASTER; } } @@ -3983,7 +4479,7 @@ static int l2cap_disconn_ind(struct hci_conn *hcon) BT_DBG("hcon %p", hcon); if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn) - return 0x13; + return HCI_ERROR_REMOTE_USER_TERM; return conn->disc_reason; } |