nimble/ll: Add VS command for setting transmit power

This command allows to override TX power configured with
BLE_LL_TX_PWR_DBM syscfg value. If 0xff is provided BLE_LL_TX_PWR_DBM
TX power is restored.

Author: sjanc

nimble/controller/src/ble_ll.c

@@ -65,6 +65,8 @@
  * right thing to do.
  */
 
+int8_t g_ble_ll_tx_power = MYNEWT_VAL(BLE_LL_TX_PWR_DBM);
+
 /* Supported states */
 #if MYNEWT_VAL(BLE_LL_ROLE_BROADCASTER)
 #define BLE_LL_S_NCA                    ((uint64_t)1 << 0)
@@ -1391,7 +1393,7 @@ ble_ll_task(void *arg)
     ble_phy_init();
 
     /* Set output power to 1mW (0 dBm) */
-    ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+    ble_phy_txpwr_set(g_ble_ll_tx_power);
 
     /* Register callback for transport */
     ble_hci_trans_cfg_ll(ble_ll_hci_cmd_rx, NULL, ble_ll_hci_acl_rx, NULL);

nimble/controller/src/ble_ll_adv.c

@@ -41,6 +41,7 @@
 #include "controller/ble_ll_utils.h"
 #include "controller/ble_ll_rfmgmt.h"
 #include "ble_ll_conn_priv.h"
+#include "ble_ll_priv.h"
 
 #if MYNEWT_VAL(BLE_LL_ROLE_BROADCASTER)
 
@@ -1023,7 +1024,7 @@ ble_ll_adv_tx_done(void *arg)
     struct ble_ll_adv_sm *advsm;
 
     /* reset power to max after advertising */
-    ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+    ble_phy_txpwr_set(g_ble_ll_tx_power);
 
     advsm = (struct ble_ll_adv_sm *)arg;
 
@@ -1687,7 +1688,7 @@ ble_ll_adv_halt(void)
 
         ble_ll_trace_u32(BLE_LL_TRACE_ID_ADV_HALT, advsm->adv_instance);
 
