링크 스트레시 기반 TX power 동적 제어

- 확인 필요

boards/nrf52840dk_nrf52840.conf

@@ -1 +1,2 @@
-CONFIG_BT_CTLR_TX_PWR_PLUS_8=y
+CONFIG_BT_CTLR_TX_PWR_PLUS_4=y
+CONFIG_BT_CTLR_TX_PWR_DYNAMIC_CONTROL=y

src/ble/ble_service.c

@@ -12,6 +12,8 @@
 #include <zephyr/kernel.h>
 #include <zephyr/bluetooth/bluetooth.h>
 #include <zephyr/bluetooth/hci.h>
+#include <zephyr/bluetooth/hci_vs.h>
+#include <zephyr/sys/byteorder.h>
 #include <zephyr/bluetooth/conn.h>
 #include <zephyr/bluetooth/uuid.h>
 #include <zephyr/bluetooth/gatt.h>
@@ -39,6 +41,26 @@ static bool advertising_unlimited;
 static uint8_t conn_param_update_attempts;
 static int64_t last_rx_conn_param_update_ms;
 
+#define BLE_TX_POWER_NORMAL_DBM        4
+#define BLE_TX_POWER_STRESS_DBM        8
+#define BLE_RSSI_STRESS_DBM          (-80)
+#define BLE_RSSI_STRESS_COUNT          3
+#define BLE_RSSI_MONITOR_INTERVAL_MS 500
+#define BLE_TX_SLOW_MS                50
+#define BLE_TX_CRITICAL_MS           100
+#define BLE_TX_STUCK_MS              200
+#define BLE_TX_SLOW_COUNT              3
+#define BLE_TX_BUSY_STRESS_RETRY       4
+
+extern int bt_hci_get_conn_handle(const struct bt_conn *conn, uint16_t *conn_handle);
+
+static uint16_t current_conn_handle;
+static bool current_conn_handle_valid;
+static int8_t current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
+static uint8_t low_rssi_count;
+static uint8_t slow_tx_count;
+static struct k_work_delayable tx_power_monitor_work;
+
 #if IS_ENABLED(CONFIG_BT_SMP)
 #define BLE_REQUIRED_SECURITY_LEVEL    BT_SECURITY_L4
 static uint32_t pairing_passkey = BT_PASSKEY_RAND;
@@ -55,6 +77,150 @@ static struct k_sem nus_tx_done_sem;
 static struct k_work adv_restart_work;
 static struct k_work_delayable adv_timeout_work;
 static struct k_work_delayable conn_param_update_work;
