diff options
Diffstat (limited to 'net/bluetooth')
| -rw-r--r-- | net/bluetooth/l2cap.c | 262 |
1 files changed, 234 insertions, 28 deletions
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 @@ 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 */ @@ -1029,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 */ @@ -1739,15 +1741,65 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) *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 @@ static int l2cap_build_conf_req(struct sock *sk, void *data) 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 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data) } } + 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; + rfc.mode = pi->mode; + } - l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, + 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 @@ 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 *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 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd 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 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr /* 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 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr 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 @@ 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)->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: |
