diff --git a/boards/nrf52840dk_nrf52840.conf b/boards/nrf52840dk_nrf52840.conf index 6b4a087..d6043d1 100644 --- a/boards/nrf52840dk_nrf52840.conf +++ b/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 diff --git a/src/ble/ble_service.c b/src/ble/ble_service.c index 8f1bccf..de29a81 100644 --- a/src/ble/ble_service.c +++ b/src/ble/ble_service.c @@ -12,6 +12,8 @@ #include #include #include +#include +#include #include #include #include @@ -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, ¤t_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,10 +896,42 @@ 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) { - DBG_ERR("[NUS TX] completion timeout (err %d)\r\n", 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); diff --git a/src/ble/ble_service.h b/src/ble/ble_service.h index 1be2649..92d3457 100644 --- a/src/ble/ble_service.h +++ b/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 *============================================================================*/ diff --git a/sysbuild/hci_ipc.conf b/sysbuild/hci_ipc.conf index 9abaada..f67c05a 100644 --- a/sysbuild/hci_ipc.conf +++ b/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