Bluetooth: Add configuration support for ERTM and Streaming mode
Add support to config_req and config_rsp to configure ERTM and Streaming
mode. If the remote device specifies ERTM or Streaming mode, then the
same mode is proposed. Otherwise ERTM or Basic mode is used. And in case
of a state 2 device, the remote device should propose the same mode. If
not, then the channel gets disconnected.
Signed-off-by: Gustavo F. Padovan <gustavo@las.ic.unicamp.br>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 7ce1a24..af0fbf9 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -966,6 +966,7 @@
case L2CAP_MODE_BASIC:
break;
case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAMING:
if (enable_ertm)
break;
/* fall through */
@@ -1029,6 +1030,7 @@
case L2CAP_MODE_BASIC:
break;
case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAMING:
if (enable_ertm)
break;
/* fall through */
@@ -1739,15 +1741,65 @@
*ptr += L2CAP_CONF_OPT_SIZE + len;
}
+static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
+{
+ u32 local_feat_mask = l2cap_feat_mask;
+ if (enable_ertm)
+ local_feat_mask |= L2CAP_FEAT_ERTM;
+
+ switch (mode) {
+ case L2CAP_MODE_ERTM:
+ return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
+ case L2CAP_MODE_STREAMING:
+ return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
+ default:
+ return 0x00;
+ }
+}
+
+static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
+{
+ switch (mode) {
+ case L2CAP_MODE_STREAMING:
+ case L2CAP_MODE_ERTM:
+ if (l2cap_mode_supported(mode, remote_feat_mask))
+ return mode;
+ /* fall through */
+ default:
+ return L2CAP_MODE_BASIC;
+ }
+}
+
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 = { .mode = L2CAP_MODE_BASIC };
+ struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_ERTM };
void *ptr = req->data;
BT_DBG("sk %p", sk);
+ if (pi->num_conf_req || pi->num_conf_rsp)
+ goto done;
+
+ switch (pi->mode) {
+ case L2CAP_MODE_STREAMING:
+ case L2CAP_MODE_ERTM:
+ pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
+ if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask)) {
+ struct l2cap_disconn_req req;
+ req.dcid = cpu_to_le16(pi->dcid);
+ req.scid = cpu_to_le16(pi->scid);
+ l2cap_send_cmd(pi->conn, l2cap_get_ident(pi->conn),
+ L2CAP_DISCONN_REQ, sizeof(req), &req);
+ }
+ break;
+ default:
+ pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
+ break;
+ }
+
+done:
switch (pi->mode) {
case L2CAP_MODE_BASIC:
if (pi->imtu != L2CAP_DEFAULT_MTU)
@@ -1756,10 +1808,22 @@
case L2CAP_MODE_ERTM:
rfc.mode = L2CAP_MODE_ERTM;
- rfc.txwin_size = L2CAP_DEFAULT_RX_WINDOW;
+ rfc.txwin_size = L2CAP_DEFAULT_TX_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.retrans_timeout = 0;
+ rfc.monitor_timeout = 0;
+ rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_RX_APDU);
+
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+ sizeof(rfc), (unsigned long) &rfc);
+ break;
+
+ 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);
l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
@@ -1825,30 +1889,83 @@
}
}
+ if (pi->num_conf_rsp || pi->num_conf_req)
+ goto done;
+
+ switch (pi->mode) {
+ case L2CAP_MODE_STREAMING:
+ case L2CAP_MODE_ERTM:
+ pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
+ if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
+ return -ECONNREFUSED;
+ break;
+ default:
+ pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
+ break;
+ }
+
+done:
+ if (pi->mode != rfc.mode) {
+ result = L2CAP_CONF_UNACCEPT;
+ rfc.mode = pi->mode;
+
+ if (pi->num_conf_rsp == 1)
+ return -ECONNREFUSED;
+
+ 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 = L2CAP_FCS_NONE;
+ pi->conf_state |= L2CAP_CONF_MODE_DONE;
+ break;
+
+ case L2CAP_MODE_ERTM:
+ pi->remote_tx_win = rfc.txwin_size;
+ pi->remote_max_tx = rfc.max_transmit;
+ pi->max_pdu_size = rfc.max_pdu_size;
+
+ rfc.retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
+ rfc.monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
+
+ pi->conf_state |= L2CAP_CONF_MODE_DONE;
+ break;
+
+ case L2CAP_MODE_STREAMING:
+ pi->remote_tx_win = rfc.txwin_size;
+ 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;
-
- l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
- sizeof(rfc), (unsigned long) &rfc);
+ rfc.mode = pi->mode;
}
- }
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+ sizeof(rfc), (unsigned long) &rfc);
+
+ if (result == L2CAP_CONF_SUCCESS)
+ pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
+ }
rsp->scid = cpu_to_le16(pi->dcid);
rsp->result = cpu_to_le16(result);
rsp->flags = cpu_to_le16(0x0000);
@@ -1856,6 +1973,73 @@
return ptr - data;
}
+static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct l2cap_conf_req *req = data;
+ 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, data);
+
+ 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);
+
+ if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
+ rfc.mode != pi->mode)
+ return -ECONNREFUSED;
+
+ pi->mode = rfc.mode;
+ pi->fcs = 0;
+
+ l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+ sizeof(rfc), (unsigned long) &rfc);
+ break;
+ }
+ }
+
+ if (*result == L2CAP_CONF_SUCCESS) {
+ switch (rfc.mode) {
+ case L2CAP_MODE_ERTM:
+ pi->remote_tx_win = rfc.txwin_size;
+ pi->retrans_timeout = rfc.retrans_timeout;
+ pi->monitor_timeout = rfc.monitor_timeout;
+ pi->max_pdu_size = le16_to_cpu(rfc.max_pdu_size);
+ break;
+ case L2CAP_MODE_STREAMING:
+ pi->max_pdu_size = le16_to_cpu(rfc.max_pdu_size);
+ break;
+ }
+ }
+
+ req->dcid = cpu_to_le16(pi->dcid);
+ req->flags = cpu_to_le16(0x0000);
+
+ return ptr - data;
+}
+
static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
{
struct l2cap_conf_rsp *rsp = data;
@@ -2042,6 +2226,7 @@
l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
l2cap_build_conf_req(sk, req), req);
+ l2cap_pi(sk)->num_conf_req++;
break;
case L2CAP_CR_PEND:
@@ -2100,10 +2285,17 @@
/* Complete config. */
len = l2cap_parse_conf_req(sk, rsp);
- if (len < 0)
+ if (len < 0) {
+ 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;
+ }
l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
+ l2cap_pi(sk)->num_conf_rsp++;
/* Reset config buffer. */
l2cap_pi(sk)->conf_len = 0;
@@ -2121,6 +2313,7 @@
u8 buf[64];
l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
l2cap_build_conf_req(sk, buf), buf);
+ l2cap_pi(sk)->num_conf_req++;
}
unlock:
@@ -2150,16 +2343,29 @@
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)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
+ int len = cmd->len - sizeof(*rsp);
+ char req[64];
+
+ /* throw out any old stored conf requests */
+ result = L2CAP_CONF_SUCCESS;
+ len = l2cap_parse_conf_rsp(sk, rsp->data,
+ len, req, &result);
+ if (len < 0) {
+ 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)->num_conf_req++;
+ if (result != L2CAP_CONF_SUCCESS)
+ goto done;
+ break;
}
default: