2009-04-30 21:16:37

by Nathan Holstein

[permalink] [raw]
Subject: Revamped Enhanced Retransmission mode L2CAP support


This is a reworking of the previous patchset I submitted. This includes fixes
from Marcel's comments.

include/net/bluetooth/bluetooth.h | 3
include/net/bluetooth/l2cap.h | 101 +++++-
net/bluetooth/l2cap.c | 628 ++++++++++++++++++++++++++++++++++----
3 files changed, 665 insertions(+), 67 deletions(-)



--Nathan


2009-04-30 21:16:47

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 10/10] Allow opening an L2CAP socket with type SOCK_STREAM

From: Nathan Holstein <[email protected]>

This patch enables use of Enhanced Retransmission and Streaming modes on an
L2CAP socket. If a socket is created with SOCK_STREAM, it will be treated as
an Enhanced Retransmission mode L2CAP connection.

Signed-off-by: Nathan Holstein <[email protected]>
---
net/bluetooth/l2cap.c | 34 +++++++++++++++++++++-------------
1 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index b7c2e94..85493ea 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -53,7 +53,11 @@

#define VERSION "2.13"

-static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_FCS;
+static u32 l2cap_feat_mask =
+ L2CAP_FEAT_FCS |
+ L2CAP_FEAT_ERTM |
+ L2CAP_FEAT_STREAM |
+ L2CAP_FEAT_FIXED_CHAN;
static u8 l2cap_fixed_chan[8] = { 0x02, };

static const struct proto_ops l2cap_sock_ops;
@@ -215,7 +219,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct so

l2cap_pi(sk)->conn = conn;

- if (sk->sk_type == SOCK_SEQPACKET) {
+ if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
/* Alloc CID for connection-oriented socket */
l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
} else if (sk->sk_type == SOCK_DGRAM) {
@@ -391,7 +395,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
bh_lock_sock(sk);

- if (sk->sk_type != SOCK_SEQPACKET) {
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) {
bh_unlock_sock(sk);
continue;
}
@@ -451,7 +455,7 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
bh_lock_sock(sk);

- if (sk->sk_type != SOCK_SEQPACKET) {
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) {
l2cap_sock_clear_timer(sk);
sk->sk_state = BT_CONNECTED;
sk->sk_state_change(sk);
@@ -660,7 +664,7 @@ static void __l2cap_sock_close(struct sock *sk, int reason)

case BT_CONNECTED:
case BT_CONFIG:
- if (sk->sk_type == SOCK_SEQPACKET) {
+ if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
struct l2cap_disconn_req req;

@@ -676,7 +680,7 @@ static void __l2cap_sock_close(struct sock *sk, int reason)
break;

case BT_CONNECT2:
- if (sk->sk_type == SOCK_SEQPACKET) {
+ if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
struct l2cap_conn_rsp rsp;
__u16 result;
@@ -794,7 +798,7 @@ static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol)

sock->state = SS_UNCONNECTED;

- if (sock->type != SOCK_SEQPACKET &&
+ if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM &&
sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
return -ESOCKTNOSUPPORT;

@@ -941,7 +945,7 @@ static int l2cap_do_connect(struct sock *sk)
l2cap_sock_set_timer(sk, sk->sk_sndtimeo);

if (hcon->state == BT_CONNECTED) {
- if (sk->sk_type != SOCK_SEQPACKET) {
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) {
l2cap_sock_clear_timer(sk);
sk->sk_state = BT_CONNECTED;
} else
@@ -974,7 +978,7 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al

lock_sock(sk);

- if (sk->sk_type == SOCK_SEQPACKET && !la.l2_psm) {
+ if ((sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) && !la.l2_psm) {
err = -EINVAL;
goto done;
}
@@ -1025,7 +1029,9 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)

lock_sock(sk);

- if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
+ if (sk->sk_state != BT_BOUND
+ || (sock->type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
+ || sock->type != SOCK_STREAM) {
err = -EBADFD;
goto done;
}
@@ -1436,7 +1442,8 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch

switch (optname) {
case BT_SECURITY:
- if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
+ && sk->sk_type != SOCK_RAW) {
err = -EINVAL;
break;
}
@@ -1583,7 +1590,8 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch

switch (optname) {
case BT_SECURITY:
- if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
+ && sk->sk_type != SOCK_RAW) {
err = -EINVAL;
break;
}
@@ -3029,7 +3037,7 @@ static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)

static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
{
- if (sk->sk_type != SOCK_SEQPACKET)
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
return;

if (encrypt == 0x00) {
--
1.6.0.6


2009-04-30 21:16:46

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 09/10] Add support for negotiating new L2CAP options after a failed configuration

From: Nathan Holstein <[email protected]>

When negotiating the configuration of an L2CAP socket, the remote may reject
the initial configuration that was sent. This patch adds allows selecting
differing configuration values.

Signed-off-by: Nathan Holstein <[email protected]>
---
net/bluetooth/l2cap.c | 106 ++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 97 insertions(+), 9 deletions(-)

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 228f7f0..b7c2e94 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -2012,6 +2012,70 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
return ptr - data;
}

+static inline int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *_req, u16 *result)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct l2cap_conf_req *req = _req;
+ void *ptr = req->data;
+ int type, hint, olen;
+ unsigned long val;
+ struct l2cap_conf_rfc rfc;
+
+ BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, _req);
+
+ while (len >= L2CAP_CONF_OPT_SIZE) {
+ len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
+
+ hint = type & L2CAP_CONF_HINT;
+ type &= 0x7f;
+
+ switch (type) {
+ case L2CAP_CONF_MTU:
+ if (val < L2CAP_DEFAULT_MIN_MTU)
+ {
+ *result = L2CAP_CONF_UNACCEPT;
+ pi->omtu = L2CAP_DEFAULT_MIN_MTU;
+ }
+ else
+ pi->omtu = val;
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
+ break;
+
+ case L2CAP_CONF_FLUSH_TO:
+ pi->flush_to = val;
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2,
+ pi->flush_to);
+ break;
+
+ case L2CAP_CONF_QOS:
+ break;
+
+ case L2CAP_CONF_RFC:
+ /* Currently, the code does not support negotiating to
+ * use a different mode than was initially created. This
+ * can be used for switching between ERTM/Streaming, and
+ * will be useful for implementing MCAP/HDP on top of
+ * eL2CAP. */
+ *result = L2CAP_CONF_UNACCEPT;
+ if (pi->mode == L2CAP_MODE_BASIC)
+ pi->fcs = 0;
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+ sizeof(rfc), (unsigned long) &rfc);
+ break;
+
+ case L2CAP_CONF_FCS:
+ pi->fcs = val;
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
+ }
+ }
+
+ req->dcid = cpu_to_le16(pi->dcid);
+ req->flags = cpu_to_le16(0x0000);
+
+ BT_DBG("ptr %p, req->data %p", ptr, req->data);
+ return ptr - _req;
+}
+
static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
{
struct l2cap_conf_rsp *rsp = data;
@@ -2233,6 +2297,15 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
if (sk->sk_state == BT_DISCONN)
goto unlock;

+ /* Reject if connected and in eL2CAP mode. */
+ if (sk->sk_state == BT_CONNECTED && l2cap_pi(sk)->mode != L2CAP_MODE_BASIC)
+ {
+ l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
+ l2cap_build_conf_rsp(sk, rsp,
+ L2CAP_CONF_REJECT, flags), rsp);
+ goto unlock;
+ }
+
/* Reject if config buffer is too small. */
len = cmd_len - sizeof(*req);
if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) {
@@ -2301,21 +2374,36 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
if (!sk)
return 0;

+ /* Reject if connected and in eL2CAP mode. */
+ if (sk->sk_state == BT_CONNECTED && l2cap_pi(sk)->mode != L2CAP_MODE_BASIC)
+ {
+ l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
+ l2cap_build_conf_rsp(sk, rsp,
+ L2CAP_CONF_REJECT, flags), rsp);
+ goto done;
+ }
+
switch (result) {
case L2CAP_CONF_SUCCESS:
break;

case L2CAP_CONF_UNACCEPT:
if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
- char req[128];
- /* It does not make sense to adjust L2CAP parameters
- * that are currently defined in the spec. We simply
- * resend config request that we sent earlier. It is
- * stupid, but it helps qualification testing which
- * expects at least some response from us. */
- l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
- l2cap_build_conf_req(sk, req), req);
- goto done;
+ int len = cmd->len - sizeof(*rsp);
+ char req[64];
+
+ /* throw out any old conf requests we stored */
+ result = L2CAP_CONF_SUCCESS;
+ len = l2cap_parse_conf_rsp(sk, rsp->data, len, req,
+ &result);
+
+ BT_DBG("len %d", len);
+ l2cap_send_cmd(conn, l2cap_get_ident(conn),
+ L2CAP_CONF_REQ, len, req);
+ if (result == L2CAP_CONF_SUCCESS)
+ break;
+ else
+ goto done;
}

