From 49db78462cf6267496a5d1efd81e9d522716243b Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Fri, 9 Aug 2024 10:25:09 +0200
Subject: [PATCH] nimble/phy/cmac: Fix setting TX power level

TX power is now configured using ble_ll_tx_power_set (instead of
ble_phy_tx_power_set) which uses ble_phy_tx_power_round to calculate
proper TX power level.

This adds missing implementation for that function in CMAC PHY driver so
power level can be properly set.
---
 nimble/drivers/dialog_cmac/src/ble_phy.c | 32 +++++++++++++++++++-----
 1 file changed, 26 insertions(+), 6 deletions(-)

diff --git a/nimble/drivers/dialog_cmac/src/ble_phy.c b/nimble/drivers/dialog_cmac/src/ble_phy.c
index 4c0e4c942a..434aaa969e 100644
--- a/nimble/drivers/dialog_cmac/src/ble_phy.c
+++ b/nimble/drivers/dialog_cmac/src/ble_phy.c
@@ -1655,26 +1655,46 @@ ble_phy_encrypt_disable(void)
 }
 #endif
 
+static int8_t
+phy_txpower_round(int8_t dbm)
+{
+    static const int8_t supported_pwr_levels[] = {-18, -12, -8, -6, -3, -2,
+                                                  -1, 0, 1, 2, 3, 4, 5, 6 };
+    int i;
+
+    for (i = 0; i < ARRAY_SIZE(supported_pwr_levels); i++) {
+        if (dbm <= supported_pwr_levels[i]) {
+            dbm = supported_pwr_levels[i];
+            break;
+        }
+    }
+
+    if (i == ARRAY_SIZE(supported_pwr_levels)) {
+        dbm = supported_pwr_levels[ARRAY_SIZE(supported_pwr_levels) - 1];
+    }
+
+    return dbm;
+}
+
 int
 ble_phy_tx_power_set(int dbm)
 {
 #if MYNEWT_VAL(CMAC_DEBUG_DATA_ENABLE)
     if (g_cmac_shm_debugdata.tx_power_ovr_enable) {
-        ble_rf_set_tx_power(g_cmac_shm_debugdata.tx_power_ovr);
-    } else {
-        ble_rf_set_tx_power(dbm);
+        dbm = g_cmac_shm_debugdata.tx_power_ovr;
     }
-#else
-    ble_rf_set_tx_power(dbm);
 #endif
 
+    dbm = phy_txpower_round(dbm);
+    ble_rf_set_tx_power(dbm);
+
     return 0;
 }
 
 int
 ble_phy_tx_power_round(int dbm)
 {
-    return 0;
+    return phy_txpower_round(dbm);
 }
 
 int