reworking MTP API, adding callback, getting GetDeviceInfo working

4 files changed, 760 insertions, 695 deletions

diff --git a/examples/device/mtp/src/mtp_fs_example.c b/examples/device/mtp/src/mtp_fs_example.c
index 9dbac746b..68140b1ae 100644
--- a/examples/device/mtp/src/mtp_fs_example.c
+++ b/examples/device/mtp/src/mtp_fs_example.c
@@ -30,6 +30,58 @@
 #define MTPD_VOLUME_IDENTIFIER "volume"
 
 //--------------------------------------------------------------------+
+// Device Info
+//--------------------------------------------------------------------+
+
+// device info string (including terminating null)
+const uint16_t dev_info_manufacturer[] = { 'T', 'i', 'n', 'y', 'U', 'S', 'B', 0 };
+const uint16_t dev_info_model[]        = { 'M', 'T', 'P', ' ', 'E', 'x', 'a', 'm', 'p', 'l', 'e', 0 };
+const uint16_t dev_info_version[]      = { '1', '.', '0', 0 };
+const uint16_t dev_info_serial[]       = { '1', '2', '3', '4', '5', '6', 0 };
+
+
+static const uint16_t supported_operations[] = {
+  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
+};
+
+static const uint16_t supported_events[] = {
+  MTP_EVENT_OBJECT_ADDED,
+};
+
+static const uint16_t supported_device_properties[] = {
+  MTP_DEV_PROP_DEVICE_FRIENDLY_NAME,
+};
+
+static const uint16_t capture_formats[] = {
+  MTP_OBJ_FORMAT_UNDEFINED,
+  MTP_OBJ_FORMAT_ASSOCIATION,
+  MTP_OBJ_FORMAT_TEXT,
+};
+
+static const uint16_t playback_formats[] = {
+  MTP_OBJ_FORMAT_UNDEFINED,
+  MTP_OBJ_FORMAT_ASSOCIATION,
+  MTP_OBJ_FORMAT_TEXT,
+};
+
+
+//--------------------------------------------------------------------+
 // RAM FILESYSTEM
 //--------------------------------------------------------------------+
 #define FS_MAX_NODES 5UL
@@ -37,56 +89,54 @@
 #define FS_MAX_NODE_NAME_LEN 64UL
 #define FS_ISODATETIME_LEN 26UL
 
-typedef struct
-{
-    uint32_t handle;
-    uint32_t parent;
-    uint32_t size;
-    bool allocated;
-    bool association;
-    char name[FS_MAX_NODE_NAME_LEN];
-    char created[FS_ISODATETIME_LEN];
-    char modified[FS_ISODATETIME_LEN];
-    uint8_t data[FS_MAX_NODE_BYTES];
+typedef struct {
+  uint32_t handle;
+  uint32_t parent;
+  uint32_t size;
+  bool allocated;
+  bool association;
+  char name[FS_MAX_NODE_NAME_LEN];
+  char created[FS_ISODATETIME_LEN];
+  char modified[FS_ISODATETIME_LEN];
+  uint8_t data[FS_MAX_NODE_BYTES];
 } fs_object_info_t;
 
 // Sample object file
 static fs_object_info_t _fs_objects[FS_MAX_NODES] = {
-    {
-        .handle = 1,
-        .parent = 0,
-        .allocated = true,
-        .association = false,
-        .name = "readme.txt",
-        .created = "20240104T111134.0",
-        .modified = "20241214T121110.0",
-        .data = "USB MTP on RAM Filesystem example\n",
-        .size = 34
-    }
+  {
+    .handle = 1,
+    .parent = 0,
+    .allocated = true,
+    .association = false,
+    .name = "readme.txt",
+    .created = "20240104T111134.0",
+    .modified = "20241214T121110.0",
+    .data = "USB MTP on RAM Filesystem example\n",
+    .size = 34
+  }
 };
 
 //--------------------------------------------------------------------+
 // OPERATING STATUS
 //--------------------------------------------------------------------+
-typedef struct
-{
-    // Session
-    uint32_t session_id;
-    // Association traversal
-    uint32_t traversal_parent;
-    uint32_t traversal_index;
-    // Object open for reading
-    uint32_t read_handle;
-    uint32_t read_pos;
-    // Object open for writing
-    uint32_t write_handle;
-    uint32_t write_pos;
-    // Unique identifier
-    uint32_t last_handle;
+typedef struct {
+  // Session
+  uint32_t session_id;
+  // Association traversal
+  uint32_t traversal_parent;
+  uint32_t traversal_index;
+  // Object open for reading
+  uint32_t read_handle;
+  uint32_t read_pos;
+  // Object open for writing
+  uint32_t write_handle;
+  uint32_t write_pos;
+  // Unique identifier
+  uint32_t last_handle;
 } fs_operation_t;
 
 static fs_operation_t _fs_operation = {
-    .last_handle = 1
+  .last_handle = 1
 };
 
 //--------------------------------------------------------------------+
@@ -94,481 +144,461 @@ static fs_operation_t _fs_operation = {
 //--------------------------------------------------------------------+
 
 // Get pointer to object info from handle
-fs_object_info_t *fs_object_get_from_handle(uint32_t handle);
+fs_object_info_t* fs_object_get_from_handle(uint32_t handle);
 
 // Get the number of allocated nodes in filesystem
 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];
-        if (obj->allocated && obj->handle == handle)
-            return obj;
-    }
-    return NULL;
+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];
+    if (obj->allocated && obj->handle == handle)
+      return obj;
+  }
+  return NULL;
+}
+
+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)
+      s++;
+  }
+  return s;
 }
 
-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)
-            s++;
+int32_t tud_mtp_data_complete_cb(uint8_t idx, mtp_container_header_t* cmd_header, mtp_generic_container_t* resp_block, tusb_xfer_result_t xfer_result, uint32_t xferred_bytes) {
+  (void) idx;
+  // switch (cmd_header->code) {
+  //   default: break;
+  // }
+  resp_block->len = MTP_CONTAINER_HEADER_LENGTH;
+  resp_block->code = (xfer_result == XFER_RESULT_SUCCESS) ? MTP_RESP_OK : MTP_RESP_GENERAL_ERROR;
+
+  tud_mtp_response_send(resp_block);
+
+  return 0;
+}
+
+int32_t tud_mtp_command_received_cb(uint8_t idx, mtp_generic_container_t* cmd_block, mtp_generic_container_t* out_block) {
+  (void)idx;
+  switch (cmd_block->code) {
+    case MTP_OP_GET_DEVICE_INFO: {
+      // Device info is already prepared up to playback formats. Application need to add string fields
+      mtp_container_add_string(out_block, TU_ARRAY_SIZE(dev_info_manufacturer), dev_info_manufacturer);
+      mtp_container_add_string(out_block, TU_ARRAY_SIZE(dev_info_model), dev_info_model);
+      mtp_container_add_string(out_block, TU_ARRAY_SIZE(dev_info_version), dev_info_version);
+      mtp_container_add_string(out_block, TU_ARRAY_SIZE(dev_info_serial), dev_info_serial);
+
+      tud_mtp_data_send(out_block);
+      break;
     }
-    return s;
+
+    case MTP_OP_OPEN_SESSION:
+      out_block->len = MTP_CONTAINER_HEADER_LENGTH;
+      out_block->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+      // TODO check if session is already opened
+      out_block->code = MTP_RESP_OK;
+
+      tud_mtp_response_send(out_block);
+      break;
+
+    default: return -1;
+  }
+
+  return 0;
 }
 
 //--------------------------------------------------------------------+
 // API
 //--------------------------------------------------------------------+
-mtp_response_t tud_mtp_storage_open_session(uint32_t *session_id)
-{
-    if (*session_id == 0)
-    {
-        TU_LOG1("Invalid session ID\r\n");
-        return MTP_RESP_INVALID_PARAMETER;
-    }
-    if (_fs_operation.session_id != 0)
-    {
-        *session_id = _fs_operation.session_id;
-        TU_LOG1("ERR: Session %ld already open\r\n", _fs_operation.session_id);
-        return MTP_RESP_SESSION_ALREADY_OPEN;
-    }
-    _fs_operation.session_id = *session_id;
-    TU_LOG1("Open session with id %ld\r\n", _fs_operation.session_id);
-    return MTP_RESP_OK;
+mtp_response_t tud_mtp_storage_open_session(uint32_t* session_id) {
+  if (*session_id == 0) {
+    TU_LOG1("Invalid session ID\r\n");
+    return MTP_RESP_INVALID_PARAMETER;
+  }
+  if (_fs_operation.session_id != 0) {
+    *session_id = _fs_operation.session_id;
+    TU_LOG1("ERR: Session %ld already open\r\n", _fs_operation.session_id);
+    return MTP_RESP_SESSION_ALREADY_OPEN;
+  }
+  _fs_operation.session_id = *session_id;
+  TU_LOG1("Open session with id %ld\r\n", _fs_operation.session_id);
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_close_session(uint32_t session_id)
-{
-    if (session_id != _fs_operation.session_id)
-    {
-        TU_LOG1("ERR: Session %ld not open\r\n", session_id);
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
-    _fs_operation.session_id = 0;
-    TU_LOG1("Session closed\r\n");
-    return MTP_RESP_OK;
+mtp_response_t tud_mtp_storage_close_session(uint32_t session_id) {
+  if (session_id != _fs_operation.session_id) {
+    TU_LOG1("ERR: Session %ld not open\r\n", session_id);
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
+  _fs_operation.session_id = 0;
+  TU_LOG1("Session closed\r\n");
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_get_storage_id(uint32_t *storage_id)
-{
-    if (_fs_operation.session_id == 0)
-    {
-        TU_LOG1("ERR: Session not open\r\n");
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
-    *storage_id = STORAGE_ID(0x0001, 0x0001);
-    TU_LOG1("Retrieved storage identifier %ld\r\n", *storage_id);
-    return MTP_RESP_OK;
+mtp_response_t tud_mtp_get_storage_id(uint32_t* storage_id) {
+  if (_fs_operation.session_id == 0) {
+    TU_LOG1("ERR: Session not open\r\n");
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
+  *storage_id = STORAGE_ID(0x0001, 0x0001);
+  TU_LOG1("Retrieved storage identifier %ld\r\n", *storage_id);
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_get_storage_info(uint32_t storage_id, mtp_storage_info_t *info)
-{
-    if (_fs_operation.session_id == 0)
-    {
-        TU_LOG1("ERR: Session not open\r\n");
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
-    if (storage_id != STORAGE_ID(0x0001, 0x0001))
-    {
-        TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
-        return MTP_RESP_INVALID_STORAGE_ID;
-    }
-    info->storage_type = MTP_STORAGE_TYPE_FIXED_RAM;
-    info->filesystem_type = MTP_FILESYSTEM_TYPE_GENERIC_HIERARCHICAL;
-    info->access_capability = MTP_ACCESS_CAPABILITY_READ_WRITE;
-    info->max_capacity_in_bytes = FS_MAX_NODES * FS_MAX_NODE_BYTES;
-    info->free_space_in_objects = FS_MAX_NODES - fs_get_object_count();
-    info->free_space_in_bytes = info->free_space_in_objects * FS_MAX_NODE_BYTES;
-    mtpd_gct_append_wstring(MTPD_STORAGE_DESCRIPTION);
-    mtpd_gct_append_wstring(MTPD_VOLUME_IDENTIFIER);
-    return MTP_RESP_OK;
+mtp_response_t tud_mtp_get_storage_info(uint32_t storage_id, mtp_storage_info_t* info) {
+  if (_fs_operation.session_id == 0) {
+    TU_LOG1("ERR: Session not open\r\n");
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
+  if (storage_id != STORAGE_ID(0x0001, 0x0001)) {
+    TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
+    return MTP_RESP_INVALID_STORAGE_ID;
+  }
+  info->storage_type = MTP_STORAGE_TYPE_FIXED_RAM;
+  info->filesystem_type = MTP_FILESYSTEM_TYPE_GENERIC_HIERARCHICAL;
+  info->access_capability = MTP_ACCESS_CAPABILITY_READ_WRITE;
+  info->max_capacity_in_bytes = FS_MAX_NODES * FS_MAX_NODE_BYTES;
+  info->free_space_in_objects = FS_MAX_NODES - fs_get_object_count();
+  info->free_space_in_bytes = info->free_space_in_objects * FS_MAX_NODE_BYTES;
+  mtpd_gct_append_wstring(MTPD_STORAGE_DESCRIPTION);
+  mtpd_gct_append_wstring(MTPD_VOLUME_IDENTIFIER);
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_format(uint32_t storage_id)
-{
-    if (_fs_operation.session_id == 0)
-    {
-        TU_LOG1("ERR: Session not open\r\n");
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
-    if (storage_id != STORAGE_ID(0x0001, 0x0001))
-    {
-        TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
-        return MTP_RESP_INVALID_STORAGE_ID;
-    }
+mtp_response_t tud_mtp_storage_format(uint32_t storage_id) {
+  if (_fs_operation.session_id == 0) {
+    TU_LOG1("ERR: Session not open\r\n");
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
+  if (storage_id != STORAGE_ID(0x0001, 0x0001)) {
+    TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
+    return MTP_RESP_INVALID_STORAGE_ID;
+  }
 
-    // Simply deallocate all entries
-    for (unsigned int i=0; i<FS_MAX_NODES; i++)
-        _fs_objects[i].allocated = false;
-    TU_LOG1("Format completed\r\n");
-    return MTP_RESP_OK;
+  // Simply deallocate all entries
+  for (unsigned int i = 0; i < FS_MAX_NODES; i++)
+    _fs_objects[i].allocated = false;
+  TU_LOG1("Format completed\r\n");
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_association_get_object_handle(uint32_t storage_id, uint32_t parent_object_handle, uint32_t *next_child_handle)
-{
-    fs_object_info_t *obj;
+mtp_response_t tud_mtp_storage_association_get_object_handle(uint32_t storage_id, uint32_t parent_object_handle,
+                                                             uint32_t* next_child_handle) {
+  fs_object_info_t* obj;
 
-    if (_fs_operation.session_id == 0)
-    {
-        TU_LOG1("ERR: Session not open\r\n");
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
-    // We just have one storage, same reply if querying all storages
-    if (storage_id != 0xFFFFFFFF && storage_id != STORAGE_ID(0x0001, 0x0001))
-    {
-        TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
-        return MTP_RESP_INVALID_STORAGE_ID;
-    }
+  if (_fs_operation.session_id == 0) {
+    TU_LOG1("ERR: Session not open\r\n");
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
+  // We just have one storage, same reply if querying all storages
+  if (storage_id != 0xFFFFFFFF && storage_id != STORAGE_ID(0x0001, 0x0001)) {
+    TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
+    return MTP_RESP_INVALID_STORAGE_ID;
+  }
 
-    // Request for objects with no parent (0xFFFFFFFF) are considered root objects
-    // Note: implementation may pass 0 as parent_object_handle
-    if (parent_object_handle == 0xFFFFFFFF)
-        parent_object_handle = 0;
+  // Request for objects with no parent (0xFFFFFFFF) are considered root objects
+  // Note: implementation may pass 0 as parent_object_handle
+  if (parent_object_handle == 0xFFFFFFFF)
+    parent_object_handle = 0;
 
-    if (parent_object_handle != _fs_operation.traversal_parent)
-    {
-        _fs_operation.traversal_parent = parent_object_handle;
-        _fs_operation.traversal_index = 0;
-    }
+  if (parent_object_handle != _fs_operation.traversal_parent) {
+    _fs_operation.traversal_parent = parent_object_handle;
+    _fs_operation.traversal_index = 0;
+  }
 
-    for (unsigned int i=_fs_operation.traversal_index; i<FS_MAX_NODES; i++)
-    {
-        obj = &_fs_objects[i];
-        if (obj->allocated && obj->parent == parent_object_handle)
-        {
-            _fs_operation.traversal_index = i+1;
-            *next_child_handle = obj->handle;
-            TU_LOG1("Association %ld -> child %ld\r\n", parent_object_handle, obj->handle);
-            return MTP_RESP_OK;
-        }
+  for (unsigned int i = _fs_operation.traversal_index; i < FS_MAX_NODES; i++) {
+    obj = &_fs_objects[i];
+    if (obj->allocated && obj->parent == parent_object_handle) {
+      _fs_operation.traversal_index = i + 1;
+      *next_child_handle = obj->handle;
+      TU_LOG1("Association %ld -> child %ld\r\n", parent_object_handle, obj->handle);
+      return MTP_RESP_OK;
     }
-    TU_LOG1("Association traversal completed\r\n");
-    _fs_operation.traversal_index = 0;
-    *next_child_handle = 0;
-    return MTP_RESP_OK;
+  }
+  TU_LOG1("Association traversal completed\r\n");
+  _fs_operation.traversal_index = 0;
+  *next_child_handle = 0;
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_object_write_info(uint32_t storage_id, uint32_t parent_object, uint32_t *new_object_handle, const mtp_object_info_t *info)
-{
-    fs_object_info_t *obj = NULL;
+mtp_response_t tud_mtp_storage_object_write_info(uint32_t storage_id, uint32_t parent_object,
+                                                 uint32_t* new_object_handle, const mtp_object_info_t* info) {
+  fs_object_info_t* obj = NULL;
 
-    if (_fs_operation.session_id == 0)
-    {
-        TU_LOG1("ERR: Session not open\r\n");
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
-    // Accept command on default storage
-    if (storage_id != 0xFFFFFFFF && storage_id != STORAGE_ID(0x0001, 0x0001))
-    {
-        TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
-        return MTP_RESP_INVALID_STORAGE_ID;
-    }
+  if (_fs_operation.session_id == 0) {
+    TU_LOG1("ERR: Session not open\r\n");
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
+  // Accept command on default storage
+  if (storage_id != 0xFFFFFFFF && storage_id != STORAGE_ID(0x0001, 0x0001)) {
+    TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
+    return MTP_RESP_INVALID_STORAGE_ID;
+  }
 
-    if (info->object_compressed_size > FS_MAX_NODE_BYTES)
-    {
-        TU_LOG1("Object size %ld is more than maximum %ld\r\n", info->object_compressed_size, FS_MAX_NODE_BYTES);
-        return MTP_RESP_STORE_FULL;
-    }
+  if (info->object_compressed_size > FS_MAX_NODE_BYTES) {
+    TU_LOG1("Object size %ld is more than maximum %ld\r\n", info->object_compressed_size, FS_MAX_NODE_BYTES);
+    return MTP_RESP_STORE_FULL;
+  }
 
-    // Request for objects with no parent (0xFFFFFFFF) are considered root objects
-    if (parent_object == 0xFFFFFFFF)
-        parent_object = 0;
+  // Request for objects with no parent (0xFFFFFFFF) are considered root objects
+  if (parent_object == 0xFFFFFFFF)
+    parent_object = 0;
 
-    // Ensure we are not creating an orphaned object outside root
-    if (parent_object != 0)
-    {
-        obj = fs_object_get_from_handle(parent_object);
-        if (obj == NULL)
-        {
-            TU_LOG1("Parent %ld does not exist\r\n", parent_object);
-            return MTP_RESP_INVALID_PARENT_OBJECT;
-        }
-        if (!obj->association)
-        {
-            TU_LOG1("Parent %ld is not an association\r\n", parent_object);
-            return MTP_RESP_INVALID_PARENT_OBJECT;
-        }
+  // Ensure we are not creating an orphaned object outside root
+  if (parent_object != 0) {
+    obj = fs_object_get_from_handle(parent_object);
+    if (obj == NULL) {
+      TU_LOG1("Parent %ld does not exist\r\n", parent_object);
+      return MTP_RESP_INVALID_PARENT_OBJECT;
     }
-
-    // Search for first free object
-    for (unsigned int i=0; i<FS_MAX_NODES; i++)
-    {
-        if (!_fs_objects[i].allocated)
-        {
-            obj = &_fs_objects[i];
-            break;
-        }
+    if (!obj->association) {
+      TU_LOG1("Parent %ld is not an association\r\n", parent_object);
+      return MTP_RESP_INVALID_PARENT_OBJECT;
     }
+  }
 
-    if (obj == NULL)
-    {
-        TU_LOG1("No space left on device\r\n");
-        return MTP_RESP_STORE_FULL;
+  // Search for first free object
+  for (unsigned int i = 0; i < FS_MAX_NODES; i++) {
+    if (!_fs_objects[i].allocated) {
+      obj = &_fs_objects[i];
+      break;
     }
+  }
 
-    // Fill-in structure
-    obj->allocated = true;
-    obj->handle = ++_fs_operation.last_handle;
-    obj->parent = parent_object;
-    obj->size = info->object_compressed_size;
-    obj->association = info->object_format == MTP_OBJ_FORMAT_ASSOCIATION;
+  if (obj == NULL) {
+    TU_LOG1("No space left on device\r\n");
+    return MTP_RESP_STORE_FULL;
+  }
 
-    // Extract variable data
-    uint16_t offset_data = sizeof(mtp_object_info_t);
-    mtpd_gct_get_string(&offset_data, obj->name, FS_MAX_NODE_NAME_LEN);
-    mtpd_gct_get_string(&offset_data, obj->created, FS_ISODATETIME_LEN);
-    mtpd_gct_get_string(&offset_data, obj->modified, FS_ISODATETIME_LEN);
+  // Fill-in structure
+  obj->allocated = true;
+  obj->handle = ++_fs_operation.last_handle;
+  obj->parent = parent_object;
+  obj->size = info->object_compressed_size;
+  obj->association = info->object_format == MTP_OBJ_FORMAT_ASSOCIATION;
 
-    TU_LOG1("Create %s %s with handle %ld, parent %ld and size %ld\r\n",
-        obj->association ? "association" : "object",
-         obj->name, obj->handle, obj->parent, obj->size);
-    *new_object_handle = obj->handle;
-    // Initialize operation
-    _fs_operation.write_handle = obj->handle;
-    _fs_operation.write_pos = 0;
-    return MTP_RESP_OK;
+  // Extract variable data
+  uint16_t offset_data = sizeof(mtp_object_info_t);
+  mtpd_gct_get_string(&offset_data, obj->name, FS_MAX_NODE_NAME_LEN);
+  mtpd_gct_get_string(&offset_data, obj->created, FS_ISODATETIME_LEN);
+  mtpd_gct_get_string(&offset_data, obj->modified, FS_ISODATETIME_LEN);
+
+  TU_LOG1("Create %s %s with handle %ld, parent %ld and size %ld\r\n",
+          obj->association ? "association" : "object",
+          obj->name, obj->handle, obj->parent, obj->size);
+  *new_object_handle = obj->handle;
+  // Initialize operation
+  _fs_operation.write_handle = obj->handle;
+  _fs_operation.write_pos = 0;
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_object_read_info(uint32_t object_handle, mtp_object_info_t *info)
-{
-    const fs_object_info_t *obj;
+mtp_response_t tud_mtp_storage_object_read_info(uint32_t object_handle, mtp_object_info_t* info) {
+  const fs_object_info_t* obj;
 
-    if (_fs_operation.session_id == 0)
-    {
-        TU_LOG1("ERR: Session not open\r\n");
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
+  if (_fs_operation.session_id == 0) {
+    TU_LOG1("ERR: Session not open\r\n");
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
 
-    obj = fs_object_get_from_handle(object_handle);
-    if (obj == NULL)
-    {
-        TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
-    }
+  obj = fs_object_get_from_handle(object_handle);
+  if (obj == NULL) {
+    TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
+    return MTP_RESP_INVALID_OBJECT_HANDLE;
+  }
 
-    memset(info, 0, sizeof(mtp_object_info_t));
-    info->storage_id = STORAGE_ID(0x0001, 0x0001);
-    if (obj->association)
-    {
-        info->object_format = MTP_OBJ_FORMAT_ASSOCIATION;
-        info->protection_status = MTP_PROTECTION_STATUS_NO_PROTECTION;
-        info->object_compressed_size = 0;
-        info->association_type = MTP_ASSOCIATION_UNDEFINED;
-    }
-    else
-    {
-        info->object_format = MTP_OBJ_FORMAT_UNDEFINED;
-        info->protection_status = MTP_PROTECTION_STATUS_NO_PROTECTION;
-        info->object_compressed_size = obj->size;
-        info->association_type = MTP_ASSOCIATION_UNDEFINED;
-    }
-    info->thumb_format = MTP_OBJ_FORMAT_UNDEFINED;
-    info->parent_object = obj->parent;
+  memset(info, 0, sizeof(mtp_object_info_t));
+  info->storage_id = STORAGE_ID(0x0001, 0x0001);
+  if (obj->association) {
+    info->object_format = MTP_OBJ_FORMAT_ASSOCIATION;
+    info->protection_status = MTP_PROTECTION_STATUS_NO_PROTECTION;
+    info->object_compressed_size = 0;
+    info->association_type = MTP_ASSOCIATION_UNDEFINED;
+  } else {
+    info->object_format = MTP_OBJ_FORMAT_UNDEFINED;
+    info->protection_status = MTP_PROTECTION_STATUS_NO_PROTECTION;
+    info->object_compressed_size = obj->size;
+    info->association_type = MTP_ASSOCIATION_UNDEFINED;
+  }
+  info->thumb_format = MTP_OBJ_FORMAT_UNDEFINED;
+  info->parent_object = obj->parent;
 
-    mtpd_gct_append_wstring(obj->name);
-    mtpd_gct_append_wstring(obj->created); // date_created
-    mtpd_gct_append_wstring(obj->modified); // date_modified
-    mtpd_gct_append_wstring(""); // keywords, not used
+  mtpd_gct_append_wstring(obj->name);
+  mtpd_gct_append_wstring(obj->created); // date_created
+  mtpd_gct_append_wstring(obj->modified); // date_modified
+  mtpd_gct_append_wstring(""); // keywords, not used
 
-    TU_LOG1("Retrieve object %s with handle %ld\r\n", obj->name, obj->handle);
+  TU_LOG1("Retrieve object %s with handle %ld\r\n", obj->name, obj->handle);
 
-    return MTP_RESP_OK;
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_object_write(uint32_t object_handle, const uint8_t *buffer, uint32_t size)
-{
-    fs_object_info_t *obj;
+mtp_response_t tud_mtp_storage_object_write(uint32_t object_handle, const uint8_t* buffer, uint32_t size) {
+  fs_object_info_t* obj;
 
-    obj = fs_object_get_from_handle(object_handle);
-    if (obj == NULL)
-    {
-        TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
-    }
-    // It's a requirement that this command is preceded by a write info
-    if (object_handle != _fs_operation.write_handle)
-    {
-        TU_LOG1("ERR: Object %ld not open for write\r\n", object_handle);
-        return MTP_RESP_NO_VALID_OBJECTINFO;
-    }
+  obj = fs_object_get_from_handle(object_handle);
+  if (obj == NULL) {
+    TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
+    return MTP_RESP_INVALID_OBJECT_HANDLE;
+  }
+  // It's a requirement that this command is preceded by a write info
+  if (object_handle != _fs_operation.write_handle) {
+    TU_LOG1("ERR: Object %ld not open for write\r\n", object_handle);
+    return MTP_RESP_NO_VALID_OBJECTINFO;
+  }
 
-    TU_LOG1("Write object %ld: data chunk at %ld/%ld bytes at offset %ld\r\n", object_handle, _fs_operation.write_pos, obj->size, size);
-    TU_ASSERT(obj->size >= _fs_operation.write_pos + size, MTP_RESP_INCOMPLETE_TRANSFER);
-    if (_fs_operation.write_pos + size < FS_MAX_NODE_BYTES)
-        memcpy(&obj->data[_fs_operation.write_pos], buffer, size);
-    _fs_operation.write_pos += size;
-    // Write operation completed
-    if (_fs_operation.write_pos == obj->size)
-    {
-        _fs_operation.write_handle = 0;
-        _fs_operation.write_pos = 0;
-    }
-    return MTP_RESP_OK;
+  TU_LOG1("Write object %ld: data chunk at %ld/%ld bytes at offset %ld\r\n", object_handle, _fs_operation.write_pos,
+          obj->size, size);
+  TU_ASSERT(obj->size >= _fs_operation.write_pos + size, MTP_RESP_INCOMPLETE_TRANSFER);
+  if (_fs_operation.write_pos + size < FS_MAX_NODE_BYTES)
+    memcpy(&obj->data[_fs_operation.write_pos], buffer, size);
+  _fs_operation.write_pos += size;
+  // Write operation completed
+  if (_fs_operation.write_pos == obj->size) {
+    _fs_operation.write_handle = 0;
+    _fs_operation.write_pos = 0;
+  }
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_object_size(uint32_t object_handle, uint32_t *size)
-{
-    const fs_object_info_t *obj;
-    obj = fs_object_get_from_handle(object_handle);
-    if (obj == NULL)
-    {
-        TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
-    }
-    *size = obj->size;
-    return MTP_RESP_OK;
+mtp_response_t tud_mtp_storage_object_size(uint32_t object_handle, uint32_t* size) {
+  const fs_object_info_t* obj;
+  obj = fs_object_get_from_handle(object_handle);
+  if (obj == NULL) {
+    TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
+    return MTP_RESP_INVALID_OBJECT_HANDLE;
+  }
+  *size = obj->size;
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_object_read(uint32_t object_handle, void *buffer, uint32_t buffer_size, uint32_t *read_count)
-{
-    const fs_object_info_t *obj;
+mtp_response_t tud_mtp_storage_object_read(uint32_t object_handle, void* buffer, uint32_t buffer_size,
+                                           uint32_t* read_count) {
+  const fs_object_info_t* obj;
 
-    obj = fs_object_get_from_handle(object_handle);
+  obj = fs_object_get_from_handle(object_handle);
 
-    if (obj == NULL)
-    {
-        TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
-    }
-    // It's not a requirement that this command is preceded by a read info
-    if (object_handle != _fs_operation.read_handle)
-    {
-        TU_LOG1("ERR: Object %ld not open for read\r\n", object_handle);
-        _fs_operation.read_handle = object_handle;
-        _fs_operation.read_pos = 0;
-    }
+  if (obj == NULL) {
+    TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
+    return MTP_RESP_INVALID_OBJECT_HANDLE;
+  }
+  // It's not a requirement that this command is preceded by a read info
+  if (object_handle != _fs_operation.read_handle) {
+    TU_LOG1("ERR: Object %ld not open for read\r\n", object_handle);
+    _fs_operation.read_handle = object_handle;
+    _fs_operation.read_pos = 0;
+  }
 
-    if (obj->size - _fs_operation.read_pos > buffer_size)
-    {
-        TU_LOG1("Read object %ld: %ld bytes at offset %ld\r\n", object_handle, buffer_size, _fs_operation.read_pos);
-        *read_count = buffer_size;
-        if (_fs_operation.read_pos + buffer_size < FS_MAX_NODE_BYTES) {
-            memcpy(buffer, &obj->data[_fs_operation.read_pos], *read_count);
-        }
-        _fs_operation.read_pos += *read_count;
+  if (obj->size - _fs_operation.read_pos > buffer_size) {
+    TU_LOG1("Read object %ld: %ld bytes at offset %ld\r\n", object_handle, buffer_size, _fs_operation.read_pos);
+    *read_count = buffer_size;
+    if (_fs_operation.read_pos + buffer_size < FS_MAX_NODE_BYTES) {
+      memcpy(buffer, &obj->data[_fs_operation.read_pos], *read_count);
     }
-    else
-    {
-        TU_LOG1("Read object %ld: %ld bytes at offset %ld\r\n", object_handle, obj->size - _fs_operation.read_pos, _fs_operation.read_pos);
-        *read_count = obj->size - _fs_operation.read_pos;
-        if (_fs_operation.read_pos + *read_count < FS_MAX_NODE_BYTES) {
-            memcpy(buffer, &obj->data[_fs_operation.read_pos], *read_count);
-        }
-        // Read operation completed
-        _fs_operation.read_handle = 0;
-        _fs_operation.read_pos = 0;
+    _fs_operation.read_pos += *read_count;
+  } else {
+    TU_LOG1("Read object %ld: %ld bytes at offset %ld\r\n", object_handle, obj->size - _fs_operation.read_pos,
+            _fs_operation.read_pos);
+    *read_count = obj->size - _fs_operation.read_pos;
+    if (_fs_operation.read_pos + *read_count < FS_MAX_NODE_BYTES) {
+      memcpy(buffer, &obj->data[_fs_operation.read_pos], *read_count);
     }
-    return MTP_RESP_OK;
+    // Read operation completed
+    _fs_operation.read_handle = 0;
+    _fs_operation.read_pos = 0;
+  }
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_object_move(uint32_t object_handle, uint32_t new_parent_object_handle)
-{
-    fs_object_info_t *obj;
+mtp_response_t tud_mtp_storage_object_move(uint32_t object_handle, uint32_t new_parent_object_handle) {
+  fs_object_info_t* obj;
 
-    if (new_parent_object_handle == 0xFFFFFFFF)
-        new_parent_object_handle = 0;
+  if (new_parent_object_handle == 0xFFFFFFFF)
+    new_parent_object_handle = 0;
 
-    // Ensure we are not moving to an nonexisting parent
-    if (new_parent_object_handle != 0)
-    {
-        obj = fs_object_get_from_handle(new_parent_object_handle);
-        if (obj == NULL)
-        {
-            TU_LOG1("Parent %ld does not exist\r\n", new_parent_object_handle);
-            return MTP_RESP_INVALID_PARENT_OBJECT;
-        }
-        if (!obj->association)
-        {
-            TU_LOG1("Parent %ld is not an association\r\n", new_parent_object_handle);
-            return MTP_RESP_INVALID_PARENT_OBJECT;
-        }
+  // Ensure we are not moving to an nonexisting parent
+  if (new_parent_object_handle != 0) {
+    obj = fs_object_get_from_handle(new_parent_object_handle);
+    if (obj == NULL) {
+      TU_LOG1("Parent %ld does not exist\r\n", new_parent_object_handle);
+      return MTP_RESP_INVALID_PARENT_OBJECT;
     }
+    if (!obj->association) {
+      TU_LOG1("Parent %ld is not an association\r\n", new_parent_object_handle);
+      return MTP_RESP_INVALID_PARENT_OBJECT;
+    }
+  }
 
-    obj = fs_object_get_from_handle(object_handle);
+  obj = fs_object_get_from_handle(object_handle);
 
-    if (obj == NULL)
-    {
-        TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
-    }
-    TU_LOG1("Move object %ld to new parent %ld\r\n", object_handle, new_parent_object_handle);
-    obj->parent = new_parent_object_handle;
-    return MTP_RESP_OK;
+  if (obj == NULL) {
+    TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
+    return MTP_RESP_INVALID_OBJECT_HANDLE;
+  }
+  TU_LOG1("Move object %ld to new parent %ld\r\n", object_handle, new_parent_object_handle);
+  obj->parent = new_parent_object_handle;
+  return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_storage_object_delete(uint32_t object_handle)
-{
-    fs_object_info_t *obj;
+mtp_response_t tud_mtp_storage_object_delete(uint32_t object_handle) {
+  fs_object_info_t* obj;
 
-    if (_fs_operation.session_id == 0)
-    {
-        TU_LOG1("ERR: Session not open\r\n");
-        return MTP_RESP_SESSION_NOT_OPEN;
-    }
+  if (_fs_operation.session_id == 0) {
+    TU_LOG1("ERR: Session not open\r\n");
+    return MTP_RESP_SESSION_NOT_OPEN;
+  }
 
-    if (object_handle == 0xFFFFFFFF)
-        object_handle = 0;
+  if (object_handle == 0xFFFFFFFF)
+    object_handle = 0;
 
-    if (object_handle != 0)
-    {
-        obj = fs_object_get_from_handle(object_handle);
+  if (object_handle != 0) {
+    obj = fs_object_get_from_handle(object_handle);
 
-        if (obj == NULL)
-        {
-            TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
-            return MTP_RESP_INVALID_OBJECT_HANDLE;
-        }
-        obj->allocated = false;
-        TU_LOG1("Delete object with handle %ld\r\n", object_handle);
+    if (obj == NULL) {
+      TU_LOG1("ERR: Object with handle %ld does not exist\r\n", object_handle);
+      return MTP_RESP_INVALID_OBJECT_HANDLE;
     }
+    obj->allocated = false;
+    TU_LOG1("Delete object with handle %ld\r\n", 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];
-            if (obj->allocated && obj->parent == object_handle)
-            {
-                tud_mtp_storage_object_delete(obj->handle);
-            }
-        }
+  if (object_handle == 0 || obj->association) {
+    // Delete also children
+    for (unsigned int i = 0; i < FS_MAX_NODES; i++) {
+      obj = &_fs_objects[i];
+      if (obj->allocated && obj->parent == object_handle) {
+        tud_mtp_storage_object_delete(obj->handle);
+      }
     }
+  }
 
-    return MTP_RESP_OK;
+  return MTP_RESP_OK;
 }
 
-void tud_mtp_storage_object_done(void)
-{
+void tud_mtp_storage_object_done(void) {
 }
 
-void tud_mtp_storage_cancel(void)
-{
-    fs_object_info_t *obj;
+void tud_mtp_storage_cancel(void) {
+  fs_object_info_t* obj;
 
-    _fs_operation.traversal_parent = 0;
-    _fs_operation.traversal_index = 0;
-    _fs_operation.read_handle = 0;
-    _fs_operation.read_pos = 0;
-    // If write operation is canceled, discard object
-    if (_fs_operation.write_handle)
-    {
-        obj = fs_object_get_from_handle(_fs_operation.write_handle);
-        if (obj)
-            obj->allocated = false;
-    }
-    _fs_operation.write_handle = 0;
-    _fs_operation.write_pos = 0;
+  _fs_operation.traversal_parent = 0;
+  _fs_operation.traversal_index = 0;
+  _fs_operation.read_handle = 0;
+  _fs_operation.read_pos = 0;
+  // If write operation is canceled, discard object
+  if (_fs_operation.write_handle) {
+    obj = fs_object_get_from_handle(_fs_operation.write_handle);
+    if (obj)
+      obj->allocated = false;
+  }
+  _fs_operation.write_handle = 0;
+  _fs_operation.write_pos = 0;
 }
 
-void tud_mtp_storage_reset(void)
-{
-    tud_mtp_storage_cancel();
-    _fs_operation.session_id = 0;
+void tud_mtp_storage_reset(void) {
+  tud_mtp_storage_cancel();
+  _fs_operation.session_id = 0;
 }
diff --git a/src/class/mtp/mtp.h b/src/class/mtp/mtp.h
index c9d2b27a9..d61f329c4 100644
--- a/src/class/mtp/mtp.h
+++ b/src/class/mtp/mtp.h
@@ -38,7 +38,6 @@
  extern "C" {
 #endif
 
-#define TU_ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
 #define STORAGE_ID(physical_id, logical_id) ( (((uint32_t)physical_id & 0xFFFF) << 16) | ((uint32_t)logical_id & 0x0000FFFF) )
 typedef uint16_t wchar16_t;
 
@@ -583,19 +582,32 @@ typedef enum
 } mtp_object_handles_t;
 
 // Datatypes
-typedef enum
-{
-  MTP_TYPE_UNDEFINED = 0x0000u,
-  MTP_TYPE_INT8      = 0x0001u,
-  MTP_TYPE_UINT8     = 0x0002u,
-  MTP_TYPE_INT16     = 0x0003u,
-  MTP_TYPE_UINT16    = 0x0004u,
-  MTP_TYPE_INT32     = 0x0005u,
-  MTP_TYPE_UINT32    = 0x0006u,
-  MTP_TYPE_INT64     = 0x0007u,
-  MTP_TYPE_UINT64    = 0x0008u,
-  MTP_TYPE_STR       = 0xFFFFu,
-} mtp_datatypes_t;
+typedef enum {
+  MTP_DATA_TYPE_UNDEFINED = 0x0000u,
+  // scalars
+  MTP_DATA_TYPE_INT8      = 0x0001u,
+  MTP_DATA_TYPE_UINT8     = 0x0002u,
+  MTP_DATA_TYPE_INT16     = 0x0003u,
+  MTP_DATA_TYPE_UINT16    = 0x0004u,
+  MTP_DATA_TYPE_INT32     = 0x0005u,
+  MTP_DATA_TYPE_UINT32    = 0x0006u,
+  MTP_DATA_TYPE_INT64     = 0x0007u,
+  MTP_DATA_TYPE_UINT64    = 0x0008u,
+  MTP_DATA_TYPE_INT128    = 0x0009u,
+  MTP_DATA_TYPE_UINT128   = 0x000Au,
+  // array
+  MTP_DATA_TYPE_AINT8     = 0x4001u,
+  MTP_DATA_TYPE_AUINT8    = 0x4002u,
+  MTP_DATA_TYPE_AINT16    = 0x4003u,
+  MTP_DATA_TYPE_AUINT16   = 0x4004u,
+  MTP_DATA_TYPE_AINT32    = 0x4005u,
+  MTP_DATA_TYPE_AUINT32   = 0x4006u,
+  MTP_DATA_TYPE_AINT64    = 0x4007u,
+  MTP_DATA_TYPE_AUINT64   = 0x4008u,
+  MTP_DATA_TYPE_AINT128   = 0x4009u,
+  MTP_DATA_TYPE_AUINT128  = 0x400Au,
+  MTP_DATA_TYPE_STR       = 0xFFFFu,
+} mtp_data_type_t;
 
 // Get/Set
 typedef enum
@@ -707,46 +719,24 @@ typedef struct TU_ATTR_PACKED {
   uint32_t data[MTP_MAX_PACKET_SIZE / sizeof(uint32_t)];
 } mtp_generic_container_t;
 
-// DeviceInfo Dataset
-#define MTP_EXTENSIONS "microsoft.com: 1.0; "
-typedef struct TU_ATTR_PACKED {
-  uint16_t standard_version;
-  uint32_t mtp_vendor_extension_id;
-  uint16_t mtp_version;
-  uint8_t   mtp_extensions_len;
-  wchar16_t mtp_extensions[TU_ARRAY_LEN(MTP_EXTENSIONS)] TU_ATTR_PACKED;
-
-  uint16_t functional_mode;
-  /* Operations supported */
-  uint32_t operations_supported_len;
-  uint16_t operations_supported[TU_ARRAY_LEN(mtp_operations_supported)] TU_ATTR_PACKED;
-  /* Events supported */
-  uint32_t events_supported_len;
-  uint16_t events_supported[TU_ARRAY_LEN(mtp_events_supported)] TU_ATTR_PACKED;
-  /* Device properties supported */
-  uint32_t device_properties_supported_len;
-  uint16_t device_properties_supported[TU_ARRAY_LEN(mtp_device_properties_supported)] TU_ATTR_PACKED;
-  /* Capture formats */
-  uint32_t capture_formats_len;
-  uint16_t capture_formats[TU_ARRAY_LEN(mtp_capture_formats)] TU_ATTR_PACKED;
-  /* Playback formats */
-  uint32_t playback_formats_len;
-  uint16_t playback_formats[TU_ARRAY_LEN(mtp_playback_formats)] TU_ATTR_PACKED;
-} mtp_device_info_t;
-// The following fields will be dynamically added to the struct at runtime:
-// - wstring manufacturer
-// - wstring model
-// - wstring device_version
-// - wstring serial_number
+#define mtp_string_t(_nchars) \
+  struct TU_ATTR_PACKED { \
+    uint8_t count; /* in characters including null */ \
+    uint16_t utf16[_nchars]; \
+  }
 
+#define mtp_array_t(_type, _count) \
+   struct TU_ATTR_PACKED { \
+     uint32_t count; \
+     _type arr[_count];\
+   }
 
-#define MTP_STRING_DEF(name, string) \
-  uint8_t name##_len; \
-  wchar16_t name[TU_ARRAY_LEN(string)];
+#define mtp_auint16_t(_count) mtp_array_t(uint16_t, _count)
 
-#define MTP_ARRAY_DEF(name, array) \
-  uint16_t name##_len; \
-  typeof(name) name[TU_ARRAY_LEN(array)];
+typedef struct TU_ATTR_PACKED {
+  uint8_t count;
+  uint16_t utf16[];
+} mtp_flexible_string_t;
 
 // StorageInfo dataset
 typedef struct TU_ATTR_PACKED {
@@ -813,6 +803,95 @@ typedef struct TU_ATTR_PACKED {
   uint32_t parent_object_handle;
 } mtp_basic_object_info_t;
 
+//--------------------------------------------------------------------+
+// Generic Container function
+//--------------------------------------------------------------------+
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add(mtp_generic_container_t* p_container, mtp_data_type_t type, const void* data) {
+  TU_VERIFY(type != MTP_DATA_TYPE_UNDEFINED, 0);
+  uint8_t scalar_size; // size of single scalar
+  uint8_t count_width; // size of count field (0, 1 or 4 bytes)
+
+  if (type == MTP_DATA_TYPE_STR) {
+    scalar_size = 2;
+    count_width = 1;
+  } else {
+    uint8_t scalar_type = type & 0x3F;
+    count_width = (type & 0x4000u) ? 4 : 0;
+    scalar_size = 1u << ((scalar_type - 1u) >> 1);
+  }
+
+  uint32_t data_len;
+  if (count_width) {
+    const uint32_t count = *(const uint32_t*) data;
+    data_len = count_width + count*scalar_size;
+  } else {
+    data_len = scalar_size;
+  }
+
+  memcpy(((uint8_t*)p_container) + p_container->len, data, data_len);
+  p_container->len += data_len;
+
+  return data_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) {
+  const uint32_t prev_len = p_container->len;
+  uint8_t* container8 = (uint8_t*) p_container;
+  if (count == 0) {
+    // count = 0 means scalar
+    memcpy(container8 + p_container->len, data, scalar_size);
+    p_container->len += scalar_size;
+  } else {
+    tu_unaligned_write32(container8 + p_container->len, count);
+    p_container->len += 4;
+    memcpy(container8 + p_container->len, data, count * scalar_size);
+  }
+
+  return p_container->len - prev_len;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_string(mtp_generic_container_t* p_container, uint8_t count, uint16_t* utf16) {
+  const uint32_t prev_len = p_container->len;
+  uint8_t* container8 = (uint8_t*) p_container;
+  *(container8 + p_container->len) = count;
+  p_container->len += 1;
+
+  memcpy(container8 + p_container->len, utf16, 2 * count);
+  p_container->len += 2 * count;
+
+  return p_container->len - prev_len;
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+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);
+}
+
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/class/mtp/mtp_device.c b/src/class/mtp/mtp_device.c
index 7e9574ab1..b64f36f2e 100644
--- a/src/class/mtp/mtp_device.c
+++ b/src/class/mtp/mtp_device.c
@@ -50,8 +50,8 @@
 //--------------------------------------------------------------------+
 // STRUCT
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
+  uint8_t rhport;
   uint8_t itf_num;
   uint8_t ep_in;
   uint8_t ep_out;
@@ -82,7 +82,7 @@ static mtp_phase_type_t mtpd_chk_generic(const char *func_name, const bool err_c
 static mtp_phase_type_t mtpd_chk_session_open(const char *func_name);
 
 // MTP commands
-static mtp_phase_type_t mtpd_handle_cmd(void);
+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_get_device_info(void);
 static mtp_phase_type_t mtpd_handle_cmd_open_session(void);
@@ -118,9 +118,9 @@ CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN static char _mtp_datestr[20];
 // Helper
 //--------------------------------------------------------------------+
 
-static bool prepare_new_command(uint8_t rhport, mtpd_interface_t* p_mtp) {
+static bool prepare_new_command(mtpd_interface_t* p_mtp) {
   p_mtp->phase = MTP_PHASE_IDLE;
-  return usbd_edpt_xfer(rhport, p_mtp->ep_out, (uint8_t *)(&_mtpd_epbuf.container), sizeof(mtp_generic_container_t));
+  return usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_out, (uint8_t *)(&_mtpd_epbuf.container), sizeof(mtp_generic_container_t));
 }
 
 
@@ -157,6 +157,8 @@ uint16_t mtpd_open(uint8_t rhport, tusb_desc_interface_t const* itf_desc, uint16
   // Max length must be at least 1 interface + 3 endpoints
   TU_ASSERT(itf_desc->bNumEndpoints == 3 && max_len >= mtpd_itf_size);
   mtpd_interface_t* p_mtp = &_mtpd_itf;
+  tu_memclr(p_mtp, sizeof(mtpd_interface_t));
+  p_mtp->rhport = rhport;
   p_mtp->itf_num = itf_desc->bInterfaceNumber;
 
   // Open interrupt IN endpoint
@@ -168,7 +170,7 @@ uint16_t mtpd_open(uint8_t rhport, tusb_desc_interface_t const* itf_desc, uint16
   // Open endpoint pair
   TU_ASSERT(usbd_open_edpt_pair(rhport, tu_desc_next(ep_desc), 2, TUSB_XFER_BULK, &p_mtp->ep_out, &p_mtp->ep_in), 0);
 
-  TU_ASSERT(prepare_new_command(rhport, p_mtp), 0);
+  TU_ASSERT(prepare_new_command(p_mtp), 0);
 
   return mtpd_itf_size;
 }
@@ -216,6 +218,29 @@ bool mtpd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
   return true;
 }
 
+bool tud_mtp_data_send(mtp_generic_container_t* data_block) {
+  mtpd_interface_t* p_mtp = &_mtpd_itf;
+  p_mtp->phase = MTP_PHASE_DATA;
+  p_mtp->total_len = data_block->len;
+  p_mtp->xferred_len = 0;
+  p_mtp->handled_len = 0;
+  p_mtp->xfer_completed = false;
+
+  data_block->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  data_block->transaction_id = p_mtp->cmd_header.transaction_id;
+  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, (uint8_t*) data_block, (uint16_t)data_block->len));
+  return true;
+}
+
+bool tud_mtp_response_send(mtp_generic_container_t* resp_block) {
+  mtpd_interface_t* p_mtp = &_mtpd_itf;
+  p_mtp->phase = MTP_PHASE_RESPONSE_QUEUED;
+  resp_block->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  resp_block->transaction_id = p_mtp->cmd_header.transaction_id;
+  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, (uint8_t*) resp_block, (uint16_t)resp_block->len));
+  return true;
+}
+
 // Transfer on bulk endpoints
 bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) {
   TU_ASSERT(event == XFER_RESULT_SUCCESS);
@@ -235,23 +260,25 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       p_mtp->phase = MTP_PHASE_COMMAND;
       TU_ATTR_FALLTHROUGH; // handle in the next case
 
-    case MTP_PHASE_COMMAND:
-      // Handle command block
-      memcpy(&p_mtp->cmd_header, p_container, sizeof(mtp_container_header_t));
-      p_mtp->phase = mtpd_handle_cmd();
-      if (p_mtp->phase == MTP_PHASE_DATA_IN) {
-        TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_mtp->queued_len));
-        p_mtp->total_len = p_container->len;
-        p_mtp->xferred_len = 0;
-        p_mtp->handled_len = 0;
-        p_mtp->xfer_completed = false;
-      } else if (p_mtp->phase == MTP_PHASE_DATA_OUT) {
-        p_mtp->xferred_len = 0;
-        p_mtp->handled_len = 0;
-        p_mtp->xfer_completed = false;
-        TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, (uint8_t*) p_container, sizeof(mtp_generic_container_t)), 0);
+    case MTP_PHASE_COMMAND: {
+      mtpd_handle_cmd(p_mtp);
+      break;
+    }
+
+    case MTP_PHASE_DATA: {
+      const uint16_t bulk_mps = (tud_speed_get() == TUSB_SPEED_HIGH) ? 512 : 64;
+      p_mtp->xferred_len += xferred_bytes;
+
+      // transfer complete if ZLP or short packet or overflow
+      if (xferred_bytes == 0 || // ZLP
+          (xferred_bytes & (bulk_mps - 1)) || // short packet
+          p_mtp->xferred_len > p_mtp->total_len) {
+        tud_mtp_data_complete_cb(0, &p_mtp->cmd_header, p_container, event, p_mtp->xferred_len);
+      } else {
+        TU_ASSERT(false);
       }
       break;
+    }
 
     case MTP_PHASE_DATA_IN:
       p_mtp->xferred_len += xferred_bytes;
@@ -328,7 +355,7 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
     case MTP_PHASE_RESPONSE_QUEUED:
       // response phase is complete -> prepare for new command
       TU_ASSERT(ep_addr == p_mtp->ep_in);
-      prepare_new_command(rhport, p_mtp);
+      prepare_new_command(p_mtp);
       break;
 
     case MTP_PHASE_RESPONSE:
@@ -339,151 +366,16 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
   }
 
   if (p_mtp->phase == MTP_PHASE_RESPONSE) {
-    p_mtp->phase = MTP_PHASE_RESPONSE_QUEUED;
-    p_container->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
-    p_container->transaction_id = p_mtp->cmd_header.transaction_id;
-    TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_container->len), 0);
+    // p_mtp->phase = MTP_PHASE_RESPONSE_QUEUED;
+    // p_container->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+    // p_container->transaction_id = p_mtp->cmd_header.transaction_id;
+    // TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_container->len), 0);
   } else if (p_mtp->phase == MTP_PHASE_ERROR) {
     // stall both IN & OUT endpoints
     usbd_edpt_stall(rhport, p_mtp->ep_out);
     usbd_edpt_stall(rhport, p_mtp->ep_in);
   }
 
-#if 0
-  // IN transfer completed
-  if (ep_addr == p_mtp->ep_in) {
-    if (p_mtp->phase == MTP_PHASE_RESPONSE) {
-      // IN transfer completed, prepare for a new command
-      TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, (uint8_t*) p_container, CFG_MTP_EP_SIZE), 0);
-      p_mtp->phase = MTP_PHASE_IDLE;
-    } else if (p_mtp->phase == MTP_PHASE_DATA_IN) {
-      p_mtp->xferred_len += xferred_bytes;
-      p_mtp->handled_len = p_mtp->xferred_len;
-
-      // Check if transfer completed.
-      if (p_mtp->xferred_len >= p_mtp->total_len && (xferred_bytes == 0 || (xferred_bytes % CFG_MTP_EP_SIZE) != 0)) {
-        p_mtp->phase = MTP_PHASE_RESPONSE;
-        p_container->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
-        p_container->code = MTP_RESP_OK;
-        p_container->len = MTP_CONTAINER_HEADER_LENGTH;
-        p_container->transaction_id = p_mtp->context.transaction_id;
-        if (p_mtp->session_id != 0) {
-          p_container->data[0] = p_mtp->session_id;
-          p_container->len += sizeof(uint32_t);
-        }
-        TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_container->len), 0);
-      } else {
-        // Send next block of DATA
-        // Send Zero-Length Packet
-        if (p_mtp->xferred_len == p_mtp->total_len) {
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, ((uint8_t *)(&p_container->data)), 0 ));
-        } else {
-          p_mtp->phase = mtpd_handle_data();
-          if (p_mtp->phase == MTP_PHASE_RESPONSE) {
-            TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_container->len));
-          } else {
-            TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, ((uint8_t *)(&p_container->data)), (uint16_t)p_mtp->queued_len));
-          }
-        }
-      }
-    } else {
-      return false;
-    }
-  }
-
-  if (ep_addr == p_mtp->ep_out) {
-    if (p_mtp->phase == MTP_PHASE_IDLE) {
-      // A new command has been received. Ensure this is the last of the sequence.
-      p_mtp->total_len = p_container->len;
-      // Stall in case of unexpected block
-      if (p_container->type != MTP_CONTAINER_TYPE_COMMAND_BLOCK) {
-        return false;
-      }
-      p_mtp->phase = MTP_PHASE_COMMAND;
-      p_mtp->total_len = p_container->len;
-      p_mtp->xferred_len = xferred_bytes;
-      p_mtp->handled_len = 0;
-      p_mtp->xfer_completed = false;
-      TU_ASSERT(p_mtp->total_len < sizeof(mtp_generic_container_t));
-    }
-
-    if (p_mtp->phase == MTP_PHASE_COMMAND) {
-      // A zero-length or a short packet termination is expected
-      if (xferred_bytes == CFG_MTP_EP_SIZE || (p_mtp->total_len - p_mtp->xferred_len) > 0) {
-        TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, (uint8_t*) p_container + p_mtp->xferred_len, (uint16_t)(p_mtp->total_len - p_mtp->xferred_len)));
-      } else {
-        // Handle command block
-        p_mtp->phase = mtpd_handle_cmd();
-        if (p_mtp->phase == MTP_PHASE_RESPONSE) {
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_container->len));
-        } else if (p_mtp->phase == MTP_PHASE_DATA_IN) {
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_mtp->queued_len));
-          p_mtp->total_len = p_container->len;
-          p_mtp->xferred_len = 0;
-          p_mtp->handled_len = 0;
-          p_mtp->xfer_completed = false;
-        } else if (p_mtp->phase == MTP_PHASE_DATA_OUT) {
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, (uint8_t*) p_container, sizeof(mtp_generic_container_t)), 0);
-          p_mtp->xferred_len = 0;
-          p_mtp->handled_len = 0;
-          p_mtp->xfer_completed = false;
-        } else {
-          usbd_edpt_stall(rhport, p_mtp->ep_out);
-          usbd_edpt_stall(rhport, p_mtp->ep_in);
-        }
-      }
-      return true;
-    }
-
-    if (p_mtp->phase == MTP_PHASE_DATA_OUT) {
-      // First block of data
-      if (p_mtp->xferred_len == 0) {
-        p_mtp->total_len = p_container->len;
-        p_mtp->handled_len = 0;
-        p_mtp->xfer_completed = false;
-      }
-      p_mtp->xferred_len += xferred_bytes;
-      // Stall in case of unexpected block
-      if (p_container->type != MTP_CONTAINER_TYPE_DATA_BLOCK) { return false; }
-
-      // A zero-length or a short packet termination
-      if (xferred_bytes < CFG_MTP_EP_SIZE) {
-        p_mtp->xfer_completed = true;
-        // Handle data block
-        p_mtp->phase = mtpd_handle_data();
-        if (p_mtp->phase == MTP_PHASE_DATA_IN || p_mtp->phase == MTP_PHASE_RESPONSE) {
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_in, (uint8_t*) p_container, (uint16_t)p_container->len));
-        } else if (p_mtp->phase == MTP_PHASE_DATA_OUT) {
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, (uint8_t*) p_container, sizeof(mtp_generic_container_t)), 0);
-          p_mtp->xferred_len = 0;
-          p_mtp->xfer_completed = false;
-        } else {
-          usbd_edpt_stall(rhport, p_mtp->ep_out);
-          usbd_edpt_stall(rhport, p_mtp->ep_in);
-        }
-      } else {
-        // Handle data block when container is full
-        if (p_mtp->xferred_len - p_mtp->handled_len >= MTP_MAX_PACKET_SIZE - CFG_MTP_EP_SIZE) {
-          p_mtp->phase = mtpd_handle_data();
-          p_mtp->handled_len = p_mtp->xferred_len;
-        }
-        // Transfer completed: wait for zero-lenght packet
-        // Some platforms may not respect EP size and xferred_bytes may be more than CFG_MTP_EP_SIZE if
-        // the OUT EP is waiting for more data. Ensure we are not waiting for more than CFG_MTP_EP_SIZE.
-        if (p_mtp->total_len == p_mtp->xferred_len) {
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, ((uint8_t *)(&p_container->data)), CFG_MTP_EP_SIZE), 0);
-        } else if (p_mtp->handled_len == 0) {
-          // First data block includes container header + container data
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, (uint8_t*) p_container + p_mtp->xferred_len, (uint16_t)TU_MIN(p_mtp->total_len - p_mtp->xferred_len, CFG_MTP_EP_SIZE)));
-        } else {
-          // Successive data block includes only container data
-          TU_ASSERT(usbd_edpt_xfer(rhport, p_mtp->ep_out, ((uint8_t *)(&p_container->data)) + p_mtp->xferred_len - p_mtp->handled_len, (uint16_t)TU_MIN(p_mtp->total_len - p_mtp->xferred_len, CFG_MTP_EP_SIZE)));
-        }
-      }
-    }
-  }
-#endif
-
   return true;
 }
 
@@ -492,26 +384,77 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
 //--------------------------------------------------------------------+
 
 // Decode command and prepare response
-mtp_phase_type_t mtpd_handle_cmd(void) {
+mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp) {
   mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  TU_ASSERT(p_container->type == MTP_CONTAINER_TYPE_COMMAND_BLOCK);
+
+  mtp_generic_container_t cmd_block; // copy command block for callback
+  memcpy(&cmd_block, p_container, p_container->len);
+  memcpy(&p_mtp->cmd_header, p_container, sizeof(mtp_container_header_t));
+  // p_container->len = MTP_CONTAINER_HEADER_LENGTH; // default data/response length
+
   if (p_container->code != MTP_OP_SEND_OBJECT) {
     _mtpd_soi.object_handle = 0;
   }
 
+  mtp_phase_type_t ret = MTP_PHASE_RESPONSE;
+
   switch (p_container->code) {
-    case MTP_OP_GET_DEVICE_INFO:
+    case MTP_OP_GET_DEVICE_INFO: {
       TU_LOG_DRV("  MTP command: MTP_OP_GET_DEVICE_INFO\n");
-      return mtpd_handle_cmd_get_device_info();
+      tud_mtp_device_info_t dev_info = {
+        .standard_version = 100,
+        .mtp_vendor_extension_id = 0xFFFFFFFFU,
+        .mtp_version = 100,
+        .mtp_extensions = {
+          .count = sizeof(CFG_TUD_MTP_DEVICEINFO_EXTENSIONS),
+          .utf16 = { 0 }
+        },
+        .functional_mode = 0x0000,
+        .supported_operations = {
+          .count = TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_SUPPORTED_OPERATIONS),
+          .arr = { CFG_TUD_MTP_DEVICEINFO_SUPPORTED_OPERATIONS }
+        },
+        .supported_events = {
+          .count = TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_SUPPORTED_EVENTS),
+          .arr = { CFG_TUD_MTP_DEVICEINFO_SUPPORTED_EVENTS }
+        },
+        .supported_device_properties = {
+          .count = TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_SUPPORTED_DEVICE_PROPERTIES),
+          .arr = { CFG_TUD_MTP_DEVICEINFO_SUPPORTED_DEVICE_PROPERTIES }
+        },
+        .capture_formats = {
+          .count = TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_CAPTURE_FORMATS),
+          .arr = { CFG_TUD_MTP_DEVICEINFO_CAPTURE_FORMATS }
+        },
+        .playback_formats = {
+          .count = TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_PLAYBACK_FORMATS),
+          .arr = { CFG_TUD_MTP_DEVICEINFO_PLAYBACK_FORMATS }
+        }
+      };
+      for (uint8_t i=0; i < dev_info.mtp_extensions.count; i++) {
+        dev_info.mtp_extensions.utf16[i] = (uint16_t)CFG_TUD_MTP_DEVICEINFO_EXTENSIONS[i];
+      }
+      p_container->len = MTP_CONTAINER_HEADER_LENGTH + sizeof(tud_mtp_device_info_t);
+      p_container->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+      p_container->code = MTP_OP_GET_DEVICE_INFO;
+      memcpy(p_container->data, &dev_info, sizeof(tud_mtp_device_info_t));
+
+      ret = MTP_PHASE_RESPONSE;
+      break;
+    }
+
     case MTP_OP_OPEN_SESSION:
       TU_LOG_DRV("  MTP command: MTP_OP_OPEN_SESSION\n");
-      return mtpd_handle_cmd_open_session();
+      break;
+
     case MTP_OP_CLOSE_SESSION:
       TU_LOG_DRV("  MTP command: MTP_OP_CLOSE_SESSION\n");
       return mtpd_handle_cmd_close_session();
+
     case MTP_OP_GET_STORAGE_IDS:
       TU_LOG_DRV("  MTP command: MTP_OP_GET_STORAGE_IDS\n");
       return mtpd_handle_cmd_get_storage_ids();
+
     case MTP_OP_GET_STORAGE_INFO:
       TU_LOG_DRV("  MTP command: MTP_OP_GET_STORAGE_INFO for ID=%lu\n", p_container->data[0]);
       return mtpd_handle_cmd_get_storage_info();
@@ -546,7 +489,9 @@ mtp_phase_type_t mtpd_handle_cmd(void) {
       TU_LOG_DRV("  MTP command: MTP_OP_UNKNOWN_COMMAND %x!!!!\n", p_container->code);
       return false;
   }
-  return true;
+
+  tud_mtp_command_received_cb(0, &cmd_block, p_container);
+  return ret;
 }
 
 mtp_phase_type_t mtpd_handle_data(void)
@@ -572,40 +517,6 @@ mtp_phase_type_t mtpd_handle_data(void)
   return true;
 }
 
-mtp_phase_type_t mtpd_handle_cmd_get_device_info(void)
-{
-  TU_VERIFY_STATIC(sizeof(mtp_device_info_t) < MTP_MAX_PACKET_SIZE, "mtp_device_info_t shall fit in MTP_MAX_PACKET_SIZE");
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-
-  p_container->len = MTP_CONTAINER_HEADER_LENGTH + sizeof(mtp_device_info_t);
-  p_container->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
-  p_container->code = MTP_OP_GET_DEVICE_INFO;
-  mtp_device_info_t *d = (mtp_device_info_t *)p_container->data;
-  d->standard_version = 100;
-  d->mtp_vendor_extension_id = 0x06;
-  d->mtp_version = 100;
-  d->mtp_extensions_len = TU_ARRAY_LEN(MTP_EXTENSIONS);
-  mtpd_wc16cpy((uint8_t *)d->mtp_extensions, MTP_EXTENSIONS);
-  d->functional_mode = 0x0000;
-  d->operations_supported_len = TU_ARRAY_LEN(mtp_operations_supported);
-  memcpy(d->operations_supported, mtp_operations_supported, sizeof(mtp_operations_supported));
-  d->events_supported_len = TU_ARRAY_LEN(mtp_events_supported);
-  memcpy(d->events_supported, mtp_events_supported, sizeof(mtp_events_supported));
-  d->device_properties_supported_len = TU_ARRAY_LEN(mtp_device_properties_supported);
-  memcpy(d->device_properties_supported, mtp_device_properties_supported, sizeof(mtp_device_properties_supported));
-  d->capture_formats_len = TU_ARRAY_LEN(mtp_capture_formats);
-  memcpy(d->capture_formats, mtp_capture_formats, sizeof(mtp_capture_formats));
-  d->playback_formats_len = TU_ARRAY_LEN(mtp_playback_formats);
-  memcpy(d->playback_formats, mtp_playback_formats, sizeof(mtp_playback_formats));
-  mtpd_gct_append_wstring(CFG_TUD_MANUFACTURER);
-  mtpd_gct_append_wstring(CFG_TUD_MODEL);
-  mtpd_gct_append_wstring(CFG_MTP_DEVICE_VERSION);
-  mtpd_gct_append_wstring(CFG_MTP_SERIAL_NUMBER);
-
-  _mtpd_itf.queued_len = p_container->len;
-  return MTP_PHASE_DATA_IN;
-}
-
 mtp_phase_type_t mtpd_handle_cmd_open_session(void)
 {
   mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
@@ -851,7 +762,7 @@ mtp_phase_type_t mtpd_handle_cmd_get_device_prop_desc(void)
       p_container->len = MTP_CONTAINER_HEADER_LENGTH + sizeof(mtp_device_prop_desc_t);
       mtp_device_prop_desc_t *d = (mtp_device_prop_desc_t *)p_container->data;
       d->device_property_code = (uint16_t)(device_prop_code);
-      d->datatype = MTP_TYPE_STR;
+      d->datatype = MTP_DATA_TYPE_STR;
       d->get_set = MTP_MODE_GET;
       mtpd_gct_append_wstring(CFG_TUD_MODEL); // factory_def_value
       mtpd_gct_append_wstring(CFG_TUD_MODEL); // current_value_len
diff --git a/src/class/mtp/mtp_device.h b/src/class/mtp/mtp_device.h
index 6531b36df..b39322f22 100644
--- a/src/class/mtp/mtp_device.h
+++ b/src/class/mtp/mtp_device.h
@@ -18,14 +18,14 @@
  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN0
  * THE SOFTWARE.
  *
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_MTP_DEVICE_H_
-#define _TUSB_MTP_DEVICE_H_
+#ifndef TUSB_MTP_DEVICE_H_
+#define TUSB_MTP_DEVICE_H_
 
 #include "common/tusb_common.h"
 #include "mtp.h"
@@ -36,15 +36,51 @@
  extern "C" {
 #endif
 
+typedef struct {
+  const mtp_container_header_t* cmd_header;
+  tusb_xfer_result_t xfer_result;
+  uint32_t xferred_bytes;
+} tud_mtp_cb_complete_data_t;
+
+// Number of supported operations, events, device properties, capture formats, playback formats
+// and max number of characters for strings manufacturer, model, device_version, serial_number
+#define MTP_DEVICE_INFO_TYPEDEF(_extension_nchars, _op_count, _event_count, _devprop_count, _capture_count, _playback_count) \
+  struct TU_ATTR_PACKED { \
+    uint16_t standard_version; \
+    uint32_t mtp_vendor_extension_id; \
+    uint16_t mtp_version; \
+    mtp_string_t(_extension_nchars) mtp_extensions; \
+    uint16_t functional_mode; \
+    mtp_auint16_t(_op_count) supported_operations; \
+    mtp_auint16_t(_event_count) supported_events; \
+    mtp_auint16_t(_devprop_count) supported_device_properties; \
+    mtp_auint16_t(_capture_count) capture_formats; \
+    mtp_auint16_t(_playback_count) playback_formats; \
+    /* string fields will be added using append function */ \
+  }
+
+typedef MTP_DEVICE_INFO_TYPEDEF(
+    sizeof(CFG_TUD_MTP_DEVICEINFO_EXTENSIONS), TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_SUPPORTED_OPERATIONS),
+    TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_SUPPORTED_EVENTS), TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_SUPPORTED_DEVICE_PROPERTIES),
+    TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_CAPTURE_FORMATS), TU_ARGS_NUM(CFG_TUD_MTP_DEVICEINFO_PLAYBACK_FORMATS)
+  ) tud_mtp_device_info_t;
+
 //--------------------------------------------------------------------+
-// Internal Class Driver API
+// Application API
 //--------------------------------------------------------------------+
-void     mtpd_init            (void);
-bool     mtpd_deinit          (void);
-void     mtpd_reset           (uint8_t rhport);
-uint16_t mtpd_open            (uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
-bool     mtpd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const *p_request);
-bool     mtpd_xfer_cb         (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+bool tud_mtp_data_send(mtp_generic_container_t* data_block);
+// bool tud_mtp_block_data_receive();
+bool tud_mtp_response_send(mtp_generic_container_t* resp_block);
+
+//--------------------------------------------------------------------+
+// Application Callbacks
+//--------------------------------------------------------------------+
+
+// Invoked when new command is received
+int32_t tud_mtp_command_received_cb(uint8_t idx, mtp_generic_container_t* cmd_block, mtp_generic_container_t* out_block);
+
+// Invoked when data phase is complete
+int32_t tud_mtp_data_complete_cb(uint8_t idx, mtp_container_header_t* cmd_header, mtp_generic_container_t* resp_block, tusb_xfer_result_t xfer_result, uint32_t xferred_bytes);
 
 //--------------------------------------------------------------------+
 // Helper functions
@@ -66,10 +102,19 @@ bool mtpd_gct_append_array(uint32_t array_size, const void *data, size_t type_si
 // The function returns true if the data fits in the available buffer space.
 bool mtpd_gct_append_date(struct tm *timeinfo);
 
+//--------------------------------------------------------------------+
+// Internal Class Driver API
+//--------------------------------------------------------------------+
+void     mtpd_init            (void);
+bool     mtpd_deinit          (void);
+void     mtpd_reset           (uint8_t rhport);
+uint16_t mtpd_open            (uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
+bool     mtpd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const *p_request);
+bool     mtpd_xfer_cb         (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+
 #ifdef __cplusplus
  }
 #endif
 
-#endif /* CFG_TUD_ENABLED && CFG_TUD_MTP */
-
-#endif /* _TUSB_MTP_DEVICE_H_ */
+#endif
+#endif