default:
--
1.6.0.6


2009-04-30 21:16:45

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 08/10] Reassemble segmented L2CAP PDUs upon reception

From: Nathan Holstein <[email protected]>

When configured in Enhanced Retransmission of Streaming modes, data sent over
L2CAP can be broken up into multiple L2CAP SDUs. This patch adds support for
reassembling these packets upon reception.

Signed-off-by: Nathan Holstein <[email protected]>
---
include/net/bluetooth/l2cap.h | 3 ++
net/bluetooth/l2cap.c | 53 +++++++++++++++++++++++++++++++++++++++++
2 files changed, 56 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 85fc21b..44e6e92 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -316,6 +316,9 @@ struct l2cap_pinfo {
__u16 monitor_timeout;
__u16 max_pdu_size;

+ __u16 sdu_next;
+ __u16 sdu_len;
+
__le16 sport;

struct sk_buff *tx_buf;
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index ad96ce7..228f7f0 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -2591,6 +2591,43 @@ static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct l2cap_hdr *lh, struct
return 0;
}

+static int l2cap_check_sar(struct l2cap_pinfo *pi, u16 control, struct sk_buff *skb)
+{
+ u16 mask = control & L2CAP_CONTROL_SAR_MASK;
+
+ switch (mask)
+ {
+ case L2CAP_SAR_UNSEGMENTED:
+ if (pi->sdu_next != 0)
+ return -1;
+ break;
+
+ case L2CAP_SAR_SDU_START:
+ if (pi->sdu_next != 0 || skb->len < 2)
+ return -1;
+ pi->sdu_next = 1;
+ pi->sdu_len = get_unaligned((__le16 *) skb->data);
+ skb_pull(skb, 2);
+ break;
+
+ case L2CAP_SAR_SDU_CONTINUE:
+ if (pi->sdu_next >= pi->sdu_len)
+ return -1;
+ ++pi->sdu_next;
+ break;
+
+ case L2CAP_SAR_SDU_END:
+ /* TODO:
+ * How do we correctly signal MSG_EOR? */
+ if (pi->sdu_next != pi->sdu_len)
+ return -1;
+ pi->sdu_next = pi->sdu_len = 0;
+ break;
+ }
+
+ return 0;
+}
+
static inline int l2cap_data_channel_i_frame(struct sock *sk, u16 rx_control, struct sk_buff *skb)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
@@ -2610,6 +2647,17 @@ static inline int l2cap_data_channel_i_frame(struct sock *sk, u16 rx_control, st
L2CAP_SEQ_NUM_INC(pi->req_seq);
tx_control = pi->req_seq << L2CAP_CONTROL_REQSEQ_SHIFT;

+ /* TODO:
+ * l2cap_check_sar() call side effects! After calling this function,
+ * we can't reject the SDU without possibly screwing up reassembly.
+ * We need to ensure the sequence number is correct, and that we can
+ * queue the skb before calling l2cap_check_sar(). */
+ if (l2cap_check_sar(pi, rx_control, skb)) {
+ tx_control |= L2CAP_SUPER_REJECT;
+ err = -1;
+ goto respond;
+ }
+
if ((err = sock_queue_rcv_skb(sk, skb))) {
tx_control |= L2CAP_SUPER_RCV_NOT_READY;
goto respond;
@@ -2690,6 +2738,11 @@ static int l2cap_data_channel_streaming(struct sock *sk, struct l2cap_hdr *lh, s
control = get_unaligned((__le16 *) skb->data);
skb_pull(skb, 2);

+ /* Todo:
+ * is this the proper behavior? */
+ if (l2cap_is_I_frame(control) && l2cap_check_sar(pi, control, skb))
+ return -1;
+
if (!sock_queue_rcv_skb(sk, skb))
return -1;

--
1.6.0.6


2009-04-30 21:16:44

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 07/10] Check the FCS of a received L2CAP packet.

From: Nathan Holstein <[email protected]>

When in Enhanced Retransmission or Streaming modes, L2CAP supports an optional
16 bit CRC to validate a packet. This patch checks incoming SDUs for a correct
FCS.

Signed-off-by: Nathan Holstein <[email protected]>
---
net/bluetooth/l2cap.c | 46 +++++++++++++++++++++++++++++++++++++++++++++-
1 files changed, 45 insertions(+), 1 deletions(-)

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index b2c8dd7..ad96ce7 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -53,7 +53,7 @@

#define VERSION "2.13"

-static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
+static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_FCS;
static u8 l2cap_fixed_chan[8] = { 0x02, };

static const struct proto_ops l2cap_sock_ops;
@@ -2563,6 +2563,34 @@ 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 l2cap_hdr *lh, struct sk_buff *skb)
+{
+ u16 our_fcs = 0;
+ u16 rcv_fcs;
+
+ if (lh->len != skb->len)
+ return -1;
+
+ if (pi->fcs)
+ {
+ if (lh->len < 4)
+ return -1;
+
+ /* Remove the FCS bytes from data */
+ skb->len -= 2;
+ rcv_fcs = get_unaligned((__le16 *) skb->data + skb->len);
+ /* Include the header in the FCS */
+ our_fcs = crc16(0, skb->data - 4, skb->len);
+
+ if (our_fcs != rcv_fcs)
+ return 0;
+ }
+ else if (lh->len < 2)
+ return -1;
+
+ return 0;
+}
+
static inline int l2cap_data_channel_i_frame(struct sock *sk, u16 rx_control, struct sk_buff *skb)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
@@ -2620,6 +2648,19 @@ static inline int l2cap_data_channel_enhanced(struct sock *sk, struct l2cap_hdr

BT_DBG("sk %p skb %p", sk, skb);

+ if (l2cap_check_fcs(pi, lh, skb))
+ {
+ BT_DBG("failed fcs");
+ control = pi->req_seq | L2CAP_CONTROL_TYPE_MASK |
+ L2CAP_SUPER_REJECT;
+ if (!l2cap_send_sframe(pi, control))
+ {
+ /* TODO:
+ * set an error state, disconnect */
+ }
+ return -1;
+ }
+
control = get_unaligned((__le16 *) skb->data);
skb_pull(skb, 2);

@@ -2643,6 +2684,9 @@ static int l2cap_data_channel_streaming(struct sock *sk, struct l2cap_hdr *lh, s
struct l2cap_pinfo *pi = l2cap_pi(sk);
u16 control;

+ if (l2cap_check_fcs(pi, lh, skb))
+ return -1;
+
control = get_unaligned((__le16 *) skb->data);
skb_pull(skb, 2);

--
1.6.0.6


2009-04-30 21:16:41

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 04/10] Append RFC option when configuring an L2CAP socket.

From: Nathan Holstein <[email protected]>

When negotiating Enhanced Retransmission or Streaming modes, append our RFC
option to the configuration list.

Signed-off-by: Nathan Holstein <[email protected]>
---
net/bluetooth/l2cap.c | 24 ++++++++++++++++++++++++
1 files changed, 24 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 7df307f..660a433 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -1863,6 +1863,7 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
struct l2cap_conf_req *req = data;
+ struct l2cap_conf_rfc rfc = { 0, };
void *ptr = req->data;

BT_DBG("sk %p", sk);
@@ -1870,6 +1871,29 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)
if (pi->imtu != L2CAP_DEFAULT_MTU)
l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);

