From 9dd64d2903a670c79493f035013d69d948400178 Mon Sep 17 00:00:00 2001
From: Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
Date: Thu, 8 Sep 2022 10:34:37 +0200
Subject: [PATCH] nimble/phy/nrf5x: Move out TX power rounding

---
 nimble/drivers/nrf5x/src/ble_phy.c   | 40 ++++------------------------
 nimble/drivers/nrf5x/src/nrf52/phy.c | 34 +++++++++++++++++++++++
 nimble/drivers/nrf5x/src/phy_priv.h  |  2 ++
 3 files changed, 41 insertions(+), 35 deletions(-)

diff --git a/nimble/drivers/nrf5x/src/ble_phy.c b/nimble/drivers/nrf5x/src/ble_phy.c
index d5113434d..b4d64d88d 100644
--- a/nimble/drivers/nrf5x/src/ble_phy.c
+++ b/nimble/drivers/nrf5x/src/ble_phy.c
@@ -1803,8 +1803,8 @@ ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans)
 int
 ble_phy_txpwr_set(int dbm)
 {
-    /* "Rail" power level if outside supported range */
-    dbm = ble_phy_txpower_round(dbm);
+    /* Get actual TX power supported by radio */
+    dbm = phy_txpower_round(dbm);
 
     NRF_RADIO->TXPOWER = dbm;
     g_ble_phy_data.phy_txpwr_dbm = dbm;
@@ -1821,40 +1821,10 @@ ble_phy_txpwr_set(int dbm)
  *
  * @return int Rounded power in dBm
  */
-int ble_phy_txpower_round(int dbm)
+int
+ble_phy_txpower_round(int dbm)
 {
-    /* TODO this should be per nRF52XXX */
-
-    /* "Rail" power level if outside supported range */
-    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm) {
-        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm;
-    }
-
-    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm) {
-        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm;
-    }
-
-    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_0dBm) {
-        return (int8_t)RADIO_TXPOWER_TXPOWER_0dBm;
-    }
-
-    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm) {
-        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm;
-    }
-
-    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm) {
-        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm;
-    }
-
-    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm) {
-        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm;
-    }
-
-    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm) {
-        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm;
-    }
-
-    return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm;
+    return phy_txpower_round(dbm);
 }
 
 /**
diff --git a/nimble/drivers/nrf5x/src/nrf52/phy.c b/nimble/drivers/nrf5x/src/nrf52/phy.c
index 4f825b8f2..763e3eef2 100644
--- a/nimble/drivers/nrf5x/src/nrf52/phy.c
+++ b/nimble/drivers/nrf5x/src/nrf52/phy.c
@@ -163,3 +163,37 @@ phy_ppi_init(void)
                                    (uint32_t)&(NRF_TIMER0->EVENTS_COMPARE[3]),
                                    (uint32_t)&(NRF_RADIO->TASKS_DISABLE));
 }
+
+int8_t
+phy_txpower_round(int8_t dbm)
+{
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos4dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Pos3dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_0dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_0dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg4dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg8dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg12dBm;
+    }
+
+    if (dbm >= (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm) {
+        return (int8_t)RADIO_TXPOWER_TXPOWER_Neg20dBm;
+    }
+
+    return (int8_t)RADIO_TXPOWER_TXPOWER_Neg40dBm;
+}
diff --git a/nimble/drivers/nrf5x/src/phy_priv.h b/nimble/drivers/nrf5x/src/phy_priv.h
index 881221ad1..b2308e61f 100644
--- a/nimble/drivers/nrf5x/src/phy_priv.h
+++ b/nimble/drivers/nrf5x/src/phy_priv.h
@@ -76,4 +76,6 @@ void phy_ppi_init(void);
 #include "nrf52/phy_ppi.h"
 #endif
 
+int8_t phy_txpower_round(int8_t dbm);
+
 #endif /* H_PHY_PRIV_ */