BLE TX power/RSSI 모니터 로직 분리

2026-07-20 12:27:12 +09:00
parent 3f2c2e774b
commit 4a3e71774f
3 changed files with 462 additions and 331 deletions
@@ -0,0 +1,364 @@
+/*******************************************************************************
+ * @file    ble_tx_power.c
+ * @brief   BLE TX power and RSSI monitor helper
+ ******************************************************************************/
+#include <zephyr/kernel.h>
+#include <zephyr/bluetooth/hci.h>
+#include <zephyr/bluetooth/hci_vs.h>
+#include <zephyr/bluetooth/conn.h>
+#include <zephyr/sys/byteorder.h>
+#include <errno.h>
+
+#include "ble_tx_power.h"
+#include "debug_print.h"
+
+#define BLE_TX_POWER_NORMAL_DBM        4        // NORMAL: 평상시에는 전류 소모를 낮추기 위해 +4 dBm을 기본값으로 사용
+#define BLE_TX_POWER_STRESS_DBM        8        // BOOST: RSSI 또는 TX 지연이 나빠지면 일시적으로 +8 dBm으로 boost
+#define BLE_RSSI_MONITOR_ENABLED       1
+#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
+#define BLE_TX_RECOVER_HOLD_MS         10000
+
+extern int bt_hci_get_conn_handle(const struct bt_conn *conn, uint16_t *conn_handle);
+
+/* HCI vendor command는 bt_conn 포인터 대신 controller 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 int64_t last_stress_ms;
+static struct k_work_delayable tx_power_monitor_work;
+
+/* Nordic vendor HCI command로 advertising/connection TX power 직접 설정 */
+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
+    {
+        if (handle_type == BT_HCI_VS_LL_HANDLE_TYPE_CONN)
+        {
+            current_tx_power_dbm = rp->selected_tx_power;
+        }
+        DBG_CORE("[BLE] TX power %d dBm reason=%s\r\n", rp->selected_tx_power, reason);
+    }
+
+    net_buf_unref(rsp);
+    return err;
+}
+
+/* 연결 handle이 유효할 때만 connection TX power 변경 */
+static int ble_set_conn_tx_power(int8_t dbm, const char *reason)
+{
+    // 연결 전이거나 이미 목표 출력이면 HCI 명령을 보내지 않음
+    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)
+{
+    // stress 발생 시각을 기록해서 이후 NORMAL 복귀 시점 판단
+    last_stress_ms = k_uptime_get();
+    (void)ble_set_conn_tx_power(BLE_TX_POWER_STRESS_DBM, reason);
+}
+
+/* 마지막 stress 이후 일정 시간 stress 없는 경우 NORMAL 출력으로 회복 */
+static void ble_tx_power_try_recover(void)
+{
+    // 이미 NORMAL이면 불필요한 HCI 명령 피함
+    if ((current_tx_power_dbm != BLE_TX_POWER_NORMAL_DBM) &&
+        ((k_uptime_get() - last_stress_ms) >= BLE_TX_RECOVER_HOLD_MS))
+    {
+        (void)ble_set_conn_tx_power(BLE_TX_POWER_NORMAL_DBM, "link-recovered");
+    }
+}
+
+/* 컨트롤러에 실제 적용된 연결 TX power 확인 */
+static int ble_read_conn_tx_power(int8_t *dbm)
+{
+    struct net_buf *buf;
+    struct net_buf *rsp;
+    struct bt_hci_cp_vs_read_tx_power_level *cp;
+    struct bt_hci_rp_vs_read_tx_power_level *rp;
+    int err;
+
+    // 연결 handle 없이는 controller에 연결 TX power 조회 불가
+    if (!current_conn_handle_valid || dbm == 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_type = BT_HCI_VS_LL_HANDLE_TYPE_CONN;
+    cp->handle = sys_cpu_to_le16(current_conn_handle);
+
+    err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_TX_POWER_LEVEL, buf, &rsp);
+    if (err)
+    {
+        return err;
+    }
+
+    rp = (void *)rsp->data;
+    if (rp->status != 0U)
+    {
+        err = -EIO;
+    }
+    else
+    {
+        *dbm = rp->tx_power_level;
+    }
+
+    net_buf_unref(rsp);
+    return err;
+}
+
+/* 현재 연결의 RSSI를 읽어 링크 품질 판단에 사용 */
+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;
+
+    // RSSI 조회도 연결 handle이 있어야 가능
+    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;
+}
+
+/* 연결 중에는 주기적으로 RSSI를 확인하고, 필요하면 TX power boost */
+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;
+    }
+
+    if (!BLE_RSSI_MONITOR_ENABLED)
+    {
+        return;
+    }
+
+    err = ble_read_conn_rssi(&rssi);
+
+    // RSSI를 못 읽는 상황도 링크 품질 저하로 간주
+    if (err)
+    {
+        ble_tx_power_stress("rssi-silence");
+    }
+    else if (rssi <= BLE_RSSI_STRESS_DBM)
+    {
+        // 순간적인 RSSI 흔들림은 무시하고, 연속 low RSSI일 때만 boost
+        if (low_rssi_count < UINT8_MAX)
+        {
+            low_rssi_count++;
+        }
+
+        if (low_rssi_count >= BLE_RSSI_STRESS_COUNT)
+        {
+            ble_tx_power_stress("rssi-low");
+        }
+    }
+    else
+    {
+        // RSSI가 회복되면 연속 low RSSI 카운터 초기화
+        low_rssi_count = 0U;
+    }
+
+    // TX가 없어도 monitor 주기마다 NORMAL 복귀 여부 확인
+    ble_tx_power_try_recover();
+
+    // 연결이 유지되는 동안 monitor work 계속 반복
+    k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
+}
+
+/* RSSI monitor용 delayable work 초기화 */
+void ble_tx_power_init(void)
+{
+    k_work_init_delayable(&tx_power_monitor_work, tx_power_monitor_handler);
+}
+
+/* 연결 직후 TX power 상태를 초기화하고 RSSI monitor를 시작 */
+void ble_tx_power_on_connected(struct bt_conn *conn)
+{
+    int8_t cur_txp = 0;
+    int txp_ret;
+
+    // 이후 HCI 명령에서 사용할 controller connection handle 저장
+    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;
+    last_stress_ms = k_uptime_get();
+
+    if (current_conn_handle_valid)
+    {
+        // 새 연결은 항상 NORMAL (4 dBm) 시작
+        (void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_CONN, current_conn_handle, BLE_TX_POWER_NORMAL_DBM, "connected");
+    }
+
+    txp_ret = ble_read_conn_tx_power(&cur_txp); // 실제 적용값
+
+    if (txp_ret == 0)
+    {
+        DBG_CORE("[BLE] TX power (current) = %d dBm\r\n", cur_txp);
+    }
+    else
+    {
+        DBG_CORE("[BLE] TX power read fail ret=%d\r\n", txp_ret);
+    }
+
+    if (BLE_RSSI_MONITOR_ENABLED)
+    {
+        // 연결이 유지되는 동안만 RSSI monitor
+        k_work_schedule(&tx_power_monitor_work, K_MSEC(BLE_RSSI_MONITOR_INTERVAL_MS));
+    }
+}
+
+/* 연결 해제 시 monitor와 링크별 상태 초기화 */
+void ble_tx_power_on_disconnected(void)
+{
+    k_work_cancel_delayable(&tx_power_monitor_work);
+    current_conn_handle_valid = false;
+    current_tx_power_dbm = BLE_TX_POWER_NORMAL_DBM;
+    low_rssi_count = 0U;
+    slow_tx_count = 0U;
+    last_stress_ms = 0;
+}
+
+/* advertising handle의 TX power를 NORMAL 값으로 설정 */
+void ble_tx_power_apply_advertising(void)
+{
+    (void)ble_tx_power_write(BT_HCI_VS_LL_HANDLE_TYPE_ADV, 0, BLE_TX_POWER_NORMAL_DBM, "adv");
+}
+
+/* ble_service.c가 TX 완료 대기 timeout을 TX power 정책값과 공유 */
+uint32_t ble_tx_power_tx_stuck_ms(void)
+{
+    return BLE_TX_STUCK_MS;
+}
+
+/* NUS TX queue가 여러 번 busy이면 링크가 불안정한 상황으로 보고 출력 boost */
+void ble_tx_power_on_tx_busy_retry(uint8_t retry_count)
+{
+    if (retry_count >= BLE_TX_BUSY_STRESS_RETRY)
+    {
+        ble_tx_power_stress("tx-queue-busy");
+    }
+}
+
+/* sent callback이 늦게 오면 TX path가 막힌 것으로 보고 boost */
+void ble_tx_power_on_tx_complete_timeout(void)
+{
+    ble_tx_power_stress("tx-complete-stuck");
+}
+
+/* TX 완료 시간이 길어지는 것도 링크 품질 저하 신호로 사용 */
+void ble_tx_power_on_tx_complete(int64_t elapsed_ms)
+{
+    if (elapsed_ms >= BLE_TX_CRITICAL_MS)
+    {
+        // 100ms 이상이면 단발성이라도 즉시 boost
+        ble_tx_power_stress("tx-complete-slow100");
+        slow_tx_count = 0U;
+    }
+    else if (elapsed_ms >= BLE_TX_SLOW_MS)
+    {
+        // 50ms 이상은 연속 발생할 때만 boost
+        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 카운터 지우고 복귀 가능성 확인
+        slow_tx_count = 0U;
+        ble_tx_power_try_recover();
+    }
+}