diff --git a/examples/device/mtp/src/mtp_fs_example.c b/examples/device/mtp/src/mtp_fs_example.c
index 19ca71d62..7e19fa75c 100644
--- a/examples/device/mtp/src/mtp_fs_example.c
+++ b/examples/device/mtp/src/mtp_fs_example.c
@@ -27,9 +27,6 @@
 #include "tusb.h"
 #include "tinyusb_logo_png.h"
 
-#define MTPD_STORAGE_DESCRIPTION "storage"
-#define MTPD_VOLUME_IDENTIFIER "volume"
-
 //--------------------------------------------------------------------+
 // Dataset
 //--------------------------------------------------------------------+
@@ -118,29 +115,6 @@ enum {
 
 static bool is_session_opened = false;
 
-//--------------------------------------------------------------------+
-// 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;
-} fs_operation_t;
-
-static fs_operation_t _fs_operation = {
-  .last_handle = 1
-};
-
 //--------------------------------------------------------------------+
 // INTERNAL FUNCTIONS
 //--------------------------------------------------------------------+
@@ -166,7 +140,6 @@ uint32_t fs_get_object_count(void) {
 
 int32_t tud_mtp_data_complete_cb(tud_mtp_cb_data_t* cb_data) {
   mtp_container_info_t* reply = &cb_data->reply;
-  reply->header->len = sizeof(mtp_container_header_t);
   reply->header->code = (cb_data->xfer_result == XFER_RESULT_SUCCESS) ? MTP_RESP_OK : MTP_RESP_GENERAL_ERROR;
   tud_mtp_response_send(reply);
   return 0;
@@ -181,30 +154,42 @@ int32_t tud_mtp_data_more_cb(tud_mtp_cb_data_t* cb_data) {
   // only a few command that need more data e.g GetObject and SendObject
   const mtp_container_command_t* command = cb_data->command;
   mtp_container_info_t* reply = &cb_data->reply;
-  switch (command->header.code) {
+  uint32_t resp_code = 0;
+  switch (command->code) {
     case MTP_OP_GET_OBJECT: {
       const uint32_t obj_handle = command->params[0];
       fs_object_info_t* obj = fs_get_object(obj_handle);
       if (obj == NULL) {
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
+        resp_code = MTP_RESP_INVALID_OBJECT_HANDLE;
+      } else {
+        // file contents offset is xferred byte minus header size
+        const uint32_t offset = cb_data->xferred_bytes - sizeof(mtp_container_header_t);
+        const uint32_t xact_len = tu_min32(obj->size - offset, reply->payload_size);
+        memcpy(reply->payload, obj->data + offset, xact_len);
+        tud_mtp_data_send(&cb_data->reply);
       }
-      // file contents offset is xferred byte minus header size
-      const uint32_t offset = cb_data->xferred_bytes - sizeof(mtp_container_header_t);
-      const uint32_t xact_len = tu_min32(obj->size - offset, reply->payload_size);
-      memcpy(reply->payload, obj->data + offset, xact_len);
-      tud_mtp_data_send(&cb_data->reply);
+      break;
     }
 
-    default: return MTP_RESP_OPERATION_NOT_SUPPORTED;
+    default:
+      resp_code = MTP_RESP_OPERATION_NOT_SUPPORTED;
+      break;
   }
 
-  return MTP_RESP_OK;
+  // send response if needed
+  if (resp_code != 0) {
+    reply->header->code = resp_code;
+    tud_mtp_response_send(reply);
+  }
+
+  return 0; // 0 mean data/response is sent already
 }
 
 int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
   const mtp_container_command_t* command = cb_data->command;
   mtp_container_info_t* reply = &cb_data->reply;
-  switch (command->header.code) {
+  uint32_t resp_code = 0;
+  switch (command->code) {
     case MTP_OP_GET_DEVICE_INFO: {
       // Device info is already prepared up to playback formats. Application only need to add string fields
       mtp_container_add_cstring(&cb_data->reply, DEV_INFO_MANUFACTURER);
@@ -218,24 +203,20 @@ int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
 
     case MTP_OP_OPEN_SESSION:
       if (is_session_opened) {
-        //return MTP_RESP_SESSION_ALREADY_OPEN;
-        reply->header->code = MTP_RESP_SESSION_ALREADY_OPEN;
+        resp_code = MTP_RESP_SESSION_ALREADY_OPEN;
       }else {
-        reply->header->code = MTP_RESP_OK;
+        resp_code = MTP_RESP_OK;
       }
       is_session_opened = true;
-      tud_mtp_response_send(&cb_data->reply);
       break;
 
     case MTP_OP_CLOSE_SESSION:
       if (!is_session_opened) {
-        // return MTP_RESP_SESSION_NOT_OPEN;
-        reply->header->code = MTP_RESP_SESSION_NOT_OPEN;
+        resp_code = MTP_RESP_SESSION_NOT_OPEN;
       } else {
-        reply->header->code = MTP_RESP_OK;
+        resp_code = MTP_RESP_OK;
       }
       is_session_opened = false;
-      tud_mtp_response_send(&cb_data->reply);
       break;
 
     case MTP_OP_GET_STORAGE_IDS: {
@@ -246,7 +227,7 @@ int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
     }
 
     case MTP_OP_GET_STORAGE_INFO: {
-      uint32_t storage_id = command->params[0];
+      const uint32_t storage_id = command->params[0];
       TU_VERIFY(SUPPORTED_STORAGE_ID == storage_id, -1);
       // update storage info with current free space
       storage_info.free_space_in_objects = FS_MAX_FILE_COUNT - fs_get_object_count();
@@ -271,7 +252,9 @@ int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
           tud_mtp_data_send(&cb_data->reply);
           break;
 
-        default: return MTP_RESP_PARAMETER_NOT_SUPPORTED;
+        default:
+          resp_code = MTP_RESP_PARAMETER_NOT_SUPPORTED;
+          break;
       }
       break;
     }
@@ -284,7 +267,9 @@ int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
           tud_mtp_data_send(&cb_data->reply);
           break;
 
-        default: return MTP_RESP_PARAMETER_NOT_SUPPORTED;
+        default:
+          resp_code = MTP_RESP_PARAMETER_NOT_SUPPORTED;
+          break;
       }
       break;
     }
@@ -295,20 +280,20 @@ int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
       (void) obj_format;
       const uint32_t parent_handle = command->params[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_FILE_COUNT] = { 0 };
-      uint32_t count = 0;
-      for (uint8_t i = 0; i < FS_MAX_FILE_COUNT; i++) {
-        fs_object_info_t* obj = &fs_objects[i];
-        if (obj->name[0] != 0 &&
-           (parent_handle == obj->parent || (parent_handle == 0xFFFFFFFF && obj->parent == 0))) {
-          handles[count++] = i + 1; // handle is index + 1
+        resp_code = MTP_RESP_INVALID_STORAGE_ID;
+      } else {
+        uint32_t handles[FS_MAX_FILE_COUNT] = { 0 };
+        uint32_t count = 0;
+        for (uint8_t i = 0; i < FS_MAX_FILE_COUNT; i++) {
+          fs_object_info_t* obj = &fs_objects[i];
+          if (obj->name[0] != 0 &&
+             (parent_handle == obj->parent || (parent_handle == 0xFFFFFFFF && obj->parent == 0))) {
+            handles[count++] = i + 1; // handle is index + 1
+          }
         }
+        mtp_container_add_auint32(&cb_data->reply, count, handles);
+        tud_mtp_data_send(&cb_data->reply);
       }
-      mtp_container_add_auint32(&cb_data->reply, count, handles);
-      tud_mtp_data_send(&cb_data->reply);
       break;
     }
 
@@ -316,32 +301,33 @@ int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
       const uint32_t obj_handle = command->params[0];
       fs_object_info_t* obj = fs_get_object(obj_handle);
       if (obj == NULL) {
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
-      }
-      mtp_object_info_header_t obj_info_header = {
-        .storage_id = SUPPORTED_STORAGE_ID,
-        .object_format = obj->format,
-        .protection_status =  MTP_PROTECTION_STATUS_NO_PROTECTION,
-        .object_compressed_size = obj->size,
-        .thumb_format = MTP_OBJ_FORMAT_UNDEFINED,
-        .thumb_compressed_size = 0,
-        .thumb_pix_width = 0,
-        .thumb_pix_height = 0,
-        .image_pix_width = 128,
-        .image_pix_height = 64,
-        .image_bit_depth = 32,
-        .parent_object = obj->parent,
-        .association_type = MTP_ASSOCIATION_UNDEFINED,
-        .association_desc = 0,
-        .sequence_number = 0
-      };
-      mtp_container_add_raw(&cb_data->reply, &obj_info_header, sizeof(obj_info_header));
-      mtp_container_add_cstring(&cb_data->reply, obj->name);
-      mtp_container_add_cstring(&cb_data->reply, FS_FIXED_DATETIME);
-      mtp_container_add_cstring(&cb_data->reply, FS_FIXED_DATETIME);
-      mtp_container_add_cstring(&cb_data->reply, ""); // keywords, not used
+        resp_code = MTP_RESP_INVALID_OBJECT_HANDLE;
+      } else {
+        mtp_object_info_header_t obj_info_header = {
+          .storage_id = SUPPORTED_STORAGE_ID,
+          .object_format = obj->format,
+          .protection_status =  MTP_PROTECTION_STATUS_NO_PROTECTION,
+          .object_compressed_size = obj->size,
+          .thumb_format = MTP_OBJ_FORMAT_UNDEFINED,
+          .thumb_compressed_size = 0,
+          .thumb_pix_width = 0,
+          .thumb_pix_height = 0,
+          .image_pix_width = 128,
+          .image_pix_height = 64,
+          .image_bit_depth = 32,
+          .parent_object = obj->parent,
+          .association_type = MTP_ASSOCIATION_UNDEFINED,
+          .association_desc = 0,
+          .sequence_number = 0
+        };
+        mtp_container_add_raw(&cb_data->reply, &obj_info_header, sizeof(obj_info_header));
+        mtp_container_add_cstring(&cb_data->reply, obj->name);
+        mtp_container_add_cstring(&cb_data->reply, FS_FIXED_DATETIME);
+        mtp_container_add_cstring(&cb_data->reply, FS_FIXED_DATETIME);
+        mtp_container_add_cstring(&cb_data->reply, ""); // keywords, not used
 
-      tud_mtp_data_send(&cb_data->reply);
+        tud_mtp_data_send(&cb_data->reply);
+      }
       break;
     }
 
@@ -349,20 +335,28 @@ int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t* cb_data) {
       const uint32_t obj_handle = command->params[0];
       fs_object_info_t* obj = fs_get_object(obj_handle);
       if (obj == NULL) {
-        return MTP_RESP_INVALID_OBJECT_HANDLE;
+        resp_code = MTP_RESP_INVALID_OBJECT_HANDLE;
+      } else {
+        // If file contents is larger than CFG_TUD_MTP_EP_BUFSIZE, only partial data is added here
+        // the rest will be sent in tud_mtp_data_more_cb
+        mtp_container_add_raw(&cb_data->reply, obj->data, obj->size);
+        tud_mtp_data_send(&cb_data->reply);
       }
-
-      // If file contents is larger than CFG_TUD_MTP_EP_BUFSIZE, only partial data is added here
-      // the rest will be sent in tud_mtp_data_more_cb
-      mtp_container_add_raw(&cb_data->reply, obj->data, obj->size);
-      tud_mtp_data_send(&cb_data->reply);
       break;
     }
 
