diff --git a/micropython/extmod/modtrezormsg/modtrezormsg-stmhal.h b/micropython/extmod/modtrezormsg/modtrezormsg-stmhal.h
index a3bc853c..dc515d3a 100644
--- a/micropython/extmod/modtrezormsg/modtrezormsg-stmhal.h
+++ b/micropython/extmod/modtrezormsg/modtrezormsg-stmhal.h
@@ -5,8 +5,7 @@
  * see LICENSE file for details
  */
 
-extern int usb_hid_read_blocking(uint8_t iface_num, uint8_t *buf, uint32_t len, uint32_t timeout);
-extern int usb_hid_write_blocking(uint8_t iface_num, const uint8_t *buf, uint32_t len, uint32_t timeout);
+#include "usb.h"
 
 void msg_init(void)
 {
@@ -14,15 +13,15 @@ void msg_init(void)
 
 ssize_t msg_recv(uint8_t *iface, uint8_t *buf, size_t len)
 {
-    *iface = 0; // TODO: return proper interface
-    return usb_hid_read_blocking(0x00, buf, len, 1);
+    int i = usb_hid_read_select(1); // 1ms timeout
+    if (i < 0) {
+        return 0;
+    }
+    *iface = i;
+    return usb_hid_read(i, buf, len);
 }
 
 ssize_t msg_send(uint8_t iface, const uint8_t *buf, size_t len)
 {
-    (void)iface; // TODO: ignore interface for now
-    if (len > 0) {
-        usb_hid_write_blocking(0x00, buf, len, 1);
-    }
-    return len;
+    return usb_hid_write_blocking(iface, buf, len, 1); // 1ms timeout
 }
diff --git a/micropython/trezorhal/usb_hid-impl.h b/micropython/trezorhal/usb_hid-impl.h
index 64ddfdce..d2d3f582 100644
--- a/micropython/trezorhal/usb_hid-impl.h
+++ b/micropython/trezorhal/usb_hid-impl.h
@@ -164,8 +164,24 @@ int usb_hid_write(uint8_t iface_num, const uint8_t *buf, uint32_t len) {
     return len;
 }
 
+int usb_hid_read_select(uint32_t timeout) {
+    const uint32_t start = HAL_GetTick();
+    for (;;) {
+        for (int i = 0; i < USBD_MAX_NUM_INTERFACES; i++) {
+            if (usb_hid_can_read(i)) {
+                return i;
+            }
+        }
+        if (HAL_GetTick() - start >= timeout) {
+            break;
+        }
+        __WFI(); // Enter sleep mode, waiting for interrupt
+    }
+    return -1; // Timeout
+}
+
 int usb_hid_read_blocking(uint8_t iface_num, uint8_t *buf, uint32_t len, uint32_t timeout) {
-    uint32_t start = HAL_GetTick();
+    const uint32_t start = HAL_GetTick();
     while (!usb_hid_can_read(iface_num)) {
         if (HAL_GetTick() - start >= timeout) {
             return 0; // Timeout
@@ -176,7 +192,7 @@ int usb_hid_read_blocking(uint8_t iface_num, uint8_t *buf, uint32_t len, uint32_
 }
 
 int usb_hid_write_blocking(uint8_t iface_num, const uint8_t *buf, uint32_t len, uint32_t timeout) {
-    uint32_t start = HAL_GetTick();
+    const uint32_t start = HAL_GetTick();
     while (!usb_hid_can_write(iface_num)) {
         if (HAL_GetTick() - start >= timeout) {
             return 0; // Timeout