+ switch (pi->mode) {
+ case L2CAP_MODE_ERTM:
+ rfc.mode = L2CAP_MODE_ERTM;
+ rfc.txwin_size = L2CAP_DEFAULT_RX_WINDOW;
+ rfc.max_transmit = L2CAP_DEFAULT_MAX_RECEIVE;
+ rfc.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
+ rfc.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
+ rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_RX_APDU);
+ break;
+
+ case L2CAP_MODE_STREAM:
+ rfc.mode = L2CAP_MODE_STREAM;
+ rfc.txwin_size = 0;
+ rfc.max_transmit = 0;
+ rfc.retrans_timeout = 0;
+ rfc.monitor_timeout = 0;
+ rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_RX_APDU);
+ break;
+ }
+ if (rfc.mode)
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
+ (unsigned long) &rfc);
+
/* FIXME: Need actual value of the flush timeout */
//if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
// l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
--
1.6.0.6


2009-04-30 21:16:43

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 06/10] Add support for receiving data over an enhanced L2CAP channel

From: Nathan Holstein <n[email protected]>

Receiving data over L2CAP socket depends upon the mode of the connection. In
Basic mode, no processing needs to be done. In Enhanced Retransmission and
Streaming modes there are two types of packets: information and supervisory.

This patch splits the control path of l2cap_data_channel() accordingly.

