From 3ac3f417ac2c4793663cede47f1964543d0485f7 Mon Sep 17 00:00:00 2001
From: Szymon Janc <szymon.janc@codecoup.pl>
Date: Mon, 26 Jun 2017 16:07:16 +0200
Subject: [PATCH] nimble/controller: Add support for setting advertising TX
 power

This allows host to set prefered TX power for advertising instance.
Controller is allowed to ignore or adjust this value.

X-Original-Commit: b6d919375838e6b03748725d47bb59aee94ece2f
---
 .../controller/include/controller/ble_phy.h   |  3 +++
 nimble/controller/src/ble_ll_adv.c            | 10 +++++++-
 nimble/drivers/native/src/ble_phy.c           | 23 +++++++++++++++++++
 nimble/drivers/nrf51/src/ble_phy.c            | 23 +++++++++++++++++++
 nimble/drivers/nrf52/src/ble_phy.c            | 23 +++++++++++++++++++
 5 files changed, 81 insertions(+), 1 deletion(-)

diff --git a/nimble/controller/include/controller/ble_phy.h b/nimble/controller/include/controller/ble_phy.h
index 734f9e940..e5267e4d5 100644
--- a/nimble/controller/include/controller/ble_phy.h
+++ b/nimble/controller/include/controller/ble_phy.h
@@ -121,6 +121,9 @@ int ble_phy_rssi_get(void);
 /* Set the transmit power */
 int ble_phy_txpwr_set(int dbm);
 
+/* Get highest allowed power from range */
+int ble_phy_txpower_round(int dbm);
+
 /* Get the transmit power */
 int ble_phy_txpwr_get(void);
 
diff --git a/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c
index 9f0722178..77f16ddd3 100644
--- a/nimble/controller/src/ble_ll_adv.c
+++ b/nimble/controller/src/ble_ll_adv.c
@@ -1709,6 +1709,7 @@ ble_ll_adv_ext_set_param(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
     uint8_t sec_phy;
     uint8_t sid;
     uint8_t scan_req_notif;
+    int8_t tx_power;
 
     if (cmdbuf[0] >= BLE_LL_ADV_INSTANCES) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1843,7 +1844,14 @@ ble_ll_adv_ext_set_param(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
         memcpy(advsm->peer_addr, &cmdbuf[12], BLE_DEV_ADDR_LEN);
     }
 
-    advsm->adv_txpwr = 0; /* TODO rspbuf[21];*/
+    tx_power = (int8_t) rspbuf[21];
+    if (tx_power == 127) {
+        /* no preference */
+        advsm->adv_txpwr = MYNEWT_VAL(BLE_LL_TX_PWR_DBM);
+    } else {
+        advsm->adv_txpwr = ble_phy_txpower_round(tx_power);
+    }
+
     advsm->own_addr_type = own_addr_type;
     advsm->peer_addr_type = peer_addr_type;
     advsm->adv_filter_policy = adv_filter_policy;
diff --git a/nimble/drivers/native/src/ble_phy.c b/nimble/drivers/native/src/ble_phy.c
index a8fd9f54d..6a39346c7 100644
--- a/nimble/drivers/native/src/ble_phy.c
+++ b/nimble/drivers/native/src/ble_phy.c
@@ -497,6 +497,29 @@ ble_phy_txpwr_set(int dbm)
     return 0;
 }
 
+/**
+ * ble phy txpwr round
+ *
+ * Get the rounded transmit output power (in dBm).
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int Rounded power in dBm
+ */
+int ble_phy_txpower_round(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+    if (dbm > BLE_XCVR_TX_PWR_MAX_DBM) {
+        dbm = BLE_XCVR_TX_PWR_MAX_DBM;
+    } else {
+        if (dbm < BLE_XCVR_TX_PWR_MIN_DBM) {
+            dbm = BLE_XCVR_TX_PWR_MIN_DBM;
+        }
+    }
+
+    return dbm;
+}
+
 /**
  * ble phy txpwr get
  *
diff --git a/nimble/drivers/nrf51/src/ble_phy.c b/nimble/drivers/nrf51/src/ble_phy.c
index 84d61d6a1..b74e7831b 100644
--- a/nimble/drivers/nrf51/src/ble_phy.c
+++ b/nimble/drivers/nrf51/src/ble_phy.c
@@ -1212,6 +1212,29 @@ ble_phy_txpwr_set(int dbm)
     return 0;
 }
 
+/**
+ * ble phy txpwr round
+ *
+ * Get the rounded transmit output power (in dBm).
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int Rounded power in dBm
+ */
+int ble_phy_txpower_round(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+    if (dbm > NRF_TX_PWR_MAX_DBM) {
+        dbm = NRF_TX_PWR_MAX_DBM;
+    } else {
+        if (dbm < NRF_TX_PWR_MIN_DBM) {
+            dbm = NRF_TX_PWR_MIN_DBM;
+        }
+    }
+
+    return dbm;
+}
+
 /**
  * ble phy txpwr get
  *
diff --git a/nimble/drivers/nrf52/src/ble_phy.c b/nimble/drivers/nrf52/src/ble_phy.c
index 81becf955..f14831448 100644
--- a/nimble/drivers/nrf52/src/ble_phy.c
+++ b/nimble/drivers/nrf52/src/ble_phy.c
@@ -1294,6 +1294,29 @@ ble_phy_txpwr_set(int dbm)
     return 0;
 }
 
+/**
+ * ble phy txpwr round
+ *
+ * Get the rounded transmit output power (in dBm).
+ *
+ * @param dbm Power output in dBm.
+ *
+ * @return int Rounded power in dBm
+ */
+int ble_phy_txpower_round(int dbm)
+{
+    /* "Rail" power level if outside supported range */
+    if (dbm > NRF_TX_PWR_MAX_DBM) {
+        dbm = NRF_TX_PWR_MAX_DBM;
+    } else {
+        if (dbm < NRF_TX_PWR_MIN_DBM) {
+            dbm = NRF_TX_PWR_MIN_DBM;
+        }
+    }
+
+    return dbm;
+}
+
 /**
  * ble phy set access addr
  *