Skip to content

Commit

Permalink
nimble/ll: Power Control Request feature support
Browse files Browse the repository at this point in the history
  • Loading branch information
@m-gorecki
m-gorecki committed Nov 12, 2024
1 parent b73cbab commit d330835
Show file tree
Hide file tree
Showing 11 changed files with 555 additions and 0 deletions.
39 changes: 39 additions & 0 deletions nimble/controller/include/controller/ble_ll_conn.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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 */
Expand Down
4 changes: 4 additions & 0 deletions nimble/controller/include/controller/ble_ll_ctrl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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);
Expand Down
4 changes: 4 additions & 0 deletions nimble/controller/src/ble_ll.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
58 changes: 58 additions & 0 deletions nimble/controller/src/ble_ll_conn.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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.
*
Expand Down Expand Up @@ -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) {
Expand Down
150 changes: 150 additions & 0 deletions nimble/controller/src/ble_ll_conn_hci.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
9 changes: 9 additions & 0 deletions nimble/controller/src/ble_ll_conn_priv.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Loading

0 comments on commit d330835

Please sign in to comment.