-    default: return MTP_RESP_OPERATION_NOT_SUPPORTED;
+    default:
+      resp_code = MTP_RESP_OPERATION_NOT_SUPPORTED;
+      break;
   }
 
-  return MTP_RESP_OK;
+  // send response if needed
+  if (resp_code != 0) {
+    reply->header->code = resp_code;
+    tud_mtp_response_send(reply);
+  }
+
+  return 0;
 }
 
 //--------------------------------------------------------------------+
@@ -556,23 +550,7 @@ void tud_mtp_storage_object_done(void) {
 #endif
 
 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_get_object(_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;
 }
diff --git a/src/class/mtp/mtp.h b/src/class/mtp/mtp.h
index d4d81492e..f1b1eaf23 100644
--- a/src/class/mtp/mtp.h
+++ b/src/class/mtp/mtp.h
@@ -666,7 +666,10 @@ typedef struct TU_ATTR_PACKED {
 TU_VERIFY_STATIC(sizeof(mtp_container_header_t) == 12, "size is not correct");
 
 typedef struct TU_ATTR_PACKED {
-  mtp_container_header_t header;
+  uint32_t len;
+  uint16_t type;
+  uint16_t code;
+  uint32_t transaction_id;
   uint32_t params[5];
 } mtp_container_command_t;
 TU_VERIFY_STATIC(sizeof(mtp_container_command_t) == 32, "size is not correct");
diff --git a/src/class/mtp/mtp_device.c b/src/class/mtp/mtp_device.c
index 92801a8ad..358114655 100644
--- a/src/class/mtp/mtp_device.c
+++ b/src/class/mtp/mtp_device.c
@@ -69,17 +69,13 @@ typedef struct {
 } mtpd_interface_t;
 
 typedef struct {
-  TUD_EPBUF_TYPE_DEF(mtp_generic_container_t, container);
+  TUD_EPBUF_DEF(buf, CFG_TUD_MTP_EP_BUFSIZE);
 } mtpd_epbuf_t;
 
 //--------------------------------------------------------------------+
 // INTERNAL FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-// Checker
-static mtp_phase_type_t mtpd_chk_generic(const char *func_name, const bool err_cd, const uint16_t ret_code, const char *message);
-
-// MTP commands
-static mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp, tud_mtp_cb_data_t* cb_data);
+static int32_t mtpd_handle_cmd(mtpd_interface_t* p_mtp, tud_mtp_cb_data_t* cb_data);
 static mtp_phase_type_t mtpd_handle_data(void);
 static mtp_phase_type_t mtpd_handle_cmd_delete_object(void);
 static mtp_phase_type_t mtpd_handle_cmd_send_object_info(void);
@@ -95,10 +91,7 @@ static mtpd_interface_t _mtpd_itf;
 CFG_TUD_MEM_SECTION static mtpd_epbuf_t _mtpd_epbuf;
 
 CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN static mtp_device_status_res_t _mtpd_device_status_res;
-CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint32_t _mtpd_get_object_handle;
 CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN static mtp_basic_object_info_t _mtpd_soi;
-CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN static char _mtp_datestr[20];
-
 
 //--------------------------------------------------------------------+
 // Debug
@@ -165,17 +158,15 @@ TU_ATTR_UNUSED static tu_lookup_table_t const _mtp_op_table = {
 
 static bool prepare_new_command(mtpd_interface_t* p_mtp) {
   p_mtp->phase = MTP_PHASE_IDLE;
-  return usbd_edpt_xfer(p_mtp->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, _mtpd_epbuf.buf, CFG_TUD_MTP_EP_BUFSIZE);
 }
 
-
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
 void mtpd_init(void) {
   tu_memclr(&_mtpd_itf, sizeof(mtpd_interface_t));
   tu_memclr(&_mtpd_soi, sizeof(mtp_basic_object_info_t));
-  _mtpd_get_object_handle = 0;
 }
 
 bool mtpd_deinit(void) {
@@ -187,7 +178,6 @@ void mtpd_reset(uint8_t rhport) {
   tu_memclr(&_mtpd_itf, sizeof(mtpd_interface_t));
   tu_memclr(&_mtpd_epbuf, sizeof(mtpd_epbuf_t));
   tu_memclr(&_mtpd_soi, sizeof(mtp_basic_object_info_t));
-  _mtpd_get_object_handle = 0;
 }
 
 uint16_t mtpd_open(uint8_t rhport, tusb_desc_interface_t const* itf_desc, uint16_t max_len) {
@@ -242,7 +232,7 @@ bool mtpd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
       TU_LOG_DRV("  MTP request: MTP_REQ_RESET\n");
       tud_mtp_storage_reset();
       // Prepare for a new command
-      TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, (uint8_t *)(&_mtpd_epbuf.container), sizeof(mtp_generic_container_t)));
+      TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, _mtpd_epbuf.buf, CFG_TUD_MTP_EP_BUFSIZE));
       break;
 
     case MTP_REQ_GET_DEVICE_STATUS: {
@@ -272,7 +262,7 @@ bool tud_mtp_data_send(mtp_container_info_t* p_container) {
     p_mtp->xferred_len = 0;
 
     p_container->header->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
-    p_container->header->transaction_id = p_mtp->command.header.transaction_id;
+    p_container->header->transaction_id = p_mtp->command.transaction_id;
     p_mtp->reply_header = *p_container->header; // save header for subsequent data
   } else {
     // subsequent data block: payload only
@@ -280,7 +270,7 @@ bool tud_mtp_data_send(mtp_container_info_t* p_container) {
   }
 
   const uint16_t xact_len = tu_min32(p_mtp->total_len - p_mtp->xferred_len, CFG_TUD_MTP_EP_BUFSIZE);
-  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, (uint8_t *)(&_mtpd_epbuf.container), xact_len));
+  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, _mtpd_epbuf.buf, xact_len));
 
   return true;
 }
