From: Andrei Emeltchenko Date: Mon, 17 Oct 2011 09:19:56 +0000 (+0300) Subject: Bluetooth: EWS: handling different Control fields X-Git-Tag: v3.3-rc1~182^2~545^2~134^2~61 X-Git-Url: https://openfabrics.org/gitweb/?a=commitdiff_plain;h=88843ab06b6f279bff1c32e4218541ac7efe2600;p=~emulex%2Finfiniband.git Bluetooth: EWS: handling different Control fields There are three different Control Field formats: the Standard Control Field, the Enhanced Control Field, and the Extended Control Field. Patch adds function to handle all those fields seamlessly. Signed-off-by: Andrei Emeltchenko Signed-off-by: Gustavo F. Padovan --- diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h index e67ecd12df8..41f0906649e 100644 --- a/include/net/bluetooth/l2cap.h +++ b/include/net/bluetooth/l2cap.h @@ -27,6 +27,8 @@ #ifndef __L2CAP_H #define __L2CAP_H +#include + /* L2CAP defaults */ #define L2CAP_DEFAULT_MTU 672 #define L2CAP_DEFAULT_MIN_MTU 48 @@ -684,6 +686,32 @@ static inline bool __is_ctrl_poll(struct l2cap_chan *chan, __u32 ctrl) else return ctrl & L2CAP_CTRL_POLL; } + +static inline __u32 __get_control(struct l2cap_chan *chan, void *p) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + return get_unaligned_le32(p); + else + return get_unaligned_le16(p); +} + +static inline void __put_control(struct l2cap_chan *chan, __u32 control, + void *p) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + return put_unaligned_le32(control, p); + else + return put_unaligned_le16(control, p); +} + +static inline __u8 __ctrl_size(struct l2cap_chan *chan) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + return L2CAP_EXT_HDR_SIZE - L2CAP_HDR_SIZE; + else + return L2CAP_ENH_HDR_SIZE - L2CAP_HDR_SIZE; +} + extern int disable_ertm; int l2cap_init_sockets(void); diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index 22133464d65..a25394231b4 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -568,7 +568,7 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, hci_send_acl(conn->hcon, 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; @@ -587,7 +587,7 @@ static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) if (chan->fcs == L2CAP_FCS_CRC16) hlen += 2; - 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); @@ -606,7 +606,8 @@ 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); @@ -623,7 +624,7 @@ static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) hci_send_acl(chan->conn->hcon, skb, flags); } -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 |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); @@ -1279,12 +1280,13 @@ static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) 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 = __get_control(chan, skb->data + L2CAP_HDR_SIZE); control |= __set_txseq(chan, chan->next_tx_seq); - put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); + __put_control(chan, control, skb->data + L2CAP_HDR_SIZE); if (chan->fcs == L2CAP_FCS_CRC16) { fcs = crc16(0, (u8 *)skb->data, skb->len - 2); @@ -1300,7 +1302,8 @@ static void l2cap_streaming_send(struct l2cap_chan *chan) 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) @@ -1323,7 +1326,8 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq) tx_skb = skb_clone(skb, GFP_ATOMIC); bt_cb(skb)->retries++; - control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); + + 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)) @@ -1332,7 +1336,7 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq) 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); @@ -1345,7 +1349,8 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 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) @@ -1363,7 +1368,7 @@ static int l2cap_ertm_send(struct l2cap_chan *chan) bt_cb(skb)->retries++; - control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); + 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)) @@ -1371,8 +1376,8 @@ static int l2cap_ertm_send(struct l2cap_chan *chan) control |= __set_reqseq(chan, chan->buffer_seq); control |= __set_txseq(chan, chan->next_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 *)skb->data, tx_skb->len - 2); @@ -1416,7 +1421,7 @@ 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 |= __set_reqseq(chan, chan->buffer_seq); @@ -1437,7 +1442,7 @@ static void l2cap_send_ack(struct l2cap_chan *chan) static void l2cap_send_srejtail(struct l2cap_chan *chan) { struct srej_list *tail; - u16 control; + u32 control; control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); control |= __set_ctrl_final(chan); @@ -1541,7 +1546,7 @@ 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; @@ -1575,7 +1580,9 @@ 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)); @@ -1596,7 +1603,7 @@ 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); @@ -1640,7 +1647,7 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) { struct sk_buff *skb; - u16 control; + u32 control; int err; /* Connectionless channel */ @@ -3180,7 +3187,7 @@ 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; @@ -3265,7 +3272,7 @@ 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; @@ -3348,7 +3355,7 @@ 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); @@ -3365,7 +3372,7 @@ 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; @@ -3401,7 +3408,7 @@ void l2cap_chan_busy(struct l2cap_chan *chan, int busy) 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)) { @@ -3428,7 +3435,7 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 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) { @@ -3447,7 +3454,7 @@ static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq) static void 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 = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); @@ -3462,7 +3469,7 @@ static void l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq) chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; } -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) { u16 tx_seq = __get_txseq(chan, rx_control); u16 req_seq = __get_reqseq(chan, rx_control); @@ -3471,7 +3478,7 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont 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 (__is_ctrl_final(chan, rx_control) && @@ -3597,9 +3604,9 @@ 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, + 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(chan, rx_control); @@ -3637,11 +3644,11 @@ 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) { 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); @@ -3658,11 +3665,11 @@ 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) { 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); @@ -3694,11 +3701,11 @@ 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) { 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; @@ -3722,9 +3729,9 @@ static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_c } } -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 (__is_ctrl_final(chan, rx_control) && test_bit(CONN_WAIT_F, &chan->conn_state)) { @@ -3759,12 +3766,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; + 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; /* @@ -3830,7 +3837,7 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk { struct l2cap_chan *chan; struct sock *sk = NULL; - u16 control; + u32 control; u16 tx_seq; int len; @@ -3872,8 +3879,8 @@ 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))