From 6a2f50aba251114b5923db007dec7cdfcf46793b Mon Sep 17 00:00:00 2001 From: Andries Kruithof Date: Fri, 22 May 2020 15:16:39 +0200 Subject: [PATCH] Bluetooth: controller: split: Update feature exchange to BTCore V5.0 The existing feature exchange procedure does not give the proper response as specified in the BT core spec 5.0. The old behaviour is that the feature-response returns the logical and of the features for both peers. The behaviour implemented here is that the feature-response returns the featureset of the peer, except for octet 0 which is the logical and of the supported features. Tested by using the bt shell, and having different featuresets on the 2 peers. This fixes #25483 Signed-off-by: Andries Kruithof --- subsys/bluetooth/controller/include/ll_feat.h | 1 + subsys/bluetooth/controller/ll_sw/ull_adv.c | 3 +- subsys/bluetooth/controller/ll_sw/ull_conn.c | 66 ++++++++++++++----- .../controller/ll_sw/ull_conn_types.h | 3 +- .../bluetooth/controller/ll_sw/ull_master.c | 3 +- 5 files changed, 58 insertions(+), 18 deletions(-) diff --git a/subsys/bluetooth/controller/include/ll_feat.h b/subsys/bluetooth/controller/include/ll_feat.h index 376aea0aecd..9c85d0a90a1 100644 --- a/subsys/bluetooth/controller/include/ll_feat.h +++ b/subsys/bluetooth/controller/include/ll_feat.h @@ -103,6 +103,7 @@ #define LL_FEAT_BIT_MASK 0x1FFFF #define LL_FEAT_BIT_MASK_VALID 0x1CF2F +#define LL_FEAT_FILTER_OCTET0 0x1FF00 #define LL_FEAT (LL_FEAT_BIT_ENC | \ LL_FEAT_BIT_CONN_PARAM_REQ | \ LL_FEAT_BIT_EXT_REJ_IND | \ diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv.c b/subsys/bluetooth/controller/ll_sw/ull_adv.c index 283d0c5721d..8f186760286 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv.c @@ -654,7 +654,8 @@ u8_t ll_adv_enable(u8_t enable) conn->llcp_rx = NULL; conn->llcp_cu.req = conn->llcp_cu.ack = 0; conn->llcp_feature.req = conn->llcp_feature.ack = 0; - conn->llcp_feature.features = LL_FEAT; + conn->llcp_feature.features_conn = LL_FEAT; + conn->llcp_feature.features_peer = 0; conn->llcp_version.req = conn->llcp_version.ack = 0; conn->llcp_version.tx = conn->llcp_version.rx = 0; conn->llcp_terminate.reason_peer = 0; diff --git a/subsys/bluetooth/controller/ll_sw/ull_conn.c b/subsys/bluetooth/controller/ll_sw/ull_conn.c index 6ae6eec3527..ecb8d4f5b29 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_conn.c +++ b/subsys/bluetooth/controller/ll_sw/ull_conn.c @@ -275,7 +275,7 @@ u8_t ll_conn_update(u16_t handle, u8_t cmd, u8_t status, u16_t interval_min, #if defined(CONFIG_BT_CTLR_CONN_PARAM_REQ) if (!conn->llcp_conn_param.disabled && (!conn->common.fex_valid || - (conn->llcp_feature.features & + (conn->llcp_feature.features_conn & BIT(BT_LE_FEAT_BIT_CONN_PARAM_REQ)))) { cmd++; } else if (conn->lll.role) { @@ -427,7 +427,7 @@ u32_t ll_length_req_send(u16_t handle, u16_t tx_octets, u16_t tx_time) if (conn->llcp_length.disabled || (conn->common.fex_valid && - !(conn->llcp_feature.features & BIT(BT_LE_FEAT_BIT_DLE)))) { + !(conn->llcp_feature.features_conn & BIT(BT_LE_FEAT_BIT_DLE)))) { return BT_HCI_ERR_UNSUPP_REMOTE_FEATURE; } @@ -534,8 +534,9 @@ u8_t ll_phy_req_send(u16_t handle, u8_t tx, u8_t flags, u8_t rx) if (conn->llcp_phy.disabled || (conn->common.fex_valid && - !(conn->llcp_feature.features & BIT(BT_LE_FEAT_BIT_PHY_2M)) && - !(conn->llcp_feature.features & BIT(BT_LE_FEAT_BIT_PHY_CODED)))) { + !(conn->llcp_feature.features_conn & BIT(BT_LE_FEAT_BIT_PHY_2M)) && + !(conn->llcp_feature.features_conn & + BIT(BT_LE_FEAT_BIT_PHY_CODED)))) { return BT_HCI_ERR_UNSUPP_REMOTE_FEATURE; } @@ -2512,7 +2513,8 @@ static inline void event_enc_reject_prep(struct ll_conn *conn, pdu->ll_id = PDU_DATA_LLID_CTRL; if (conn->common.fex_valid && - (conn->llcp_feature.features & BIT(BT_LE_FEAT_BIT_EXT_REJ_IND))) { + (conn->llcp_feature.features_conn & + BIT(BT_LE_FEAT_BIT_EXT_REJ_IND))) { struct pdu_data_llctrl_reject_ext_ind *p; pdu->llctrl.opcode = PDU_DATA_LLCTRL_TYPE_REJECT_EXT_IND; @@ -2740,7 +2742,7 @@ static inline void event_fex_prep(struct ll_conn *conn) pdu->llctrl.opcode = PDU_DATA_LLCTRL_TYPE_FEATURE_RSP; (void)memset(&pdu->llctrl.feature_rsp.features[0], 0x00, sizeof(pdu->llctrl.feature_rsp.features)); - sys_put_le24(conn->llcp_feature.features, + sys_put_le24(conn->llcp_feature.features_peer, pdu->llctrl.feature_req.features); /* enqueue feature rsp structure into rx queue */ @@ -2758,7 +2760,7 @@ static inline void event_fex_prep(struct ll_conn *conn) conn->llcp_feature.ack--; /* use initial feature bitmap */ - conn->llcp_feature.features = LL_FEAT; + conn->llcp_feature.features_conn = LL_FEAT; /* place the feature exchange req packet as next in tx queue */ pdu->ll_id = PDU_DATA_LLID_CTRL; @@ -2770,7 +2772,7 @@ static inline void event_fex_prep(struct ll_conn *conn) (void)memset(&pdu->llctrl.feature_req.features[0], 0x00, sizeof(pdu->llctrl.feature_req.features)); - sys_put_le24(conn->llcp_feature.features, + sys_put_le24(conn->llcp_feature.features_conn, pdu->llctrl.feature_req.features); ctrl_tx_enqueue(conn, tx); @@ -3175,14 +3177,14 @@ static inline void dle_max_time_get(const struct ll_conn *conn, #if defined(CONFIG_BT_CTLR_PHY) #if defined(CONFIG_BT_CTLR_PHY_CODED) - feature_coded_phy = (conn->llcp_feature.features & + feature_coded_phy = (conn->llcp_feature.features_conn & BIT(BT_LE_FEAT_BIT_PHY_CODED)); #else feature_coded_phy = 0; #endif #if defined(CONFIG_BT_CTLR_PHY_2M) - feature_phy_2m = (conn->llcp_feature.features & + feature_phy_2m = (conn->llcp_feature.features_conn & BIT(BT_LE_FEAT_BIT_PHY_2M)); #else feature_phy_2m = 0; @@ -4020,12 +4022,28 @@ static inline u32_t feat_get(u8_t *features) return feat; } +/* + * Perform a logical and on octet0 and keep the remaining bits of the + * first input parameter + */ +static inline u32_t feat_land_octet0(u32_t feat_to_keep, u32_t feat_octet0) +{ + u32_t feat_result; + + feat_result = feat_to_keep & feat_octet0; + feat_result &= 0xFF; + feat_result |= feat_to_keep & LL_FEAT_FILTER_OCTET0; + + return feat_result; +} + static int feature_rsp_send(struct ll_conn *conn, struct node_rx_pdu *rx, struct pdu_data *pdu_rx) { struct pdu_data_llctrl_feature_req *req; struct node_tx *tx; struct pdu_data *pdu_tx; + u32_t feat; /* acquire tx mem */ tx = mem_acquire(&mem_conn_tx_ctrl.free); @@ -4035,7 +4053,14 @@ static int feature_rsp_send(struct ll_conn *conn, struct node_rx_pdu *rx, /* AND the feature set to get Feature USED */ req = &pdu_rx->llctrl.feature_req; - conn->llcp_feature.features &= feat_get(&req->features[0]); + conn->llcp_feature.features_conn &= feat_get(&req->features[0]); + /* + * Get all the features of peer, except octet 0. + * Octet 0 is the actual features used on the link + * See BTCore V5.2, Vol. 6, Part B, chapter 5.1.4 + */ + conn->llcp_feature.features_peer = + feat_land_octet0(feat_get(&req->features[0]), LL_FEAT); /* features exchanged */ conn->common.fex_valid = 1U; @@ -4044,12 +4069,16 @@ static int feature_rsp_send(struct ll_conn *conn, struct node_rx_pdu *rx, pdu_tx = (void *)tx->pdu; pdu_tx->ll_id = PDU_DATA_LLID_CTRL; pdu_tx->len = offsetof(struct pdu_data_llctrl, feature_rsp) + - sizeof(struct pdu_data_llctrl_feature_rsp); + sizeof(struct pdu_data_llctrl_feature_rsp); pdu_tx->llctrl.opcode = PDU_DATA_LLCTRL_TYPE_FEATURE_RSP; (void)memset(&pdu_tx->llctrl.feature_rsp.features[0], 0x00, sizeof(pdu_tx->llctrl.feature_rsp.features)); - sys_put_le24(conn->llcp_feature.features, - pdu_tx->llctrl.feature_req.features); + /* + * On feature response we send the local supported features. + * See BTCore V5.2 VOl 6 Part B, chapter 5.1.4 + */ + feat = feat_land_octet0(LL_FEAT, conn->llcp_feature.features_conn); + sys_put_le24(feat, pdu_tx->llctrl.feature_rsp.features); ctrl_tx_sec_enqueue(conn, tx); @@ -4066,7 +4095,14 @@ static void feature_rsp_recv(struct ll_conn *conn, struct pdu_data *pdu_rx) rsp = &pdu_rx->llctrl.feature_rsp; /* AND the feature set to get Feature USED */ - conn->llcp_feature.features &= feat_get(&rsp->features[0]); + conn->llcp_feature.features_conn &= feat_get(&rsp->features[0]); + /* + * Get all the features of peer, except octet 0. + * Octet 0 is the actual features used on the link + * See BTCore V5.2, Vol. 6, Part B, chapter 5.1.4 + */ + conn->llcp_feature.features_peer = + feat_land_octet0(feat_get(&rsp->features[0]), LL_FEAT); /* features exchanged */ conn->common.fex_valid = 1U; diff --git a/subsys/bluetooth/controller/ll_sw/ull_conn_types.h b/subsys/bluetooth/controller/ll_sw/ull_conn_types.h index f9f7356ae64..a9fea8cc238 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_conn_types.h +++ b/subsys/bluetooth/controller/ll_sw/ull_conn_types.h @@ -140,7 +140,8 @@ struct ll_conn { struct { u8_t req; u8_t ack; - u32_t features; + u32_t features_conn; + u32_t features_peer; } llcp_feature; struct { diff --git a/subsys/bluetooth/controller/ll_sw/ull_master.c b/subsys/bluetooth/controller/ll_sw/ull_master.c index f26816da438..4f331803085 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_master.c +++ b/subsys/bluetooth/controller/ll_sw/ull_master.c @@ -201,7 +201,8 @@ u8_t ll_create_connection(u16_t scan_interval, u16_t scan_window, conn->llcp_rx = NULL; conn->llcp_cu.req = conn->llcp_cu.ack = 0; conn->llcp_feature.req = conn->llcp_feature.ack = 0; - conn->llcp_feature.features = LL_FEAT; + conn->llcp_feature.features_conn = LL_FEAT; + conn->llcp_feature.features_peer = 0; conn->llcp_version.req = conn->llcp_version.ack = 0; conn->llcp_version.tx = conn->llcp_version.rx = 0U; conn->llcp_terminate.reason_peer = 0U;