@@ -289,22 +279,25 @@ bool tud_mtp_response_send(mtp_container_info_t* p_container) {
   mtpd_interface_t* p_mtp = &_mtpd_itf;
   p_mtp->phase = MTP_PHASE_RESPONSE_QUEUED;
   p_container->header->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
-  p_container->header->transaction_id = p_mtp->command.header.transaction_id;
-  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, (uint8_t *)(&_mtpd_epbuf.container), (uint16_t)p_container->header->len));
+  p_container->header->transaction_id = p_mtp->command.transaction_id;
+  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, _mtpd_epbuf.buf, (uint16_t)p_container->header->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);
-
   if (ep_addr == _mtpd_itf.ep_event) {
     // nothing to do
     return true;
   }
 
   mtpd_interface_t* p_mtp = &_mtpd_itf;
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
+  mtp_generic_container_t* p_container = (mtp_generic_container_t*) _mtpd_epbuf.buf;
+
+#if CFG_TUSB_DEBUG >= CFG_TUD_MTP_LOG_LEVEL
+  tu_lookup_find(&_mtp_op_table, p_mtp->command.code);
+  TU_LOG_DRV("  MTP %s phase = %u\r\n", (const char *) tu_lookup_find(&_mtp_op_table, p_mtp->command.code), p_mtp->phase);
+#endif
 
   tud_mtp_cb_data_t cb_data;
   cb_data.idx = 0;
