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

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;

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;

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;
         }

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;
             }
         }

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);
         }
     }