diff --git a/micropython/trezorhal/usb.c b/micropython/trezorhal/usb.c
index 71fadef1..691a5539 100644
--- a/micropython/trezorhal/usb.c
+++ b/micropython/trezorhal/usb.c
@@ -134,14 +134,20 @@ static const USBD_DescriptorsTypeDef usb_descriptors = {
     .GetInterfaceStrDescriptor     = usb_get_interface_str_descriptor,
 };
 
+static usb_iface_t *usb_get_iface(uint8_t iface_num) {
+    if (iface_num < USBD_MAX_NUM_INTERFACES) {
+        return &usb_ifaces[iface_num];
+    } else {
+        return NULL; // Invalid interface number
+    }
+}
+
 static void *usb_desc_alloc_iface(size_t desc_len) {
-    if (usb_config_desc->wTotalLength + desc_len > USB_MAX_CONFIG_DESC_SIZE) {
-        return NULL;  // Not enough space in the descriptor
+    if (usb_config_desc->wTotalLength + desc_len < USB_MAX_CONFIG_DESC_SIZE) {
+        return usb_next_iface_desc;
+    } else {
+        return NULL; // Not enough space in the descriptor
     }
-    if (usb_config_desc->bNumInterfaces + 1 >= USBD_MAX_NUM_INTERFACES) {
-        return NULL;  // Already using all the interfaces
-    }
-    return usb_next_iface_desc;
 }
 
 static void usb_desc_add_iface(size_t desc_len) {
diff --git a/micropython/trezorhal/usb_hid-defs.h b/micropython/trezorhal/usb_hid-defs.h
index 499b40bc..f57d3390 100644
--- a/micropython/trezorhal/usb_hid-defs.h
+++ b/micropython/trezorhal/usb_hid-defs.h
@@ -27,38 +27,33 @@ typedef enum {
 } usb_hid_protocol_t;
 
 typedef struct {
-    // Interface configuration
-    uint8_t iface_num;  // Address of this HID interface
-    uint8_t ep_in;      // Address of IN endpoint (with the highest bit set)
-    uint8_t ep_out;     // Address of OUT endpoint
-
-    // HID configuration
-    uint8_t subclass;          // usb_iface_subclass_t
-    uint8_t protocol;          // usb_iface_protocol_t
-    uint8_t max_packet_len;    // rx_buffer should be big enough
-    uint8_t polling_interval;  // In units of 1ms
+    uint8_t iface_num;          // Address of this HID interface
+    uint8_t ep_in;              // Address of IN endpoint (with the highest bit set)
+    uint8_t ep_out;             // Address of OUT endpoint
+    uint8_t subclass;           // usb_iface_subclass_t
+    uint8_t protocol;           // usb_iface_protocol_t
+    uint8_t max_packet_len;     // rx_buffer should be big enough
+    uint8_t polling_interval;   // In units of 1ms
     uint8_t report_desc_len;
     const uint8_t *report_desc;
-
-    // HID read buffer
-    uint8_t *rx_buffer;  // Big enough for max_packet_len
+    uint8_t *rx_buffer;         // Big enough for max_packet_len
 } usb_hid_info_t;
 
 typedef struct {
-    // HID state
     uint8_t in_idle;       // Set to 1 after IN endpoint gets idle
     uint8_t protocol;      // For SET_PROTOCOL/GET_PROTOCOL setup reqs
     uint8_t idle_rate;     // For SET_IDLE/GET_IDLE setup reqs
     uint8_t alt_setting;   // For SET_INTERFACE/GET_INTERFACE setup reqs
     uint8_t rx_buffer_len; // Length of data read into rx_buffer
 
-    // HID configuration (copied from usb_hid_info_t on init)
+    // Configuration (copied from usb_hid_info_t on init)
     uint8_t ep_in;
     uint8_t ep_out;
     uint8_t max_packet_len;
     uint8_t report_desc_len;
     uint8_t *rx_buffer;
     const uint8_t *report_desc;
+
     const usb_hid_descriptor_block_t *desc_block;
 } usb_hid_state_t;
 
diff --git a/micropython/trezorhal/usb_hid-impl.h b/micropython/trezorhal/usb_hid-impl.h
index fb1348f2..9a63d0a9 100644
--- a/micropython/trezorhal/usb_hid-impl.h
+++ b/micropython/trezorhal/usb_hid-impl.h
@@ -9,20 +9,33 @@
 /* usb_hid_add adds and configures new USB HID interface according to
  * configuration options passed in `info`. */
 int usb_hid_add(const usb_hid_info_t *info) {
-    if ((info->iface_num >= USBD_MAX_NUM_INTERFACES) || (info->iface_num < usb_config_desc->bNumInterfaces)) {
-        return -1; // Invalid interface number
+
+    usb_iface_t *iface = usb_get_iface(info->iface_num);
+
+    if (iface == NULL) {
+        return 1; // Invalid interface number
     }
-    if (((info->ep_in & USB_EP_DIR_MSK) == 0) || ((info->ep_out & USB_EP_DIR_MSK) != 0)) {
-        return -2; // Invalid endpoints
-    }
-    if ((info->rx_buffer == NULL) || (info->report_desc == NULL)) {
-        return -3; // Invalid buffers
+    if (iface->type != USB_IFACE_TYPE_DISABLED) {
+        return 1; // Interface is already enabled
     }
 
     usb_hid_descriptor_block_t *d = usb_desc_alloc_iface(sizeof(usb_hid_descriptor_block_t));
 
-    if (!d) {
-        return -4; // Not enough space in the configuration descriptor
+    if (d == NULL) {
+        return 1; // Not enough space in the configuration descriptor
+    }
+
+    if ((info->ep_in & USB_EP_DIR_MSK) != USB_EP_DIR_IN) {
+        return 1; // IN EP is invalid
+    }
+    if ((info->ep_out & USB_EP_DIR_MSK) != USB_EP_DIR_OUT) {
+        return 1; // OUT EP is invalid
+    }
+    if (info->rx_buffer == NULL) {
+        return 1;
+    }
+    if (info->report_desc == NULL) {
+        return 1;
     }
 
     // Interface descriptor
@@ -65,59 +78,61 @@ int usb_hid_add(const usb_hid_info_t *info) {
     usb_desc_add_iface(sizeof(usb_hid_descriptor_block_t));
 
     // Interface state
-    usb_iface_t *i = &usb_ifaces[info->iface_num];
-    i->type = USB_IFACE_TYPE_HID;
-    i->hid.ep_in = info->ep_in;
-    i->hid.ep_out = info->ep_out;
-    i->hid.rx_buffer = info->rx_buffer;
-    i->hid.max_packet_len = info->max_packet_len;
-    i->hid.report_desc_len = info->report_desc_len;
-    i->hid.report_desc = info->report_desc;
-    i->hid.desc_block = d;
+    iface->type = USB_IFACE_TYPE_HID;
+    iface->hid.ep_in = info->ep_in;
+    iface->hid.ep_out = info->ep_out;
+    iface->hid.rx_buffer = info->rx_buffer;
+    iface->hid.max_packet_len = info->max_packet_len;
+    iface->hid.report_desc_len = info->report_desc_len;
+    iface->hid.report_desc = info->report_desc;
+    iface->hid.desc_block = d;
 
     return 0;
 }
 
 int usb_hid_can_read(uint8_t iface_num) {
-    if (usb_dev_handle.dev_state != USBD_STATE_CONFIGURED) {
-        return 0; // Device is not configured
-    }
-    if (iface_num >= USBD_MAX_NUM_INTERFACES) {
+    usb_iface_t *iface = usb_get_iface(iface_num);
+    if (iface == NULL) {
         return 0; // Invalid interface number
     }
-    if (usb_ifaces[iface_num].type != USB_IFACE_TYPE_HID) {
+    if (iface->type != USB_IFACE_TYPE_HID) {
         return 0; // Invalid interface type
     }
-    if (usb_ifaces[iface_num].hid.rx_buffer_len == 0) {
+    if (iface->hid.rx_buffer_len == 0) {
         return 0; // Nothing in the receiving buffer
     }
+    if (usb_dev_handle.dev_state != USBD_STATE_CONFIGURED) {
+        return 0; // Device is not configured
+    }
     return 1;
 }
 
 int usb_hid_can_write(uint8_t iface_num) {
-    if (usb_dev_handle.dev_state != USBD_STATE_CONFIGURED) {
-        return 0; // Device is not configured
-    }
-    if (iface_num >= USBD_MAX_NUM_INTERFACES) {
+    usb_iface_t *iface = usb_get_iface(iface_num);
+    if (iface == NULL) {
         return 0; // Invalid interface number
     }
-    if (usb_ifaces[iface_num].type != USB_IFACE_TYPE_HID) {
+    if (iface->type != USB_IFACE_TYPE_HID) {
         return 0; // Invalid interface type
     }
-    if (usb_ifaces[iface_num].hid.in_idle == 0) {
+    if (iface->hid.in_idle == 0) {
         return 0; // Last transmission is not over yet
     }
+    if (usb_dev_handle.dev_state != USBD_STATE_CONFIGURED) {
+        return 0; // Device is not configured
+    }
     return 1;
 }
 
 int usb_hid_read(uint8_t iface_num, uint8_t *buf, uint32_t len) {
-    if (iface_num >= USBD_MAX_NUM_INTERFACES) {
+    usb_iface_t *iface = usb_get_iface(iface_num);
+    if (iface == NULL) {
         return -1; // Invalid interface number
     }
-    if (usb_ifaces[iface_num].type != USB_IFACE_TYPE_HID) {
+    if (iface->type != USB_IFACE_TYPE_HID) {
         return -2; // Invalid interface type
     }
-    usb_hid_state_t *state = &usb_ifaces[iface_num].hid;
+    usb_hid_state_t *state = &iface->hid;
 
     // Copy maximum possible amount of data and truncate the buffer length
     if (len < state->rx_buffer_len) {
@@ -134,13 +149,14 @@ int usb_hid_read(uint8_t iface_num, uint8_t *buf, uint32_t len) {
 }
 
 int usb_hid_write(uint8_t iface_num, const uint8_t *buf, uint32_t len) {
-    if (iface_num >= USBD_MAX_NUM_INTERFACES) {
+    usb_iface_t *iface = usb_get_iface(iface_num);
+    if (iface == NULL) {
         return -1; // Invalid interface number
     }
-    if (usb_ifaces[iface_num].type != USB_IFACE_TYPE_HID) {
+    if (iface->type != USB_IFACE_TYPE_HID) {
         return -2; // Invalid interface type
     }
-    usb_hid_state_t *state = &usb_ifaces[iface_num].hid;
+    usb_hid_state_t *state = &iface->hid;
 
     state->in_idle = 0;
     USBD_LL_Transmit(&usb_dev_handle, state->ep_in, UNCONST(buf), (uint16_t)len);
@@ -256,9 +272,6 @@ static int usb_hid_class_setup(USBD_HandleTypeDef *dev, usb_hid_state_t *state,
 
 static uint8_t usb_hid_class_data_in(USBD_HandleTypeDef *dev, usb_hid_state_t *state, uint8_t ep_num) {
     if ((ep_num | USB_EP_DIR_IN) == state->ep_in) {
-        // Ensure that the FIFO is empty before a new transfer,
-        // this condition could be caused by a new transfer
-        // before the end of the previous transfer.
         state->in_idle = 1;
     }
     return USBD_OK;