From d12cc82893ede76e9ef6c356292734347648bb65 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?D=C3=A1niel=20Buga?= <bugadani@gmail.com>
Date: Wed, 5 Feb 2025 10:47:44 +0100
Subject: [PATCH] Fix BLE controller test

---
 hil-test/tests/esp_wifi_ble_controller.rs | 20 +++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

diff --git a/hil-test/tests/esp_wifi_ble_controller.rs b/hil-test/tests/esp_wifi_ble_controller.rs
index 0cc431c0a5..c3d26ee20d 100644
--- a/hil-test/tests/esp_wifi_ble_controller.rs
+++ b/hil-test/tests/esp_wifi_ble_controller.rs
@@ -72,7 +72,7 @@ mod tests {
         // so if we don't get the expected response we'll timeout and fail
         let mut buf = [0u8; 255];
         loop {
-            let len = connector.read(&mut buf).unwrap();
+            let len = read_packet(&mut connector, &mut buf);
             if len == 7 {
                 assert_eq!(buf[0], 4, "Expected packet type = 4 (EVENT)");
                 assert_eq!(buf[1], 14, "Expected event code = 14 (COMMAND_COMPLETE)");
@@ -91,3 +91,21 @@ mod tests {
         }
     }
 }
+
+fn read_packet(connector: &mut BleConnector, buf: &mut [u8]) -> usize {
+    // Read header
+    read_all(connector, &mut buf[..3]);
+
+    // Read payload
+    let payload_len = buf[2] as usize;
+    read_all(connector, &mut buf[3..][..payload_len]);
+
+    3 + payload_len
+}
+
+fn read_all(connector: &mut BleConnector, mut buf: &mut [u8]) {
+    while !buf.is_empty() {
+        let len = connector.read(buf).unwrap();
+        buf = &mut buf[len..];
+    }
+}