Add code to config_req and config_rsp to configure ERTM and Streaming mode.
The code treat state1 and state2 devices as said in specification, also
uses the precendence list for state 1 device: Streaming mode, ERTM, Basic
Mode(highest).
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
include/net/bluetooth/l2cap.h | 27 ++++-
net/bluetooth/l2cap.c | 271 ++++++++++++++++++++++++++++++++++++-----
2 files changed, 267 insertions(+), 31 deletions(-)
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 06b072f..1c85694 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -27,6 +27,7 @@
/* L2CAP defaults */
#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
@@ -272,6 +273,11 @@ struct l2cap_pinfo {
__u16 omtu;
__u16 flush_to;
__u8 mode;
+ __u8 state;
+ __u8 dev_type;
+ __u8 nr_conf_req;
+ __u8 nr_conf_rsp;
+
__u8 fcs;
__u8 sec_level;
__u8 role_switch;
@@ -280,10 +286,15 @@ struct l2cap_pinfo {
__u8 conf_req[64];
__u8 conf_len;
__u8 conf_state;
- __u8 conf_retry;
__u8 ident;
+ __u8 remote_tx_win;
+ __u8 remote_max_tx;
+ __u16 retrans_timeout;
+ __u16 monitor_timeout;
+ __u16 max_pdu_size;
+
__le16 sport;
struct l2cap_conn *conn;
@@ -294,9 +305,21 @@ 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_MAX_RETRIES 2
+#define L2CAP_CONF_MAX_CONF_REQ 2
+#define L2CAP_CONF_MAX_CONF_RSP 2
+
+#define L2CAP_CONF_STATE1_DEVICE 0x01
+#define L2CAP_CONF_STATE2_DEVICE 0x02
+#define L2CAP_CONF_STATE1_LOWER 0x03
+#define L2CAP_CONF_STATE1_HIGHER 0x04
+
+#define L2CAP_CONF_DEV_LOCAL 0x01
+#define L2CAP_CONF_DEV_REMOTE 0x02
+
void l2cap_load(void);
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 82feeb7..44836bb 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -718,6 +718,7 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
pi->imtu = l2cap_pi(parent)->imtu;
pi->omtu = l2cap_pi(parent)->omtu;
pi->mode = l2cap_pi(parent)->mode;
+ pi->state = l2cap_pi(parent)->state;
pi->fcs = l2cap_pi(parent)->fcs;
pi->sec_level = l2cap_pi(parent)->sec_level;
pi->role_switch = l2cap_pi(parent)->role_switch;
@@ -965,6 +966,7 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al
case L2CAP_MODE_BASIC:
break;
case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAMING:
if (enable_ertm)
break;
/* fall through */
@@ -1028,6 +1030,7 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)
case L2CAP_MODE_BASIC:
break;
case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAMING:
if (enable_ertm)
break;
/* fall through */
@@ -1297,6 +1300,11 @@ 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;
+ if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM ||
+ l2cap_pi(sk)->mode == L2CAP_MODE_STREAMING)
+ l2cap_pi(sk)->state = L2CAP_CONF_STATE2_DEVICE;
+ else
+ l2cap_pi(sk)->state = L2CAP_CONF_STATE1_DEVICE;
break;
case L2CAP_LM:
@@ -1738,7 +1746,50 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
*ptr += L2CAP_CONF_OPT_SIZE + len;
}
-static int l2cap_build_conf_req(struct sock *sk, void *data)
+
+static __u32 l2cap_mode_to_feat_mask(__u8 mode)
+{
+ switch(mode) {
+ case L2CAP_MODE_ERTM:
+ return L2CAP_FEAT_ERTM;
+ case L2CAP_MODE_STREAMING:
+ return L2CAP_FEAT_STREAMING;
+ default:
+ return 0x00;
+ }
+}
+
+static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
+{
+ return (l2cap_mode_to_feat_mask(mode) & feat_mask);
+}
+
+static __u8 l2cap_next_state1_mode_supported(__u16 local_feat_mask, __u16 remote_feat_mask)
+{
+ if (local_feat_mask & remote_feat_mask & L2CAP_FEAT_STREAMING)
+ return L2CAP_MODE_STREAMING;
+ else if (local_feat_mask & remote_feat_mask & L2CAP_FEAT_ERTM)
+ return L2CAP_MODE_ERTM;
+ else
+ return L2CAP_MODE_BASIC;
+}
+
+static int l2cap_get_state1_type(__u8 local_mode, __u8 remote_mode)
+{
+ if (remote_mode == L2CAP_MODE_STREAMING)
+ return L2CAP_CONF_STATE1_LOWER;
+ else if (remote_mode == L2CAP_MODE_ERTM) {
+ if (local_mode == L2CAP_MODE_STREAMING)
+ return L2CAP_CONF_STATE1_HIGHER;
+ else
+ return L2CAP_CONF_STATE1_LOWER;
+ }
+ else
+ return L2CAP_CONF_STATE1_HIGHER;
+}
+
+
+static int l2cap_build_conf_req(struct sock *sk, struct l2cap_conn *conn, void *data)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
struct l2cap_conf_req *req = data;
@@ -1747,6 +1798,22 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)
BT_DBG("sk %p", sk);
+ if (pi->nr_conf_req == 0 && pi->nr_conf_req == 0) {
+ if (pi->state == L2CAP_CONF_STATE2_DEVICE) {
+ if (!l2cap_mode_supported(pi->mode, conn->feat_mask) ||
+ !l2cap_mode_supported(pi->mode, l2cap_feat_mask)) {
+ struct l2cap_disconn_req req;
+ req.dcid = cpu_to_le16(pi->dcid);
+ req.scid = cpu_to_le16(pi->scid);
+ l2cap_send_cmd(conn, l2cap_get_ident(conn),
+ L2CAP_DISCONN_REQ, sizeof(req), &req);
+ }
+ }
+ else {
+ pi->mode = l2cap_next_state1_mode_supported(l2cap_feat_mask, conn->feat_mask);
+ }
+ }
+
switch (pi->mode) {
case L2CAP_MODE_BASIC:
if (pi->imtu != L2CAP_DEFAULT_MTU)
@@ -1760,11 +1827,20 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)
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;
- l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
- sizeof(rfc), (unsigned long) &rfc);
+ case L2CAP_MODE_STREAMING:
+ rfc.mode = L2CAP_MODE_STREAMING;
+ 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)
@@ -1776,7 +1852,7 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)
return ptr - data;
}
-static int l2cap_parse_conf_req(struct sock *sk, void *data)
+static int l2cap_parse_conf_req(struct sock *sk, struct l2cap_conn *conn, void *data, int *disconnect)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
struct l2cap_conf_rsp *rsp = data;
@@ -1824,24 +1900,70 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
}
}
+ if (pi->nr_conf_rsp == 0 && pi->nr_conf_req == 0) {
+ if (pi->state == L2CAP_CONF_STATE2_DEVICE) {
+ if (!l2cap_mode_supported(pi->mode, conn->feat_mask) ||
+ !l2cap_mode_supported(pi->mode, l2cap_feat_mask)) {
+ *disconnect = 1;
+ }
+ }
+ else {
+ pi->mode = l2cap_next_state1_mode_supported(l2cap_feat_mask, conn->feat_mask);
+ }
+ }
+
+ if (pi->mode != rfc.mode) {
+ result = L2CAP_CONF_UNACCEPT;
+ rfc.mode = pi->mode;
+ pi->state = l2cap_get_state1_type(pi->mode, rfc.mode);
+
+ if (pi->nr_conf_rsp == 0) {
+ if (pi->dev_type == L2CAP_CONF_DEV_REMOTE &&
+ pi->state == L2CAP_CONF_STATE1_LOWER)
+ *disconnect = 1;
+ }
+ else
+ *disconnect = 1;
+
+ /*FIXME: when disconnecting we don't need this */
+ 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 (rfc.mode == L2CAP_MODE_BASIC) {
- if (mtu < pi->omtu)
- result = L2CAP_CONF_UNACCEPT;
- else {
- pi->omtu = mtu;
- pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
- }
+ 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);
- l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
- } else {
+ switch (rfc.mode) {
+ case L2CAP_MODE_BASIC:
+ pi->fcs = 0;
+ pi->conf_state |= L2CAP_CONF_MODE_DONE;
+ break;
+
+ case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAMING:
+ 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);
@@ -1855,6 +1977,66 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
return ptr - data;
}
+static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *_req, u16 *result, int *disconnect)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct l2cap_conf_req *req = _req;
+ void *ptr = req->data;
+ int type, 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);
+
+ 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_RFC:
+ if (olen == sizeof(rfc)) {
+ memcpy(&rfc, (void *) val, olen);
+ }
+ *result = L2CAP_CONF_UNACCEPT;
+ if ((rfc.mode != pi->mode) &&
+ (pi->state == L2CAP_CONF_STATE2_DEVICE)) {
+ *disconnect = 1;
+ }
+ if (pi->mode == L2CAP_MODE_BASIC)
+ pi->fcs = 0;
+ pi->mode = rfc.mode;
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+ sizeof(rfc), (unsigned long) &rfc);
+ break;
+ }
+ }
+
+ req->dcid = cpu_to_le16(pi->dcid);
+ req->flags = cpu_to_le16(0x0000);
+
+ return ptr - _req;
+}
+
+
+
+
+
static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
{
struct l2cap_conf_rsp *rsp = data;
@@ -2040,7 +2222,9 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
- l2cap_build_conf_req(sk, req), req);
+ l2cap_build_conf_req(sk, conn, req), req);
+ l2cap_pi(sk)->nr_conf_req++;
+ l2cap_pi(sk)->dev_type = L2CAP_CONF_DEV_LOCAL;
break;
case L2CAP_CR_PEND:
@@ -2062,7 +2246,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
u16 dcid, flags;
u8 rsp[64];
struct sock *sk;
- int len;
+ int len, disconn = 0;
dcid = __le16_to_cpu(req->dcid);
flags = __le16_to_cpu(req->flags);
@@ -2098,11 +2282,22 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
}
/* Complete config. */
- len = l2cap_parse_conf_req(sk, rsp);
+ len = l2cap_parse_conf_req(sk, conn, rsp, &disconn);
if (len < 0)
goto unlock;
- l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
+ if (disconn) {
+ struct l2cap_disconn_req req;
+ req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
+ req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
+ l2cap_send_cmd(conn, l2cap_get_ident(conn),
+ L2CAP_DISCONN_REQ, sizeof(req), &req);
+ goto unlock;
+ }
+ else {
+ l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
+ l2cap_pi(sk)->nr_conf_rsp++;
+ }
/* Reset config buffer. */
l2cap_pi(sk)->conf_len = 0;
@@ -2119,7 +2314,10 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) {
u8 buf[64];
l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
- l2cap_build_conf_req(sk, buf), buf);
+ l2cap_build_conf_req(sk, conn, buf), buf);
+ l2cap_pi(sk)->nr_conf_req++;
+ l2cap_pi(sk)->dev_type = L2CAP_CONF_DEV_REMOTE;
+
}
unlock:
@@ -2132,6 +2330,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
u16 scid, flags, result;
struct sock *sk;
+ int disconn;
scid = __le16_to_cpu(rsp->scid);
flags = __le16_to_cpu(rsp->flags);
@@ -2149,16 +2348,30 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
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;
+ if (l2cap_pi(sk)->nr_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
+ 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, &disconn);
+ if (disconn) {
+ struct l2cap_disconn_req req;
+ req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
+ req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
+ l2cap_send_cmd(conn, l2cap_get_ident(conn),
+ L2CAP_DISCONN_REQ, sizeof(req), &req);
+ goto done;
+ }
+
+ l2cap_send_cmd(conn, l2cap_get_ident(conn),
+ L2CAP_CONF_REQ, len, req);
+ l2cap_pi(sk)->nr_conf_req++;
+ if (result == L2CAP_CONF_SUCCESS)
+ break;
+ else
+ goto done;
}
default:
--
1.6.0.6