add tx power to cordio hci driver

Author: paul-szczepanek-arm

features/FEATURE_BLE/targets/TARGET_CORDIO/driver/CordioHCIDriver.cpp

@@ -287,6 +287,10 @@ void CordioHCIDriver::on_host_stack_inactivity()
 {
 }
 
+ble_error_t CordioHCIDriver::set_tx_power(int8_t level_db) {
+    return BLE_ERROR_NOT_IMPLEMENTED;
+}
+
 } // namespace cordio
 } // namespace vendor
 } // namespace ble

features/FEATURE_BLE/targets/TARGET_CORDIO/driver/CordioHCIDriver.h

@@ -22,6 +22,7 @@
 #include <BLETypes.h>
 #include "wsf_buf.h"
 #include "CordioHCITransportDriver.h"
+#include "ble/blecommon.h"
 
 namespace ble {
 namespace vendor {
@@ -144,6 +145,16 @@ class CordioHCIDriver {
      */
      virtual void on_host_stack_inactivity();
 
+     /**
+      * Set desired transmit power. Value equal or bigger will be used from available levels.
+      * Consult chip documentation for available values. Actual TX power is not guaranteed
+      * and is down to the implementation.
+      *
+      * @param level_db Signal level in dBm.
+      * @return BLE_ERROR_NONE on success.
+      */
+     virtual ble_error_t set_tx_power(int8_t level_db);
+
 protected:
     /**
      * Return a default set of memory pool that the Cordio stack can use.

features/FEATURE_BLE/targets/TARGET_CORDIO/mbed_lib.json

@@ -63,6 +63,10 @@
             "help": "Where the CBC MAC calculatio is performed. Valid values are 0 (host) and 1 (controller through HCI).",
             "value": 1,
             "macro_name": "SEC_CCM_CFG"
+        },
+        "preferred-tx-power": {
+            "help": "Preferred value of tx power in dbm (-128,127). This value is not guaranteed and relies on existing support in the HCI driver.",
+            "value": 0
         }
     }
 }

features/FEATURE_BLE/targets/TARGET_STM/TARGET_STM32WB/HCIDriver.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <stdio.h>
+#include "ble/blecommon.h"
 #include "CordioBLE.h"
 #include "CordioHCIDriver.h"
 #include "CordioHCITransportDriver.h"
@@ -131,6 +132,33 @@ class HCIDriver : public cordio::CordioHCIDriver {
         HciResetCmd();
     }
 
+    static uint8_t convert_db_to_tx_power_index(int8_t level_db) {
+        const int8_t conversion[] = {
+            -40, -21 - 10 - 19, -18, -16, -15, -14, -13, -12, -11, -10, -9, -8,
+            -7, -6, -5, -4, -3, -2, -2, -1, -1, 0, 0, 0, 1, 2, 3, 4, 5, 6,
+        };
+
+        uint8_t index;
+        for (index = 0; index < sizeof(conversion); ++index) {
+            if (level_db <= conversion[index]) {
+                break;
+            }
+        }
+        return index;
+    }
+
+    virtual ble_error_t set_tx_power(int8_t level_db) {
+
+
+        uint8_t buf[2];
+        buf[0] = 0x1; // Enable high power mode - deprecated and ignored on STM32WB
+        buf[1] = convert_db_to_tx_power_index(level_db);
+
+        HciVendorSpecificCmd(ACI_HAL_SET_TX_POWER_LEVEL, 2, buf);
+
+        return BLE_ERROR_NONE;
+    }
+
     /**
      * @see CordioHCIDriver::handle_reset_sequence
      */
@@ -177,15 +205,15 @@ class HCIDriver : public cordio::CordioHCIDriver {
                     } else {
                         tr_info("could not find BDaddr");
                         /*  Skip to next step */
-                        aciSetTxPowerLevel();
+                        set_tx_power(MBED_CONF_CORDIO_PREFERRED_TX_POWER);
                     }
                     break;
 
                 case ACI_WRITE_CONFIG_DATA_OPCODE:
                     tr_debug("Bluetooth Device address set");
                     /* set the event mask to control which events are generated by the
                      * controller for the host */
-                    aciSetTxPowerLevel();
+                    set_tx_power(MBED_CONF_CORDIO_PREFERRED_TX_POWER);
                     break;
 
 
@@ -359,16 +387,6 @@ class HCIDriver : public cordio::CordioHCIDriver {
 
 private:
     uint8_t bd_addr[6];
-    void aciSetTxPowerLevel()
-    {
-        uint8_t *pBuf = hciCmdAlloc(ACI_HAL_SET_TX_POWER_LEVEL, 2);
-        if (!pBuf) {
-            return;
-        }
-        pBuf[HCI_CMD_HDR_LEN] = 0x1;
-        pBuf[HCI_CMD_HDR_LEN + 1] = 0x18;
-        hciCmdSend(pBuf);
-    }
 
     void aciReadConfigParameter(uint8_t offset)
     {