apache · m-gorecki · Nov 6, 2024
diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h
@@ -131,6 +131,9 @@ struct ble_ll_conn_sm_flags {
 #if MYNEWT_VAL(BLE_LL_CONN_INIT_AUTO_DLE)
     uint32_t pending_initiate_dle : 1;
 #endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+    uint32_t power_request_host_w4event : 1;
+#endif
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ENHANCED_CONN_UPDATE)
     uint8_t subrate_trans : 1;
     uint8_t subrate_ind_txd : 1;
@@ -201,6 +204,38 @@ struct ble_ll_conn_subrate_req_params {
     uint16_t supervision_tmo;
 };
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+struct ble_ll_conn_power_control_data {
+    /*
+     * Current local TxPower.
+     * Updated when PHY is changed or when current PHY's TxPower is changed
+     */
+    int8_t curr_local_tx_power;
+
+    /*
+     * Stores remote TxPower for each PHY mode.
+     * Updated when power control response PDU or power change IND is received
+     */
+    int8_t remote_tx_power[BLE_PHY_NUM_MODE];
+
+    /* Stores local TxPower for each PHY mode */
+    int8_t local_tx_power[BLE_PHY_NUM_MODE];
+
+    /* Stores min max flags for each PHY mode */
+    int8_t local_min_max[BLE_PHY_NUM_MODE];
+
+    /* Indicates on which PHY we requested Power Control Request Procedure */
+    uint8_t req_phy_mode;
+
+    /* Stores delta for Power Control Request */
+    int8_t req_delta;
+
+    /* Flags that indicate if reports are enabled */
+    uint8_t local_reports_enabled : 1;
+    uint8_t remote_reports_enabled : 1;
+};
+#endif
+
 /* Connection state machine */
 struct ble_ll_conn_sm
 {
@@ -397,6 +432,10 @@ struct ble_ll_conn_sm
     uint16_t css_slot_idx_pending;
     uint8_t css_period_idx;
 #endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+    struct ble_ll_conn_power_control_data pwr_ctrl;
+#endif
 };
 
 /* Role */

diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -44,6 +44,7 @@ extern "C" {
 #define BLE_LL_CTRL_PROC_SUBRATE_REQ    (12)
 #define BLE_LL_CTRL_PROC_SUBRATE_UPDATE (13)
 #define BLE_LL_CTRL_PROC_NUM            (14)
+#define BLE_LL_CTRL_PROC_POWER_CTRL_REQ (15)
 #define BLE_LL_CTRL_PROC_IDLE           (255)
 
 /* Checks if a particular control procedure is running */
@@ -338,6 +339,9 @@ void ble_ll_hci_ev_send_vs_assert(const char *file, uint32_t line);
 void ble_ll_hci_ev_send_vs_printf(uint8_t id, const char *fmt, ...);
 void ble_ll_hci_ev_send_vs_llcp_trace(uint8_t type, uint16_t handle, uint16_t count,
                                       void *pdu, size_t length);
+void ble_ll_hci_ev_transmit_power_report(struct ble_ll_conn_sm *connsm, uint8_t status,
+                                         uint8_t reason, uint8_t phy_mode, int8_t tx_power,
+                                         uint8_t tx_power_flags, int8_t delta);
 
 uint8_t ble_ll_ctrl_phy_tx_transition_get(uint8_t phy_mask);
 uint8_t ble_ll_ctrl_phy_from_phy_mask(uint8_t phy_mask);

diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c
@@ -1935,6 +1935,10 @@ ble_ll_init(void)
     features |= BLE_LL_FEAT_CS_PCT_QUALITY_IND;
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+    features |= BLE_LL_FEAT_POWER_CTRL_REQ;
+#endif
+
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
     features |= BLE_LL_FEAT_PERIODIC_ADV_ADI;
 #endif

diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c
@@ -1526,8 +1526,12 @@ ble_ll_conn_event_start_cb(struct ble_ll_sched_item *sch)
     ble_phy_mode_set(connsm->phy_data.tx_phy_mode, connsm->phy_data.rx_phy_mode);
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+    ble_ll_tx_power_set(connsm->pwr_ctrl.curr_local_tx_power);
+#else
     /* Set the power */
     ble_ll_tx_power_set(g_ble_ll_tx_power);
+#endif
 
     switch (connsm->conn_role) {
 #if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL)
@@ -1746,6 +1750,21 @@ ble_ll_conn_auth_pyld_timer_start(struct ble_ll_conn_sm *connsm)
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+static void
+ble_ll_conn_init_tx_power(struct ble_ll_conn_sm *connsm)
+{
+    int i;
+
+    for (i = 0; i < BLE_PHY_NUM_MODE; i++) {
+        connsm->pwr_ctrl.local_tx_power[i] = g_ble_ll_tx_power;
+        connsm->pwr_ctrl.remote_tx_power[i] = 127;
+    }
+
+    connsm->pwr_ctrl.curr_local_tx_power = g_ble_ll_tx_power;
+}
+#endif
+
 #if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL)
 static void
 ble_ll_conn_central_common_init(struct ble_ll_conn_sm *connsm)
@@ -2018,6 +2037,10 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm)
     connsm->phy_tx_transition = 0;
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+    ble_ll_conn_init_tx_power(connsm);
+#endif
+
     /* Reset current control procedure */
     connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_IDLE;
     connsm->pending_ctrl_procs = 0;
@@ -2325,6 +2348,38 @@ ble_ll_conn_move_anchor(struct ble_ll_conn_sm *connsm, uint16_t offset)
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+void
+ble_ll_conn_update_current_local_tx_power(struct ble_ll_conn_sm *connsm)
+{
+    uint8_t curr_phy_mode;
+    int8_t new_tx_power;
+    int8_t delta;
+
+#if MYNEWT_VAL(BLE_LL_PHY)
+    curr_phy_mode = connsm->phy_data.tx_phy_mode;
+#else
+    curr_phy_mode = BLE_PHY_MODE_1M;
+#endif
+
+    new_tx_power = connsm->pwr_ctrl.local_tx_power[curr_phy_mode];
+    if (connsm->pwr_ctrl.curr_local_tx_power == new_tx_power) {
+        return;
+    }
+
+    delta = new_tx_power - connsm->pwr_ctrl.curr_local_tx_power;
+    connsm->pwr_ctrl.curr_local_tx_power = new_tx_power;
+
+    if (connsm->pwr_ctrl.local_reports_enabled) {
+        ble_ll_hci_ev_transmit_power_report(connsm, BLE_ERR_SUCCESS, 0x00,
+                                            curr_phy_mode,
+                                            connsm->pwr_ctrl.curr_local_tx_power,
+                                            connsm->pwr_ctrl.local_min_max[curr_phy_mode],
+                                            delta);
+    }
+}
+#endif
+
 /**
  * Called to move to the next connection event.
  *
@@ -2659,6 +2714,9 @@ ble_ll_conn_next_event(struct ble_ll_conn_sm *connsm)
             connsm->phy_data.tx_phy_mode =
                                 ble_ll_phy_to_phy_mode(connsm->phy_data.cur_tx_phy,
                                                    connsm->phy_data.pref_opts);
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+            ble_ll_conn_update_current_local_tx_power(connsm);
+#endif
         }
 
         if (connsm->phy_data.new_rx_phy) {

diff --git a/nimble/controller/src/ble_ll_conn_hci.c b/nimble/controller/src/ble_ll_conn_hci.c
@@ -2176,4 +2176,154 @@ ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len)
     return BLE_ERR_SUCCESS;
 }
 #endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+int
+ble_ll_conn_hci_enhanced_read_tx_power_level(const uint8_t *cmdbuf, uint8_t len,
+                                             uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_enh_read_transmit_power_level_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_enh_read_transmit_power_level_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint8_t phy_mode;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    switch (cmd->phy) {
+    case 0x1:
+        phy_mode = BLE_PHY_MODE_1M;
+        break;
+#if MYNEWT_VAL(BLE_PHY_2M)
+    case 0x2:
+        phy_mode = BLE_PHY_MODE_2M;
+        break;
+#endif
+#if MYNEWT_VAL(BLE_PHY_CODED)
+    case 0x3:
+        phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+        break;
+    case 0x4:
+        phy_mode = BLE_PHY_MODE_CODED_125KBPS;
+        break;
+#endif
+    default:
+        rc = BLE_ERR_UNSUPPORTED;
+        goto done;
+    }
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto done;
+    }
+
+    rsp->phy = cmd->phy;
+    rsp->curr_tx_power_level = connsm->pwr_ctrl.local_tx_power[phy_mode];
+    rsp->max_tx_power_level = ble_phy_tx_power_round(INT8_MAX);
+
+    rc = BLE_ERR_SUCCESS;
+done:
+    *rsplen = sizeof(*rsp);
+    rsp->conn_handle = cmd->conn_handle;
+    return rc;
+}
+
+int
+ble_ll_conn_hci_read_remote_tx_power_level(const uint8_t *cmdbuf, uint8_t len)
+{
+    const struct ble_hci_le_read_remote_transmit_power_level_cp *cmd = (const void *) cmdbuf;
+    struct ble_ll_conn_sm *connsm;
+
+    if (len != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        return BLE_ERR_UNK_CONN_ID;
+    }
+
+    switch (cmd->phy) {
+    case 0x1:
+        connsm->pwr_ctrl.req_phy_mode = BLE_PHY_MODE_1M;
+        break;
+#if MYNEWT_VAL(BLE_PHY_2M)
+    case 0x2:
+        connsm->pwr_ctrl.req_phy_mode = BLE_PHY_MODE_2M;
+        break;
+#endif
+#if MYNEWT_VAL(BLE_PHY_CODED)
+    case 0x3:
+        connsm->pwr_ctrl.req_phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+        break;
+    case 0x4:
+        connsm->pwr_ctrl.req_phy_mode = BLE_PHY_MODE_CODED_125KBPS;
+        break;
+#endif
+    default:
+        return BLE_ERR_UNSUPPORTED;
+    }
+
+    connsm->flags.power_request_host_w4event = 1;
+
+    ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_POWER_CTRL_REQ, NULL);
+
+    return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_conn_hci_set_tx_power_report_enable(const uint8_t *cmdbuf, uint8_t len,
+                                           uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_le_set_transmit_power_report_enable_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_le_set_transmit_power_report_enable_rp *rsp = (void *) rspbuf;
+    struct ble_ll_conn_sm *connsm;
+    uint8_t send_req;
+    int i;
+    int rc;
+
+    if (len != sizeof(*cmd)) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+    if (!connsm) {
+        rc = BLE_ERR_UNK_CONN_ID;
+        goto done;
+    }
+
+    if (cmd->local_enable & 0xfe || cmd->remote_enable & 0xfe) {
+        rc = BLE_ERR_INV_HCI_CMD_PARMS;
+        goto done;
+    }
+
+    connsm->pwr_ctrl.local_reports_enabled = cmd->local_enable;
+    connsm->pwr_ctrl.remote_reports_enabled = cmd->remote_enable;
+
+    send_req = 1;
+    for (i = 0; i < BLE_PHY_NUM_MODE; i++) {
+        if (connsm->pwr_ctrl.remote_tx_power[i] != 127) {
+            send_req = 0;
+            break;
+        }
+    }
+
+    if (send_req) {
+        connsm->pwr_ctrl.req_phy_mode = connsm->phy_data.rx_phy_mode;
+        ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_POWER_CTRL_REQ, NULL);
+    }
+
+    rc = BLE_ERR_SUCCESS;
+
+done:
+    *rsplen = sizeof(*rsp);
+    rsp->conn_handle = cmd->conn_handle;
+    return rc;
+}
+#endif
 #endif
diff --git a/nimble/controller/src/ble_ll_conn_priv.h b/nimble/controller/src/ble_ll_conn_priv.h
@@ -274,6 +274,15 @@ int ble_ll_set_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len,
 int ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len);
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL)
+int ble_ll_conn_hci_enhanced_read_tx_power_level(const uint8_t *cmdbuf, uint8_t len,
+                                                 uint8_t *rspbuf, uint8_t *rsplen);
+int ble_ll_conn_hci_read_remote_tx_power_level(const uint8_t *cmdbuf, uint8_t len);
+int ble_ll_conn_hci_set_tx_power_report_enable(const uint8_t *cmdbuf, uint8_t len,
+                                               uint8_t *rspbuf, uint8_t *rsplen);
+void ble_ll_conn_update_current_local_tx_power(struct ble_ll_conn_sm *connsm);
+#endif
+
 #if MYNEWT_VAL(BLE_LL_CONN_STRICT_SCHED)
 void ble_ll_conn_css_set_next_slot(uint16_t slot_idx);
 uint16_t ble_ll_conn_css_get_next_slot(void);