From db989bf770af1fb77bbb8ea82fd9caf627d268eb Mon Sep 17 00:00:00 2001 From: Andrzej Kaczmarek Date: Mon, 3 Jul 2017 16:11:50 +0200 Subject: [PATCH] nimble/controller: Fix features handling Octets 2-8 in LL_FEATURE_REQ and LL_FEATURE_RSP shall be always set to features supported by controller which sends PDU. Only 1st octet shall be set to features supported by both controllers. This patch renames "common_features" to "conn_features" to indicate that this stores features used on connection (as called by spec) and introduces new "remote_features" which caches remaining 7 octets of remote features so we can properly reply on LL and HCI. X-Original-Commit: b661780a48470bbd10fcef833c81240bf5bc6487 --- .../include/controller/ble_ll_conn.h | 3 ++- nimble/controller/src/ble_ll_conn.c | 3 ++- nimble/controller/src/ble_ll_conn_hci.c | 2 +- nimble/controller/src/ble_ll_ctrl.c | 27 +++++++++++++------ nimble/controller/src/ble_ll_hci_ev.c | 3 ++- 5 files changed, 26 insertions(+), 12 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h index 33079eda9..08ec11fdc 100644 --- a/nimble/controller/include/controller/ble_ll_conn.h +++ b/nimble/controller/include/controller/ble_ll_conn.h @@ -228,7 +228,8 @@ struct ble_ll_conn_sm uint8_t disconnect_reason; uint8_t rxd_disconnect_reason; uint8_t vers_nr; - uint32_t common_features; + uint8_t conn_features; + uint8_t remote_features[7]; uint16_t pending_ctrl_procs; uint16_t event_cntr; uint16_t completed_pkts; diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 494e04172..6461ba83a 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -1821,7 +1821,8 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm) connsm->event_cntr = 0; connsm->conn_state = BLE_LL_CONN_STATE_IDLE; connsm->disconnect_reason = 0; - connsm->common_features = 0; + connsm->conn_features = 0; + memset(connsm->remote_features, 0, sizeof(connsm->remote_features)); connsm->vers_nr = 0; connsm->comp_id = 0; connsm->sub_vers_nr = 0; diff --git a/nimble/controller/src/ble_ll_conn_hci.c b/nimble/controller/src/ble_ll_conn_hci.c index 78a07512f..d95373023 100644 --- a/nimble/controller/src/ble_ll_conn_hci.c +++ b/nimble/controller/src/ble_ll_conn_hci.c @@ -813,7 +813,7 @@ ble_ll_conn_hci_update(uint8_t *cmdbuf) } /* See if this feature is supported on both sides */ - if ((connsm->common_features & BLE_LL_FEAT_CONN_PARM_REQ) == 0) { + if ((connsm->conn_features & BLE_LL_FEAT_CONN_PARM_REQ) == 0) { if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { return BLE_ERR_CMD_DISALLOWED; } diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c index 9b4253e6e..0b9aae2ad 100644 --- a/nimble/controller/src/ble_ll_ctrl.c +++ b/nimble/controller/src/ble_ll_ctrl.c @@ -1530,7 +1530,7 @@ ble_ll_ctrl_rx_feature_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf, uint8_t opcode) { uint8_t rsp_opcode; - uint32_t remote_feat; + uint32_t our_feat; /* * Only accept slave feature requests if we are a master and feature @@ -1547,13 +1547,23 @@ ble_ll_ctrl_rx_feature_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, } } - remote_feat = get_le32(dptr); + our_feat = ble_ll_read_supp_features(); - /* Set common features and reply */ rsp_opcode = BLE_LL_CTRL_FEATURE_RSP; - connsm->common_features = remote_feat & ble_ll_read_supp_features(); + + /* + * 1st octet of features should be common features of local and remote + * controller - we call this 'connection features' + * remaining octets are features of controller which sends PDU, in this case + * it's our controller + * + * See: Vol 6, Part B, section 2.4.2.10 + */ + + connsm->conn_features = dptr[0] & our_feat; memset(rspbuf + 1, 0, 8); - put_le32(rspbuf + 1, connsm->common_features); + put_le32(rspbuf + 1, our_feat); + rspbuf[1] = connsm->conn_features; return rsp_opcode; } @@ -2132,7 +2142,7 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) features = ble_ll_read_supp_features(); if ((features & feature) == 0) { if (opcode == BLE_LL_CTRL_ENC_REQ) { - if (connsm->common_features & BLE_LL_FEAT_EXTENDED_REJ) { + if (connsm->conn_features & BLE_LL_FEAT_EXTENDED_REJ) { rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT; rspbuf[1] = opcode; rspbuf[2] = BLE_ERR_UNSUPP_REM_FEATURE; @@ -2201,8 +2211,9 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) break; /* XXX: check to see if ctrl procedure was running? Do we care? */ case BLE_LL_CTRL_FEATURE_RSP: + connsm->conn_features = dptr[0]; + memcpy(connsm->remote_features, dptr + 1, 7); /* Stop the control procedure */ - connsm->common_features = dptr[0]; if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG)) { ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_SUCCESS); ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_FEATURE_XCHG); @@ -2321,7 +2332,7 @@ ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm, uint8_t rej_opcode, rspbuf = om->om_data; opcode = BLE_LL_CTRL_REJECT_IND_EXT; if (rej_opcode == BLE_LL_CTRL_ENC_REQ) { - if ((connsm->common_features & BLE_LL_FEAT_EXTENDED_REJ) == 0) { + if ((connsm->conn_features & BLE_LL_FEAT_EXTENDED_REJ) == 0) { opcode = BLE_LL_CTRL_REJECT_IND; } } diff --git a/nimble/controller/src/ble_ll_hci_ev.c b/nimble/controller/src/ble_ll_hci_ev.c index 70ba68f9c..6abfdd6d5 100644 --- a/nimble/controller/src/ble_ll_hci_ev.c +++ b/nimble/controller/src/ble_ll_hci_ev.c @@ -198,7 +198,8 @@ ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, uint8_t status) evbuf[3] = status; put_le16(evbuf + 4, connsm->conn_handle); memset(evbuf + 6, 0, BLE_HCI_RD_LOC_SUPP_FEAT_RSPLEN); - put_le32(evbuf + 6, connsm->common_features); + evbuf[6] = connsm->conn_features; + memcpy(evbuf + 7, connsm->remote_features, 7); ble_ll_hci_event_send(evbuf); } }