Signed-off-by: Nathan Holstein <[email protected]>
---
include/net/bluetooth/l2cap.h | 30 ++++++++++-
net/bluetooth/l2cap.c | 124 ++++++++++++++++++++++++++++++++++++++---
2 files changed, 145 insertions(+), 9 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 9221c4a..85fc21b 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -327,7 +327,7 @@ struct l2cap_pinfo {
#define L2CAP_CONF_REQ_SENT 0x01
#define L2CAP_CONF_INPUT_DONE 0x02
#define L2CAP_CONF_MTU_DONE 0x04
-#define L2CAP_CONF_MODE_DONE 0x04
+#define L2CAP_CONF_MODE_DONE 0x08
#define L2CAP_CONF_CONNECT_PEND 0x80
#define L2CAP_CONF_OUTPUT_DONE(state) (\
((state) & L2CAP_CONF_MTU_DONE) && \
@@ -335,6 +335,34 @@ struct l2cap_pinfo {

#define L2CAP_CONF_MAX_RETRIES 2

+static inline u8 l2cap_control_txseq(u16 control)
+{
+ return (control & L2CAP_CONTROL_TXSEQ_MASK) >>
+ L2CAP_CONTROL_TXSEQ_SHIFT;
+}
+
+static inline u8 l2cap_control_reqseq(u16 control)
+{
+ return (control & L2CAP_CONTROL_REQSEQ_MASK) >>
+ L2CAP_CONTROL_REQSEQ_SHIFT;
+}
+
+static inline u16 l2cap_txseq_to_reqseq(u16 control)
+{
+ return (control & L2CAP_CONTROL_TXSEQ_MASK) <<
+ (L2CAP_CONTROL_REQSEQ_SHIFT - L2CAP_CONTROL_TXSEQ_SHIFT);
+}
+
+static inline int l2cap_is_I_frame(u16 control)
+{
+ return !(control & L2CAP_CONTROL_TYPE_MASK);
+}
+
+static inline int l2cap_is_S_frame(u16 control)
+{
+ return (control & L2CAP_CONTROL_TYPE_MASK);
+}
+
void l2cap_load(void);

#endif /* __L2CAP_H */
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 423999e..b2c8dd7 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -2563,7 +2563,96 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
kfree_skb(skb);
}

-static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
+static inline int l2cap_data_channel_i_frame(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u8 tx_seq = l2cap_control_txseq(rx_control);
+ u16 tx_control;
+ int err = 0;
+
+ BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+ if (tx_seq != pi->req_seq) {
+ tx_control = pi->req_seq << L2CAP_CONTROL_REQSEQ_SHIFT;
+ tx_control |= L2CAP_SUPER_SELECT_REJECT;
+ err = -1;
+ goto respond;
+ }
+
+ L2CAP_SEQ_NUM_INC(pi->req_seq);
+ tx_control = pi->req_seq << L2CAP_CONTROL_REQSEQ_SHIFT;
+
+ if ((err = sock_queue_rcv_skb(sk, skb))) {
+ tx_control |= L2CAP_SUPER_RCV_NOT_READY;
+ goto respond;
+ }
+
+ tx_control |= L2CAP_SUPER_RCV_READY;
+
+respond:
+ if (!l2cap_send_sframe(pi, tx_control)) {
+ /* TODO:
+ * set an error state, disconnect */
+ return -1;
+ }
+
+ return err;
+}
+
+static inline int l2cap_data_channel_s_frame(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+ u16 tx_control;
+ BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+ if (rx_control & L2CAP_CONTROL_POLL_MASK) {
+ tx_control = rx_control & L2CAP_CONTROL_REQSEQ_MASK;
+ tx_control |= L2CAP_CONTROL_FINAL_MASK;
+ return l2cap_send_sframe(l2cap_pi(sk), tx_control);
+ }
+
+ return -1;
+}
+
+static inline int l2cap_data_channel_enhanced(struct sock *sk, struct l2cap_hdr *lh, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u16 control;
+
+ BT_DBG("sk %p skb %p", sk, skb);
+
+ control = get_unaligned((__le16 *) skb->data);
+ skb_pull(skb, 2);
+
+ if (l2cap_control_txseq(control) != pi->req_seq)
+ {
+ BT_DBG("wrong tx seq");
+ /* TODO:
+ * once we increase our tx window, we can queue this. For now,
+ * just silently drop it. */
+ //return -1;
+ }
+
+ if (l2cap_is_I_frame(control))
+ return l2cap_data_channel_i_frame(sk, control, skb);
+ else
+ return l2cap_data_channel_s_frame(sk, control, skb);
+}
+
+static int l2cap_data_channel_streaming(struct sock *sk, struct l2cap_hdr *lh, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u16 control;
+
+ control = get_unaligned((__le16 *) skb->data);
+ skb_pull(skb, 2);
+
+ if (!sock_queue_rcv_skb(sk, skb))
+ return -1;
+
+ return 0;
+}
+
+static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct l2cap_hdr *lh, struct sk_buff *skb)
{
struct sock *sk;

@@ -2581,13 +2670,32 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
if (l2cap_pi(sk)->imtu < skb->len)
goto drop;

- /* If socket recv buffers overflows we drop data here
- * which is *bad* because L2CAP has to be reliable.
- * But we don't have any other choice. L2CAP doesn't
- * provide flow control mechanism. */
+ switch (l2cap_pi(sk)->mode)
+ {
+ case L2CAP_MODE_BASIC:
+ /* If socket recv buffers overflows we drop data here
+ * which is *bad* because L2CAP has to be reliable.
+ * But we don't have any other choice. L2CAP doesn't
+ * provide flow control mechanism. */

- if (!sock_queue_rcv_skb(sk, skb))
- goto done;
+ if (!sock_queue_rcv_skb(sk, skb))
+ goto done;
+ break;
+
+ case L2CAP_MODE_ERTM:
+ if (!l2cap_data_channel_enhanced(sk, lh, skb));
+ goto done;
+ break;
+
+ case L2CAP_MODE_STREAM:
+ if (!l2cap_data_channel_streaming(sk, lh, skb));
+ goto done;
+ break;
+
+ default:
+ BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
+ break;
+ }

drop:
kfree_skb(skb);
@@ -2651,7 +2759,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
break;

default:
- l2cap_data_channel(conn, cid, skb);
+ l2cap_data_channel(conn, cid, lh, skb);
break;
}
}
--
1.6.0.6


