Skip to content

Commit

Permalink
Bluetooth: Add support for FCS option to L2CAP
Browse files Browse the repository at this point in the history 
Implement CRC16 check for L2CAP packets. FCS is used by Streaming Mode and
Enhanced Retransmission Mode and is a extra check for the packet content.

Using CRC16 is the default, L2CAP won't use FCS only when both side send
a "No FCS" request.

Initially based on a patch from Nathan Holstein <nathan@lampreynetworks.com>

Signed-off-by: Gustavo F. Padovan <gustavo@las.ic.unicamp.br>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
  • Loading branch information
Gustavo F. Padovan authored and Marcel Holtmann committed Aug 22, 2009
1 parent 6840ed0 commit fcc203c
Show file tree
Hide file tree
Showing 2 changed files with 97 additions and 6 deletions.
2 changes: 2 additions & 0 deletions include/net/bluetooth/l2cap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ struct l2cap_options {
__u16 imtu;
__u16 flush_to;
__u8 mode;
__u8 fcs;
};

#define L2CAP_CONNINFO 0x02
Expand Down Expand Up @@ -348,6 +349,7 @@ struct l2cap_pinfo {
#define L2CAP_CONF_MTU_DONE 0x08
#define L2CAP_CONF_MODE_DONE 0x10
#define L2CAP_CONF_CONNECT_PEND 0x20
#define L2CAP_CONF_NO_FCS_RECV 0x40
#define L2CAP_CONF_STATE2_DEVICE 0x80

#define L2CAP_CONF_MAX_CONF_REQ 2
Expand Down
101 changes: 95 additions & 6 deletions net/bluetooth/l2cap.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <linux/list.h>
#include <linux/device.h>
#include <linux/uaccess.h>
#include <linux/crc16.h>
#include <net/sock.h>

#include <asm/system.h>
Expand Down Expand Up @@ -338,22 +339,30 @@ static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
struct sk_buff *skb;
struct l2cap_hdr *lh;
struct l2cap_conn *conn = pi->conn;
int count;
int count, hlen = L2CAP_HDR_SIZE + 2;

if (pi->fcs == L2CAP_FCS_CRC16)
hlen += 2;

BT_DBG("pi %p, control 0x%2.2x", pi, control);

count = min_t(unsigned int, conn->mtu, L2CAP_HDR_SIZE + 2);
count = min_t(unsigned int, conn->mtu, hlen);
control |= L2CAP_CTRL_FRAME_TYPE;

skb = bt_skb_alloc(count, GFP_ATOMIC);
if (!skb)
return -ENOMEM;

lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
lh->len = cpu_to_le16(2);
lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
lh->cid = cpu_to_le16(pi->dcid);
put_unaligned_le16(control, skb_put(skb, 2));

if (pi->fcs == L2CAP_FCS_CRC16) {
u16 fcs = crc16(0, (u8 *)lh, count - 2);
put_unaligned_le16(fcs, skb_put(skb, 2));
}

return hci_send_acl(pi->conn->hcon, skb, 0);
}

Expand Down Expand Up @@ -1249,7 +1258,7 @@ static int l2cap_streaming_send(struct sock *sk)
{
struct sk_buff *skb, *tx_skb;
struct l2cap_pinfo *pi = l2cap_pi(sk);
u16 control;
u16 control, fcs;
int err;

while ((skb = sk->sk_send_head)) {
Expand All @@ -1259,6 +1268,11 @@ static int l2cap_streaming_send(struct sock *sk)
control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);

if (l2cap_pi(sk)->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);
}

err = l2cap_do_send(sk, tx_skb);
if (err < 0) {
l2cap_send_disconn_req(pi->conn, sk);
Expand All @@ -1282,7 +1296,7 @@ static int l2cap_ertm_send(struct sock *sk)
{
struct sk_buff *skb, *tx_skb;
struct l2cap_pinfo *pi = l2cap_pi(sk);
u16 control;
u16 control, fcs;
int err;

if (pi->conn_state & L2CAP_CONN_WAIT_F)
Expand All @@ -1305,6 +1319,11 @@ static int l2cap_ertm_send(struct sock *sk)
put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);


if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
}

err = l2cap_do_send(sk, tx_skb);
if (err < 0) {
l2cap_send_disconn_req(pi->conn, sk);
Expand Down Expand Up @@ -1428,6 +1447,9 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *m
if (sdulen)
hlen += 2;

if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
hlen += 2;

count = min_t(unsigned int, (conn->mtu - hlen), len);
skb = bt_skb_send_alloc(sk, count + hlen,
msg->msg_flags & MSG_DONTWAIT, &err);
Expand All @@ -1448,6 +1470,9 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *m
return ERR_PTR(err);
}

if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
put_unaligned_le16(0, skb_put(skb, 2));

