MTU에 따른 BLE 패킷 분할 - IMU FIFO 응답 패킷

- 협상된 MTU 기준으로 rim: 응답 패킷 크기 판단
- 기존 rim:이 MTU에 들어가지 않으면 ric: chunk 분할 전송

CMakeLists.txt

@@ -31,6 +31,7 @@ target_sources(app PRIVATE
     src/power_control.c
     src/measurement/piezo_measure.c
     src/ble/ble_service.c
+    src/ble/ble_tx_power.c
     src/drivers/battery/battery_adc.c
     src/drivers/echo_adc/echo_adc.c
     src/drivers/led/led_control.c

src/ble/ble_service.c

@@ -37,6 +37,7 @@ static bool dfu_advertising_mode;
 static bool advertising_unlimited;
 static uint8_t conn_param_update_attempts;
 static int64_t last_rx_conn_param_update_ms;
+static uint16_t current_notify_mtu = 23U;
 
 #if IS_ENABLED(CONFIG_BT_SMP)
 #define BLE_REQUIRED_SECURITY_LEVEL    BT_SECURITY_L4
@@ -55,6 +56,62 @@ static struct k_work adv_restart_work;
 static struct k_work_delayable adv_timeout_work;
 static struct k_work_delayable conn_param_update_work;
 
+/*
+ * Store the UATT MTU used by normal GATT/NUS notifications.
+ * NUS notifications can carry application payload up to UATT MTU - 3 bytes.
+ */
+static void att_mtu_updated(struct bt_conn *conn, uint16_t tx, uint16_t rx)
+{
+    uint16_t uatt = bt_gatt_get_uatt_mtu(conn);
+
+    if (uatt != 0U)
+    {
+        current_notify_mtu = uatt;
+    }
+    else
+    {
+        current_notify_mtu = (tx < rx) ? tx : rx;
+    }
+
+    DBG_PRINTF("[BLE] ATT MTU updated tx=%u rx=%u uatt=%u used=%u notify_payload=%u\r\n",
+               tx, rx, uatt, current_notify_mtu,
+               (current_notify_mtu > 3U) ? (current_notify_mtu - 3U) : 0U);
+}
+
+
+/* MTU Exchange 연결 직후 요청 */
+static void mtu_exchange_cb(struct bt_conn *conn, uint8_t err, struct bt_gatt_exchange_params *params)
+{
+    uint16_t uatt;
+
+    ARG_UNUSED(params);
+
+    if (err)
+    {
+        DBG_ERR("[BLE] MTU exchange failed err=%u\r\n", err);
+        return;
+    }
+
+    uatt = bt_gatt_get_uatt_mtu(conn);
+    if (uatt != 0U)
+    {
+        current_notify_mtu = uatt;
+    }
+
+    DBG_PRINTF("[BLE] MTU exchange complete uatt=%u notify_payload=%u\r\n",
+               uatt,
+               (uatt > 3U) ? (uatt - 3U) : 0U);
+}
+
+static struct bt_gatt_exchange_params mtu_exchange_params =
+{
+    .func = mtu_exchange_cb,
+};
+static struct bt_gatt_cb gatt_callbacks =
+{
+    .att_mtu_updated = att_mtu_updated,
+};
+
 /* 로컬 BLE MAC 주소 로그 */
 static void ble_log_local_identity(void)
 {
@@ -388,6 +445,16 @@ static void connected(struct bt_conn *conn, uint8_t err)
     bt_addr_le_to_str(bt_conn_get_dst(conn), addr_str, sizeof(addr_str));
     DBG_CORE("[BLE] Peer: %s\r\n", addr_str); // 연결 정보 출력
 
+
+    err = bt_gatt_exchange_mtu(conn, &mtu_exchange_params);
+    if (err)
+    {
+        DBG_ERR("[BLE] MTU exchange request failed err=%d\r\n", err);
+    }
+    else
+    {
+        DBG_PRINTF("[BLE] MTU exchange requested\r\n");
+    }
     conn_param_update_attempts = 0U;
     last_rx_conn_param_update_ms = 0;
 
@@ -424,6 +491,7 @@ static void disconnected(struct bt_conn *conn, uint8_t reason)
     }
 
     ble_connection_st = false;
+    current_notify_mtu = 23U;
     if (dfu_advertising_mode)
     {
         // DFU 광고는 미연결 10분 타임아웃 적용
@@ -578,6 +646,7 @@ int ble_service_init(ble_data_rx_cb_t rx_cb)
 
     ble_log_local_identity();
 
+    bt_gatt_cb_register(&gatt_callbacks);
 #if IS_ENABLED(CONFIG_BT_SMP)
     err = ble_security_init();
     if (err)
@@ -790,3 +859,9 @@ bool ble_is_connected(void)
 {
     return (current_conn != NULL);
 }
+
+/* ATT MTU used to calculate notify payload size */
+uint16_t ble_current_mtu(void)
+{
+    return current_notify_mtu;
+}

src/ble/ble_service.h

@@ -31,6 +31,7 @@ int ble_service_init(ble_data_rx_cb_t rx_cb);
 int ble_advertising_start(void);
 int ble_advertising_stop(void);
 int ble_data_send(const uint8_t *data, uint16_t len);
+uint16_t ble_current_mtu(void);
 int ble_dfu_advertising_enable(void);
 bool ble_dfu_advertising_is_enabled(void);
 bool ble_is_connected(void);

src/command/cmd_common.c

@@ -4,6 +4,7 @@
  ******************************************************************************/
 #include <zephyr/kernel.h>
 #include <zephyr/sys/reboot.h>
+#include <zephyr/sys/util.h>
 #include <string.h>
 
 #include "cmd_common.h"
@@ -22,6 +23,14 @@ static uint8_t tx_id_buf[4 + HW_NO_LENGTH + SERIAL_NO_LENGTH + SERIAL_NO_LENGTH
 static uint8_t tx_rim_buf[4 + 2 + (IMU_FIFO_RIM_TARGET_SAMPLES * IMU_FIFO_SAMPLE_BYTES) + 2];
 static uint8_t g_echo_session;
 
+/* ATT MTU에 따른 패킷 분할 */
+#define ATT_NOTIFY_HEADER_SIZE 3U
+#define REB_OVERHEAD_SIZE      10U
+#define REC_OVERHEAD_SIZE      14U
+#define ECHO_SAMPLE_SIZE       2U
+#define RIM_OVERHEAD_SIZE      8U
+#define RIC_OVERHEAD_SIZE      12U
+
 uint8_t cmd_next_echo_session(void)
 {
     return g_echo_session++;
@@ -185,7 +194,27 @@ int cmd_send_response_imu(const int16_t accel[3], const int16_t gyro[3])
     return ble_data_send(buf, 18);
 }
 
-void cmd_send_response_echo(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples)
+/* ATT MTU에 따라 패킷 분할이 필요한지 계산 */
+static uint16_t calc_rec_samples_per_packet(uint16_t notify_payload_max)
+{
+    uint16_t samples_per_pkt = 1U;
+
+    // rec:는 고정 오버헤드가 14B, chunk 크기 다시 계산
+    if (notify_payload_max > REC_OVERHEAD_SIZE)
+    {
+        samples_per_pkt = (uint16_t)((notify_payload_max - REC_OVERHEAD_SIZE) / ECHO_SAMPLE_SIZE);
+    }
+
+    // 방어코드
+    if (samples_per_pkt == 0U)
+    {
+        samples_per_pkt = 1U;
+    }
+
+    return samples_per_pkt;
+}
+
+static void cmd_send_response_echo_reb(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples)
 {
     // 정적 버퍼 사용 (한 번에 한 명령만 처리)
     uint8_t *buf = tx_echo_buf;
@@ -210,6 +239,87 @@ void cmd_send_response_echo(uint8_t session, uint8_t channel, const uint16_t *sa
     ble_data_send(buf, payload_len + 2);
 }
 
+/*
+* rec: packet layout:
+*   tag (4B): "rec:"
+*   session (1B): 측정 세션 번호
+*   channel (1B): piezo 채널 번호
+*   offset (2B): 전체 샘플 배열에서 시작 인덱스
+*   total_samples (2B): 전체 샘플 수
+*   chunk_samples (2B): 현재 패킷에 들어간 샘플 수
+*   sample data (N): Piezo ADC samples
+*   crc (2B): tag부터 sample data까지 CRC16
+*/
+static void cmd_send_response_echo_rec(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples, uint16_t samples_per_pkt)
+{
+    uint8_t *buf = tx_echo_buf;
+
+    for (uint16_t off = 0U; off < num_samples; off = (uint16_t)(off + samples_per_pkt))
+    {
+        uint16_t remain = (uint16_t)(num_samples - off);
+        uint16_t chunk_samples = MIN(samples_per_pkt, remain);
+
+        buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'c'; buf[3] = ':';
+        buf[4] = session;
+        buf[5] = channel;
+        buf[6] = (uint8_t)(off >> 8);
+        buf[7] = (uint8_t)(off & 0xFF);
+        buf[8] = (uint8_t)(num_samples >> 8);
+        buf[9] = (uint8_t)(num_samples & 0xFF);
+        buf[10] = (uint8_t)(chunk_samples >> 8);
+        buf[11] = (uint8_t)(chunk_samples & 0xFF);
+
+        // 마지막 chunk는 samples_per_pkt보다 작을 수 있음
+        for (uint16_t i = 0U; i < chunk_samples; i++)
+        {
+            buf[12 + i * 2] = (uint8_t)(samples[off + i] >> 8);
+            buf[13 + i * 2] = (uint8_t)(samples[off + i] & 0xFF);
+        }
+
+        uint16_t payload_len = (uint16_t)(REC_OVERHEAD_SIZE - 2U + (chunk_samples * ECHO_SAMPLE_SIZE));
+        uint16_t crc = cmd_crc16_compute(buf, payload_len);
+        DBG_PRINTF("[CMD] rec tx ch=%u off=%u total=%u chunk=%u len=%u\r\n",
+                   channel, off, num_samples, chunk_samples, (uint16_t)(payload_len + 2U));
+
+        buf[payload_len] = (uint8_t)(crc & 0xFF);
+        buf[payload_len + 1U] = (uint8_t)(crc >> 8);
+
+        // ble_data_send()는 NUS sent 콜백을 기다리고 다음 chunk 전송
+        if (ble_data_send(buf, (uint16_t)(payload_len + 2U)) != 0)
+        {
+            break;
+        }
+    }
+}
+
+/* Piezo echo ADC 응답 패킷 */
+void cmd_send_response_echo(uint8_t session, uint8_t channel, const uint16_t *samples, uint16_t num_samples)
+{
+    uint16_t notify_payload_max = 0U;
+    uint16_t mtu = ble_current_mtu();
+    uint16_t reb_len = (uint16_t)(REB_OVERHEAD_SIZE + (num_samples * ECHO_SAMPLE_SIZE));
+
+    // 협상된 ATT MTU에서 최대 payload
+    if (mtu > ATT_NOTIFY_HEADER_SIZE)
+    {
+        notify_payload_max = (uint16_t)(mtu - ATT_NOTIFY_HEADER_SIZE);
+    }
+
+    // 분할이 필요 없는 경우(기존 reb: 패킷이 MTU 안에 들어가는 경우) reb: 응답
+    if ((notify_payload_max == 0U) || (reb_len <= notify_payload_max))
+    {
+        cmd_send_response_echo_reb(session, channel, samples, num_samples);
+        return;
+    }
+
+    // 기존 reb: 패킷이 현재 MTU 보다 큰 경우 rec:로 전환
+    uint16_t samples_per_pkt = calc_rec_samples_per_packet(notify_payload_max);
+
+    DBG_PRINTF("[CMD] rec split mtu=%u samples=%u chunk=%u\r\n", mtu, num_samples, samples_per_pkt);
+
+    cmd_send_response_echo_rec(session, channel, samples, num_samples, samples_per_pkt);
+}
+
 void cmd_send_response_bundle(uint16_t batt_mv, const int16_t accel[3], const int16_t gyro[3], int16_t temp_cdeg)
 {
     uint8_t *buf = tx_bundle_buf;
@@ -239,7 +349,7 @@ void cmd_send_response_bundle(uint16_t batt_mv, const int16_t accel[3], const in
     ble_data_send(buf, sizeof(tx_bundle_buf));
 }
 
-void cmd_send_response_rim(const uint8_t *sample_bytes, uint16_t sample_count)
+static void cmd_send_response_rim_legacy(const uint8_t *sample_bytes, uint16_t sample_count)
 {
     uint8_t *buf = tx_rim_buf;
     uint16_t payload_len;
@@ -261,9 +371,98 @@ void cmd_send_response_rim(const uint8_t *sample_bytes, uint16_t sample_count)
     buf[payload_len + 1U] = (uint8_t)(crc >> 8);
 
     DBG_PRINTF("[MTB] tx rim samples=%u len=%u\r\n", sample_count, (uint16_t)(payload_len + 2U));
+
     ble_data_send(buf, (uint16_t)(payload_len + 2U));
 }
 
+static uint16_t calc_ric_samples_per_packet(uint16_t notify_payload_max)
+{
+    if (notify_payload_max < (RIC_OVERHEAD_SIZE + IMU_FIFO_SAMPLE_BYTES))
+    {
+        return 0U;
+    }
+
+    return (uint16_t)((notify_payload_max - RIC_OVERHEAD_SIZE) / IMU_FIFO_SAMPLE_BYTES);
+}
+
+/*
+* ric: packet layout:
+*   tag (4B): "ric:"
+*   offset (2B): 전체 샘플 배열에서 시작 인덱스
+*   total_samples (2B): 전체 IMU FIFO 샘플 수
+*   chunk_samples (2B): 현재 패킷에 들어간 샘플 수
+*   sample data (N): IMU FIFO samples
+*   crc (2B): tag부터 sample data까지 CRC16
+*/
+static void cmd_send_response_rim_ric(const uint8_t *sample_bytes, uint16_t sample_count, uint16_t samples_per_pkt)
+{
+    uint8_t *buf = tx_rim_buf;
+
+    for (uint16_t off = 0U; off < sample_count; off = (uint16_t)(off + samples_per_pkt))
+    {
+        uint16_t remain = (uint16_t)(sample_count - off);
+        uint16_t chunk_samples = MIN(samples_per_pkt, remain);
+
+        buf[0] = 'r'; buf[1] = 'i'; buf[2] = 'c'; buf[3] = ':';
+        buf[4] = (uint8_t)(off >> 8);
+        buf[5] = (uint8_t)(off & 0xFF);
+        buf[6] = (uint8_t)(sample_count >> 8);
+        buf[7] = (uint8_t)(sample_count & 0xFF);
+        buf[8] = (uint8_t)(chunk_samples >> 8);
+        buf[9] = (uint8_t)(chunk_samples & 0xFF);
+
+        memcpy(&buf[10], &sample_bytes[off * IMU_FIFO_SAMPLE_BYTES], (size_t)chunk_samples * IMU_FIFO_SAMPLE_BYTES);
+
+        uint16_t payload_len = (uint16_t)(RIC_OVERHEAD_SIZE - 2U + (chunk_samples * IMU_FIFO_SAMPLE_BYTES));
+        uint16_t crc = cmd_crc16_compute(buf, payload_len);
+        DBG_PRINTF("[CMD] ric tx off=%u total=%u chunk=%u len=%u\r\n",
+                   off, sample_count, chunk_samples, (uint16_t)(payload_len + 2U));
+
+        buf[payload_len] = (uint8_t)(crc & 0xFF);
+        buf[payload_len + 1U] = (uint8_t)(crc >> 8);
+
+        if (ble_data_send(buf, (uint16_t)(payload_len + 2U)) != 0)
+        {
+            break;
+        }
+    }
+}
+
+void cmd_send_response_rim(const uint8_t *sample_bytes, uint16_t sample_count)
+{
+    uint16_t notify_payload_max = 0U;
+    uint16_t mtu = ble_current_mtu();
+
+    if (sample_count > IMU_FIFO_RIM_TARGET_SAMPLES)
+    {
+        sample_count = IMU_FIFO_RIM_TARGET_SAMPLES;
+    }
+
+    uint16_t rim_len = (uint16_t)(RIM_OVERHEAD_SIZE + (sample_count * IMU_FIFO_SAMPLE_BYTES));
+
+    if (mtu > ATT_NOTIFY_HEADER_SIZE)
+    {
+        notify_payload_max = (uint16_t)(mtu - ATT_NOTIFY_HEADER_SIZE);
+    }
+
+    if ((notify_payload_max == 0U) || (rim_len <= notify_payload_max))
+    {
+        cmd_send_response_rim_legacy(sample_bytes, sample_count);
+        return;
+    }
+
+    uint16_t samples_per_pkt = calc_ric_samples_per_packet(notify_payload_max);
+
+    if (samples_per_pkt == 0U)
+    {
+        DBG_ERR("[CMD] ric skip mtu=%u payload=%u samples=%u\r\n", mtu, notify_payload_max, sample_count);
+        return;
+    }
+
+    DBG_PRINTF("[CMD] ric split mtu=%u samples=%u chunk=%u\r\n", mtu, sample_count, samples_per_pkt);
+    cmd_send_response_rim_ric(sample_bytes, sample_count, samples_per_pkt);
+}
+
 void cmd_send_response_piezo_config(const char *tag, uint16_t freq, uint16_t cycles, uint16_t avg, uint16_t delay_us, uint16_t samples)
 {
     uint8_t *buf = tx_cfg_buf;