2009-04-30 21:16:40

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 03/10] Add support for sending L2CAP S-frames.

From: Nathan Holstein <[email protected]>

This patch creates the function l2cap_send_sframe() which builds a sk_buff
contains the specified control field and then sends the frame over the ACL
connection.

Signed-off-by: Nathan Holstein <[email protected]>
---
net/bluetooth/l2cap.c | 45 +++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 45 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 7255c97..7df307f 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -68,6 +68,7 @@ static void l2cap_sock_kill(struct sock *sk);

static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
u8 code, u8 ident, u16 dlen, void *data);
+static struct sk_buff *l2cap_build_sframe(struct l2cap_pinfo *pi, u16 control);

/* ---- L2CAP timers ---- */
static void l2cap_sock_timeout(unsigned long arg)
@@ -332,6 +333,18 @@ static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16
return hci_send_acl(conn->hcon, skb, 0);
}

+static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
+{
+ struct sk_buff *skb = l2cap_build_sframe(pi, control);
+
+ BT_DBG("control 0x%2.2x", control);
+
+ if (!skb)
+ return -ENOMEM;
+
+ return hci_send_acl(pi->conn->hcon, skb, 0);
+}
+
static void l2cap_do_start(struct sock *sk)
{
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
@@ -1751,6 +1764,38 @@ fail:
return NULL;
}

+static struct sk_buff *l2cap_build_sframe(struct l2cap_pinfo *pi, u16 control)
+{
+ struct sk_buff *skb;
+ struct l2cap_hdr *lh;
+ int len;
+
+ BT_DBG("pi %p, control 0x%2.2x", pi, control);
+
+ len = L2CAP_HDR_SIZE + 2;
+ control |= L2CAP_CONTROL_TYPE_MASK;
+
+ if (pi->fcs);
+ len += 2;
+
+ skb = bt_skb_alloc(len, GFP_ATOMIC);
+ if (!skb)
+ return NULL;
+
+ lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+ lh->len = cpu_to_le16(pi->fcs ? 4 : 2);
+ lh->cid = cpu_to_le16(pi->dcid);
+
+ put_unaligned(control, (__le16 *)skb_put(skb, 2));
+ if (pi->fcs)
+ {
+ u16 fcs = crc16(0, (u8 *)lh, len);
+ put_unaligned(fcs, (__le16 *)skb_put(skb, 2));
+ }
+
+ return skb;
+}
+
static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
{
struct l2cap_conf_opt *opt = *ptr;
--
1.6.0.6


2009-04-30 21:16:42

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 05/10] Add support for parsing enhanced L2CAP configuration options.

From: Nathan Holstein <[email protected]>

When negotiating for either Enhanced Retransmission or Streaming modes, both
sides of the connection must agree upon the configuration. This patch adds
support for parsing and validating the configuration receiverd from the remote.

Signed-off-by: Nathan Holstein <[email protected]>
---
include/net/bluetooth/l2cap.h | 12 ++++++++-
net/bluetooth/l2cap.c | 55 +++++++++++++++++++++++++++++++---------
2 files changed, 53 insertions(+), 14 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index e67b1ff..9221c4a 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -310,6 +310,12 @@ struct l2cap_pinfo {

__u8 ident;

+ __u8 remote_tx_win;
+ __u8 remote_max_tx;
+ __u16 retrans_timeout;
+ __u16 monitor_timeout;
+ __u16 max_pdu_size;
+
__le16 sport;

struct sk_buff *tx_buf;
@@ -320,8 +326,12 @@ struct l2cap_pinfo {

#define L2CAP_CONF_REQ_SENT 0x01
#define L2CAP_CONF_INPUT_DONE 0x02
-#define L2CAP_CONF_OUTPUT_DONE 0x04
+#define L2CAP_CONF_MTU_DONE 0x04
+#define L2CAP_CONF_MODE_DONE 0x04
#define L2CAP_CONF_CONNECT_PEND 0x80
+#define L2CAP_CONF_OUTPUT_DONE(state) (\
+ ((state) & L2CAP_CONF_MTU_DONE) && \
+ ((state) & L2CAP_CONF_MODE_DONE))

#define L2CAP_CONF_MAX_RETRIES 2

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 660a433..423999e 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -747,6 +747,9 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
pi->mode = L2CAP_MODE_BASIC;
}