+
+static int ble_tx_power_write(uint8_t handle_type, uint16_t handle, int8_t dbm,
+                              const char *reason)
+{
+    struct net_buf *buf;
+    struct net_buf *rsp;
+    struct bt_hci_cp_vs_write_tx_power_level *cp;
+    struct bt_hci_rp_vs_write_tx_power_level *rp;
+    int err;
+
+    buf = bt_hci_cmd_alloc(K_NO_WAIT);
+    if (buf == NULL)
+    {
+        return -ENOBUFS;
+    }
+
+    cp = net_buf_add(buf, sizeof(*cp));
+    cp->handle_type = handle_type;
+    cp->handle = sys_cpu_to_le16(handle);
+    cp->tx_power_level = dbm;
+
+    err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_WRITE_TX_POWER_LEVEL, buf, &rsp);
+    if (err)
+    {
+        DBG_ERR("[BLE] TX power set fail reason=%s err=%d\r\n", reason, err);
+        return err;
+    }
+
+    rp = (void *)rsp->data;
+    if (rp->status != 0U)
+    {
+        err = -EIO;
+        DBG_ERR("[BLE] TX power set rejected reason=%s status=0x%02x\r\n",
+                reason, rp->status);
+    }
+    else
+    {
+        current_tx_power_dbm = rp->selected_tx_power;
+        DBG_CORE("[BLE] TX power %d dBm reason=%s\r\n",
+                 current_tx_power_dbm, reason);
+    }
+
+    net_buf_unref(rsp);
+    return err;
+}
+
+static int ble_set_conn_tx_power(int8_t dbm, const char *reason)
+{
+    if (!current_conn_handle_valid || current_tx_power_dbm == dbm)
+    {
+        return 0;
+    }
+
+    return ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN,
+                              current_conn_handle,
+                              dbm,
+                              reason ? reason : "unknown");
+}
+
+static void ble_tx_power_stress(const char *reason)
+{
+    (void)ble_set_conn_tx_power(BLE_TX_POWER_STRESS_DBM, reason);
+}
+
+static int ble_read_conn_rssi(int8_t *rssi)
+{
+    struct net_buf *buf;
+    struct net_buf *rsp;
+    struct bt_hci_cp_read_rssi *cp;
+    struct bt_hci_rp_read_rssi *rp;
+    int err;
+
+    if (!current_conn_handle_valid || rssi == NULL)
+    {
+        return -ENOTCONN;
+    }
+
+    buf = bt_hci_cmd_alloc(K_NO_WAIT);
+    if (buf == NULL)
+    {
+        return -ENOBUFS;
+    }
+
+    cp = net_buf_add(buf, sizeof(*cp));
+    cp->handle = sys_cpu_to_le16(current_conn_handle);
+
+    err = bt_hci_cmd_send_sync(BT_HCI_OP_READ_RSSI, buf, &rsp);
+    if (err)
+    {
+        return err;
+    }
+
+    rp = (void *)rsp->data;
+    if (rp->status != 0U)
+    {
+        err = -EIO;
+    }
+    else
+    {
+        *rssi = rp->rssi;
+    }
+
+    net_buf_unref(rsp);
+    return err;
+}
+
+static void tx_power_monitor_handler(struct k_work *work)
+{
+    int8_t rssi = 0;
+    int err;
+
+    ARG_UNUSED(work);
+
+    if (!current_conn_handle_valid)
+    {
+        return;
+    }
+
+    err = ble_read_conn_rssi(&rssi);
+    if (err)
+    {
+        ble_tx_power_stress("rssi-silence");
+    }
+    else if (rssi <= BLE_RSSI_STRESS_DBM)
+    {
+        if (low_rssi_count < UINT8_MAX)
+        {
+            low_rssi_count++;
+        }
+
+        if (low_rssi_count >= BLE_RSSI_STRESS_COUNT)
+        {
+            ble_tx_power_stress("rssi-low");
+        }
+    }
+    else
+    {
+        low_rssi_count = 0U;
+    }
+
+    k_work_schedule(&tx_power_monitor_work,
+                    K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
+}
+
 static const char *ble_addr_type_str(uint8_t type)
 {
     switch (type)
@@ -357,6 +523,12 @@ static void connected(struct bt_conn *conn, uint8_t err)
 
     current_conn = bt_conn_ref(conn);
     ble_connection_st = true;
+    current_conn_handle_valid = (bt_hci_get_conn_handle(conn, &current_conn_handle) == 0);
+    current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
+    low_rssi_count = 0U;
+    slow_tx_count = 0U;
+    (void)ble_set_conn_tx_power(BLE_TX_POWER_NORMAL_DBM, "connected");
+    k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
 
     /* 연결 정보 출력 */
     char addr_str[BT_ADDR_LE_STR_LEN];
@@ -386,8 +558,13 @@ static void connected(struct bt_conn *conn, uint8_t err)
 static void disconnected(struct bt_conn *conn, uint8_t reason)
 {
     k_work_cancel_delayable(&conn_param_update_work);
+    k_work_cancel_delayable(&tx_power_monitor_work);
     conn_param_update_attempts = 0U;
     last_rx_conn_param_update_ms = 0;
+    current_conn_handle_valid = false;
+    current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
+    low_rssi_count = 0U;
+    slow_tx_count = 0U;
 
     if (current_conn)
     {
@@ -533,6 +710,7 @@ int ble_service_init(ble_data_rx_cb_t rx_cb)
     k_work_init_delayable(&adv_timeout_work, adv_timeout_handler);
     k_work_init_delayable(&conn_param_update_work, conn_param_update_handler);
 
+    k_work_init_delayable(&tx_power_monitor_work, tx_power_monitor_handler);
     /* Enable BLE stack */
     err = bt_enable(NULL);
     if (err)
@@ -666,6 +844,7 @@ bool ble_dfu_advertising_is_enabled(void)
 int ble_data_send(const uint8_t *data, uint16_t len)
 {
     int err;
+    int64_t tx_start_ms;
 
     if (!current_conn)
     {
@@ -689,6 +868,7 @@ int ble_data_send(const uint8_t *data, uint16_t len)
     }
 
     //DBG_CORE("[NUS TX] send len=%u\r\n", len);
+    tx_start_ms = k_uptime_get();
     for (int retry = 0; ; retry++)
     {
         err = bt_nus_send(current_conn, data, len);
@@ -701,6 +881,10 @@ int ble_data_send(const uint8_t *data, uint16_t len)
          * 바로 포기하지 않고 짧게 쉬었다가 다시 시도
          */
         DBG_ERR("[NUS TX] busy, retry=%d\r\n", retry + 1);
+        if ((retry + 1) >= BLE_TX_BUSY_STRESS_RETRY)
+        {
+            ble_tx_power_stress("tx-queue-busy");
+        }
         k_msleep(5);
     }
 
@@ -712,11 +896,43 @@ int ble_data_send(const uint8_t *data, uint16_t len)
     }
 
     /* bt_nus_send() 성공 후 sent 콜백이 올 때까지 기다림 (다음 패킷을 보내도 되는 시점) */
-    err = k_sem_take(&nus_tx_done_sem, K_MSEC(1000));
+    err = k_sem_take(&nus_tx_done_sem, K_MSEC(BLE_TX_STUCK_MS));
+    if (err)
+    {
+        ble_tx_power_stress("tx-complete-stuck");
+        err = k_sem_take(&nus_tx_done_sem, K_MSEC(1000 - BLE_TX_STUCK_MS));
         if (err)
         {
             DBG_ERR("[NUS TX] completion timeout (err %d)\r\n", err);
         }
+    }
+
+    if (!err)
+    {
+        int64_t tx_elapsed_ms = k_uptime_get() - tx_start_ms;
+
+        if (tx_elapsed_ms >= BLE_TX_CRITICAL_MS)
+        {
+            ble_tx_power_stress("tx-complete-slow100");
+            slow_tx_count = 0U;
+        }
+        else if (tx_elapsed_ms >= BLE_TX_SLOW_MS)
+        {
+            if (slow_tx_count < UINT8_MAX)
+            {
+                slow_tx_count++;
+            }
+
+            if (slow_tx_count >= BLE_TX_SLOW_COUNT)
+            {
+                ble_tx_power_stress("tx-complete-slow50");
+            }
+        }
+        else
+        {
+            slow_tx_count = 0U;
+        }
+    }
 
     k_mutex_unlock(&nus_tx_lock);
     return err;

src/ble/ble_service.h

@@ -27,8 +27,6 @@
 #define BLE_CONN_PARAM_RETRY_INTERVAL_MS    30000
 #define BLE_CONN_PARAM_MAX_ATTEMPTS         3
 
-#define BLE_TX_POWER                8       /* +8 dBm */
-
 /*==============================================================================
  * Callback types
  *============================================================================*/

sysbuild/hci_ipc.conf

@@ -1,2 +1,3 @@
-CONFIG_BT_CTLR_TX_PWR_PLUS_8=y
+CONFIG_BT_CTLR_TX_PWR_PLUS_4=y
+CONFIG_BT_CTLR_TX_PWR_DYNAMIC_CONTROL=y
 CONFIG_BT_CTLR_DATA_LENGTH_MAX=251