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 <Andries.Kruithof@nordicsemi.no>
diff --git a/subsys/bluetooth/controller/include/ll_feat.h b/subsys/bluetooth/controller/include/ll_feat.h
index 376aea0..9c85d0a 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 3235780..ee21bc3 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_adv.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_adv.c
@@ -650,7 +650,8 @@
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 a0f4e7e..77fcf5b 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_conn.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_conn.c
@@ -275,7 +275,7 @@
#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) {
@@ -2499,7 +2499,8 @@
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;
@@ -2727,7 +2728,7 @@
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 */
@@ -2745,7 +2746,7 @@
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;
@@ -2757,7 +2758,7 @@
(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);
@@ -3162,14 +3163,14 @@
#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;
@@ -4007,12 +4008,28 @@
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);
@@ -4022,7 +4039,14 @@
/* 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;
@@ -4031,12 +4055,16 @@
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);
@@ -4053,7 +4081,14 @@
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 decf8ad..3f9c28c 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 {
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 25ec71c..77a3d62 100644
--- a/subsys/bluetooth/controller/ll_sw/ull_master.c
+++ b/subsys/bluetooth/controller/ll_sw/ull_master.c
@@ -197,7 +197,8 @@
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;