+ if (pi->mode == L2CAP_MODE_BASIC)
+ pi->conf_state |= L2CAP_CONF_MODE_DONE;
+
/* Default config options */
pi->conf_len = 0;
pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
@@ -1942,6 +1945,10 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
memcpy(&rfc, (void *) val, olen);
break;

+ case L2CAP_CONF_FCS:
+ pi->fcs = val;
+ break;
+
default:
if (hint)
break;
@@ -1952,27 +1959,49 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
}
}

+ if (pi->mode != rfc.mode) {
+ result = L2CAP_CONF_UNACCEPT;
+ rfc.mode = pi->mode;
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+ 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. */
+ if (mtu < L2CAP_DEFAULT_MIN_MTU)
+ result = L2CAP_CONF_UNACCEPT;
+ else {
+ pi->omtu = mtu;
+ pi->conf_state |= L2CAP_CONF_MTU_DONE;
+ }
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);

- if (rfc.mode == L2CAP_MODE_BASIC) {
- if (mtu < pi->omtu)
- result = L2CAP_CONF_UNACCEPT;
- else {
- pi->omtu = mtu;
- pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
- }
+ switch (rfc.mode) {
+ case L2CAP_MODE_BASIC:
+ pi->fcs = 0;
+ pi->conf_state |= L2CAP_CONF_MODE_DONE;
+ break;

- l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
- } else {
+ case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAM:
+ pi->remote_tx_win = rfc.txwin_size;
+ pi->remote_max_tx = rfc.max_transmit;
+ pi->retrans_timeout = rfc.retrans_timeout;
+ pi->monitor_timeout = rfc.monitor_timeout;
+ pi->max_pdu_size = rfc.max_pdu_size;
+
+ pi->conf_state |= L2CAP_CONF_MODE_DONE;
+ break;
+
+ default:
result = L2CAP_CONF_UNACCEPT;

memset(&rfc, 0, sizeof(rfc));
- rfc.mode = L2CAP_MODE_BASIC;
+ rfc.mode = pi->mode;

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

@@ -2235,7 +2264,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
/* Reset config buffer. */
l2cap_pi(sk)->conf_len = 0;

- if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE))
+ if (!L2CAP_CONF_OUTPUT_DONE(l2cap_pi(sk)->conf_state))
goto unlock;

if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
@@ -2308,7 +2337,7 @@ 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_CONF_OUTPUT_DONE(l2cap_pi(sk)->conf_state)) {
sk->sk_state = BT_CONNECTED;
l2cap_chan_ready(sk);
}
--
1.6.0.6


2009-04-30 21:16:39

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 02/10] Add support for sending enhanced L2CAP data

From: Nathan Holstein <[email protected]>

When functioning in Enhanced Retransmission or Streaming mode, the complexity
of an L2CAP SDU is greatly increased. This patch splits the control stream of
the original l2cap_sock_sendmsg() function dependent upon the socket's mode.

Previously, l2cap_do_send() did the work of copying data from an iovec into a
sk_buff. Since both Basic and Enhanced modes need this functionality, a new
function, l2cap_skbuff_fromiovec() was added to do this work.

Signed-off-by: Nathan Holstein <[email protected]>
---
include/net/bluetooth/bluetooth.h | 3 +-
include/net/bluetooth/l2cap.h | 8 ++
net/bluetooth/l2cap.c | 139 ++++++++++++++++++++++++++++++++----
3 files changed, 133 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 5d005e6..a18b2d1 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -144,8 +144,9 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock);
struct bt_skb_cb {
__u8 pkt_type;
__u8 incoming;
+ __u8 tx_seq;
};
-#define bt_cb(skb) ((struct bt_skb_cb *)(skb->cb))
+#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))

