try to add logo png to mtp example

2 files changed, 93 insertions, 63 deletions

diff --git a/src/class/mtp/mtp.h b/src/class/mtp/mtp.h
index c844b08ab..c44b99fda 100644
--- a/src/class/mtp/mtp.h
+++ b/src/class/mtp/mtp.h
@@ -470,7 +470,7 @@ typedef enum {
   MTP_OP_DELETE_OBJECT              = 0x100Bu,
   MTP_OP_SEND_OBJECT_INFO           = 0x100Cu,
   MTP_OP_SEND_OBJECT                = 0x100Du,
-  MTP_OP_INITIAL_CAPTURE            = 0x100Eu,
+  MTP_OP_INITIATE_CAPTURE           = 0x100Eu,
   MTP_OP_FORMAT_STORE               = 0x100Fu,
   MTP_OP_RESET_DEVICE               = 0x1010u,
   MTP_OP_SELF_TEST                  = 0x1011u,
@@ -734,6 +734,10 @@ typedef struct TU_ATTR_PACKED {
   uint16_t association_type;
   uint32_t association_desc;
   uint32_t sequence_number;
+  // mtp_string_t() filename
+  // mtp_string_t() date_created
+  // mtp_string_t() date_modified
+  // mtp_string_t() keywords
 } mtp_object_info_header_t;
 
 // Device property desc up to get/set
@@ -775,12 +779,15 @@ typedef struct TU_ATTR_PACKED {
 //--------------------------------------------------------------------+
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_raw(mtp_generic_container_t* p_container, const void* data, uint32_t len) {
+  TU_ASSERT(p_container->len + len < sizeof(mtp_generic_container_t), 0);
   memcpy((uint8_t*) p_container + p_container->len, data, len);
   p_container->len += len;
   return len;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_array(mtp_generic_container_t* p_container, uint8_t scalar_size, uint32_t count, const void* data) {
+  const uint32_t added_len = 4 + count * scalar_size;
+  TU_ASSERT(p_container->len + added_len < sizeof(mtp_generic_container_t), 0);
   uint8_t* container8 = (uint8_t*)p_container;
 
   tu_unaligned_write32(container8 + p_container->len, count);
@@ -789,32 +796,43 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_array(mtp_generic
   memcpy(container8 + p_container->len, data, count * scalar_size);
   p_container->len += count * scalar_size;
 
-  return 4 + count * scalar_size;
+  return added_len;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_string(mtp_generic_container_t* p_container, uint8_t count, uint16_t* utf16) {
+  const uint32_t added_len = 1 + 2 * count;
+  TU_ASSERT(p_container->len + added_len < sizeof(mtp_generic_container_t), 0);
   uint8_t* container8 = (uint8_t*) p_container;
+
   container8[p_container->len] = count;
   p_container->len++;
 
   memcpy(container8 + p_container->len, utf16, 2 * count);
   p_container->len += 2 * count;
 
-  return 1 + 2 * count;
+  return added_len;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_cstring(mtp_generic_container_t* p_container, const char* str) {
-  uint8_t* container8 = (uint8_t*) p_container;
   const uint8_t len = (uint8_t) (strlen(str) + 1); // include null
+  TU_ASSERT(p_container->len + 1 + 2 * len < sizeof(mtp_generic_container_t), 0);
+
+  uint8_t* container8 = (uint8_t*) p_container;
   container8[p_container->len] = len;
   p_container->len++;
 
-  for (uint8_t i = 0; i < len; i++) {
-    container8[p_container->len] = str[i];
-    container8[p_container->len + 1] = 0;
-    p_container->len += 2;
+  if (len == 1) {
+    // empty string (null only)
+    container8[p_container->len] = 0;
+    return 1;
+  } else {
+    for (uint8_t i = 0; i < len; i++) {
+      container8[p_container->len] = str[i];
+      container8[p_container->len + 1] = 0;
+      p_container->len += 2;
+    }
+    return 1 + 2 * len;
   }
-  return 1 + 2 * len;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_uint8(mtp_generic_container_t* p_container, uint8_t data) {
diff --git a/src/class/mtp/mtp_device.c b/src/class/mtp/mtp_device.c
index ca594158e..91df13111 100644
--- a/src/class/mtp/mtp_device.c
+++ b/src/class/mtp/mtp_device.c
@@ -103,6 +103,65 @@ CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN static char _mtp_datestr[20];
 
 
 //--------------------------------------------------------------------+
+// Debug
+//--------------------------------------------------------------------+
+#if CFG_TUSB_DEBUG >= CFG_TUD_MTP_LOG_LEVEL
+
+TU_ATTR_UNUSED static tu_lookup_entry_t const _mpt_op_lookup[] = {
+{.key = MTP_OP_UNDEFINED                    , .data = "Undefined"                 } ,
+{.key = MTP_OP_GET_DEVICE_INFO              , .data = "GetDeviceInfo"             } ,
+{.key = MTP_OP_OPEN_SESSION                 , .data = "OpenSession"               } ,
+{.key = MTP_OP_CLOSE_SESSION                , .data = "CloseSession"              } ,
+{.key = MTP_OP_GET_STORAGE_IDS              , .data = "GetStorageIDs"             } ,
+{.key = MTP_OP_GET_STORAGE_INFO             , .data = "GetStorageInfo"            } ,
+{.key = MTP_OP_GET_NUM_OBJECTS              , .data = "GetNumObjects"             } ,
+{.key = MTP_OP_GET_OBJECT_HANDLES           , .data = "GetObjectHandles"          } ,
+{.key = MTP_OP_GET_OBJECT_INFO              , .data = "GetObjectInfo"             } ,
+{.key = MTP_OP_GET_OBJECT                   , .data = "GetObject"                 } ,
+{.key = MTP_OP_GET_THUMB                    , .data = "GetThumb"                  } ,
+{.key = MTP_OP_DELETE_OBJECT                , .data = "DeleteObject"              } ,
+{.key = MTP_OP_SEND_OBJECT_INFO             , .data = "SendObjectInfo"            } ,
+{.key = MTP_OP_SEND_OBJECT                  , .data = "SendObject"                } ,
+{.key = MTP_OP_INITIATE_CAPTURE             , .data = "InitiateCapture"           } ,
+{.key = MTP_OP_FORMAT_STORE                 , .data = "FormatStore"               } ,
+{.key = MTP_OP_RESET_DEVICE                 , .data = "ResetDevice"               } ,
+{.key = MTP_OP_SELF_TEST                    , .data = "SelfTest"                  } ,
+{.key = MTP_OP_SET_OBJECT_PROTECTION        , .data = "SetObjectProtection"       } ,
+{.key = MTP_OP_POWER_DOWN                   , .data = "PowerDown"                 } ,
+{.key = MTP_OP_GET_DEVICE_PROP_DESC         , .data = "GetDevicePropDesc"         } ,
+{.key = MTP_OP_GET_DEVICE_PROP_VALUE        , .data = "GetDevicePropValue"        } ,
+{.key = MTP_OP_SET_DEVICE_PROP_VALUE        , .data = "SetDevicePropValue"        } ,
+{.key = MTP_OP_RESET_DEVICE_PROP_VALUE      , .data = "ResetDevicePropValue"      } ,
+{.key = MTP_OP_TERMINATE_OPEN_CAPTURE       , .data = "TerminateOpenCapture"      } ,
+{.key = MTP_OP_MOVE_OBJECT                  , .data = "MoveObject"                } ,
+{.key = MTP_OP_COPY_OBJECT                  , .data = "CopyObject"                } ,
+{.key = MTP_OP_GET_PARTIAL_OBJECT           , .data = "GetPartialObject"          } ,
+{.key = MTP_OP_INITIATE_OPEN_CAPTURE        , .data = "InitiateOpenCapture"       } ,
+{.key = MTP_OP_GET_OBJECT_PROPS_SUPPORTED   , .data = "GetObjectPropsSupported"   } ,
+{.key = MTP_OP_GET_OBJECT_PROP_DESC         , .data = "GetObjectPropDesc"         } ,
+{.key = MTP_OP_GET_OBJECT_PROP_VALUE        , .data = "GetObjectPropValue"        } ,
+{.key = MTP_OP_SET_OBJECT_PROP_VALUE        , .data = "SetObjectPropValue"        } ,
+{.key = MTP_OP_GET_OBJECT_PROPLIST          , .data = "GetObjectPropList"         } ,
+{.key = MTP_OP_GET_OBJECT_PROP_REFERENCES   , .data = "GetObjectPropReferences"   } ,
+{.key = MTP_OP_GET_SERVICE_IDS              , .data = "GetServiceIDs"             } ,
+{.key = MTP_OP_GET_SERVICE_INFO             , .data = "GetServiceInfo"            } ,
+{.key = MTP_OP_GET_SERVICE_CAPABILITIES     , .data = "GetServiceCapabilities"    } ,
+{.key = MTP_OP_GET_SERVICE_PROP_DESC        , .data = "GetServicePropDesc"        } ,
+{.key = MTP_OP_GET_OBJECT_PROP_LIST         , .data = "GetObjectPropList"         } ,
+{.key = MTP_OP_SET_OBJECT_PROP_LIST         , .data = "SetObjectPropList"         } ,
+{.key = MTP_OP_GET_INTERDEPENDENT_PROP_DESC , .data = "GetInterdependentPropDesc" } ,
+{.key = MTP_OP_SEND_OBJECT_PROP_LIST        , .data = "SendObjectPropList"        }
+};
+
+TU_ATTR_UNUSED static tu_lookup_table_t const _mtp_op_table = {
+  .count = TU_ARRAY_SIZE(_mpt_op_lookup),
+  .items = _mpt_op_lookup
+};
+
+#endif
+
+
+//--------------------------------------------------------------------+
 // Helper
 //--------------------------------------------------------------------+
 
@@ -382,11 +441,12 @@ mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp) {
     _mtpd_soi.object_handle = 0;
   }
 
-  mtp_phase_type_t ret = MTP_PHASE_RESPONSE;
+  tu_lookup_find(&_mtp_op_table, cmd_block.code);
+  TU_LOG_DRV("  MTP command: %s\r\n", (char const*) tu_lookup_find(&_mtp_op_table, cmd_block.code));
 
-  switch (p_container->code) {
+  mtp_phase_type_t ret = MTP_PHASE_RESPONSE;
+  switch (cmd_block.code) {
     case MTP_OP_GET_DEVICE_INFO: {
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_DEVICE_INFO\n");
       tud_mtp_device_info_t dev_info = {
         .standard_version = 100,
         .mtp_vendor_extension_id = 6, // MTP specs say 0xFFFFFFFF but libMTP check for value 6
@@ -435,63 +495,14 @@ mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp) {
       break;
     }
 
-    case MTP_OP_OPEN_SESSION:
-      TU_LOG_DRV("  MTP command: MTP_OP_OPEN_SESSION\n");
-      break;
-
-    case MTP_OP_CLOSE_SESSION:
-      TU_LOG_DRV("  MTP command: MTP_OP_CLOSE_SESSION\n");
-      break;
-
-    case MTP_OP_GET_STORAGE_IDS:
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_STORAGE_IDS\n");
-      break;
-
-    case MTP_OP_GET_STORAGE_INFO:
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_STORAGE_INFO for ID=%lu\n", p_container->data[0]);
-      break;
-
-    case MTP_OP_GET_OBJECT_HANDLES:
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_OBJECT_HANDLES\n");
-      break;
-
-    case MTP_OP_GET_OBJECT_INFO:
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_OBJECT_INFO\n");
-      break;
-
-    case MTP_OP_GET_OBJECT:
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_OBJECT\n");
-      break;
-
-    case MTP_OP_DELETE_OBJECT:
-      TU_LOG_DRV("  MTP command: MTP_OP_DELETE_OBJECT\n");
-      return mtpd_handle_cmd_delete_object();
-
-    case MTP_OP_GET_DEVICE_PROP_DESC:
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_DEVICE_PROP_DESC\n");
-      break;
-
-    case MTP_OP_GET_DEVICE_PROP_VALUE:
-      TU_LOG_DRV("  MTP command: MTP_OP_GET_DEVICE_PROP_VALUE\n");
-      break;
-
-    case MTP_OP_SEND_OBJECT_INFO:
-      TU_LOG_DRV("  MTP command: MTP_OP_SEND_OBJECT_INFO\n");
-      return mtpd_handle_cmd_send_object_info();
-    case MTP_OP_SEND_OBJECT:
-      TU_LOG_DRV("  MTP command: MTP_OP_SEND_OBJECT\n");
-      return mtpd_handle_cmd_send_object();
-    case MTP_OP_FORMAT_STORE:
-      TU_LOG_DRV("  MTP command: MTP_OP_FORMAT_STORE\n");
-      return mtpd_handle_cmd_format_store();
     default:
-      TU_LOG_DRV("  MTP command: MTP_OP_UNKNOWN_COMMAND %x!!!!\n", p_container->code);
-      return false;
+      break;
   }
 
   tud_mtp_command_received_cb(0, &cmd_block, p_container);
   return ret;
 }
+#if 0
 
 mtp_phase_type_t mtpd_handle_data(void)
 {
@@ -614,6 +625,7 @@ mtp_phase_type_t mtpd_handle_cmd_format_store(void)
   p_container->len = MTP_CONTAINER_HEADER_LENGTH;
   return MTP_PHASE_RESPONSE;
 }
+#endif
 
 //--------------------------------------------------------------------+
 // Checker