From d330835a6109d38ec1bd6b8d4f0c8cd38a733ff2 Mon Sep 17 00:00:00 2001 From: Michal Gorecki Date: Wed, 6 Nov 2024 11:36:00 +0100 Subject: [PATCH] nimble/ll: Power Control Request feature support --- .../include/controller/ble_ll_conn.h | 39 ++++ .../include/controller/ble_ll_ctrl.h | 4 + nimble/controller/src/ble_ll.c | 4 + nimble/controller/src/ble_ll_conn.c | 58 +++++ nimble/controller/src/ble_ll_conn_hci.c | 150 +++++++++++++ nimble/controller/src/ble_ll_conn_priv.h | 9 + nimble/controller/src/ble_ll_ctrl.c | 212 ++++++++++++++++++ nimble/controller/src/ble_ll_hci.c | 16 ++ nimble/controller/src/ble_ll_hci_ev.c | 51 +++++ nimble/controller/syscfg.yml | 8 + nimble/include/nimble/hci_common.h | 4 + 11 files changed, 555 insertions(+) diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h index 1d29063bf8..0351ab5eba 100644 --- 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 index 89502a45a3..9dc5c23e02 100644 --- 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 index f2fc37d6d1..afb9195310 100644 --- 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 index 789b948aad..efef0479a1 100644 --- 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 index a7bb136f70..abe48fd481 100644 --- 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 index 954a27663a..e0bfb0e8e1 100644 --- 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); diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c index 6902ba4958..71a1dee35a 100644 --- a/nimble/controller/src/ble_ll_ctrl.c +++ b/nimble/controller/src/ble_ll_ctrl.c @@ -33,6 +33,7 @@ #include "controller/ble_ll_sync.h" #include "controller/ble_ll_tmr.h" #include "ble_ll_conn_priv.h" +#include "ble_ll_priv.h" #if MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL) || MYNEWT_VAL(BLE_LL_ROLE_CENTRAL) @@ -1313,6 +1314,200 @@ ble_ll_ctrl_rx_subrate_ind(struct ble_ll_conn_sm *connsm, uint8_t *req, } #endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL) +static int8_t +ble_ll_ctrl_rx_power_control_phy_bit_to_mode(uint8_t phy_bit) +{ + switch (phy_bit) { + case 0x1: + return BLE_PHY_MODE_1M; +#if MYNEWT_VAL(BLE_PHY_2M) + case 0x2: + return BLE_PHY_MODE_2M; +#endif +#if MYNEWT_VAL(BLE_PHY_CODED) + case 0x4: + return BLE_PHY_MODE_CODED_500KBPS; + case 0x8: + return BLE_PHY_MODE_CODED_125KBPS; +#endif + default: + return -1; + } +} + +static void +ble_ll_ctrl_power_control_req_make(struct ble_ll_conn_sm *connsm, uint8_t *ctrdata) +{ + switch (connsm->pwr_ctrl.req_phy_mode) { + case BLE_PHY_MODE_1M: + ctrdata[0] = 0x1; + break; +#if MYNEWT_VAL(BLE_PHY_2M) + case BLE_PHY_MODE_2M: + ctrdata[0] = 0x2; + break; +#endif +#if MYNEWT_VAL(BLE_PHY_CODED) + case BLE_PHY_MODE_CODED_500KBPS: + ctrdata[0] = 0x4; + break; + case BLE_PHY_MODE_CODED_125KBPS: + ctrdata[0] = 0x8; + break; +#endif + default: + BLE_LL_ASSERT(0); + } + + ctrdata[1] = connsm->pwr_ctrl.req_delta; + ctrdata[2] = connsm->pwr_ctrl.local_tx_power[connsm->pwr_ctrl.req_phy_mode]; +} + +static void +ble_ll_ctrl_power_control_rsp_make(struct ble_ll_conn_sm *connsm, uint8_t *ctrdata, + uint8_t phy_mode, int8_t delta) +{ + int8_t prev_tx_power; + int8_t new_tx_power; + int8_t max_tx_power; + int8_t min_tx_power; + int16_t result; + int increase; + + prev_tx_power = connsm->pwr_ctrl.local_tx_power[phy_mode]; + max_tx_power = ble_ll_tx_power_round(INT8_MAX); + min_tx_power = ble_ll_tx_power_round(INT8_MIN); + + if (delta == 0x7F) { + new_tx_power = max_tx_power; + } else if (delta == 0) { + new_tx_power = prev_tx_power; + } else { + result = prev_tx_power + delta; + increase = 0; + /* + * ble_ll_tx_power_round returns tx_power lower or equal to the result, + * while we need higher or equal, so we increase the result until new_tx_power + * is not less than it. + */ + do { + new_tx_power = ble_ll_tx_power_round(result + increase); + increase++; + } while((new_tx_power < result) && (new_tx_power != max_tx_power)); + } + + if (new_tx_power == max_tx_power) { + connsm->pwr_ctrl.local_min_max[phy_mode] = 0x02; + ctrdata[0] = 0x02; + } else if (new_tx_power == min_tx_power) { + connsm->pwr_ctrl.local_min_max[phy_mode] = 0x01; + ctrdata[0] = 0x01; + } else { + connsm->pwr_ctrl.local_min_max[phy_mode] = 0x0; + ctrdata[0] = 0x0; + } + + connsm->pwr_ctrl.local_tx_power[phy_mode] = new_tx_power; + + ctrdata[1] = connsm->pwr_ctrl.local_tx_power[phy_mode] - prev_tx_power; + ctrdata[2] = connsm->pwr_ctrl.local_tx_power[phy_mode]; + /* For now, we don't support Acceptable Power Reduction (Vol 6 Part B 5.1.17.1) */ + ctrdata[3] = 0xff; +} + +static uint8_t +ble_ll_ctrl_rx_power_control_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + uint8_t phy_bit; + int8_t phy_mode; + int8_t delta; + int8_t tx_power; + + phy_bit = dptr[0]; + delta = (int8_t) dptr[1]; + tx_power = (int8_t) dptr[2]; + + phy_mode = ble_ll_ctrl_rx_power_control_phy_bit_to_mode(phy_bit); + + if (phy_mode < 0) { + rspbuf[0] = BLE_LL_CTRL_POWER_CONTROL_REQ; + rspbuf[1] = BLE_ERR_UNSUPP_LMP_LL_PARM; + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + if (tx_power == 126) { + rspbuf[0] = BLE_LL_CTRL_POWER_CONTROL_REQ; + rspbuf[1] = BLE_ERR_INV_LMP_LL_PARM; + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ble_ll_ctrl_power_control_rsp_make(connsm, rspbuf, phy_mode, delta); + ble_ll_conn_update_current_local_tx_power(connsm); + + return BLE_LL_CTRL_POWER_CONTROL_RSP; +} + +static void +ble_ll_ctrl_rx_power_control_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t tx_power_flags; + int8_t delta; + int8_t tx_power; + + if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_POWER_CTRL_REQ) { + ble_npl_callout_stop(&connsm->ctrl_proc_rsp_timer); + + tx_power = (int8_t) dptr[2]; + if (tx_power == 126) { + return; + } + + tx_power_flags = dptr[0]; + delta = (int8_t) dptr[1]; + connsm->pwr_ctrl.remote_tx_power[connsm->pwr_ctrl.req_phy_mode] = tx_power; + + if (connsm->flags.power_request_host_w4event) { + ble_ll_hci_ev_transmit_power_report(connsm, BLE_ERR_SUCCESS, 0x02, + connsm->pwr_ctrl.req_phy_mode, + tx_power, tx_power_flags, delta); + connsm->flags.power_request_host_w4event = 0; + } + + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_POWER_CTRL_REQ); + } +} + +void +ble_ll_ctrl_rx_power_change_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t phy_bit; + uint8_t tx_power_flags; + int8_t delta; + int8_t tx_power; + uint8_t phy_mode; + + phy_bit = dptr[0]; + tx_power_flags = dptr[1]; + delta = (int8_t) dptr[2]; + tx_power = (int8_t) dptr[3]; + + phy_mode = ble_ll_ctrl_rx_power_control_phy_bit_to_mode(phy_bit); + + if (phy_mode < 0) { + return; + } + + connsm->pwr_ctrl.remote_tx_power[phy_mode] = tx_power; + + if (connsm->pwr_ctrl.remote_reports_enabled) { + ble_ll_hci_ev_transmit_power_report(connsm, BLE_ERR_SUCCESS, 0x01, + phy_mode, tx_power, tx_power_flags, delta); + } +} +#endif + /** * Create a link layer length request or length response PDU. * @@ -2548,6 +2743,12 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data) &connsm->subrate_trans); break; #endif +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL) + case BLE_LL_CTRL_PROC_POWER_CTRL_REQ: + opcode = BLE_LL_CTRL_POWER_CONTROL_REQ; + ble_ll_ctrl_power_control_req_make(connsm, ctrdata); + break; #endif default: BLE_LL_ASSERT(0); @@ -3018,6 +3219,17 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) case BLE_LL_CTRL_SUBRATE_IND: rsp_opcode = ble_ll_ctrl_rx_subrate_ind(connsm, dptr, rspdata); break; +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL) + case BLE_LL_CTRL_POWER_CONTROL_REQ: + rsp_opcode = ble_ll_ctrl_rx_power_control_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_POWER_CONTROL_RSP: + ble_ll_ctrl_rx_power_control_rsp(connsm, dptr); + break; + case BLE_LL_CTRL_POWER_CHANGE_IND: + ble_ll_ctrl_rx_power_change_ind(connsm, dptr); + break; #endif default: /* Nothing to do here */ diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c index 7e2d707bd8..f4e08f588e 100644 --- a/nimble/controller/src/ble_ll_hci.c +++ b/nimble/controller/src/ble_ll_hci.c @@ -677,6 +677,9 @@ ble_ll_hci_le_cmd_send_cmd_status(uint16_t ocf) #endif #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE) case BLE_HCI_OCF_LE_REQ_PEER_SCA: +#endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL) + case BLE_HCI_OCF_LE_READ_REMOTE_TRANSMIT_POWER_LEVEL: #endif case BLE_HCI_OCF_LE_SUBRATE_REQ: rc = 1; @@ -1303,6 +1306,19 @@ ble_ll_hci_le_cmd_proc(const uint8_t *cmdbuf, uint8_t len, uint16_t ocf, rc = ble_ll_set_host_feat(cmdbuf, len); break; #endif +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL) + case BLE_HCI_OCF_LE_ENH_READ_TRANSMIT_POWER_LEVEL: + rc = ble_ll_conn_hci_enhanced_read_tx_power_level(cmdbuf, len, + rspbuf, rsplen); + break; + case BLE_HCI_OCF_LE_READ_REMOTE_TRANSMIT_POWER_LEVEL: + rc = ble_ll_conn_hci_read_remote_tx_power_level(cmdbuf, len); + break; + case BLE_HCI_OCF_LE_SET_TRANS_PWR_REPORT_ENABLE: + rc = ble_ll_conn_hci_set_tx_power_report_enable(cmdbuf, len, + rspbuf, rsplen); + break; +#endif /* BLE_LL_CFG_FEAT_LL_POWER_CONTROL */ #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_SCA_UPDATE) case BLE_HCI_OCF_LE_REQ_PEER_SCA: rc = ble_ll_conn_req_peer_sca(cmdbuf, len, diff --git a/nimble/controller/src/ble_ll_hci_ev.c b/nimble/controller/src/ble_ll_hci_ev.c index 8d088d9da8..6e5c694d5b 100644 --- a/nimble/controller/src/ble_ll_hci_ev.c +++ b/nimble/controller/src/ble_ll_hci_ev.c @@ -494,6 +494,57 @@ ble_ll_hci_ev_sca_update(struct ble_ll_conn_sm *connsm, uint8_t status, #endif + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_POWER_CONTROL) +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) +{ + struct ble_hci_ev_le_subev_transmit_power_report *ev; + struct ble_hci_ev *hci_ev; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_TRANSMIT_POWER_REPORT)) { + return; + } + + hci_ev = ble_transport_alloc_evt(0); + if (!hci_ev) { + return; + } + + hci_ev->opcode = BLE_HCI_EVCODE_LE_META; + hci_ev->length = sizeof(*ev); + ev = (void *) hci_ev->data; + + ev->subev_code = BLE_HCI_LE_SUBEV_TRANSMIT_POWER_REPORT; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->reason = reason; + + switch (phy_mode) { + case BLE_PHY_MODE_1M: + case BLE_PHY_MODE_2M: + ev->phy = phy_mode; + break; + case BLE_PHY_MODE_CODED_500KBPS: + ev->phy = 0x03; + break; + case BLE_PHY_MODE_CODED_125KBPS: + ev->phy = 0x04; + break; + default: + return; + } + + ev->transmit_power_level = tx_power; + ev->transmit_power_level_flag = tx_power_flags; + ev->delta = delta; + + ble_ll_hci_event_send(hci_ev); +} +#endif + #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_ENHANCED_CONN_UPDATE) void ble_ll_hci_ev_subrate_change(struct ble_ll_conn_sm *connsm, uint8_t status) diff --git a/nimble/controller/syscfg.yml b/nimble/controller/syscfg.yml index 862a36a523..875e281703 100644 --- a/nimble/controller/syscfg.yml +++ b/nimble/controller/syscfg.yml @@ -357,6 +357,14 @@ syscfg.defs: restrictions: - '(BLE_VERSION >= 53) if 1' + BLE_LL_CFG_FEAT_LL_POWER_CONTROL: + description: > + This option is used to enable/disable support for Power + Control Feature. + value: MYNEWT_VAL(BLE_POWER_CONTROL) + restrictions: + - '(BLE_VERSION >= 52) if 1' + BLE_LL_PERIODIC_ADV_SYNC_BIGINFO_REPORTS: description: > This option is used to enable/disable support for diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 252b033973..0b87635569 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -1130,6 +1130,10 @@ struct ble_hci_le_set_transmit_power_report_enable_cp { uint8_t local_enable; uint8_t remote_enable; } __attribute__((packed)); +struct ble_hci_le_set_transmit_power_report_enable_rp { + uint16_t conn_handle; +} __attribute__((packed)); + #define BLE_HCI_OCF_LE_SET_DEFAULT_SUBRATE (0x007D) struct ble_hci_le_set_default_subrate_cp {