static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how)
{
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 21e84fb..e67b1ff 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -101,6 +101,8 @@ struct l2cap_conninfo {
#define L2CAP_CONTROL_TXSEQ_SHIFT 1
#define L2CAP_CONTROL_REQSEQ_SHIFT 8

+#define L2CAP_SEQ_NUM_INC(seq) ((seq) = (seq + 1) % 64)
+
/* L2CAP Segmentation and Reassembly */
#define L2CAP_SAR_UNSEGMENTED 0x0000
#define L2CAP_SAR_SDU_START 0x4000
@@ -295,16 +297,22 @@ struct l2cap_pinfo {
__u8 sec_level;
__u8 role_switch;
__u8 force_reliable;
+ __u8 fcs;
+ __u8 mode;

__u8 conf_req[64];
__u8 conf_len;
__u8 conf_state;
__u8 conf_retry;

+ __u8 tx_seq;
+ __u8 req_seq;
+
__u8 ident;

__le16 sport;

+ struct sk_buff *tx_buf;
struct l2cap_conn *conn;
struct sock *next_c;
struct sock *prev_c;
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 31901be..7255c97 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -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>
@@ -718,12 +719,19 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
pi->sec_level = l2cap_pi(parent)->sec_level;
pi->role_switch = l2cap_pi(parent)->role_switch;
pi->force_reliable = l2cap_pi(parent)->force_reliable;
+ pi->fcs = l2cap_pi(parent)->fcs;
+ pi->mode = l2cap_pi(parent)->mode;
} else {
pi->imtu = L2CAP_DEFAULT_MTU;
pi->omtu = 0;
pi->sec_level = BT_SECURITY_LOW;
pi->role_switch = 0;
pi->force_reliable = 0;
+ pi->fcs = L2CAP_FCS_CRC16;
+ if (sk->sk_type == SOCK_STREAM)
+ pi->mode = L2CAP_MODE_ERTM;
+ else
+ pi->mode = L2CAP_MODE_BASIC;
}

/* Default config options */
@@ -1116,24 +1124,59 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
return 0;
}

-static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
+static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ int err;
+
+ BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
+
+ if (pi->tx_buf) {
+ err = -EAGAIN;
+ goto fail;
+ }
+
+ pi->tx_buf = skb;
+ err = hci_send_acl(pi->conn->hcon, skb, 0);
+ if (unlikely(err) < 0) {
+ pi->tx_buf = 0;
+ goto fail;
+ }
+
+ return 0;
+
+fail:
+ kfree_skb(skb);
+ return err;
+}
+
+static int l2cap_skbuff_fromiovec(
+ struct sock *sk, /* the socket */
+ struct msghdr *msg, /* data to send */
+ u16 *control, /* if != 0, ptr to control value */
+ u16 sdu_len, /* if != 0, fragmented SDU */
+ int len, /* the data length in the PDU */
+ struct sk_buff **out_skb) /* the resulting sk_buff */
{
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
struct sk_buff *skb, **frag;
- int err, hlen, count, sent = 0;
+ int err, count, hlen=0, sent=0, fcs=0;
struct l2cap_hdr *lh;

BT_DBG("sk %p len %d", sk, len);

/* First fragment (with L2CAP header) */
if (sk->sk_type == SOCK_DGRAM)
- hlen = L2CAP_HDR_SIZE + 2;
- else
- hlen = L2CAP_HDR_SIZE;
-
- count = min_t(unsigned int, (conn->mtu - hlen), len);
-
- skb = bt_skb_send_alloc(sk, hlen + count,
+ hlen += 2;
+ if (control)
+ hlen += 2;
+ if (sdu_len)
+ hlen += 2;
+ if (l2cap_pi(sk)->fcs)
+ hlen += 2;
+
+ count = min_t(unsigned int, conn->mtu - hlen, len);
+ skb = bt_skb_send_alloc(sk, count + hlen + L2CAP_HDR_SIZE,
msg->msg_flags & MSG_DONTWAIT, &err);
if (!skb)
return err;
@@ -1141,16 +1184,27 @@ static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
/* Create L2CAP header */
lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
- lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+ lh->len = cpu_to_le16(len + hlen);

if (sk->sk_type == SOCK_DGRAM)
put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
+ else {
+ if (control)
+ put_unaligned(*control, (__le16 *) skb_put(skb, 2));
+ if (sdu_len)
+ put_unaligned(sdu_len, (__le16 *) skb_put(skb, 2));
+ }

if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
err = -EFAULT;
goto fail;
}

+ if (l2cap_pi(sk)->fcs) {
+ fcs = crc16(0, (u8 *)lh, count + hlen + L2CAP_HDR_SIZE - 2);
+ put_unaligned(fcs, (__le16 *) skb_put(skb, 2));
+ }
+
sent += count;
len -= count;

@@ -1168,15 +1222,18 @@ static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
goto fail;
}

+ if (l2cap_pi(sk)->fcs) {
+ u16 fcs = crc16(0, skb->data, count);
+ put_unaligned(fcs, (__le16 *) skb_put(skb, 2));
+ }
+
sent += count;
len -= count;

frag = &(*frag)->next;
}
- err = hci_send_acl(conn->hcon, skb, 0);
- if (err < 0)
- goto fail;

+ *out_skb = skb;
return sent;

fail:
@@ -1184,9 +1241,36 @@ fail:
return err;
}