@@ -323,8 +316,10 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       TU_ATTR_FALLTHROUGH; // handle in the next case
 
     case MTP_PHASE_COMMAND: {
-      memcpy(&p_mtp->command, p_container, sizeof(mtp_container_command_t)); // copy new command
-      mtpd_handle_cmd(p_mtp, &cb_data);
+      memcpy(&p_mtp->command, p_container, sizeof(mtp_container_command_t)); // save new command
+      if (mtpd_handle_cmd(p_mtp, &cb_data) < 0) {
+        p_mtp->phase = MTP_PHASE_ERROR;
+      }
       break;
     }
 
@@ -337,6 +332,7 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       if (xferred_bytes == 0 || // ZLP
           (xferred_bytes & (bulk_mps - 1)) || // short packet
           p_mtp->xferred_len > p_mtp->total_len) {
+        cb_data.reply.header->len = sizeof(mtp_container_header_t);
         tud_mtp_data_complete_cb(&cb_data);
       } else {
         // payload only packet
@@ -450,15 +446,11 @@ 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(mtpd_interface_t* p_mtp, tud_mtp_cb_data_t* cb_data) {
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  p_container->len = sizeof(mtp_container_header_t); // default data/response length
-
-  tu_lookup_find(&_mtp_op_table, p_mtp->command.header.code);
-  TU_LOG_DRV("  MTP command: %s\r\n", (char const*) tu_lookup_find(&_mtp_op_table, p_mtp->command.header.code));
+int32_t mtpd_handle_cmd(mtpd_interface_t* p_mtp, tud_mtp_cb_data_t* cb_data) {
+  cb_data->reply.header->len = sizeof(mtp_container_header_t);
 
   // pre-processed commands
-  switch (p_mtp->command.header.code) {
+  switch (p_mtp->command.code) {
     case MTP_OP_GET_DEVICE_INFO: {
       tud_mtp_device_info_t dev_info = {
         .standard_version = 100,
@@ -500,7 +492,6 @@ mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp, tud_mtp_cb_data_t* cb_
       }
 #endif
       mtp_container_add_raw(&cb_data->reply, &dev_info, sizeof(tud_mtp_device_info_t));
-      p_container->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
       break;
     }
 
@@ -508,14 +499,13 @@ mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp, tud_mtp_cb_data_t* cb_
       break;
   }
 
-  tud_mtp_command_received_cb(cb_data);
-  return MTP_PHASE_RESPONSE;
+  return tud_mtp_command_received_cb(cb_data);
 }
 #if 0
 
 mtp_phase_type_t mtpd_handle_data(void)
 {
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
+  mtp_generic_container_t* p_container = &_mtpd_epbuf.buf;
   TU_ASSERT(p_container->type == MTP_CONTAINER_TYPE_DATA_BLOCK);
 
   switch(p_container->code)
@@ -535,7 +525,7 @@ mtp_phase_type_t mtpd_handle_data(void)
 
 mtp_phase_type_t mtpd_handle_cmd_delete_object(void)
 {
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
+  mtp_generic_container_t* p_container = &_mtpd_epbuf.buf;
   uint32_t object_handle = p_container->data[0];
   uint32_t object_code_format = p_container->data[1]; // not used
   (void) object_code_format;
@@ -552,7 +542,7 @@ mtp_phase_type_t mtpd_handle_cmd_delete_object(void)
 
 mtp_phase_type_t mtpd_handle_cmd_send_object_info(void)
 {
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
+  mtp_generic_container_t* p_container = &_mtpd_epbuf.buf;
   _mtpd_soi.storage_id = p_container->data[0];
   _mtpd_soi.parent_object_handle = (p_container->data[1] == 0xFFFFFFFF ? 0 : p_container->data[1]);
 
@@ -562,7 +552,7 @@ mtp_phase_type_t mtpd_handle_cmd_send_object_info(void)
 
 mtp_phase_type_t mtpd_handle_dto_send_object_info(void)
 {
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
+  mtp_generic_container_t* p_container = &_mtpd_epbuf.buf;
   uint32_t new_object_handle = 0;
   mtp_response_t res = tud_mtp_storage_object_write_info(_mtpd_soi.storage_id, _mtpd_soi.parent_object_handle, &new_object_handle, (mtp_object_info_header_t *)p_container->data);
   mtp_phase_type_t phase;
@@ -589,7 +579,7 @@ mtp_phase_type_t mtpd_handle_cmd_send_object(void)
 
 mtp_phase_type_t mtpd_handle_dto_send_object(void)
 {
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
+  mtp_generic_container_t* p_container = &_mtpd_epbuf.buf;
   uint8_t *buffer = (uint8_t *)&p_container->data;
   uint32_t buffer_size = _mtpd_itf.xferred_len - _mtpd_itf.handled_len;
   // First block of DATA
@@ -622,7 +612,7 @@ mtp_phase_type_t mtpd_handle_dto_send_object(void)
 
 mtp_phase_type_t mtpd_handle_cmd_format_store(void)
 {
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
+  mtp_generic_container_t* p_container = &_mtpd_epbuf.buf;
   uint32_t storage_id = p_container->data[0];
   uint32_t file_system_format = p_container->data[1]; // not used
   (void) file_system_format;
@@ -636,124 +626,4 @@ mtp_phase_type_t mtpd_handle_cmd_format_store(void)
 }
 #endif
 
-//--------------------------------------------------------------------+
-// Checker
-//--------------------------------------------------------------------+
-mtp_phase_type_t mtpd_chk_generic(const char *func_name, const bool err_cd, const uint16_t ret_code, const char *message)
-{
-  (void)func_name;
-  (void)message;
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  if (err_cd)
-  {
-    TU_LOG_DRV("  MTP error in %s: (%x) %s\n", func_name, ret_code, message);
-    p_container->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
-    p_container->code = ret_code;
-    p_container->len = sizeof(mtp_container_header_t);
-     return MTP_PHASE_RESPONSE;
-  }
-  return MTP_PHASE_NONE;
-}
-
-//--------------------------------------------------------------------+
-// Generic container data
-//--------------------------------------------------------------------+
-void mtpd_wc16cpy(uint8_t *dest, const char *src)
-{
-  wchar16_t s;
-  while(true)
-  {
-    s = *src;
-    memcpy(dest, &s, sizeof(wchar16_t));
-    if (*src == 0) break;
-    ++src;
-    dest += sizeof(wchar16_t);
-  }
-}
-
-//--------------------------------------------------------------------+
-// Generic container function
-//--------------------------------------------------------------------+
-bool mtpd_gct_append_uint8(const uint8_t value)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  uint8_t *p_value = ((uint8_t *)p_container) + p_container->len;
-  p_container->len += sizeof(uint8_t);
-  // Verify space requirement (8 bit string length, number of wide characters including terminator)
-  TU_ASSERT(p_container->len < sizeof(mtp_generic_container_t));
-  *p_value = value;
-  return true;
-}
-
-bool mtpd_gct_append_object_handle(const uint32_t object_handle)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  p_container->len += sizeof(uint32_t);
-  TU_ASSERT(p_container->len < sizeof(mtp_generic_container_t));
-  p_container->data[0]++;
-  p_container->data[p_container->data[0]] = object_handle;
-  return true;
-}
-
-bool mtpd_gct_append_wstring(const char *s)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  size_t len = strlen(s) + 1;
-  TU_ASSERT(len <= UINT8_MAX);
-  uint8_t *p_len = ((uint8_t *)p_container)+p_container->len;
-  p_container->len += sizeof(uint8_t) + sizeof(wchar16_t) * len;
-  // Verify space requirement (8 bit string length, number of wide characters including terminator)
-  TU_ASSERT(p_container->len < sizeof(mtp_generic_container_t));
-  *p_len = (uint8_t)len;
-  uint8_t *p_str = p_len + sizeof(uint8_t);
-  mtpd_wc16cpy(p_str, s);
-  return true;
-}
-
-bool mtpd_gct_get_string(uint16_t *offset_data, char *string, const uint16_t max_size)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  uint16_t size = *(((uint8_t *)&p_container->data) + *offset_data);
-  if (size > max_size)
-      size = max_size;
-  TU_ASSERT(*offset_data + size < sizeof(p_container->data));
-
-  uint8_t *s = ((uint8_t *)&p_container->data) + *offset_data + sizeof(uint8_t);
-  for(uint16_t i = 0; i < size; i++)
-  {
-    string[i] = *s;
-    s += sizeof(wchar16_t);
-  }
-  *offset_data += (uint16_t)(sizeof(uint8_t) + size * sizeof(wchar16_t));
-  return true;
-}
-
-bool mtpd_gct_append_array(uint32_t array_size, const void *data, size_t type_size)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  TU_ASSERT(p_container->len + sizeof(uint32_t) + array_size * type_size < sizeof(p_container->data));
-  uint8_t *p = ((uint8_t *)p_container) + p_container->len;
-  memcpy(p, &array_size, sizeof(uint32_t));
-  p += sizeof(uint32_t);
-  memcpy(p, data, array_size * type_size);
-  p_container->len += sizeof(uint32_t) + array_size * type_size;
-  return true;
-}
-
-bool mtpd_gct_append_date(struct tm *timeinfo)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  // strftime is not supported by all platform, this implementation is just for reference
-  int len = snprintf(_mtp_datestr, sizeof(p_container->data) - p_container->len, "%04d%02d%02dT%02d%02d%02dZ",
-    timeinfo->tm_year + 1900,
-    timeinfo->tm_mon + 1,
-    timeinfo->tm_mday,
-    timeinfo->tm_hour,
-    timeinfo->tm_min,
-    timeinfo->tm_sec);
-  if (len == 0)
-    return false;
-  return mtpd_gct_append_wstring(_mtp_datestr);
-}
-
-#endif // (CFG_TUD_ENABLED && CFG_TUD_MTP)
+#endif
diff --git a/src/class/mtp/mtp_device.h b/src/class/mtp/mtp_device.h
index 88960b1aa..28c01ddc6 100644
--- a/src/class/mtp/mtp_device.h
+++ b/src/class/mtp/mtp_device.h
@@ -107,9 +107,9 @@ bool tud_mtp_response_send(mtp_container_info_t* p_container);
 // Bulk only protocol Callbacks
 //--------------------------------------------------------------------+
 
-// Invoked when new command is received. Application fill the out_block with either DATA or RESPONSE container
-// and call tud_mtp_data_send() or tud_mtp_response_send().
-// return MTP response code
+/* Invoked when new command is received. Application fill the cb_data->reply with either DATA or RESPONSE and call
+ * tud_mtp_data_send() or tud_mtp_response_send(). Return negative to stall the endpoints
+ */
 int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t * cb_data);
 
 // Invoked when a data packet is received/sent, and more data is expected
@@ -121,26 +121,6 @@ int32_t tud_mtp_data_complete_cb(tud_mtp_cb_data_t* cb_data);
 // Invoked when response phase is complete
 int32_t tud_mtp_response_complete_cb(tud_mtp_cb_data_t* cb_data);
 
-//--------------------------------------------------------------------+
-// Helper functions
-//--------------------------------------------------------------------+
-// Generic container function
-void mtpd_wc16cpy(uint8_t *dest, const char *src);
-bool mtpd_gct_append_uint8(const uint8_t value);
-bool mtpd_gct_append_object_handle(const uint32_t object_handle);
-bool mtpd_gct_append_wstring(const char *s);
-bool mtpd_gct_get_string(uint16_t *offset_data, char *string, const uint16_t max_size);
-
-// Append the given array to the global context buffer
-// The function returns true if the data fits in the available buffer space.
-bool mtpd_gct_append_array(uint32_t array_size, const void *data, size_t type_size);
-
-// Append an UTC date string to the global context buffer
-// Required format is 'YYYYMMDDThhmmss.s' optionally added 'Z' for UTC or +/-hhmm for time zone
-// This function is provided for reference and only supports UTC format without partial seconds
-// 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
 //--------------------------------------------------------------------+