Add MTP class device

10 files changed, 1701 insertions, 0 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 55c52033c..9918407b3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -19,6 +19,7 @@ function(tinyusb_target_add TARGET)
     ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/hid/hid_device.c
     ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/midi/midi_device.c
     ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/msc/msc_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/mtp/mtp_device.c
     ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/net/ecm_rndis_device.c
     ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/net/ncm_device.c
     ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/usbtmc/usbtmc_device.c
diff --git a/src/class/mtp/mtp.h b/src/class/mtp/mtp.h
new file mode 100644
index 000000000..33c6bb552
--- /dev/null
+++ b/src/class/mtp/mtp.h
@@ -0,0 +1,438 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2025 Ennebi Elettronica (https://ennebielettronica.com)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * 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
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#ifndef _TUSB_MTP_H_
+#define _TUSB_MTP_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <time.h>
+#include "tusb_option.h"
+
+#if (CFG_TUD_ENABLED && CFG_TUD_MTP)
+
+#ifdef __cplusplus
+ 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;
+
+//--------------------------------------------------------------------+
+// Media Transfer Protocol Class Constant
+//--------------------------------------------------------------------+
+
+// Media Transfer Protocol Subclass
+typedef enum
+{
+  MTP_SUBCLASS = 1
+} mtp_subclass_type_t;
+
+// MTP Protocol.
+typedef enum
+{
+  MTP_PROTOCOL_STILL_IMAGE = 1,
+} mtp_protocol_type_t;
+
+// PTP/MTP protocol phases
+typedef enum
+{
+  MTP_PHASE_IDLE = 0,
+  MTP_PHASE_COMMAND,
+  MTP_PHASE_DATA_IN,
+  MTP_PHASE_DATA_OUT,
+  MTP_PHASE_RESPONSE,
+  MTP_PHASE_ERROR,
+  MTP_PHASE_NONE,
+} mtp_phase_type_t;
+
+// PTP/MTP Class requests
+typedef enum
+{
+  MTP_REQ_CANCEL             = 0x64,
+  MTP_REQ_GET_EXT_EVENT_DATA = 0x65,
+  MTP_REQ_RESET              = 0x66,
+  MTP_REQ_GET_DEVICE_STATUS  = 0x67,
+} mtp_class_request_t;
+
+#define MTP_GENERIC_DATA_BLOCK_LENGTH 12
+#define MTP_MAX_PACKET_SIZE 512
+
+// PTP/MTP Generic container
+typedef struct TU_ATTR_PACKED
+{
+  uint32_t container_length;
+  uint16_t container_type;
+  uint16_t code;
+  uint32_t transaction_id;
+  uint32_t data[MTP_MAX_PACKET_SIZE / sizeof(uint32_t)];
+} mtp_generic_container_t;
+
+// PTP/MTP Container type
+typedef enum
+{
+  MTP_CONTAINER_TYPE_UNDEFINED      = 0,
+  MTP_CONTAINER_TYPE_COMMAND_BLOCK  = 1,
+  MTP_CONTAINER_TYPE_DATA_BLOCK     = 2,
+  MTP_CONTAINER_TYPE_RESPONSE_BLOCK = 3,
+  MTP_CONTAINER_TYPE_EVENT_BLOCK    = 4,
+} mtp_container_type_t;
+
+// Supported OperationCode
+typedef enum
+{
+  MTP_OPEC_GET_DEVICE_INFO            = 0x1001u,
+  MTP_OPEC_OPEN_SESSION               = 0x1002u,
+  MTP_OPEC_CLOSE_SESSION              = 0x1003u,
+  MTP_OPEC_GET_STORAGE_IDS            = 0x1004u,
+  MTP_OPEC_GET_STORAGE_INFO           = 0x1005u,
+  MTP_OPEC_GET_NUM_OBJECTS            = 0x1006u,
+  MTP_OPEC_GET_OBJECT_HANDLES         = 0x1007u,
+  MTP_OPEC_GET_OBJECT_INFO            = 0x1008u,
+  MTP_OPEC_GET_OBJECT                 = 0x1009u,
+  MTP_OPEC_GET_THUMB                  = 0x100Au,
+  MTP_OPEC_DELETE_OBJECT              = 0x100Bu,
+  MTP_OPEC_SEND_OBJECT_INFO           = 0x100Cu,
+  MTP_OPEC_SEND_OBJECT                = 0x100Du,
+  MTP_OPEC_INITIAL_CAPTURE            = 0x100Eu,
+  MTP_OPEC_FORMAT_STORE               = 0x100Fu,
+  MTP_OPEC_RESET_DEVICE               = 0x1010u,
+  MTP_OPEC_SELF_TEST                  = 0x1011u,
+  MTP_OPEC_SET_OBJECT_PROTECTION      = 0x1012u,
+  MTP_OPEC_POWER_DOWN                 = 0x1013u,
+  MTP_OPEC_GET_DEVICE_PROP_DESC       = 0x1014u,
+  MTP_OPEC_GET_DEVICE_PROP_VALUE      = 0x1015u,
+  MTP_OPEC_SET_DEVICE_PROP_VALUE      = 0x1016u,
+  MTP_OPEC_RESET_DEVICE_PROP_VALUE    = 0x1017u,
+  MTP_OPEC_TERMINATE_OPEN_CAPTURE     = 0x1018u,
+  MTP_OPEC_MOVE_OBJECT                = 0x1019u,
+  MTP_OPEC_COPY_OBJECT                = 0x101Au,
+  MTP_OPEC_GET_PARTIAL_OBJECT         = 0x101Bu,
+  MTP_OPEC_INITIATE_OPEN_CAPTURE      = 0x101Bu,
+  MTP_OPEC_GET_OBJECT_PROPS_SUPPORTED = 0x9801u,
+  MTP_OPEC_GET_OBJECT_PROP_DESC       = 0x9802u,
+  MTP_OPEC_GET_OBJECT_PROP_VALUE      = 0x9803u,
+  MTP_OPEC_SET_OBJECT_PROP_VALUE      = 0x9804u,
+  MTP_OPEC_GET_OBJECT_PROPLIST        = 0x9805u,
+  MTP_OPEC_GET_OBJECT_PROP_REFERENCES = 0x9810u,
+  MTP_OPEC_GETSERVICEIDS              = 0x9301u,
+  MTP_OPEC_GETSERVICEINFO             = 0x9302u,
+  MTP_OPEC_GETSERVICECAPABILITIES     = 0x9303u,
+  MTP_OPEC_GETSERVICEPROPDESC         = 0x9304u,
+} mtp_operation_code_t;
+
+// Supported EventCode
+typedef enum
+{
+  MTP_EVTC_OBJECT_ADDED = 0x4002,
+} mtp_event_code_t;
+
+
+// Supported Device Properties
+typedef enum
+{
+  MTP_DEVP_UNDEFINED            = 0x5000u,
+  MTP_DEVP_BATTERY_LEVEL        = 0x5001u,
+  MTP_DEVP_DEVICE_FRIENDLY_NAME = 0xD402u,
+} mtp_event_properties_t;
+
+// Supported Object Properties
+typedef enum
+{
+  MTP_OBJP_STORAGE_ID                          = 0xDC01u,
+  MTP_OBJP_OBJECT_FORMAT                       = 0xDC02u,
+  MTP_OBJP_PROTECTION_STATUS                   = 0xDC03u,
+  MTP_OBJP_OBJECT_SIZE                         = 0xDC04u,
+  MTP_OBJP_ASSOCIATION_TYPE                    = 0xDC05u,
+  MTP_OBJP_OBJECT_FILE_NAME                    = 0xDC07u,
+  MTP_OBJP_PARENT_OBJECT                       = 0xDC0Bu,
+  MTP_OBJP_PERSISTENT_UNIQUE_OBJECT_IDENTIFIER = 0xDC41u,
+  MTP_OBJP_NAME                                = 0xDC44u,
+} mtp_object_properties_t;
+
+// Object formats
+typedef enum
+{
+  MTP_OBJF_UNDEFINED   = 0x3000u,
+  MTP_OBJF_ASSOCIATION = 0x3001u,
+  MTP_OBJF_TEXT        = 0x3004u,
+} mtp_object_formats_t;
+
+// Predefined Object handles
+typedef enum
+{
+  MTP_OBJH_ROOT = 0x0000,
+} 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;
+
+// Get/Set
+typedef enum
+{
+  MTP_MODE_GET     = 0x00u,
+  MTP_MODE_GET_SET = 0x01u,
+} mtp_mode_get_set_t;
+
+tu_static const uint16_t mtp_operations_supported[] = {
+  MTP_OPEC_GET_DEVICE_INFO,
+  MTP_OPEC_OPEN_SESSION,
+  MTP_OPEC_CLOSE_SESSION,
+  MTP_OPEC_GET_STORAGE_IDS,
+  MTP_OPEC_GET_STORAGE_INFO,
+  MTP_OPEC_GET_NUM_OBJECTS,
+  MTP_OPEC_GET_OBJECT_HANDLES,
+  MTP_OPEC_GET_OBJECT_INFO,
+  MTP_OPEC_GET_OBJECT,
+  MTP_OPEC_DELETE_OBJECT,
+  MTP_OPEC_SEND_OBJECT_INFO,
+  MTP_OPEC_SEND_OBJECT,
+  MTP_OPEC_FORMAT_STORE,
+  MTP_OPEC_RESET_DEVICE,
+  MTP_OPEC_GET_DEVICE_PROP_DESC,
+  MTP_OPEC_GET_DEVICE_PROP_VALUE,
+  MTP_OPEC_SET_DEVICE_PROP_VALUE,
+};
+
+tu_static const uint16_t mtp_events_supported[] = {
+  MTP_EVTC_OBJECT_ADDED,
+};
+
+tu_static const uint16_t mtp_device_properties_supported[] = {
+  MTP_DEVP_DEVICE_FRIENDLY_NAME,
+};
+
+tu_static const uint16_t mtp_capture_formats[] = {
+  MTP_OBJF_UNDEFINED,
+  MTP_OBJF_ASSOCIATION,
+  MTP_OBJF_TEXT,
+};
+
+tu_static const uint16_t mtp_playback_formats[] = {
+  MTP_OBJF_UNDEFINED,
+  MTP_OBJF_ASSOCIATION,
+  MTP_OBJF_TEXT,
+};
+
+//--------------------------------------------------------------------+
+// Data structures
+//--------------------------------------------------------------------+
+
+// 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_DEF(name, string) \
+  uint8_t name##_len; \
+  wchar16_t name[TU_ARRAY_LEN(string)];
+
+#define MTP_ARRAY_DEF(name, array) \
+  uint16_t name##_len; \
+  typeof(name) name[TU_ARRAY_LEN(array)];
+
+// StorageInfo dataset
+typedef struct TU_ATTR_PACKED {
+  uint16_t storage_type;
+  uint16_t filesystem_type;
+  uint16_t access_capability;
+  uint64_t max_capacity_in_bytes;
+  uint64_t free_space_in_bytes;
+  uint32_t free_space_in_objects;
+} mtp_storage_info_t;
+// The following fields will be dynamically added to the struct at runtime:
+// - wstring storage_description
+// - wstring volume_identifier
+
+// ObjectInfo Dataset
+typedef struct TU_ATTR_PACKED {
+  uint32_t storage_id;
+  uint16_t object_format;
+  uint16_t protection_status;
+  uint32_t object_compressed_size;
+  uint16_t thumb_format; // unused
+  uint32_t thumb_compressed_size; // unused
+  uint32_t thumb_pix_width; // unused
+  uint32_t thumb_pix_height; // unused
+  uint32_t image_pix_width; // unused
+  uint32_t image_pix_height; // unused
+  uint32_t image_bit_depth; // unused
+  uint32_t parent_object; // 0: root
+  uint16_t association_type;
+  uint32_t association_description; // not used
+  uint32_t sequence_number; // not used
+} mtp_object_info_t;
+// The following fields will be dynamically added to the struct at runtime:
+// - wstring filename;
+// - datetime_wstring date_created;
+// - datetime_wstring date_modified;
+// - wstring keywords;
+
+// Storage IDs
+typedef struct TU_ATTR_PACKED {
+  uint32_t storage_ids_len;
+  uint32_t storage_ids[CFG_MTP_STORAGE_ID_COUNT];
+} mtp_storage_ids_t;
+
+// DevicePropDesc Dataset
+typedef struct TU_ATTR_PACKED {
+  uint16_t device_property_code;
+  uint16_t datatype;
+  uint8_t  get_set;
+} mtp_device_prop_desc_t;
+// The following fields will be dynamically added to the struct at runtime:
+// - wstring factory_def_value;
+// - wstring current_value_len;
+// - uint8_t form_flag;
+
+typedef struct TU_ATTR_PACKED {
+  uint16_t wLength;
+  uint16_t code;
+} mtp_device_status_res_t;
+
+typedef struct TU_ATTR_PACKED {
+  uint32_t object_handle;
+  uint32_t storage_id;
+  uint32_t parent_object_handle;
+} mtp_basic_object_info_t;
+
+//--------------------------------------------------------------------+
+// Definitions
+//--------------------------------------------------------------------+
+
+typedef enum {
+    MTP_STORAGE_TYPE_UNDEFINED     = 0x0000u,
+    MTP_STORAGE_TYPE_FIXED_ROM     = 0x0001u,
+    MTP_STORAGE_TYPE_REMOVABLE_ROM = 0x0002u,
+    MTP_STORAGE_TYPE_FIXED_RAM     = 0x0003u,
+    MTP_STORAGE_TYPE_REMOVABLE_RAM = 0x0004u,
+} mtp_storage_type_t;
+
+typedef enum {
+    MTP_FILESYSTEM_TYPE_UNDEFINED            = 0x0000u,
+    MTP_FILESYSTEM_TYPE_GENERIC_FLAT         = 0x0001u,
+    MTP_FILESYSTEM_TYPE_GENERIC_HIERARCHICAL = 0x0002u,
+    MTP_FILESYSTEM_TYPE_DCF                  = 0x0003u,
+} mtp_filesystem_type_t;
+
+typedef enum {
+    MTP_ACCESS_CAPABILITY_READ_WRITE                        = 0x0000u,
+    MTP_ACCESS_CAPABILITY_READ_ONLY_WITHOUT_OBJECT_DELETION = 0x0001u,
+    MTP_ACCESS_CAPABILITY_READ_ONLY_WITH_OBJECT_DELETION    = 0x0002u,
+} mtp_access_capability_t;
+
+typedef enum {
+    MTP_PROTECTION_STATUS_NO_PROTECTION  = 0x0000u,
+    MTP_PROTECTION_STATUS_READ_ONLY      = 0x0001u,
+    MTP_PROTECTION_STATUS_READ_ONLY_DATA = 0x8002u,
+    MTP_PROTECTION_NON_TRANSFERABLE_DATA = 0x8003u,
+} mtp_protection_status_t;
+
+typedef enum {
+    MTP_ASSOCIATION_UNDEFINED            = 0x0000u,
+    MTP_ASSOCIATION_GENERIC_FOLDER       = 0x0001u,
+    MTP_ASSOCIATION_GENERIC_ALBUM        = 0x0002u,
+    MTP_ASSOCIATION_TIME_SEQUENCE        = 0x0003u,
+    MTP_ASSOCIATION_HORIZONTAL_PANORAMIC = 0x0004u,
+    MTP_ASSOCIATION_VERTICAL_PANORAMIC   = 0x0005u,
+    MTP_ASSOCIATION_2D_PANORAMIC         = 0x0006u,
+} mtp_association_t;
+
+// Responses
+typedef enum {
+// Supported ResponseCode
+  MTP_RESC_UNDEFINED                           = 0x2000u,
+  MTP_RESC_OK                                  = 0x2001u,
+  MTP_RESC_GENERAL_ERROR                       = 0x2002u,
+  MTP_RESC_SESSION_NOT_OPEN                    = 0x2003u,
+  MTP_RESC_INVALID_TRANSACTION_ID              = 0x2004u,
+  MTP_RESC_OPERATION_NOT_SUPPORTED             = 0x2005u,
+  MTP_RESC_PARAMETER_NOT_SUPPORTED             = 0x2006u,
+  MTP_RESC_INCOMPLETE_TRANSFER                 = 0x2007u,
+  MTP_RESC_INVALID_STORAGE_ID                  = 0x2008u,
+  MTP_RESC_INVALID_OBJECT_HANDLE               = 0x2009u,
+  MTP_RESC_STORE_FULL                          = 0x200Cu,
+  MTP_RESC_OBJECT_WRITE_PROTECTED              = 0x200Du,
+  MTP_RESC_STORE_NOT_AVAILABLE                 = 0x2013u,
+  MTP_RESC_SPECIFICATION_BY_FORMAT_UNSUPPORTED = 0x2014u,
+  MTP_RESC_NO_VALID_OBJECTINFO                 = 0x2015u,
+  MTP_RESC_DEVICE_BUSY                         = 0x2019u,
+  MTP_RESC_INVALID_PARENT_OBJECT               = 0x201Au,
+  MTP_RESC_INVALID_DEVICE_PROP_FORMAT          = 0x201Bu,
+  MTP_RESC_INVALID_DEVICE_PROP_VALUE           = 0x201Cu,
+  MTP_RESC_INVALID_PARAMETER                   = 0x201Du,
+  MTP_RESC_SESSION_ALREADY_OPEN                = 0x201Eu,
+  MTP_RESC_TRANSACTION_CANCELLED               = 0x201Fu,
+} mtp_response_t;
+
+#ifdef __cplusplus
+ }
+#endif
+
+#endif /* CFG_TUD_ENABLED && CFG_TUD_MTP */
+
+#endif /* _TUSB_MTP_H_ */
diff --git a/src/class/mtp/mtp_device.c b/src/class/mtp/mtp_device.c
new file mode 100644
index 000000000..854d4f10e
--- /dev/null
+++ b/src/class/mtp/mtp_device.c
@@ -0,0 +1,997 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2025 Ennebi Elettronica (https://ennebielettronica.com)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * 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
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#include "tusb_option.h"
+
+#if (CFG_TUD_ENABLED && CFG_TUD_MTP)
+
+//--------------------------------------------------------------------+
+// INCLUDE
+//--------------------------------------------------------------------+
+#include "device/dcd.h"         // for faking dcd_event_xfer_complete
+#include "device/usbd.h"
+#include "device/usbd_pvt.h"
+
+#include "mtp_device.h"
+#include "mtp_device_storage.h"
+
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_MTP_LOG_LEVEL
+  #define CFG_TUD_MTP_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_MTP_LOG_LEVEL, __VA_ARGS__)
+
+//--------------------------------------------------------------------+
+// STRUCT
+//--------------------------------------------------------------------+
+typedef struct
+{
+  uint8_t itf_num;
+  uint8_t ep_in;
+  uint8_t ep_out;
+  uint8_t ep_evt;
+
+  // Bulk Only Transfer (BOT) Protocol
+  uint8_t  phase;
+
+  uint32_t queued_len;  // number of bytes queued from the DataIN Stage
+  uint32_t total_len;   // byte to be transferred, can be smaller than total_bytes in cbw
+  uint32_t xferred_len; // number of bytes transferred so far in the Data Stage
+  uint32_t handled_len; // number of bytes already handled in the Data Stage
+  bool     xfer_completed; // true when DATA-IN/DATA-OUT transfer is completed
+
+} mtpd_interface_t;
+
+typedef struct
+{
+  uint32_t session_id;
+  uint32_t transaction_id;
+} mtpd_context_t;
+
+//--------------------------------------------------------------------+
+// INTERNAL FUNCTION DECLARATION
+//--------------------------------------------------------------------+
+// Checker
+tu_static mtp_phase_type_t mtpd_chk_generic(const char *func_name, const bool err_cd, const uint16_t ret_code, const char *message);
+tu_static mtp_phase_type_t mtpd_chk_session_open(const char *func_name);
+
+// MTP commands
+tu_static mtp_phase_type_t mtpd_handle_cmd(void);
+tu_static mtp_phase_type_t mtpd_handle_data(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_device_info(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_open_session(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_close_session(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_storage_info(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_storage_ids(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_object_handles(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_object_info(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_object(void);
+tu_static mtp_phase_type_t mtpd_handle_dti_get_object(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_delete_object(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_device_prop_desc(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_get_device_prop_value(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_send_object_info(void);
+tu_static mtp_phase_type_t mtpd_handle_dto_send_object_info(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_send_object(void);
+tu_static mtp_phase_type_t mtpd_handle_dto_send_object(void);
+tu_static mtp_phase_type_t mtpd_handle_cmd_format_store(void);
+
+//--------------------------------------------------------------------+
+// MTP variable declaration
+//--------------------------------------------------------------------+
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static mtpd_interface_t _mtpd_itf;
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static mtp_generic_container_t _mtpd_gct;
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static mtpd_context_t _mtpd_ctx;
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static mtp_device_status_res_t _mtpd_device_status_res;
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static uint32_t _mtpd_get_object_handle;
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static mtp_basic_object_info_t _mtpd_soi;
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static char _mtp_datestr[20];
+
+//--------------------------------------------------------------------+
+// USBD Driver API
+//--------------------------------------------------------------------+
+void mtpd_init(void) {
+  TU_LOG_DRV("  MTP mtpd_init\n");
+  tu_memclr(&_mtpd_itf, sizeof(mtpd_interface_t));
+  tu_memclr(&_mtpd_ctx, sizeof(mtpd_context_t));
+  _mtpd_get_object_handle = 0;
+  tu_memclr(&_mtpd_soi, sizeof(mtp_basic_object_info_t));
+}
+
+bool mtpd_deinit(void) {
+  TU_LOG_DRV("  MTP mtpd_deinit\n");
+  // nothing to do
+  return true;
+}
+
+void mtpd_reset(uint8_t rhport)
+{
+  TU_LOG_DRV("  MTP mtpd_reset\n");
+  (void) rhport;
+
+  // Close all endpoints
+  dcd_edpt_close_all(rhport);
+  tu_memclr(&_mtpd_itf, sizeof(mtpd_interface_t));
+  tu_memclr(&_mtpd_ctx, sizeof(mtpd_context_t));
+  tu_memclr(&_mtpd_gct, sizeof(mtp_generic_container_t));
+  _mtpd_get_object_handle = 0;
+  tu_memclr(&_mtpd_soi, sizeof(mtp_basic_object_info_t));
+}
+
+uint16_t mtpd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
+{
+  TU_LOG_DRV("  MTP mtpd_open\n");
+  tusb_desc_endpoint_t const *ep_desc;
+  // only support SCSI's BOT protocol
+  TU_VERIFY(TUSB_CLASS_IMAGE          == itf_desc->bInterfaceClass &&
+            MTP_SUBCLASS              == itf_desc->bInterfaceSubClass &&
+            MTP_PROTOCOL_STILL_IMAGE  == itf_desc->bInterfaceProtocol, 0);
+
+  // mtp driver length is fixed
+  uint16_t const mtpd_itf_size = sizeof(tusb_desc_interface_t) + 3 * sizeof(tusb_desc_endpoint_t);
+
+  // Max length must be at least 1 interface + 3 endpoints
+  TU_ASSERT(itf_desc->bNumEndpoints == 3 && max_len >= mtpd_itf_size);
+
+  _mtpd_itf.itf_num = itf_desc->bInterfaceNumber;
+
+  // Open interrupt IN endpoint
+  ep_desc = (tusb_desc_endpoint_t const *)tu_desc_next(itf_desc);
+  TU_ASSERT(ep_desc->bDescriptorType == TUSB_DESC_ENDPOINT && ep_desc->bmAttributes.xfer == TUSB_XFER_INTERRUPT, 0);
+  TU_ASSERT(usbd_edpt_open(rhport, ep_desc), 0);
+  _mtpd_itf.ep_evt = ep_desc->bEndpointAddress;
+
+  // Open endpoint pair
+  TU_ASSERT( usbd_open_edpt_pair(rhport, tu_desc_next(ep_desc), 2, TUSB_XFER_BULK, &_mtpd_itf.ep_out, &_mtpd_itf.ep_in), 0 );
+
+  // Prepare rx on bulk out EP
+  TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct)), CFG_MTP_EP_SIZE), 0);
+
+  return mtpd_itf_size;
+}
+
+// Invoked when a control transfer occurred on an interface of this class
+// Driver response accordingly to the request and the transfer stage (setup/data/ack)
+// return false to stall control endpoint (e.g unsupported request)
+bool mtpd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request)
+{
+  TU_LOG_DRV("  MTP mtpd_control_xfer_cb: bmRequest=0x%2x, bRequest=0x%2x\n", request->bmRequestType, request->bRequest);
+  // nothing to do with DATA & ACK stage
+  if (stage != CONTROL_STAGE_SETUP) return true;
+
+  uint16_t len = 0;
+
+  switch ( request->bRequest )
+  {
+    case MTP_REQ_CANCEL:
+      TU_LOG_DRV("  MTP request: MTP_REQ_CANCEL\n");
+      tud_mtp_storage_cancel();
+    break;
+    case MTP_REQ_GET_EXT_EVENT_DATA:
+      TU_LOG_DRV("  MTP request: MTP_REQ_GET_EXT_EVENT_DATA\n");
+    break;
+    case MTP_REQ_RESET:
+      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_gct)), CFG_MTP_EP_SIZE) );
+    break;
+    case MTP_REQ_GET_DEVICE_STATUS:
+      TU_LOG_DRV("  MTP request: MTP_REQ_GET_DEVICE_STATUS\n");
+      len = 4;
+      _mtpd_device_status_res.wLength = len;
+      // Cancel is synchronous, always answer OK
+      _mtpd_device_status_res.code = MTP_RESC_OK;
+      TU_ASSERT( tud_control_xfer(rhport, request, (uint8_t *)&_mtpd_device_status_res , len) );
+    break;
+
+    default:
+      TU_LOG_DRV("  MTP request: invalid request\r\n");
+      return false; // stall unsupported request
+    }
+  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)
+{
+  const unsigned dir = tu_edpt_dir(ep_addr);
+
+  if (event != XFER_RESULT_SUCCESS)
+    return false;
+
+  // IN transfer completed
+  if (dir == TUSB_DIR_IN)
+  {
+    if (_mtpd_itf.phase == MTP_PHASE_RESPONSE)
+    {
+      // IN transfer completed, prepare for a new command
+      TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct)), CFG_MTP_EP_SIZE), 0);
+      _mtpd_itf.phase = MTP_PHASE_IDLE;
+    }
+    else if (_mtpd_itf.phase == MTP_PHASE_DATA_IN)
+    {
+      _mtpd_itf.xferred_len += xferred_bytes;
+      _mtpd_itf.handled_len = _mtpd_itf.xferred_len;
+
+      // Check if transfer completed
+      if (_mtpd_itf.xferred_len >= _mtpd_itf.total_len && (xferred_bytes == 0 || (xferred_bytes % CFG_MTP_EP_SIZE) != 0))
+      {
+        _mtpd_itf.phase = MTP_PHASE_RESPONSE;
+        _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+        _mtpd_gct.code = MTP_RESC_OK;
+        _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+        _mtpd_gct.transaction_id = _mtpd_ctx.transaction_id;
+        if (_mtpd_ctx.session_id != 0)
+        {
+          _mtpd_gct.data[0] = _mtpd_ctx.session_id;
+          _mtpd_gct.container_length += sizeof(uint32_t);
+        }
+        TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_in, ((uint8_t *)(&_mtpd_gct)), (uint16_t)_mtpd_gct.container_length), 0);
+      }
+      else
+      // Send next block of DATA
+      {
+        // Send Zero-Lenght Packet
+        if (_mtpd_itf.xferred_len == _mtpd_itf.total_len)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_in, ((uint8_t *)(&_mtpd_gct.data)), 0 ));
+        }
+        else
+        {
+          _mtpd_itf.phase = mtpd_handle_data();
+          if (_mtpd_itf.phase == MTP_PHASE_RESPONSE)
+            TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_in, ((uint8_t *)(&_mtpd_gct)), (uint16_t)_mtpd_gct.container_length));
+          else
+            TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_in, ((uint8_t *)(&_mtpd_gct.data)), (uint16_t)_mtpd_itf.queued_len));
+        }
+      }
+    }
+    else
+    {
+      return false;
+    }
+  }
+
+  if (dir == TUSB_DIR_OUT)
+  {
+    if (_mtpd_itf.phase == MTP_PHASE_IDLE)
+    {
+      // A new command has been received. Ensure this is the last of the sequence.
+      _mtpd_itf.total_len = _mtpd_gct.container_length;
+      // Stall in case of unexpected block
+      if (_mtpd_gct.container_type != MTP_CONTAINER_TYPE_COMMAND_BLOCK)
+      {
+        return false;
+      }
+      _mtpd_itf.phase = MTP_PHASE_COMMAND;
+      _mtpd_itf.total_len = _mtpd_gct.container_length;
+      _mtpd_itf.xferred_len = xferred_bytes;
+      _mtpd_itf.handled_len = 0;
+      _mtpd_itf.xfer_completed = false;
+      TU_ASSERT(_mtpd_itf.total_len < sizeof(mtp_generic_container_t));
+    }
+
+    if (_mtpd_itf.phase == MTP_PHASE_COMMAND)
+    {
+      // A zero-length or a short packet termination is expected
+      if (xferred_bytes == CFG_MTP_EP_SIZE || (_mtpd_itf.total_len - _mtpd_itf.xferred_len) > 0 )
+      {
+        TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct)) + _mtpd_itf.xferred_len, (uint16_t)(_mtpd_itf.total_len - _mtpd_itf.xferred_len)));
+      }
+      else
+      {
+        // Handle command block
+        _mtpd_itf.phase = mtpd_handle_cmd();
+        if (_mtpd_itf.phase == MTP_PHASE_RESPONSE)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_in, ((uint8_t *)(&_mtpd_gct)), (uint16_t)_mtpd_gct.container_length));
+        }
+        else if (_mtpd_itf.phase == MTP_PHASE_DATA_IN)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_in, ((uint8_t *)(&_mtpd_gct)), (uint16_t)_mtpd_itf.queued_len));
+          _mtpd_itf.total_len = _mtpd_gct.container_length;
+          _mtpd_itf.xferred_len = 0;
+          _mtpd_itf.handled_len = 0;
+          _mtpd_itf.xfer_completed = false;
+        }
+        else if (_mtpd_itf.phase == MTP_PHASE_DATA_OUT)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct)), CFG_MTP_EP_SIZE), 0);
+          _mtpd_itf.xferred_len = 0;
+          _mtpd_itf.handled_len = 0;
+          _mtpd_itf.xfer_completed = false;
+        }
+        else
+        {
+          usbd_edpt_stall(rhport, _mtpd_itf.ep_out);
+          usbd_edpt_stall(rhport, _mtpd_itf.ep_in);
+        }
+      }
+      return true;
+    }
+
+    if (_mtpd_itf.phase == MTP_PHASE_DATA_OUT)
+    {
+      // First block of data
+      if (_mtpd_itf.xferred_len == 0)
+      {
+        _mtpd_itf.total_len = _mtpd_gct.container_length;
+        _mtpd_itf.handled_len = 0;
+        _mtpd_itf.xfer_completed = false;
+      }
+      _mtpd_itf.xferred_len += xferred_bytes;
+      // Stall in case of unexpected block
+      if (_mtpd_gct.container_type != MTP_CONTAINER_TYPE_DATA_BLOCK)
+      {
+        return false;
+      }
+
+      // A zero-length or a short packet termination
+      if (xferred_bytes < CFG_MTP_EP_SIZE)
+      {
+        _mtpd_itf.xfer_completed = true;
+        // Handle data block
+        _mtpd_itf.phase = mtpd_handle_data();
+        if (_mtpd_itf.phase == MTP_PHASE_DATA_IN || _mtpd_itf.phase == MTP_PHASE_RESPONSE)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_in, ((uint8_t *)(&_mtpd_gct)), (uint16_t)_mtpd_gct.container_length));
+        }
+        else if (_mtpd_itf.phase == MTP_PHASE_DATA_OUT)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct)), CFG_MTP_EP_SIZE), 0);
+          _mtpd_itf.xferred_len = 0;
+          _mtpd_itf.xfer_completed = false;
+        }
+        else
+        {
+          usbd_edpt_stall(rhport, _mtpd_itf.ep_out);
+          usbd_edpt_stall(rhport, _mtpd_itf.ep_in);
+        }
+      }
+      else
+      {
+        // Handle data block when container is full
+        if (_mtpd_itf.xferred_len - _mtpd_itf.handled_len >= MTP_MAX_PACKET_SIZE - CFG_MTP_EP_SIZE)
+        {
+          _mtpd_itf.phase = mtpd_handle_data();
+          _mtpd_itf.handled_len = _mtpd_itf.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 (_mtpd_itf.total_len == _mtpd_itf.xferred_len)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct.data)), CFG_MTP_EP_SIZE), 0);
+        }
+        // First data block includes container header + container data
+        else if (_mtpd_itf.handled_len == 0)
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct)) + _mtpd_itf.xferred_len, (uint16_t)TU_MIN(_mtpd_itf.total_len - _mtpd_itf.xferred_len, CFG_MTP_EP_SIZE)));
+        }
+        else
+        // Successive data block includes only container data
+        {
+          TU_ASSERT(usbd_edpt_xfer(rhport, _mtpd_itf.ep_out, ((uint8_t *)(&_mtpd_gct.data)) + _mtpd_itf.xferred_len - _mtpd_itf.handled_len, (uint16_t)TU_MIN(_mtpd_itf.total_len - _mtpd_itf.xferred_len, CFG_MTP_EP_SIZE)));
+        }
+      }
+    }
+  }
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// MTPD Internal functionality
+//--------------------------------------------------------------------+
+
+// Decode command and prepare response
+mtp_phase_type_t mtpd_handle_cmd(void)
+{
+  TU_ASSERT(_mtpd_gct.container_type == MTP_CONTAINER_TYPE_COMMAND_BLOCK);
+  _mtpd_ctx.transaction_id = _mtpd_gct.transaction_id;
+  if (_mtpd_gct.code != MTP_OPEC_SEND_OBJECT)
+    _mtpd_soi.object_handle = 0;
+
+  switch(_mtpd_gct.code)
+  {
+    case MTP_OPEC_GET_DEVICE_INFO:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_DEVICE_INFO\n");
+      return mtpd_handle_cmd_get_device_info();
+    case MTP_OPEC_OPEN_SESSION:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_OPEN_SESSION\n");
+      return mtpd_handle_cmd_open_session();
+    case MTP_OPEC_CLOSE_SESSION:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_CLOSE_SESSION\n");
+      return mtpd_handle_cmd_close_session();
+    case MTP_OPEC_GET_STORAGE_IDS:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_STORAGE_IDS\n");
+      return mtpd_handle_cmd_get_storage_ids();
+    case MTP_OPEC_GET_STORAGE_INFO:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_STORAGE_INFO for ID=%lu\n", _mtpd_gct.data[0]);
+      return mtpd_handle_cmd_get_storage_info();
+    case MTP_OPEC_GET_OBJECT_HANDLES:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_OBJECT_HANDLES\n");
+      return mtpd_handle_cmd_get_object_handles();
+    case MTP_OPEC_GET_OBJECT_INFO:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_OBJECT_INFO\n");
+      return mtpd_handle_cmd_get_object_info();
+    case MTP_OPEC_GET_OBJECT:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_OBJECT\n");
+      return mtpd_handle_cmd_get_object();
+    case MTP_OPEC_DELETE_OBJECT:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_DELETE_OBJECT\n");
+      return mtpd_handle_cmd_delete_object();
+    case MTP_OPEC_GET_DEVICE_PROP_DESC:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_DEVICE_PROP_DESC\n");
+      return mtpd_handle_cmd_get_device_prop_desc();
+    case MTP_OPEC_GET_DEVICE_PROP_VALUE:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_DEVICE_PROP_VALUE\n");
+      return mtpd_handle_cmd_get_device_prop_value();
+    case MTP_OPEC_SEND_OBJECT_INFO:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_SEND_OBJECT_INFO\n");
+      return mtpd_handle_cmd_send_object_info();
+    case MTP_OPEC_SEND_OBJECT:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_SEND_OBJECT\n");
+      return mtpd_handle_cmd_send_object();
+    case MTP_OPEC_FORMAT_STORE:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_FORMAT_STORE\n");
+      return mtpd_handle_cmd_format_store();
+    default:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_UNKNOWN_COMMAND %x!!!!\n", _mtpd_gct.code);
+      return false;
+  }
+  return true;
+}
+
+mtp_phase_type_t mtpd_handle_data(void)
+{
+  TU_ASSERT(_mtpd_gct.container_type == MTP_CONTAINER_TYPE_DATA_BLOCK);
+  _mtpd_ctx.transaction_id = _mtpd_gct.transaction_id;
+
+  switch(_mtpd_gct.code)
+  {
+    case MTP_OPEC_GET_OBJECT:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_GET_OBJECT-DATA_IN\n");
+      return mtpd_handle_dti_get_object();
+    case MTP_OPEC_SEND_OBJECT_INFO:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_SEND_OBJECT_INFO-DATA_OUT\n");
+      return mtpd_handle_dto_send_object_info();
+    case MTP_OPEC_SEND_OBJECT:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_SEND_OBJECT-DATA_OUT\n");
+      return mtpd_handle_dto_send_object();
+    default:
+      TU_LOG_DRV("  MTP command: MTP_OPEC_UNKNOWN_COMMAND %x!!!!\n", _mtpd_gct.code);
+      return false;
+  }
+  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");
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH + sizeof(mtp_device_info_t);
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  _mtpd_gct.code = MTP_OPEC_GET_DEVICE_INFO;
+  mtp_device_info_t *d = (mtp_device_info_t *)_mtpd_gct.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 = _mtpd_gct.container_length;
+  return MTP_PHASE_DATA_IN;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_open_session(void)
+{
+  uint32_t session_id = _mtpd_gct.data[0];
+
+  mtp_response_t res = tud_mtp_storage_open_session(&session_id);
+  if (res == MTP_RESC_SESSION_ALREADY_OPEN)
+  {
+    _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+    _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+    _mtpd_gct.code = res;
+    _mtpd_gct.container_length += sizeof(_mtpd_gct.data[0]);
+    _mtpd_gct.data[0] = session_id;
+    _mtpd_ctx.session_id = session_id;
+    return MTP_PHASE_RESPONSE;
+  }
+
+  mtp_phase_type_t phase;
+  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+
+  _mtpd_ctx.session_id = session_id;
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  _mtpd_gct.code = MTP_RESC_OK;
+
+  return MTP_PHASE_RESPONSE;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_close_session(void)
+{
+  uint32_t session_id = _mtpd_gct.data[0];
+
+  mtp_response_t res = tud_mtp_storage_close_session(session_id);
+
+  _mtpd_ctx.session_id = session_id;
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  _mtpd_gct.code = res;
+
+  return MTP_PHASE_RESPONSE;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_get_storage_ids(void)
+{
+  TU_VERIFY_STATIC(sizeof(mtp_storage_ids_t) < MTP_MAX_PACKET_SIZE, "mtp_storage_ids_t shall fit in MTP_MAX_PACKET_SIZE");
+
+  uint32_t storage_id;
+  mtp_response_t res = tud_mtp_get_storage_id(&storage_id);
+  mtp_phase_type_t phase;
+  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH + sizeof(mtp_storage_ids_t);
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  _mtpd_gct.code = MTP_OPEC_GET_STORAGE_IDS;
+  mtp_storage_ids_t *d = (mtp_storage_ids_t *)_mtpd_gct.data;
+  if (storage_id == 0)
+  {
+    // Storage not accessible
+    d->storage_ids_len = 0;
+    d->storage_ids[0] = 0;
+  }
+  else
+  {
+    d->storage_ids_len = 1;
+    d->storage_ids[0] = storage_id;
+  }
+
+  _mtpd_itf.queued_len = _mtpd_gct.container_length;
+  return MTP_PHASE_DATA_IN;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_get_storage_info(void)
+{
+  TU_VERIFY_STATIC(sizeof(mtp_storage_info_t) < MTP_MAX_PACKET_SIZE, "mtp_storage_info_t shall fit in MTP_MAX_PACKET_SIZE");
+
+  uint32_t storage_id = _mtpd_gct.data[0];
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH + sizeof(mtp_storage_info_t);
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  _mtpd_gct.code = MTP_OPEC_GET_STORAGE_INFO;
+
+  mtp_response_t res = tud_mtp_get_storage_info(storage_id, (mtp_storage_info_t *)_mtpd_gct.data);
+  mtp_phase_type_t phase;
+  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+
+  _mtpd_itf.queued_len = _mtpd_gct.container_length;
+  return MTP_PHASE_DATA_IN;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_get_object_handles(void)
+{
+  uint32_t storage_id = _mtpd_gct.data[0];
+  uint32_t object_format_code = _mtpd_gct.data[1]; // optional, not managed
+  uint32_t parent_object_handle = _mtpd_gct.data[2]; // folder specification, 0xffffffff=objects with no parent
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH + sizeof(uint32_t);
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  _mtpd_gct.code = MTP_OPEC_GET_OBJECT_HANDLES;
+  _mtpd_gct.data[0] = 0;
+
+  mtp_phase_type_t phase;
+  if ((phase = mtpd_chk_generic(__func__, (object_format_code != 0), MTP_RESC_SPECIFICATION_BY_FORMAT_UNSUPPORTED, "specification by format unsupported")) != MTP_PHASE_NONE) return phase;
+  //list of all object handles on all storages, not managed
+  if ((phase = mtpd_chk_generic(__func__, (storage_id == 0xFFFFFFFF), MTP_RESC_OPERATION_NOT_SUPPORTED, "list of all object handles on all storages unsupported")) != MTP_PHASE_NONE) return phase;
+
+  tud_mtp_storage_object_done();
+  uint32_t next_child_handle = 0;
+  while(true)
+  {
+    mtp_response_t res = tud_mtp_storage_association_get_object_handle(storage_id, parent_object_handle, &next_child_handle);
+    if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+    if (next_child_handle == 0)
+      break;
+    mtpd_gct_append_object_handle(next_child_handle);
+  }
+  tud_mtp_storage_object_done();
+
+  _mtpd_itf.queued_len = _mtpd_gct.container_length;
+  return MTP_PHASE_DATA_IN;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_get_object_info(void)
+{
+  TU_VERIFY_STATIC(sizeof(mtp_object_info_t) < MTP_MAX_PACKET_SIZE, "mtp_object_info_t shall fit in MTP_MAX_PACKET_SIZE");
+
+  uint32_t object_handle = _mtpd_gct.data[0];
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH + sizeof(mtp_object_info_t);
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  _mtpd_gct.code = MTP_OPEC_GET_OBJECT_INFO;
+  mtp_response_t res = tud_mtp_storage_object_read_info(object_handle, (mtp_object_info_t *)_mtpd_gct.data);
+  mtp_phase_type_t phase;
+  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+
+  _mtpd_itf.queued_len = _mtpd_gct.container_length;
+  return MTP_PHASE_DATA_IN;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_get_object(void)
+{
+  _mtpd_get_object_handle = _mtpd_gct.data[0];
+
+  // Continue with DATA-IN
+  return mtpd_handle_dti_get_object();
+}
+
+mtp_phase_type_t mtpd_handle_dti_get_object(void)
+{
+  mtp_response_t res;
+  mtp_phase_type_t phase;
+  uint32_t file_size = 0;
+  res = tud_mtp_storage_object_size(_mtpd_get_object_handle, &file_size);
+  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH + file_size;
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  _mtpd_gct.code = MTP_OPEC_GET_OBJECT;
+
+  uint32_t buffer_size;
+  uint32_t read_count;
+  // Data block must be multiple of EP size
+  if (_mtpd_itf.handled_len == 0)
+  {
+    // First data block: include container header
+    buffer_size = ((MTP_MAX_PACKET_SIZE + MTP_GENERIC_DATA_BLOCK_LENGTH) / CFG_MTP_EP_SIZE) * CFG_MTP_EP_SIZE - MTP_GENERIC_DATA_BLOCK_LENGTH;
+    res = tud_mtp_storage_object_read(_mtpd_get_object_handle, (void *)&_mtpd_gct.data, buffer_size, &read_count);
+    if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+    _mtpd_itf.queued_len = MTP_GENERIC_DATA_BLOCK_LENGTH + read_count;
+  }
+  else
+  {
+    // Successive data block: consider only container data
+    buffer_size = (MTP_MAX_PACKET_SIZE / CFG_MTP_EP_SIZE) * CFG_MTP_EP_SIZE;
+    res = tud_mtp_storage_object_read(_mtpd_get_object_handle, (void *)&_mtpd_gct.data, buffer_size, &read_count);
+    if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+    _mtpd_itf.queued_len = read_count;
+  }
+
+  // File completed
+  if (read_count < buffer_size)
+  {
+    tud_mtp_storage_object_done();
+  }
+
+  return MTP_PHASE_DATA_IN;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_delete_object(void)
+{
+  uint32_t object_handle = _mtpd_gct.data[0];
+  uint32_t object_code_format = _mtpd_gct.data[1]; // not used
+  (void) object_code_format;
+
+  mtp_response_t res = tud_mtp_storage_object_delete(object_handle);
+  mtp_phase_type_t phase;
+  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  _mtpd_gct.code = MTP_RESC_OK;
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+  return MTP_PHASE_RESPONSE;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_get_device_prop_desc(void)
+{
+  uint32_t device_prop_code = _mtpd_gct.data[0];
+
+  mtp_phase_type_t rt;
+  if ((rt = mtpd_chk_session_open(__func__)) != MTP_PHASE_NONE) return rt;
+
+  switch(device_prop_code)
+  {
+    case MTP_DEVP_DEVICE_FRIENDLY_NAME:
+    {
+      TU_VERIFY_STATIC(sizeof(mtp_device_prop_desc_t) < MTP_MAX_PACKET_SIZE, "mtp_device_info_t shall fit in MTP_MAX_PACKET_SIZE");
+      _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+      _mtpd_gct.code = MTP_OPEC_GET_DEVICE_PROP_DESC;
+      _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH + sizeof(mtp_device_prop_desc_t);
+      mtp_device_prop_desc_t *d = (mtp_device_prop_desc_t *)_mtpd_gct.data;
+      d->device_property_code = (uint16_t)(device_prop_code);
+      d->datatype = MTP_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
+      mtpd_gct_append_uint8(0x00); // form_flag
+      _mtpd_itf.queued_len = _mtpd_gct.container_length;
+      return MTP_PHASE_DATA_IN;
+    }
+    default:
+      break;
+  }
+
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  _mtpd_gct.code = MTP_RESC_PARAMETER_NOT_SUPPORTED;
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+  return MTP_PHASE_RESPONSE;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_get_device_prop_value(void)
+{
+  uint32_t device_prop_code = _mtpd_gct.data[0];
+
+  mtp_phase_type_t rt;
+  if ((rt = mtpd_chk_session_open(__func__)) != MTP_PHASE_NONE) return rt;
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+  _mtpd_gct.code = MTP_OPEC_GET_DEVICE_PROP_VALUE;
+
+  switch(device_prop_code)
+  {
+    // TODO support more device properties
+    case MTP_DEVP_DEVICE_FRIENDLY_NAME:
+      mtpd_gct_append_wstring(CFG_TUD_MODEL);
+      _mtpd_itf.queued_len = _mtpd_gct.container_length;
+      return MTP_PHASE_DATA_IN;
+    default:
+      _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+      _mtpd_gct.code = MTP_RESC_PARAMETER_NOT_SUPPORTED;
+      return MTP_PHASE_RESPONSE;
+  }
+}
+
+mtp_phase_type_t mtpd_handle_cmd_send_object_info(void)
+{
+  _mtpd_soi.storage_id = _mtpd_gct.data[0];
+  _mtpd_soi.parent_object_handle = (_mtpd_gct.data[1] == 0xFFFFFFFF ? 0 : _mtpd_gct.data[1]);
+
+  // Enter OUT phase and wait for DATA BLOCK
+  return MTP_PHASE_DATA_OUT;
+}
+
+mtp_phase_type_t mtpd_handle_dto_send_object_info(void)
+{
+  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_t *)_mtpd_gct.data);
+  mtp_phase_type_t phase;
+  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+
+  // Save send_object_info
+  _mtpd_soi.object_handle = new_object_handle;
+
+  // Response
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH +  3 * sizeof(uint32_t);
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  _mtpd_gct.code = MTP_RESC_OK;
+  _mtpd_gct.data[0] = _mtpd_soi.storage_id;
+  _mtpd_gct.data[1] = _mtpd_soi.parent_object_handle;
+  _mtpd_gct.data[2] = _mtpd_soi.object_handle;
+  return MTP_PHASE_RESPONSE;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_send_object(void)
+{
+  // Enter OUT phase and wait for DATA BLOCK
+  return MTP_PHASE_DATA_OUT;
+}
+
+mtp_phase_type_t mtpd_handle_dto_send_object(void)
+{
+  uint8_t *buffer = (uint8_t *)&_mtpd_gct.data;
+  uint32_t buffer_size = _mtpd_itf.xferred_len - _mtpd_itf.handled_len;
+  // First block of DATA
+  if (_mtpd_itf.handled_len == 0)
+  {
+    buffer_size -= MTP_GENERIC_DATA_BLOCK_LENGTH;
+  }
+
+  if (buffer_size > 0)
+  {
+    mtp_response_t res = tud_mtp_storage_object_write(_mtpd_soi.object_handle, buffer, buffer_size);
+    mtp_phase_type_t phase;
+    if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESC_OK), res, "")) != MTP_PHASE_NONE) return phase;
+  }
+
+  if (!_mtpd_itf.xfer_completed)
+  {
+    // Continue with next DATA BLOCK
+    return MTP_PHASE_DATA_OUT;
+  }
+
+  // Send completed
+  tud_mtp_storage_object_done();
+
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  _mtpd_gct.code = MTP_RESC_OK;
+  return MTP_PHASE_RESPONSE;
+}
+
+mtp_phase_type_t mtpd_handle_cmd_format_store(void)
+{
+  uint32_t storage_id = _mtpd_gct.data[0];
+  uint32_t file_system_format = _mtpd_gct.data[1]; // not used
+  (void) file_system_format;
+
+  mtp_response_t res = tud_mtp_storage_format(storage_id);
+
+  _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  _mtpd_gct.code = res;
+  _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+  return MTP_PHASE_RESPONSE;
+}
+
+//--------------------------------------------------------------------+
+// Checker
+//--------------------------------------------------------------------+
+mtp_phase_type_t mtpd_chk_session_open(const char *func_name)
+{
+  (void)func_name;
+  if (_mtpd_ctx.session_id == 0)
+  {
+    TU_LOG_DRV("  MTP error: %s session not open\n", func_name);
+    _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+    _mtpd_gct.code = MTP_RESC_SESSION_NOT_OPEN;
+    _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+    return MTP_PHASE_RESPONSE;
+  }
+  return MTP_PHASE_NONE;
+}
+
+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;
+  if (err_cd)
+  {
+    TU_LOG_DRV("  MTP error in %s: (%x) %s\n", func_name, ret_code, message);
+    _mtpd_gct.container_type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+    _mtpd_gct.code = ret_code;
+    _mtpd_gct.container_length = MTP_GENERIC_DATA_BLOCK_LENGTH;
+     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)
+{
+  uint8_t *p_value = ((uint8_t *)&_mtpd_gct) + _mtpd_gct.container_length;
+  _mtpd_gct.container_length += sizeof(uint8_t);
+  // Verify space requirement (8 bit string length, number of wide characters including terminator)
+  TU_ASSERT(_mtpd_gct.container_length < sizeof(mtp_generic_container_t));
+  *p_value = value;
+  return true;
+}
+
+bool mtpd_gct_append_object_handle(const uint32_t object_handle)
+{
+  _mtpd_gct.container_length += sizeof(uint32_t);
+  TU_ASSERT(_mtpd_gct.container_length < sizeof(mtp_generic_container_t));
+  _mtpd_gct.data[0]++;
+  _mtpd_gct.data[_mtpd_gct.data[0]] = object_handle;
+  return true;
+}
+
+bool mtpd_gct_append_wstring(const char *s)
+{
+  size_t len = strlen(s) + 1;
+  TU_ASSERT(len <= UINT8_MAX);
+  uint8_t *p_len = ((uint8_t *)&_mtpd_gct)+_mtpd_gct.container_length;
+  _mtpd_gct.container_length += sizeof(uint8_t) + sizeof(wchar16_t) * len;
+  // Verify space requirement (8 bit string length, number of wide characters including terminator)
+  TU_ASSERT(_mtpd_gct.container_length < 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)
+{
+  uint16_t size = *(((uint8_t *)&_mtpd_gct.data) + *offset_data);
+  if (size > max_size)
+      size = max_size;
+  TU_ASSERT(*offset_data + size < sizeof(_mtpd_gct.data));
+
+  uint8_t *s = ((uint8_t *)&_mtpd_gct.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)
+{
+  TU_ASSERT(_mtpd_gct.container_length + sizeof(uint32_t) + array_size * type_size < sizeof(_mtpd_gct.data));
+  uint8_t *p = ((uint8_t *)&_mtpd_gct) + _mtpd_gct.container_length;
+  memcpy(p, &array_size, sizeof(uint32_t));
+  p += sizeof(uint32_t);
+  memcpy(p, data, array_size * type_size);
+  _mtpd_gct.container_length += sizeof(uint32_t) + array_size * type_size;
+  return true;
+}
+
+bool mtpd_gct_append_date(struct tm *timeinfo)
+{
+  // strftime is not supported by all platform, this implementation is just for reference
+  int len = snprintf(_mtp_datestr, sizeof(_mtpd_gct.data) - _mtpd_gct.container_length, "%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)
diff --git a/src/class/mtp/mtp_device.h b/src/class/mtp/mtp_device.h
new file mode 100644
index 000000000..6531b36df
--- /dev/null
+++ b/src/class/mtp/mtp_device.h
@@ -0,0 +1,75 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2025 Ennebi Elettronica (https://ennebielettronica.com)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * 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
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#ifndef _TUSB_MTP_DEVICE_H_
+#define _TUSB_MTP_DEVICE_H_
+
+#include "common/tusb_common.h"
+#include "mtp.h"
+
+#if (CFG_TUD_ENABLED && CFG_TUD_MTP)
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+//--------------------------------------------------------------------+
+// 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);
+
+//--------------------------------------------------------------------+
+// 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);
+
+#ifdef __cplusplus
+ }
+#endif
+
+#endif /* CFG_TUD_ENABLED && CFG_TUD_MTP */
+
+#endif /* _TUSB_MTP_DEVICE_H_ */
diff --git a/src/class/mtp/mtp_device_storage.h b/src/class/mtp/mtp_device_storage.h
new file mode 100644
index 000000000..a9bbc9b90
--- /dev/null
+++ b/src/class/mtp/mtp_device_storage.h
@@ -0,0 +1,149 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2025 Ennebi Elettronica (https://ennebielettronica.com)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * 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
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#ifndef _TUSB_MTP_DEVICE_STORAGE_H_
+#define _TUSB_MTP_DEVICE_STORAGE_H_
+
+#include "common/tusb_common.h"
+#include "mtp.h"
+
+#if (CFG_TUD_ENABLED && CFG_TUD_MTP)
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+//--------------------------------------------------------------------+
+// Storage Application Callbacks
+//--------------------------------------------------------------------+
+
+/*
+ * The entire MTP functionality is based on object handles, as described in MTP Specs v. 1.1 under 3.4.1.
+ * The major weakness of the protocol is that those handles are supposed to be unique within a session
+ * and for every enumerated object. There is no specified lifetime limit or way to control the expiration:
+ * once given, they have to persist for an indefinite time and number of iterations.
+ * If the filesystem does not provide unique persistent object handle for every entry, the best approach
+ * would be to keep a full association between generated handles and full file paths. The suggested
+ * approach with memory constrained devices is to keep a hard ID associated with each file or a volatile
+ * ID generated on the fly and invalidated on each operation that may rearrange the order.
+ * In order to invalidate existing IDS, it might be necessary to invalidate the whole session from
+ * the device side.
+ * Depending on the application, the handle could be also be the file name or a tag (i.e. host-only file access)
+ */
+
+
+// Initialize MTP storage subsystem
+//
+// The function shall check if the session is already opened and, in case, set session_id to the
+// ID of the current session.
+mtp_response_t tud_mtp_storage_open_session(uint32_t *session_id);
+
+// Close an open session
+mtp_response_t tud_mtp_storage_close_session(uint32_t session_id);
+
+// Get a storage ID valid within the current session
+//
+// TODO: while multiple storage IDs could be used, the implementation currently supports only 1.
+mtp_response_t tud_mtp_get_storage_id(uint32_t *storage_id);
+
+// Get storage information for the given ID
+//
+// The implementation shall fill all the fields required by the specification.
+// Note that the variable information (e.g. wstring file name, dates and tags shall be written by using the library functions)
+// In addition to the fixed mtp_storage_info_t structure, the function shall add storage descriptor string and
+// volume identifier string via tud_mtp_gct_append_wstring function.
+mtp_response_t tud_mtp_get_storage_info(uint32_t storage_id, mtp_storage_info_t *info);
+
+// Format the specified storage
+mtp_response_t tud_mtp_storage_format(uint32_t storage_id);
+
+// Traverse the given parent object handle and return a child handle for each call
+//
+// If the parent object has not been opened (or closed before) the function returns the first handle.
+// When next_child_handle is 0 all the handles have been listed.
+// TODO: traverse by ObjectFormatCode and ObjectHandle association. For now they are unsupported.
+mtp_response_t tud_mtp_storage_association_get_object_handle(uint32_t session_handle, uint32_t parent_object_handle, uint32_t *next_child_handle);
+
+// Called with the creation of a new object is requested.
+// The handle of the new object shall be returned in new_object_handle.
+// The structure info contains the information to be used for file creation, as passted by the host.
+// Note that the variable information (e.g. wstring file name, dates and tags shall be retrieved by using the library functions)
+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);
+
+// Get object information related to a given object handle
+//
+// The structure info shall be filled according to MTP specifications. Note that
+// in addition to filling the fixed mtp_object_info_t structure, the caller must add the following fields via
+// library calls
+// - Filename (string, use tud_mtp_gct_append_wstring)
+// - Date created (string, use tud_gct_append_date or empty string)
+// - Date modified (string, use tud_gct_append_date or empty string)
+// - Keywords (string containing list of kw, separated by space, use tud_mtp_gct_append_wstring)
+// Note that the variable information (e.g. wstring file name, dates and tags shall be written by using the library functions)
+mtp_response_t tud_mtp_storage_object_read_info(uint32_t object_handle, mtp_object_info_t *info);
+
+// Get the object size.
+//
+// The object may be already open when this function is called.
+// The implementation shall not assume a specific call order between this function and tud_mtp_storage_object_read.
+// The function may leave the file open.
+mtp_response_t tud_mtp_storage_object_size(uint32_t object_handle, uint32_t *size);
+
+// Write object data
+//
+// The function shall open the object for writing if not already open.
+// The binary data shall be written to the file in full before this function is returned.
+mtp_response_t tud_mtp_storage_object_write(uint32_t object_handle, const uint8_t *buffer, uint32_t buffer_size);
+
+// Get object data
+//
+// The function shall open the object for reading if not already open.
+// The amount of data returned shall be the given size parameter.
+// read_count shall contain the effective number of bytes written. Iteration is terminated when read_count < buffer_size.
+mtp_response_t tud_mtp_storage_object_read(uint32_t object_handle, void *buffer, uint32_t buffer_size, uint32_t *read_count);
+
+// Move an object to a new parent
+mtp_response_t tud_mtp_storage_object_move(uint32_t object_handle, uint32_t new_parent_object_handle);
+
+// Delete the specified object
+mtp_response_t tud_mtp_storage_object_delete(uint32_t object_handle);
+
+// Issued when IO operation has been terminated (e.g. read, traverse), close open file handles
+void tud_mtp_storage_object_done(void);
+
+// Cancel any pending operation. Current operation shall be discarded.
+void tud_mtp_storage_cancel(void);
+
+// Restore the operation out of reset. Cancel any pending operation and close the session.
+void tud_mtp_storage_reset(void);
+
+#ifdef __cplusplus
+ }
+#endif
+
+#endif /* CFG_TUD_ENABLED && CFG_TUD_MTP */
+
+#endif /* _TUSB_MTP_DEVICE_STORAGE_H_ */
diff --git a/src/device/usbd.c b/src/device/usbd.c
index fb5cec49d..8f50d815f 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -309,6 +309,19 @@ tu_static usbd_class_driver_t const _usbd_driver[] = {
         .sof              = NULL
     },
     #endif
+
+    #if CFG_TUD_MTP
+    {
+        .name             = DRIVER_NAME("MTP"),
+        .init             = mtpd_init,
+        .deinit           = mtpd_deinit,
+        .reset            = mtpd_reset,
+        .open             = mtpd_open,
+        .control_xfer_cb  = mtpd_control_xfer_cb,
+        .xfer_cb          = mtpd_xfer_cb,
+        .sof              = NULL
+    },
+    #endif
 };
 
 enum { BUILTIN_DRIVER_COUNT = TU_ARRAY_SIZE(_usbd_driver) };
diff --git a/src/device/usbd.h b/src/device/usbd.h
index de6007fb3..b89a0200b 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -270,6 +270,25 @@ bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_requ
 
 
 //--------------------------------------------------------------------+
+// MTP Descriptor Templates
+//--------------------------------------------------------------------+
+
+// Length of template descriptor: 30 bytes
+#define TUD_MTP_DESC_LEN    (9 + 7 + 7 + 7)
+
+// Interface number, string index, EP Out & EP In address, EP size
+#define TUD_MTP_DESCRIPTOR(_itfnum, _stridx, _ep_evt, _ep_evt_size, _ep_evt_polling_interval, _epout, _epin, _epsize) \
+  /* Interface */\
+  9, TUSB_DESC_INTERFACE, _itfnum, 0, 3, TUSB_CLASS_IMAGE, MTP_SUBCLASS, MTP_PROTOCOL_STILL_IMAGE, _stridx,\
+  /* Endpoint Interrupt */\
+  7, TUSB_DESC_ENDPOINT, _ep_evt, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_ep_evt_size), _ep_evt_polling_interval,\
+  /* Endpoint Out */\
+  7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0,\
+  /* Endpoint In */\
+  7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0
+
+
+//--------------------------------------------------------------------+
 // HID Descriptor Templates
 //--------------------------------------------------------------------+
 
diff --git a/src/tinyusb.mk b/src/tinyusb.mk
index a9f623c24..8f9d52de9 100644
--- a/src/tinyusb.mk
+++ b/src/tinyusb.mk
@@ -12,6 +12,7 @@ TINYUSB_SRC_C += \
 	src/class/hid/hid_device.c \
 	src/class/midi/midi_device.c \
 	src/class/msc/msc_device.c \
+	src/class/mtp/mtp_device.c \
 	src/class/net/ecm_rndis_device.c \
 	src/class/net/ncm_device.c \
 	src/class/usbtmc/usbtmc_device.c \
diff --git a/src/tusb.h b/src/tusb.h
index dfba21ddf..abe3e3c9b 100644
--- a/src/tusb.h
+++ b/src/tusb.h
@@ -88,6 +88,10 @@
     #include "class/msc/msc_device.h"
   #endif
 
+  #if CFG_TUD_MTP
+    #include "class/mtp/mtp_device.h"
+  #endif
+
   #if CFG_TUD_AUDIO
     #include "class/audio/audio_device.h"
   #endif
diff --git a/src/tusb_option.h b/src/tusb_option.h
index 29fdcb0d6..e36a56de9 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -493,6 +493,10 @@
   #define CFG_TUD_MSC             0
 #endif
 
+#ifndef CFG_TUD_MTP
+  #define CFG_TUD_MTP             0
+#endif
+
 #ifndef CFG_TUD_HID
   #define CFG_TUD_HID             0
 #endif