-        ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+        ble_phy_txpwr_set(g_ble_ll_tx_power);
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV)
         if (advsm->flags & BLE_LL_ADV_SM_FLAG_PERIODIC_SYNC_SENDING) {
@@ -2112,8 +2113,8 @@ ble_ll_adv_sync_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
 static void
 ble_ll_adv_sync_tx_done(struct ble_ll_adv_sm *advsm)
 {
-    /* reset power to max after advertising */
-    ble_phy_txpwr_set(MYNEWT_VAL(BLE_LL_TX_PWR_DBM));
+    /* reset power to default after advertising */
+    ble_phy_txpwr_set(g_ble_ll_tx_power);
 
     /* for sync we trace a no pri nor sec set */
     ble_ll_trace_u32x2(BLE_LL_TRACE_ID_ADV_TXDONE, advsm->adv_instance, 0);

nimble/controller/src/ble_ll_hci_vs.c

@@ -21,7 +21,12 @@
 #include "syscfg/syscfg.h"
 #include "controller/ble_ll.h"
 #include "controller/ble_ll_hci.h"
+#include "controller/ble_ll_sync.h"
+#include "controller/ble_ll_adv.h"
+#include "controller/ble_ll_scan.h"
 #include "controller/ble_hw.h"
+#include "ble_ll_conn_priv.h"
+#include "ble_ll_priv.h"
 
 #if MYNEWT_VAL(BLE_LL_HCI_VS)
 
@@ -51,9 +56,89 @@ ble_ll_hci_vs_rd_static_addr(uint16_t ocf,
     return BLE_ERR_SUCCESS;
 }
 
+/* disallow changing TX power if there is any radio activity
+ * note: we could allow to change it if there is no TX activity (eg only
+ * passive scan or sync) but lets just keep this simple for now
+ */
+static int
+ble_ll_hci_vs_is_controller_busy(void)
+{
+#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL) || MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL)
+    struct ble_ll_conn_sm *cur;
+    int i = 0;
+#endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) && MYNEWT_VAL(BLE_LL_ROLE_OBSERVER)
+    if (ble_ll_sync_enabled()) {
+        return 1;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_ROLE_BROADCASTER)
+    if (ble_ll_adv_enabled()) {
+        return 1;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_ROLE_OBSERVER)
+    if (ble_ll_scan_enabled()) {
+        return 1;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL)
+    if (g_ble_ll_conn_create_sm.connsm) {
+        return 1;
+    }
+#endif
+
+#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL) || MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL)
+    STAILQ_FOREACH(cur, &g_ble_ll_conn_free_list, free_stqe) {
+        i++;
+    }
+
+    /* check if all connection objects are free */
+    if (i < MYNEWT_VAL(BLE_MAX_CONNECTIONS)) {
+        return 1;
+    }
+#endif
+
+    return 0;
+}
+
+static int
+ble_ll_hci_vs_set_tx_power(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen,
+                           uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_vs_set_tx_pwr_cp *cmd = (const void *) cmdbuf;
+    struct ble_hci_vs_set_tx_pwr_rp *rsp = (void *) rspbuf;
+
+    if (cmdlen != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_hci_vs_is_controller_busy()) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    if (cmd->tx_power == 127) {
+        /* restore reset default */
+        g_ble_ll_tx_power = MYNEWT_VAL(BLE_LL_TX_PWR_DBM);
+    } else {
+        g_ble_ll_tx_power = ble_phy_txpower_round(cmd->tx_power);
+    }
+
+    rsp->tx_power = g_ble_ll_tx_power;
+    *rsplen = sizeof(*rsp);
+
+    return BLE_ERR_SUCCESS;
+}
+
 static struct ble_ll_hci_vs_cmd g_ble_ll_hci_vs_cmds[] = {
     BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_RD_STATIC_ADDR,
                       ble_ll_hci_vs_rd_static_addr),
+    BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_SET_TX_PWR,
+            ble_ll_hci_vs_set_tx_power),
 };
 
 static struct ble_ll_hci_vs_cmd *

nimble/controller/src/ble_ll_priv.h

@@ -24,6 +24,8 @@
 extern "C" {
 #endif
 
+extern int8_t g_ble_ll_tx_power;
+
 #ifdef MYNEWT
 
 #include "syscfg/syscfg.h"

nimble/controller/syscfg.yml

@@ -68,7 +68,9 @@ syscfg.defs:
         range: 0..500
 
     BLE_LL_TX_PWR_DBM:
-        description: 'Transmit power level.'
+        description: >
+            Default Transmit power level (in dBm). Actual transmit power
+            may be rounded up or down depending on used radio.
         value: '0'
 
     BLE_LL_NUM_COMP_PKT_ITVL_MS:

nimble/include/nimble/hci_common.h

@@ -1062,11 +1062,23 @@ struct ble_hci_le_set_host_feat_cp {
 } __attribute__((packed));
 
 /* --- Vendor specific commands (OGF 0x003F) */
-#define BLE_HCI_OCF_VS_RD_STATIC_ADDR                 (0x0001)
+/* Read Random Static Address */
+#define BLE_HCI_OCF_VS_RD_STATIC_ADDR                   (0x0001)
 struct ble_hci_vs_rd_static_addr_rp {
     uint8_t addr[6];
 } __attribute__((packed));
 
+/* Set default transmit power. Actual selected TX power is returned
+ * in reply. Setting 0xff restores controller reset default.
+ */
+#define BLE_HCI_OCF_VS_SET_TX_PWR                       (0x0002)
+struct ble_hci_vs_set_tx_pwr_cp {
+    int8_t tx_power;
+} __attribute__((packed));
+struct ble_hci_vs_set_tx_pwr_rp {
+    int8_t tx_power;
+} __attribute__((packed));
+
 /* Command Specific Definitions */
 /* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */
 #define BLE_HCI_CTLR_TO_HOST_FC_OFF         (0)