diff --git a/examples/device/mtp/src/mtp_fs_example.c b/examples/device/mtp/src/mtp_fs_example.c
index e0dafb5ab..add7d6bf5 100644
--- a/examples/device/mtp/src/mtp_fs_example.c
+++ b/examples/device/mtp/src/mtp_fs_example.c
@@ -62,10 +62,10 @@ typedef struct {
 } fs_object_info_t;
 
 // Sample object file
-static fs_object_info_t _fs_objects[FS_MAX_NODES] = {
+static fs_object_info_t fs_objects[FS_MAX_NODES] = {
   {
     .handle = 1,
-    .parent = 0,
+    .parent = 0xffffffff,
     .allocated = true,
     .association = false,
     .name = "readme.txt",
@@ -142,7 +142,7 @@ unsigned int fs_get_object_count(void);
 fs_object_info_t* fs_object_get_from_handle(uint32_t handle) {
   fs_object_info_t* obj;
   for (unsigned int i = 0; i < FS_MAX_NODES; i++) {
-    obj = &_fs_objects[i];
+    obj = &fs_objects[i];
     if (obj->allocated && obj->handle == handle)
       return obj;
   }
@@ -152,7 +152,7 @@ fs_object_info_t* fs_object_get_from_handle(uint32_t handle) {
 unsigned int fs_get_object_count(void) {
   unsigned int s = 0;
   for (unsigned int i = 0; i < FS_MAX_NODES; i++) {
-    if (_fs_objects[i].allocated)
+    if (fs_objects[i].allocated)
       s++;
   }
   return s;
@@ -249,9 +249,26 @@ int32_t tud_mtp_command_received_cb(uint8_t idx, mtp_generic_container_t* cmd_bl
       break;
     }
 
-    case MTP_OP_GET_OBJECT_HANDLES:
+    case MTP_OP_GET_OBJECT_HANDLES: {
+      const uint32_t storage_id = cmd_block->data[0];
+      const uint32_t obj_format = cmd_block->data[1]; // optional
+      (void) obj_format;
+      const uint32_t parent_handle = cmd_block->data[2]; // folder handle, 0xFFFFFFFF is root
+      if (storage_id != 0xFFFFFFFF && storage_id != SUPPORTED_STORAGE_ID) {
+        return MTP_RESP_INVALID_STORAGE_ID;
+      }
 
+      uint32_t handles[FS_MAX_NODES] = { 0 };
+      uint32_t count = 0;
+      for (uint8_t i = 0, h = 0; i < FS_MAX_NODES; i++) {
+        if (fs_objects[i].allocated && parent_handle == fs_objects[i].parent) {
+          handles[count++] = fs_objects[i].handle;
+        }
+      }
+      mtp_container_add_auint32(out_block, count, handles);
+      tud_mtp_data_send(out_block);
       break;
+    }
 
     default: return MTP_RESP_OPERATION_NOT_SUPPORTED;
   }
@@ -284,7 +301,7 @@ mtp_response_t tud_mtp_storage_format(uint32_t storage_id) {
 
   // Simply deallocate all entries
   for (unsigned int i = 0; i < FS_MAX_NODES; i++)
-    _fs_objects[i].allocated = false;
+    fs_objects[i].allocated = false;
   TU_LOG1("Format completed\r\n");
   return MTP_RESP_OK;
 }
@@ -314,7 +331,7 @@ mtp_response_t tud_mtp_storage_association_get_object_handle(uint32_t storage_id
   }
 
   for (unsigned int i = _fs_operation.traversal_index; i < FS_MAX_NODES; i++) {
-    obj = &_fs_objects[i];
+    obj = &fs_objects[i];
     if (obj->allocated && obj->parent == parent_object_handle) {
       _fs_operation.traversal_index = i + 1;
       *next_child_handle = obj->handle;
@@ -366,8 +383,8 @@ mtp_response_t tud_mtp_storage_object_write_info(uint32_t storage_id, uint32_t p
 
   // Search for first free object
   for (unsigned int i = 0; i < FS_MAX_NODES; i++) {
-    if (!_fs_objects[i].allocated) {
-      obj = &_fs_objects[i];
+    if (!fs_objects[i].allocated) {
+      obj = &fs_objects[i];
       break;
     }
   }
@@ -572,7 +589,7 @@ mtp_response_t tud_mtp_storage_object_delete(uint32_t object_handle) {
   if (object_handle == 0 || obj->association) {
     // Delete also children
     for (unsigned int i = 0; i < FS_MAX_NODES; i++) {
-      obj = &_fs_objects[i];
+      obj = &fs_objects[i];
       if (obj->allocated && obj->parent == object_handle) {
         tud_mtp_storage_object_delete(obj->handle);
       }
diff --git a/src/class/mtp/mtp.h b/src/class/mtp/mtp.h
index 138cb1b86..c6ff2946a 100644
--- a/src/class/mtp/mtp.h
+++ b/src/class/mtp/mtp.h
@@ -654,47 +654,6 @@ typedef enum {
   MTP_ASSOCIATION_2D_PANORAMIC         = 0x0006u,
 } mtp_association_t;
 
-
-tu_static const uint16_t mtp_operations_supported[] = {
-  MTP_OP_GET_DEVICE_INFO,
-  MTP_OP_OPEN_SESSION,
-  MTP_OP_CLOSE_SESSION,
-  MTP_OP_GET_STORAGE_IDS,
-  MTP_OP_GET_STORAGE_INFO,
-  MTP_OP_GET_NUM_OBJECTS,
-  MTP_OP_GET_OBJECT_HANDLES,
-  MTP_OP_GET_OBJECT_INFO,
-  MTP_OP_GET_OBJECT,
-  MTP_OP_DELETE_OBJECT,
-  MTP_OP_SEND_OBJECT_INFO,
-  MTP_OP_SEND_OBJECT,
-  MTP_OP_FORMAT_STORE,
-  MTP_OP_RESET_DEVICE,
-  MTP_OP_GET_DEVICE_PROP_DESC,
-  MTP_OP_GET_DEVICE_PROP_VALUE,
-  MTP_OP_SET_DEVICE_PROP_VALUE,
-};
-
-tu_static const uint16_t mtp_events_supported[] = {
-  MTP_EVENT_OBJECT_ADDED,
-};
-
-tu_static const uint16_t mtp_device_properties_supported[] = {
-  MTP_DEV_PROP_DEVICE_FRIENDLY_NAME,
-};
-
-tu_static const uint16_t mtp_capture_formats[] = {
-  MTP_OBJ_FORMAT_UNDEFINED,
-  MTP_OBJ_FORMAT_ASSOCIATION,
-  MTP_OBJ_FORMAT_TEXT,
-};
-
-tu_static const uint16_t mtp_playback_formats[] = {
-  MTP_OBJ_FORMAT_UNDEFINED,
-  MTP_OBJ_FORMAT_ASSOCIATION,
-  MTP_OBJ_FORMAT_TEXT,
-};
-
 //--------------------------------------------------------------------+
 // Data structures
 //--------------------------------------------------------------------+
@@ -738,7 +697,7 @@ typedef struct TU_ATTR_PACKED {
   uint16_t utf16[];
 } mtp_flexible_string_t;
 
- typedef union TU_ATTR_PACKED {
+typedef union TU_ATTR_PACKED {
   struct {
     uint16_t physical; // physical location
     uint16_t logical;  // logical within physical
@@ -827,21 +786,16 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_raw(mtp_generic_c
   return len;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_field(mtp_generic_container_t* p_container, uint8_t scalar_size, uint32_t count, const void* data) {
-  if (count == 0) {
-    // count = 0 means scalar
-    return mtp_container_add_raw(p_container, data, scalar_size);
-  } else {
-    uint8_t* container8 = (uint8_t*) p_container;
+TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_array(mtp_generic_container_t* p_container, uint8_t scalar_size, uint32_t count, const void* data) {
+  uint8_t* container8 = (uint8_t*)p_container;
 
-    tu_unaligned_write32(container8 + p_container->len, count);
-    p_container->len += 4;
+  tu_unaligned_write32(container8 + p_container->len, count);
+  p_container->len += 4;
 
-    memcpy(container8 + p_container->len, data, count * scalar_size);
-    p_container->len += count * scalar_size;
+  memcpy(container8 + p_container->len, data, count * scalar_size);
+  p_container->len += count * scalar_size;
 
-    return 4 + count * scalar_size;
-  }
+  return 4 + count * scalar_size;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_string(mtp_generic_container_t* p_container, uint8_t count, uint16_t* utf16) {
@@ -870,34 +824,37 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_cstring(mtp_gener
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_uint8(mtp_generic_container_t* p_container, uint8_t data) {
-  return mtp_container_add_field(p_container, sizeof(uint8_t), 0, &data);
+  return mtp_container_add_raw(p_container, &data, 1);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_uint16(mtp_generic_container_t* p_container, uint16_t data) {
-  return mtp_container_add_field(p_container, sizeof(uint16_t), 0, &data);
+  return mtp_container_add_raw(p_container, &data, 2);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_uint32(mtp_generic_container_t* p_container, uint32_t data) {
-  return mtp_container_add_field(p_container, sizeof(uint32_t), 0, &data);
+  return mtp_container_add_raw(p_container, &data, 4);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_uint64(mtp_generic_container_t* p_container, uint64_t data) {
-  return mtp_container_add_field(p_container, 8, 0, &data);
+  return mtp_container_add_raw(p_container, &data, 8);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_uint128(mtp_generic_container_t* p_container, const void* data) {
+  return mtp_container_add_raw(p_container, data, 16);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_auint8(mtp_generic_container_t* p_container, uint32_t count, const uint8_t* data) {
-  return mtp_container_add_field(p_container, sizeof(uint8_t), count, data);
+  return mtp_container_add_array(p_container, sizeof(uint8_t), count, data);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_auint16(mtp_generic_container_t* p_container, uint32_t count, const uint16_t* data) {
-  return mtp_container_add_field(p_container, sizeof(uint16_t), count, data);
+  return mtp_container_add_array(p_container, sizeof(uint16_t), count, data);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_auint32(mtp_generic_container_t* p_container, uint32_t count, const uint32_t* data) {
-  return mtp_container_add_field(p_container, sizeof(uint32_t), count, data);
+  return mtp_container_add_array(p_container, sizeof(uint32_t), count, data);
 }
 
-
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/class/mtp/mtp_device.c b/src/class/mtp/mtp_device.c
index cb7bb9574..8e403f9ac 100644
--- a/src/class/mtp/mtp_device.c
+++ b/src/class/mtp/mtp_device.c
@@ -85,7 +85,6 @@ static mtp_phase_type_t mtpd_chk_session_open(const char *func_name);
 static mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp);
 static mtp_phase_type_t mtpd_handle_data(void);
 static mtp_phase_type_t mtpd_handle_cmd_close_session(void);
-static mtp_phase_type_t mtpd_handle_cmd_get_object_handles(void);
 static mtp_phase_type_t mtpd_handle_cmd_get_object_info(void);
 static mtp_phase_type_t mtpd_handle_cmd_get_object(void);
 static mtp_phase_type_t mtpd_handle_dti_get_object(void);
@@ -451,7 +450,8 @@ mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp) {
 
     case MTP_OP_GET_OBJECT_HANDLES:
       TU_LOG_DRV("  MTP command: MTP_OP_GET_OBJECT_HANDLES\n");
-      return mtpd_handle_cmd_get_object_handles();
+      break;
+
     case MTP_OP_GET_OBJECT_INFO:
       TU_LOG_DRV("  MTP command: MTP_OP_GET_OBJECT_INFO\n");
       return mtpd_handle_cmd_get_object_info();
@@ -528,46 +528,6 @@ mtp_phase_type_t mtpd_handle_cmd_close_session(void)
   return MTP_PHASE_RESPONSE;
 }
 
-mtp_phase_type_t mtpd_handle_cmd_get_object_handles(void)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  uint32_t storage_id = p_container->data[0];
-  uint32_t object_format_code = p_container->data[1]; // optional, not managed
-  uint32_t parent_object_handle = p_container->data[2]; // folder specification, 0xffffffff=objects with no parent
-
-  p_container->len = MTP_CONTAINER_HEADER_LENGTH + sizeof(uint32_t);
-  p_container->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
-  p_container->code = MTP_OP_GET_OBJECT_HANDLES;
-  p_container->data[0] = 0;
-
-  mtp_phase_type_t phase;
-  if ((phase = mtpd_chk_generic(__func__, (object_format_code != 0), MTP_RESP_SPECIFICATION_BY_FORMAT_UNSUPPORTED, "specification by format unsupported")) != MTP_PHASE_NONE) {
-    return phase;
-  }
-  //list of all object handles on all storages, not managed
-  if ((phase = mtpd_chk_generic(__func__, (storage_id == 0xFFFFFFFF), MTP_RESP_OPERATION_NOT_SUPPORTED, "list of all object handles on all storages unsupported")) != MTP_PHASE_NONE) {
-    return phase;
-  }
-
-  tud_mtp_storage_object_done();
-  uint32_t next_child_handle = 0;
-  while(true)
-  {
-    mtp_response_t res = tud_mtp_storage_association_get_object_handle(storage_id, parent_object_handle, &next_child_handle);
-    if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESP_OK), res, "")) != MTP_PHASE_NONE) {
-      return phase;
-    }
-    if (next_child_handle == 0) {
-      break;
-    }
-    mtpd_gct_append_object_handle(next_child_handle);
-  }
-  tud_mtp_storage_object_done();
-
-  _mtpd_itf.queued_len = p_container->len;
-  return MTP_PHASE_DATA_IN;
-}
-
 mtp_phase_type_t mtpd_handle_cmd_get_object_info(void)
 {
   TU_VERIFY_STATIC(sizeof(mtp_object_info_t) < MTP_MAX_PACKET_SIZE, "mtp_object_info_t shall fit in MTP_MAX_PACKET_SIZE");