rework get storageIDs and get storage info

5 files changed, 87 insertions, 192 deletions

diff --git a/examples/device/mtp/src/mtp_fs_example.c b/examples/device/mtp/src/mtp_fs_example.c
index 68140b1ae..fcd01f54a 100644
--- a/examples/device/mtp/src/mtp_fs_example.c
+++ b/examples/device/mtp/src/mtp_fs_example.c
@@ -34,11 +34,10 @@
 //--------------------------------------------------------------------+
 
 // device info string (including terminating null)
-const uint16_t dev_info_manufacturer[] = { 'T', 'i', 'n', 'y', 'U', 'S', 'B', 0 };
-const uint16_t dev_info_model[]        = { 'M', 'T', 'P', ' ', 'E', 'x', 'a', 'm', 'p', 'l', 'e', 0 };
-const uint16_t dev_info_version[]      = { '1', '.', '0', 0 };
-const uint16_t dev_info_serial[]       = { '1', '2', '3', '4', '5', '6', 0 };
-
+static const uint16_t dev_info_manufacturer[] = { 'T', 'i', 'n', 'y', 'U', 'S', 'B', 0 };
+static const uint16_t dev_info_model[]        = { 'M', 'T', 'P', ' ', 'E', 'x', 'a', 'm', 'p', 'l', 'e', 0 };
+static const uint16_t dev_info_version[]      = { '1', '.', '0', 0 };
+static const uint16_t dev_info_serial[]       = { '1', '2', '3', '4', '5', '6', 0 };
 
 static const uint16_t supported_operations[] = {
   MTP_OP_GET_DEVICE_INFO,
@@ -80,7 +79,6 @@ static const uint16_t playback_formats[] = {
   MTP_OBJ_FORMAT_TEXT,
 };
 
-
 //--------------------------------------------------------------------+
 // RAM FILESYSTEM
 //--------------------------------------------------------------------+
@@ -116,6 +114,32 @@ static fs_object_info_t _fs_objects[FS_MAX_NODES] = {
   }
 };
 
+//------------- Storage Info -------------//
+#define STORAGE_DESCRIPTRION { 'd', 'i', 's', 'k', 0 }
+#define VOLUME_IDENTIFIER { 'v', 'o', 'l', 0 }
+
+typedef MTP_STORAGE_INFO_TYPEDEF(TU_ARRAY_SIZE((uint16_t[]) STORAGE_DESCRIPTRION),
+                                 TU_ARRAY_SIZE(((uint16_t[])VOLUME_IDENTIFIER))
+) storage_info_t;
+
+storage_info_t storage_info = {
+  .storage_type = MTP_STORAGE_TYPE_FIXED_RAM,
+  .filesystem_type = MTP_FILESYSTEM_TYPE_GENERIC_HIERARCHICAL,
+  .access_capability = MTP_ACCESS_CAPABILITY_READ_WRITE,
+  .max_capacity_in_bytes = FS_MAX_NODES * FS_MAX_NODE_BYTES,
+  .free_space_in_bytes = FS_MAX_NODES * FS_MAX_NODE_BYTES,
+  .free_space_in_objects = FS_MAX_NODES,
+  .storage_description = {
+    .count = (TU_FIELD_SZIE(storage_info_t, storage_description)-1) / sizeof(uint16_t),
+    .utf16 = STORAGE_DESCRIPTRION
+  },
+  .volume_identifier = {
+    .count = (TU_FIELD_SZIE(storage_info_t, volume_identifier)-1) / sizeof(uint16_t),
+    .utf16 = VOLUME_IDENTIFIER
+  }
+};
+
+
 //--------------------------------------------------------------------+
 // OPERATING STATUS
 //--------------------------------------------------------------------+
