diff --git a/nimble/controller/include/controller/ble_fem.h b/nimble/controller/include/controller/ble_fem.h index bd0b93e13a..e12842ccb4 100644 --- a/nimble/controller/include/controller/ble_fem.h +++ b/nimble/controller/include/controller/ble_fem.h @@ -36,6 +36,9 @@ int ble_fem_pa_tx_power_set(int tx_power); /* returns rounded FEM TX power */ int ble_fem_pa_tx_power_round(int tx_power); + +/* returns rounded up FEM TX power */ +int ble_fem_pa_tx_power_round(int tx_power); #endif #endif diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h index 1d29063bf8..8c8f947233 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,32 @@ 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 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 */ + int8_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 +426,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/include/controller/ble_phy.h b/nimble/controller/include/controller/ble_phy.h index fe70044234..d8e7fc8bd8 100644 --- a/nimble/controller/include/controller/ble_phy.h +++ b/nimble/controller/include/controller/ble_phy.h @@ -121,6 +121,9 @@ int ble_phy_tx_power_set(int dbm); /* Get highest allowed power from range */ int ble_phy_tx_power_round(int dbm); +/* Get the next allowed power level above the specified value */ +int ble_phy_tx_power_round_up(int dbm); + /* Get the transmit power */ int ble_phy_tx_power_get(void); diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index f2fc37d6d1..cb694b9c0e 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -2030,6 +2030,24 @@ ble_ll_tx_power_round(int tx_power) return tx_power; } +/* TODO(m): is this necessary? */ +int +ble_ll_tx_power_round_up(int tx_power) +{ +#if MYNEWT_VAL(BLE_FEM_PA) +#if MYNEWT_VAL(BLE_FEM_PA_GAIN_TUNABLE) + tx_power = ble_fem_pa_tx_power_round_up(tx_power); +#else + tx_power = ble_phy_tx_power_round_up(tx_power); + tx_power += MYNEWT_VAL(BLE_FEM_PA_GAIN); +#endif +#else + tx_power = ble_phy_tx_power_round_up(tx_power); +#endif + + return tx_power; +} + void ble_ll_tx_power_set(int tx_power) { diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 789b948aad..fbf5860c97 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,19 @@ 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.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 +2035,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 +2346,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_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 +2712,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_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..f8cc650609 100644 --- a/nimble/controller/src/ble_ll_conn_hci.c +++ b/nimble/controller/src/ble_ll_conn_hci.c @@ -2176,4 +2176,134 @@ 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); + + connsm->flags.power_request_host_w4event = 1; + + 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; + } + + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_POWER_CTRL_REQ, NULL); + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_conn_hci_set_trans_pwr_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; + 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; + } + + connsm->pwr_ctrl.local_reports_enabled = cmd->local_enable; + connsm->pwr_ctrl.remote_reports_enabled = cmd->remote_enable; + + 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..d97cd5bd6e 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_trans_pwr_report_enable(const uint8_t *cmdbuf, uint8_t len, + uint8_t *rspbuf, uint8_t *rsplen); +void ble_ll_conn_update_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..cd07eb5368 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,188 @@ 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; + case BLE_PHY_MODE_2M: + ctrdata[0] = 0x2; + break; + case BLE_PHY_MODE_CODED_500KBPS: + ctrdata[0] = 0x4; + break; + case BLE_PHY_MODE_CODED_125KBPS: + ctrdata[0] = 0x8; + break; + 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; + + 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; + new_tx_power = ble_ll_tx_power_round_up(result); + } + + 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 = dptr[1]; + tx_power = 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); + +#if MYNEWT_VAL(BLE_LL_PHY) + if (connsm->phy_data.tx_phy_mode == phy_mode) { + ble_ll_conn_update_local_tx_power(connsm); + } +#else + ble_ll_conn_update_local_tx_power(connsm, connsm->pwr_ctrl.local_phy_mode_tx_power[phy_mode]); +#endif + + 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 = dptr[2]; + if (tx_power == 126) { + return; + } + + tx_power_flags = dptr[0]; /* TODO(m): check this */ + delta = dptr[1]; + + 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; + } + } +} + +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 = dptr[2]; + tx_power = dptr[3]; + + phy_mode = ble_ll_ctrl_rx_power_control_phy_bit_to_mode(phy_bit); + + if (phy_mode < 0) { + return; + } + + 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 +2731,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 +3207,16 @@ 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); #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..31b7b03ed2 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_trans_pwr_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/src/ble_ll_priv.h b/nimble/controller/src/ble_ll_priv.h index f0498fea8a..92db83e1a5 100644 --- a/nimble/controller/src/ble_ll_priv.h +++ b/nimble/controller/src/ble_ll_priv.h @@ -30,6 +30,7 @@ extern int8_t g_ble_ll_rx_power_compensation; int ble_ll_tx_power_round(int tx_power); void ble_ll_tx_power_set(int tx_power); +int ble_ll_tx_power_round_up(int tx_power); static inline int ble_ll_rx_gain(void) diff --git a/nimble/controller/syscfg.yml b/nimble/controller/syscfg.yml index 862a36a523..c1e3f50f9f 100644 --- a/nimble/controller/syscfg.yml +++ b/nimble/controller/syscfg.yml @@ -357,6 +357,12 @@ syscfg.defs: restrictions: - '(BLE_VERSION >= 53) if 1' + BLE_LL_CFG_FEAT_LL_POWER_CONTROL: + description: > + This option is use to enable/disable support for Periodic + Advertising Sync Transfer Feature. + value: MYNEWT_VAL(BLE_POWER_CONTROL) + BLE_LL_PERIODIC_ADV_SYNC_BIGINFO_REPORTS: description: > This option is used to enable/disable support for @@ -440,7 +446,8 @@ syscfg.defs: BLE_FEM_PA_GAIN_TUNABLE: description: > PA TX gain is tunable and not constant. If enabled - ble_fem_pa_tx_power_set() and ble_fem_pa_tx_power_round() + ble_fem_pa_tx_power_set(), ble_fem_pa_tx_power_round() + and ble_fem_pa_tx_power_round_up() shall be implemented (see ble_fem.h for details). value: 0 BLE_FEM_PA_GPIO: diff --git a/nimble/drivers/nrf5x/src/ble_phy.c b/nimble/drivers/nrf5x/src/ble_phy.c index f556f65024..851c3f422d 100644 --- a/nimble/drivers/nrf5x/src/ble_phy.c +++ b/nimble/drivers/nrf5x/src/ble_phy.c @@ -2050,6 +2050,21 @@ ble_phy_tx_power_round(int dbm) return phy_txpower_round(dbm); } +/** + * ble phy txpwr round + * + * Get the rounded up transmit output power (in dBm). + * + * @param dbm Power output in dBm. + * + * @return int Rounded up power in dBm + */ +int +ble_phy_tx_power_round_up(int dbm) +{ + return phy_txpower_round_up(dbm); +} + /** * ble phy set access addr * diff --git a/nimble/drivers/nrf5x/src/nrf52/phy.c b/nimble/drivers/nrf5x/src/nrf52/phy.c index 30a19966b7..055b658742 100644 --- a/nimble/drivers/nrf5x/src/nrf52/phy.c +++ b/nimble/drivers/nrf5x/src/nrf52/phy.c @@ -218,3 +218,43 @@ phy_txpower_round(int8_t dbm) return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm; } + +/* TODO(m): Add this to other BSPs */ +int8_t +phy_txpower_round_up(int8_t dbm) +{ + if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm; + } + + if (dbm > (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm; + } + + if (dbm > (int8_t)RADIO_TXPOWER_TXPOWER_0dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm; + } + + if (dbm > (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_0dBm; + } + + if (dbm > (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm; + } + + if (dbm > (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm; + } + + if (dbm > (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm; + } + + if (dbm > (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm) { + return (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm; + } + + return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm; +} + diff --git a/nimble/drivers/nrf5x/src/phy_priv.h b/nimble/drivers/nrf5x/src/phy_priv.h index db0664da2b..489bfd6fe3 100644 --- a/nimble/drivers/nrf5x/src/phy_priv.h +++ b/nimble/drivers/nrf5x/src/phy_priv.h @@ -85,6 +85,7 @@ void phy_ppi_init(void); void phy_txpower_set(int8_t dbm); int8_t phy_txpower_round(int8_t dbm); +int8_t phy_txpower_round_up(int8_t dbm); #ifdef NRF52_SERIES #include "nrf52/phy_ppi.h" 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 {