bt_cb(skb)->retries = 0;
return skb;
}
Expand Down Expand Up @@ -1633,6 +1658,7 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
opts.omtu = l2cap_pi(sk)->omtu;
opts.flush_to = l2cap_pi(sk)->flush_to;
opts.mode = l2cap_pi(sk)->mode;
opts.fcs = l2cap_pi(sk)->fcs;

len = min_t(unsigned int, sizeof(opts), optlen);
if (copy_from_user((char *) &opts, optval, len)) {
Expand All @@ -1643,6 +1669,7 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
l2cap_pi(sk)->imtu = opts.imtu;
l2cap_pi(sk)->omtu = opts.omtu;
l2cap_pi(sk)->mode = opts.mode;
l2cap_pi(sk)->fcs = opts.fcs;
break;

case L2CAP_LM:
Expand Down Expand Up @@ -1756,6 +1783,7 @@ static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __us
opts.omtu = l2cap_pi(sk)->omtu;
opts.flush_to = l2cap_pi(sk)->flush_to;
opts.mode = l2cap_pi(sk)->mode;
opts.fcs = l2cap_pi(sk)->fcs;

len = min_t(unsigned int, len, sizeof(opts));
if (copy_to_user(optval, (char *) &opts, len))
Expand Down Expand Up @@ -2154,6 +2182,15 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)

l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
sizeof(rfc), (unsigned long) &rfc);

if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
break;

if (pi->fcs == L2CAP_FCS_NONE ||
pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
pi->fcs = L2CAP_FCS_NONE;
l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
}
break;

case L2CAP_MODE_STREAMING:
Expand All @@ -2166,6 +2203,15 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)

l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
sizeof(rfc), (unsigned long) &rfc);

if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
break;

if (pi->fcs == L2CAP_FCS_NONE ||
pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
pi->fcs = L2CAP_FCS_NONE;
l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
}
break;
}

Expand Down Expand Up @@ -2217,6 +2263,12 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
memcpy(&rfc, (void *) val, olen);
break;

case L2CAP_CONF_FCS:
if (val == L2CAP_FCS_NONE)
pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;

break;

default:
if (hint)
break;
Expand Down Expand Up @@ -2638,6 +2690,10 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
goto unlock;

if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV)
|| l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;

sk->sk_state = BT_CONNECTED;
l2cap_pi(sk)->next_tx_seq = 0;
l2cap_pi(sk)->expected_ack_seq = 0;
Expand Down Expand Up @@ -2722,6 +2778,10 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;

if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV)
|| l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;

sk->sk_state = BT_CONNECTED;
l2cap_pi(sk)->expected_tx_seq = 0;
l2cap_pi(sk)->num_to_ack = 0;
Expand Down Expand Up @@ -2809,7 +2869,8 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
if (enable_ertm)
feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
| L2CAP_FEAT_FCS;
put_unaligned(cpu_to_le32(feat_mask), (__le32 *) rsp->data);
l2cap_send_cmd(conn, cmd->ident,
L2CAP_INFO_RSP, sizeof(buf), buf);
Expand Down Expand Up @@ -2961,6 +3022,22 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
kfree_skb(skb);
}

static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct sk_buff *skb)
{
u16 our_fcs, rcv_fcs;
int hdr_size = L2CAP_HDR_SIZE + 2;

if (pi->fcs == L2CAP_FCS_CRC16) {
skb_trim(skb, skb->len - 2);
rcv_fcs = get_unaligned_le16(skb->data + skb->len);
our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);

if (our_fcs != rcv_fcs)
return -EINVAL;
}
return 0;
}

static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
Expand Down Expand Up @@ -3174,6 +3251,9 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
if (__is_sar_start(control))
len -= 2;

if (pi->fcs == L2CAP_FCS_CRC16)
len -= 2;

/*
* We can just drop the corrupted I-frame here.
* Receiver will miss it and start proper recovery
Expand All @@ -3182,6 +3262,9 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
if (len > L2CAP_DEFAULT_MAX_PDU_SIZE)
goto drop;

if (l2cap_check_fcs(pi, skb))
goto drop;

if (__is_iframe(control))
err = l2cap_data_channel_iframe(sk, control, skb);
else
Expand All @@ -3199,9 +3282,15 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
if (__is_sar_start(control))
len -= 2;

if (pi->fcs == L2CAP_FCS_CRC16)
len -= 2;

if (len > L2CAP_DEFAULT_MAX_PDU_SIZE || __is_sframe(control))
goto drop;

if (l2cap_check_fcs(pi, skb))
goto drop;

tx_seq = __get_txseq(control);

if (pi->expected_tx_seq == tx_seq)
Expand Down

0 comments on commit fcc203c

Please sign in to comment.