+static inline int l2cap_create_enhanced_sdu(struct sock *sk, struct msghdr *msg, size_t len, struct sk_buff **skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u8 tx_seq = pi->tx_seq;
+ u16 control = tx_seq << L2CAP_CONTROL_TXSEQ_SHIFT;
+ u16 omtu = (pi->fcs) ? pi->omtu - 2 : pi->omtu;
+ int err;
+
+ BT_DBG("sk %p, len %d", sk, (int)len);
+
+ /* Entire SDU fits into one PDU */
+ if (len <= omtu) {
+ err = l2cap_skbuff_fromiovec(sk, msg, &control, 0, len, skb);
+ if (unlikely(err < 0))
+ return err;
+ L2CAP_SEQ_NUM_INC(pi->tx_seq);
+ bt_cb(*skb)->tx_seq = tx_seq;
+ }
+ else {
+ /* Segmentation added later */
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
{
struct sock *sk = sock->sk;
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
int err = 0;

BT_DBG("sock %p, sk %p", sock, sk);
@@ -1199,16 +1283,39 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
return -EOPNOTSUPP;

/* Check outgoing MTU */
- if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)
+ if ((sk->sk_type != SOCK_RAW || pi->mode == L2CAP_MODE_BASIC) &&
+ len > l2cap_pi(sk)->omtu)
return -EINVAL;

lock_sock(sk);

- if (sk->sk_state == BT_CONNECTED)
- err = l2cap_do_send(sk, msg, len);
+ if (sk->sk_state == BT_CONNECTED) {
+ struct sk_buff *skb = 0;
+
+ switch (pi->mode) {
+ case L2CAP_MODE_BASIC:
+ err = l2cap_skbuff_fromiovec(sk, msg, 0, 0, len, &skb);
+ break;
+
+ case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAM:
+ err = l2cap_create_enhanced_sdu(sk, msg, len, &skb);
+ break;
+
+ default:
+ BT_DBG("bad state %1.1x", pi->mode);
+ err = -EINVAL;
+ }
+
+ if (err < 0)
+ goto done;
+ if (!(err = l2cap_do_send(sk, skb)))
+ err = len;
+ }
else
err = -ENOTCONN;

+done:
release_sock(sk);
return err;
}
--
1.6.0.6


2009-04-30 21:16:38

by Nathan Holstein

[permalink] [raw]
Subject: [PATCH 01/10] Add #defines for enhanced L2CAP constants.

From: Nathan Holstein <[email protected]>

This patch #defines the constants needed for implementation of Enhanced
Retranmission and Streaming modes within L2CAP.

Signed-off-by: Nathan Holstein <[email protected]>
---
include/net/bluetooth/l2cap.h | 50 +++++++++++++++++++++++++++++++++++++---
net/bluetooth/l2cap.c | 6 ++--
2 files changed, 49 insertions(+), 7 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 18dcb33..21e84fb 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -26,12 +26,24 @@
#define __L2CAP_H

/* L2CAP defaults */
-#define L2CAP_DEFAULT_MTU 672
-#define L2CAP_DEFAULT_FLUSH_TO 0xFFFF
+#define L2CAP_DEFAULT_MTU 672
+#define L2CAP_DEFAULT_MIN_MTU 48
+#define L2CAP_DEFAULT_FLUSH_TO 0xFFFF
+#define L2CAP_DEFAULT_RX_WINDOW 1
+#define L2CAP_DEFAULT_MAX_RECEIVE 1
+#define L2CAP_DEFAULT_RETRANS_TO 300 /* 300 milliseconds */
+#define L2CAP_DEFAULT_MONITOR_TO 1000 /* 1 second */
+#define L2CAP_DEFAULT_MAX_RX_APDU 0xFFF7

#define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
#define L2CAP_INFO_TIMEOUT (4000) /* 4 seconds */

+/* L2CAP feature bits */
+#define L2CAP_FEAT_ERTM 0x00000008
+#define L2CAP_FEAT_STREAM 0x00000010
+#define L2CAP_FEAT_FCS 0x00000020
+#define L2CAP_FEAT_FIXED_CHAN 0x00000080
+
/* L2CAP socket address */
struct sockaddr_l2 {
sa_family_t l2_family;
@@ -76,6 +88,35 @@ struct l2cap_conninfo {
#define L2CAP_INFO_REQ 0x0a
#define L2CAP_INFO_RSP 0x0b

+/* L2CAP Control Field bit masks */
+#define L2CAP_CONTROL_SAR_MASK 0xC000
+#define L2CAP_CONTROL_REQSEQ_MASK 0x3F00
+#define L2CAP_CONTROL_TXSEQ_MASK 0x007E
+#define L2CAP_CONTROL_RETRANS_MASK 0x0080
+#define L2CAP_CONTROL_FINAL_MASK 0x0080
+#define L2CAP_CONTROL_POLL_MASK 0x0010
+#define L2CAP_CONTROL_SUPERVISE_MASK 0x000C
+#define L2CAP_CONTROL_TYPE_MASK 0x0001 /* I- or S-Frame */
+
+#define L2CAP_CONTROL_TXSEQ_SHIFT 1
+#define L2CAP_CONTROL_REQSEQ_SHIFT 8
+
+/* L2CAP Segmentation and Reassembly */
+#define L2CAP_SAR_UNSEGMENTED 0x0000
+#define L2CAP_SAR_SDU_START 0x4000
+#define L2CAP_SAR_SDU_END 0x8000
+#define L2CAP_SAR_SDU_CONTINUE 0xC000
+
+/* L2CAP Supervisory Function */
+#define L2CAP_SUPER_RCV_READY 0x0000
+#define L2CAP_SUPER_REJECT 0x0004
+#define L2CAP_SUPER_RCV_NOT_READY 0x0008
+#define L2CAP_SUPER_SELECT_REJECT 0x000C
+
+/* L2CAP Optional FCS */
+#define L2CAP_FCS_NONE 0x00
+#define L2CAP_FCS_CRC16 0x01
+
/* L2CAP structures */
struct l2cap_hdr {
__le16 len;
@@ -155,6 +196,7 @@ struct l2cap_conf_opt {
#define L2CAP_CONF_FLUSH_TO 0x02
#define L2CAP_CONF_QOS 0x03
#define L2CAP_CONF_RFC 0x04
+#define L2CAP_CONF_FCS 0x05

#define L2CAP_CONF_MAX_SIZE 22

@@ -168,8 +210,8 @@ struct l2cap_conf_rfc {
} __attribute__ ((packed));

#define L2CAP_MODE_BASIC 0x00
-#define L2CAP_MODE_RETRANS 0x01
-#define L2CAP_MODE_FLOWCTL 0x02
+#define L2CAP_MODE_ERTM 0x03
+#define L2CAP_MODE_STREAM 0x04

struct l2cap_disconn_req {
__le16 dcid;
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index f6a82f2..31901be 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -52,7 +52,7 @@

#define VERSION "2.13"

-static u32 l2cap_feat_mask = 0x0080;
+static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
static u8 l2cap_fixed_chan[8] = { 0x02, };

static const struct proto_ops l2cap_sock_ops;
@@ -1747,7 +1747,7 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
len -= l2cap_get_conf_opt(&req, &type, &olen, &val);

hint = type & L2CAP_CONF_HINT;
- type &= 0x7f;
+ type &= (0xFF ^ L2CAP_CONF_HINT);

switch (type) {
case L2CAP_CONF_MTU:
@@ -2244,7 +2244,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
if (type == L2CAP_IT_FEAT_MASK) {
conn->feat_mask = get_unaligned_le32(rsp->data);

- if (conn->feat_mask & 0x0080) {
+ if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
struct l2cap_info_req req;
req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);

--
1.6.0.6