BLE: Add tx power control to cordio HCI driver

features/FEATURE_BLE/targets/TARGET_CORDIO/doc/HCIAbstraction.md

@@ -46,3 +46,9 @@ specific to the controller used.
 The driver also provides an interface to perform RF testing on the BLE trasnmitter.
 This is done using the LE Receiver/Transmitter Test command and LE Test End command
 as described in the Bluetooth Core spec in Vol.2, Part E, 7.8.28-30. 
+
+The driver allows to set preferred TX power. This is an optional feature
+(check return code) and the support for it and the extent of control is
+down to the BLE chip. Chips may have different allowed values, although the
+command will pick the closest value no smaller than requested. Please refer
+to the controller code or BLE chip documentation.

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

@@ -364,6 +364,10 @@ ble_error_t CordioHCIDriver::rf_test_end()
     return BLE_ERROR_NO_MEM;
 }
 
+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

@@ -187,6 +187,16 @@ class CordioHCIDriver {
       */
      ble_error_t rf_test_end();
 
+     /**
+      * 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

@@ -67,6 +67,10 @@
         "route_unhandled_command_complete_events": {
             "help": "If enabled the stack will forward to the user all HCI events not handled by the stack.",
             "value": 1
+        },
+        "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,39 @@ 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, -20, -19,
+            -18, -16, -15, -14,
+            -13, -12, -11, -10,
+             -9,  -8,  -7,  -6,
+             -5,  -4,  -3,  -2,
+             -1,  -1,  -1,  -1,
+              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 +211,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 +393,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)
     {