@@ -185,7 +209,7 @@ int32_t tud_mtp_command_received_cb(uint8_t idx, mtp_generic_container_t* cmd_bl
   (void)idx;
   switch (cmd_block->code) {
     case MTP_OP_GET_DEVICE_INFO: {
-      // Device info is already prepared up to playback formats. Application need to add string fields
+      // Device info is already prepared up to playback formats. Application only need to add string fields
       mtp_container_add_string(out_block, TU_ARRAY_SIZE(dev_info_manufacturer), dev_info_manufacturer);
       mtp_container_add_string(out_block, TU_ARRAY_SIZE(dev_info_model), dev_info_model);
       mtp_container_add_string(out_block, TU_ARRAY_SIZE(dev_info_version), dev_info_version);
@@ -196,14 +220,26 @@ int32_t tud_mtp_command_received_cb(uint8_t idx, mtp_generic_container_t* cmd_bl
     }
 
     case MTP_OP_OPEN_SESSION:
-      out_block->len = MTP_CONTAINER_HEADER_LENGTH;
-      out_block->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
-      // TODO check if session is already opened
       out_block->code = MTP_RESP_OK;
-
       tud_mtp_response_send(out_block);
       break;
 
+    case MTP_OP_GET_STORAGE_IDS: {
+      uint32_t storage_ids [] = { 0x00010001u }; // physical = 1, logical = 1
+      mtp_container_add_auint32(out_block, 1, storage_ids);
+      tud_mtp_data_send(out_block);
+      break;
+    }
+
+    case MTP_OP_GET_STORAGE_INFO: {
+      // update storage info with current free space
+      storage_info.free_space_in_objects = FS_MAX_NODES - fs_get_object_count();
+      storage_info.free_space_in_bytes = storage_info.free_space_in_objects * FS_MAX_NODE_BYTES;
+      mtp_container_add_raw(out_block, &storage_info, sizeof(storage_info));
+      tud_mtp_data_send(out_block);
+      break;
+    }
+
     default: return -1;
   }
 
@@ -213,21 +249,6 @@ int32_t tud_mtp_command_received_cb(uint8_t idx, mtp_generic_container_t* cmd_bl
 //--------------------------------------------------------------------+
 // API
 //--------------------------------------------------------------------+
-mtp_response_t tud_mtp_storage_open_session(uint32_t* session_id) {
-  if (*session_id == 0) {
-    TU_LOG1("Invalid session ID\r\n");
-    return MTP_RESP_INVALID_PARAMETER;
-  }
-  if (_fs_operation.session_id != 0) {
-    *session_id = _fs_operation.session_id;
-    TU_LOG1("ERR: Session %ld already open\r\n", _fs_operation.session_id);
-    return MTP_RESP_SESSION_ALREADY_OPEN;
-  }
-  _fs_operation.session_id = *session_id;
-  TU_LOG1("Open session with id %ld\r\n", _fs_operation.session_id);
-  return MTP_RESP_OK;
-}
-
 mtp_response_t tud_mtp_storage_close_session(uint32_t session_id) {
   if (session_id != _fs_operation.session_id) {
     TU_LOG1("ERR: Session %ld not open\r\n", session_id);
@@ -238,36 +259,6 @@ mtp_response_t tud_mtp_storage_close_session(uint32_t session_id) {
   return MTP_RESP_OK;
 }
 
-mtp_response_t tud_mtp_get_storage_id(uint32_t* storage_id) {
-  if (_fs_operation.session_id == 0) {
-    TU_LOG1("ERR: Session not open\r\n");
-    return MTP_RESP_SESSION_NOT_OPEN;
-  }
-  *storage_id = STORAGE_ID(0x0001, 0x0001);
-  TU_LOG1("Retrieved storage identifier %ld\r\n", *storage_id);
-  return MTP_RESP_OK;
-}
-
-mtp_response_t tud_mtp_get_storage_info(uint32_t storage_id, mtp_storage_info_t* info) {
-  if (_fs_operation.session_id == 0) {
-    TU_LOG1("ERR: Session not open\r\n");
-    return MTP_RESP_SESSION_NOT_OPEN;
-  }
-  if (storage_id != STORAGE_ID(0x0001, 0x0001)) {
-    TU_LOG1("ERR: Unexpected storage id %ld\r\n", storage_id);
-    return MTP_RESP_INVALID_STORAGE_ID;
-  }
-  info->storage_type = MTP_STORAGE_TYPE_FIXED_RAM;
-  info->filesystem_type = MTP_FILESYSTEM_TYPE_GENERIC_HIERARCHICAL;
-  info->access_capability = MTP_ACCESS_CAPABILITY_READ_WRITE;
-  info->max_capacity_in_bytes = FS_MAX_NODES * FS_MAX_NODE_BYTES;
-  info->free_space_in_objects = FS_MAX_NODES - fs_get_object_count();
-  info->free_space_in_bytes = info->free_space_in_objects * FS_MAX_NODE_BYTES;
-  mtpd_gct_append_wstring(MTPD_STORAGE_DESCRIPTION);
-  mtpd_gct_append_wstring(MTPD_VOLUME_IDENTIFIER);
-  return MTP_RESP_OK;
-}
-
 mtp_response_t tud_mtp_storage_format(uint32_t storage_id) {
   if (_fs_operation.session_id == 0) {
     TU_LOG1("ERR: Session not open\r\n");
diff --git a/src/class/mtp/mtp.h b/src/class/mtp/mtp.h
index d61f329c4..a28ce40df 100644
--- a/src/class/mtp/mtp.h
+++ b/src/class/mtp/mtp.h
@@ -738,7 +738,16 @@ typedef struct TU_ATTR_PACKED {
   uint16_t utf16[];
 } mtp_flexible_string_t;
 
-// StorageInfo dataset
+ typedef union TU_ATTR_PACKED {
+  struct {
+    uint16_t physical; // physical location
+    uint16_t logical;  // logical within physical
+  };
+
+  uint32_t id;
+} mtp_storage_id_t;
+
+// StorageInfo dataset (excluding storage description and volume identifier)
 typedef struct TU_ATTR_PACKED {
   uint16_t storage_type;
   uint16_t filesystem_type;
@@ -746,10 +755,20 @@ typedef struct TU_ATTR_PACKED {
   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
+  // storage description and volume identifier are added dynamically
+} mtp_storage_info_nostring_t;
+
+#define MTP_STORAGE_INFO_TYPEDEF(_storage_desc_chars, _volume_id_chars) \
+  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_string_t(_storage_desc_chars) storage_description; \
+    mtp_string_t(_volume_id_chars) volume_identifier; \
+  }
 
 // ObjectInfo Dataset
 typedef struct TU_ATTR_PACKED {
@@ -807,48 +826,27 @@ typedef struct TU_ATTR_PACKED {
 // Generic Container function
 //--------------------------------------------------------------------+
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add(mtp_generic_container_t* p_container, mtp_data_type_t type, const void* data) {
-  TU_VERIFY(type != MTP_DATA_TYPE_UNDEFINED, 0);
-  uint8_t scalar_size; // size of single scalar
-  uint8_t count_width; // size of count field (0, 1 or 4 bytes)
-
-  if (type == MTP_DATA_TYPE_STR) {
-    scalar_size = 2;
-    count_width = 1;
-  } else {
-    uint8_t scalar_type = type & 0x3F;
-    count_width = (type & 0x4000u) ? 4 : 0;
-    scalar_size = 1u << ((scalar_type - 1u) >> 1);
-  }
-
-  uint32_t data_len;
-  if (count_width) {
-    const uint32_t count = *(const uint32_t*) data;
-    data_len = count_width + count*scalar_size;
-  } else {
-    data_len = scalar_size;
-  }
-
-  memcpy(((uint8_t*)p_container) + p_container->len, data, data_len);
-  p_container->len += data_len;
-
-  return data_len;
+TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_raw(mtp_generic_container_t* p_container, const void* data, uint32_t len) {
+  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_field(mtp_generic_container_t* p_container, uint8_t scalar_size, uint32_t count, const void* data) {
-  const uint32_t prev_len = p_container->len;
-  uint8_t* container8 = (uint8_t*) p_container;
   if (count == 0) {
     // count = 0 means scalar
-    memcpy(container8 + p_container->len, data, scalar_size);
-    p_container->len += scalar_size;
+    return mtp_container_add_raw(p_container, data, scalar_size);
   } else {
+    uint8_t* container8 = (uint8_t*) p_container;
+
     tu_unaligned_write32(container8 + p_container->len, count);
     p_container->len += 4;
+
     memcpy(container8 + p_container->len, data, count * scalar_size);
-  }
+    p_container->len += count * scalar_size;
 
-  return p_container->len - prev_len;
+    return 4 + count * scalar_size;
+  }
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_string(mtp_generic_container_t* p_container, uint8_t count, uint16_t* utf16) {
diff --git a/src/class/mtp/mtp_device.c b/src/class/mtp/mtp_device.c
index b64f36f2e..857fafd0c 100644
--- a/src/class/mtp/mtp_device.c
+++ b/src/class/mtp/mtp_device.c
@@ -84,11 +84,7 @@ static mtp_phase_type_t mtpd_chk_session_open(const char *func_name);
 // MTP commands
 static mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp);
 static mtp_phase_type_t mtpd_handle_data(void);
-static mtp_phase_type_t mtpd_handle_cmd_get_device_info(void);
-static mtp_phase_type_t mtpd_handle_cmd_open_session(void);
 static mtp_phase_type_t mtpd_handle_cmd_close_session(void);
-static mtp_phase_type_t mtpd_handle_cmd_get_storage_info(void);
-static mtp_phase_type_t mtpd_handle_cmd_get_storage_ids(void);
 static mtp_phase_type_t mtpd_handle_cmd_get_object_handles(void);
 static mtp_phase_type_t mtpd_handle_cmd_get_object_info(void);
 static mtp_phase_type_t mtpd_handle_cmd_get_object(void);
@@ -390,7 +386,7 @@ mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp) {
   mtp_generic_container_t cmd_block; // copy command block for callback
   memcpy(&cmd_block, p_container, p_container->len);
   memcpy(&p_mtp->cmd_header, p_container, sizeof(mtp_container_header_t));
-  // p_container->len = MTP_CONTAINER_HEADER_LENGTH; // default data/response length
+  p_container->len = MTP_CONTAINER_HEADER_LENGTH; // default data/response length
 
   if (p_container->code != MTP_OP_SEND_OBJECT) {
     _mtpd_soi.object_handle = 0;
@@ -453,11 +449,12 @@ mtp_phase_type_t mtpd_handle_cmd(mtpd_interface_t* p_mtp) {
 
     case MTP_OP_GET_STORAGE_IDS:
       TU_LOG_DRV("  MTP command: MTP_OP_GET_STORAGE_IDS\n");
-      return mtpd_handle_cmd_get_storage_ids();
+      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]);
-      return mtpd_handle_cmd_get_storage_info();
+      break;
+
     case MTP_OP_GET_OBJECT_HANDLES:
       TU_LOG_DRV("  MTP command: MTP_OP_GET_OBJECT_HANDLES\n");
       return mtpd_handle_cmd_get_object_handles();
@@ -517,34 +514,6 @@ mtp_phase_type_t mtpd_handle_data(void)
   return true;
 }
 
-mtp_phase_type_t mtpd_handle_cmd_open_session(void)
-{
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  uint32_t session_id = p_container->data[0];
-
-  mtp_response_t res = tud_mtp_storage_open_session(&session_id);
-  if (res == MTP_RESP_SESSION_ALREADY_OPEN)
-  {
-    p_container->len = MTP_CONTAINER_HEADER_LENGTH;
-    p_container->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
-    p_container->code = res;
-    p_container->len += sizeof(p_container->data[0]);
-    p_container->data[0] = session_id;
-    _mtpd_itf.session_id = session_id;
-    return MTP_PHASE_RESPONSE;
-  }
-
-  mtp_phase_type_t phase;
-  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESP_OK), res, "")) != MTP_PHASE_NONE) return phase;
-
-  _mtpd_itf.session_id = session_id;
-
-  p_container->len = MTP_CONTAINER_HEADER_LENGTH;
-  p_container->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
-  p_container->code = MTP_RESP_OK;
-
-  return MTP_PHASE_RESPONSE;
-}
 
 mtp_phase_type_t mtpd_handle_cmd_close_session(void)
 {
@@ -562,56 +531,6 @@ mtp_phase_type_t mtpd_handle_cmd_close_session(void)
   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");
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-
-  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_RESP_OK), res, "")) != MTP_PHASE_NONE) return phase;
-
-  p_container->len = MTP_CONTAINER_HEADER_LENGTH + sizeof(mtp_storage_ids_t);
-  p_container->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
-  p_container->code = MTP_OP_GET_STORAGE_IDS;
-  mtp_storage_ids_t *d = (mtp_storage_ids_t *)p_container->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 = p_container->len;
-  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");
-  mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
-  uint32_t storage_id = p_container->data[0];
-
-  p_container->len = MTP_CONTAINER_HEADER_LENGTH + sizeof(mtp_storage_info_t);
-  p_container->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
-  p_container->code = MTP_OP_GET_STORAGE_INFO;
-
-  mtp_response_t res = tud_mtp_get_storage_info(storage_id, (mtp_storage_info_t *)p_container->data);
-  mtp_phase_type_t phase;
-  if ((phase = mtpd_chk_generic(__func__, (res != MTP_RESP_OK), res, "")) != MTP_PHASE_NONE) {
-    return phase;
-  }
-
-  _mtpd_itf.queued_len = p_container->len;
-  return MTP_PHASE_DATA_IN;
-}
-
 mtp_phase_type_t mtpd_handle_cmd_get_object_handles(void)
 {
   mtp_generic_container_t* p_container = &_mtpd_epbuf.container;
diff --git a/src/class/mtp/mtp_device_storage.h b/src/class/mtp/mtp_device_storage.h
index a9bbc9b90..6c12d38e3 100644
--- a/src/class/mtp/mtp_device_storage.h
+++ b/src/class/mtp/mtp_device_storage.h
@@ -59,24 +59,10 @@
 //
 // 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);
 
diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index e35d3e6fe..76764bbba 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -35,6 +35,7 @@
 // Macros Helper
 //--------------------------------------------------------------------+
 #define TU_ARRAY_SIZE(_arr)   ( sizeof(_arr) / sizeof(_arr[0]) )
+#define TU_FIELD_SZIE(_type, _field)  (sizeof(((_type *)0)->_field))
 #define TU_MIN(_x, _y)        ( ( (_x) < (_y) ) ? (_x) : (_y) )
 #define TU_MAX(_x, _y)        ( ( (_x) > (_y) ) ? (_x) : (_y) )
 #define TU_DIV_CEIL(n, d)     (((n) + (d) - 1) / (d))