unify callback argument. support multiple packet get object

3 files changed, 52 insertions, 40 deletions

diff --git a/src/class/mtp/mtp.h b/src/class/mtp/mtp.h
index 8e519bdc0..d4d81492e 100644
--- a/src/class/mtp/mtp.h
+++ b/src/class/mtp/mtp.h
@@ -657,9 +657,6 @@ typedef enum {
 //--------------------------------------------------------------------+
 // Data structures
 //--------------------------------------------------------------------+
-
-#define MTP_MAX_PACKET_SIZE 512
-
 typedef struct TU_ATTR_PACKED {
   uint32_t len;
   uint16_t type;
@@ -694,6 +691,7 @@ typedef struct {
     uint16_t* payload16;
     uint32_t* payload32;
   };
+  uint32_t payload_size;
 } mtp_container_info_t;
 
 #define mtp_string_t(_nchars) \
@@ -713,14 +711,6 @@ typedef struct {
 #define mtp_auint32_t(_count) mtp_array_t(uint32_t, _count)
 #define mtp_auint64_t(_count) mtp_array_t(uint64_t, _count)
 
-typedef union TU_ATTR_PACKED {
-  struct {
-    uint16_t physical; // physical location
-    uint16_t logical;  // logical within physical
-  };
-  uint32_t id;
-} mtp_storage_id_t;
-
 #define MTP_STORAGE_INFO_STRUCT(_storage_desc_chars, _volume_id_chars) \
   struct TU_ATTR_PACKED { \
     uint16_t storage_type; \
@@ -795,12 +785,25 @@ typedef struct TU_ATTR_PACKED {
 // return number of bytes added
 //--------------------------------------------------------------------+
 
+// return payload buffer for next write
+TU_ATTR_ALWAYS_INLINE static inline uint8_t* mtp_container_payload_next(mtp_container_info_t* p_container) {
+  // only 1st packet include header
+  uint32_t pos = p_container->header->len - sizeof(mtp_container_header_t);
+  while (pos > CFG_TUD_MTP_EP_BUFSIZE) {
+    pos -= CFG_TUD_MTP_EP_BUFSIZE;
+  }
+  return p_container->payload + pos;
+}
+
+// only add_raw does partial copy
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_raw(mtp_container_info_t* p_container, const void* data, uint32_t len) {
-  TU_ASSERT(p_container->header->len + len < sizeof(mtp_generic_container_t), 0);
-  uint8_t* buf = p_container->payload + p_container->header->len - sizeof(mtp_container_header_t);
-  memcpy(buf, data, len);
-  p_container->header->len += len;
-  return len;
+  uint8_t* buf = mtp_container_payload_next(p_container);
+  const uint32_t added_len = tu_min32(len, sizeof(mtp_generic_container_t) - p_container->header->len);
+  if (added_len > 0) {
+    memcpy(buf, data, added_len);
+  }
+  p_container->header->len += len; // always increase len, even partial copy
+  return added_len;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t mtp_container_add_array(mtp_container_info_t* p_container, uint8_t scalar_size, uint32_t count, const void* data) {
diff --git a/src/class/mtp/mtp_device.c b/src/class/mtp/mtp_device.c
index 71ead732d..92801a8ad 100644
--- a/src/class/mtp/mtp_device.c
+++ b/src/class/mtp/mtp_device.c
@@ -60,15 +60,12 @@ typedef struct {
   // 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
+  uint32_t total_len;
+  uint32_t xferred_len;
 
   uint32_t session_id;
   mtp_container_command_t command;
-  // mtp_container_header_t reply_header;
+  mtp_container_header_t reply_header;
 } mtpd_interface_t;
 
 typedef struct {
@@ -268,14 +265,23 @@ bool mtpd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
 
 bool tud_mtp_data_send(mtp_container_info_t* p_container) {
   mtpd_interface_t* p_mtp = &_mtpd_itf;
-  p_mtp->phase = MTP_PHASE_DATA;
-  p_mtp->total_len = p_container->header->len;
-  p_mtp->xferred_len = 0;
-  p_mtp->handled_len = 0;
-  p_mtp->xfer_completed = false;
+  if (p_mtp->phase == MTP_PHASE_COMMAND) {
+    // 1st data block: header + payload
+    p_mtp->phase = MTP_PHASE_DATA;
+    p_mtp->total_len = p_container->header->len;
+    p_mtp->xferred_len = 0;
+
+    p_container->header->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
+    p_container->header->transaction_id = p_mtp->command.header.transaction_id;
+    p_mtp->reply_header = *p_container->header; // save header for subsequent data
+  } else {
+    // subsequent data block: payload only
+    TU_ASSERT(p_mtp->phase == MTP_PHASE_DATA);
+  }
+
+  const uint16_t xact_len = tu_min32(p_mtp->total_len - p_mtp->xferred_len, CFG_TUD_MTP_EP_BUFSIZE);
+  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, (uint8_t *)(&_mtpd_epbuf.container), xact_len));
 
-  p_container->header->type = MTP_CONTAINER_TYPE_DATA_BLOCK;
-  TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, (uint8_t *)(&_mtpd_epbuf.container), (uint16_t)p_container->header->len));
   return true;
 }
 
@@ -283,6 +289,7 @@ bool tud_mtp_response_send(mtp_container_info_t* p_container) {
   mtpd_interface_t* p_mtp = &_mtpd_itf;
   p_mtp->phase = MTP_PHASE_RESPONSE_QUEUED;
   p_container->header->type = MTP_CONTAINER_TYPE_RESPONSE_BLOCK;
+  p_container->header->transaction_id = p_mtp->command.header.transaction_id;
   TU_ASSERT(usbd_edpt_xfer(p_mtp->rhport, p_mtp->ep_in, (uint8_t *)(&_mtpd_epbuf.container), (uint16_t)p_container->header->len));
   return true;
 }
@@ -304,7 +311,9 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
   cb_data.command = &p_mtp->command;
   cb_data.reply.header = (mtp_container_header_t*) p_container;
   cb_data.reply.payload32 = p_container->data;
-  cb_data.offset = 0;
+  cb_data.reply.payload_size = CFG_TUD_MTP_EP_BUFSIZE - sizeof(mtp_container_header_t);
+  cb_data.xferred_bytes = 0;
+  cb_data.xfer_result = event;
 
   switch (p_mtp->phase) {
     case MTP_PHASE_IDLE:
@@ -322,6 +331,7 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
     case MTP_PHASE_DATA: {
       const uint16_t bulk_mps = (tud_speed_get() == TUSB_SPEED_HIGH) ? 512 : 64;
       p_mtp->xferred_len += xferred_bytes;
+      cb_data.xferred_bytes = p_mtp->xferred_len;
 
       // transfer complete if ZLP or short packet or overflow
       if (xferred_bytes == 0 || // ZLP
@@ -329,7 +339,11 @@ bool mtpd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
           p_mtp->xferred_len > p_mtp->total_len) {
         tud_mtp_data_complete_cb(&cb_data);
       } else {
-        TU_ASSERT(false);
+        // payload only packet
+        cb_data.reply.header = &p_mtp->reply_header;
+        cb_data.reply.payload = (uint8_t*) p_container;
+        cb_data.reply.payload_size = CFG_TUD_MTP_EP_BUFSIZE;
+        tud_mtp_data_more_cb(&cb_data);
       }
       break;
     }
diff --git a/src/class/mtp/mtp_device.h b/src/class/mtp/mtp_device.h
index 0524edcf6..88960b1aa 100644
--- a/src/class/mtp/mtp_device.h
+++ b/src/class/mtp/mtp_device.h
@@ -41,15 +41,6 @@ typedef struct {
   const mtp_container_command_t* command;
   mtp_container_info_t reply;
 
-  union {
-    uint8_t* buffer;
-    uint16_t* buffer16;
-    uint32_t* buffer32;
-  };
-  uint32_t bufsize;
-  uint32_t offset; // offset from start of header, since data can span multiple xfers
-
-
   tusb_xfer_result_t xfer_result;
   uint32_t xferred_bytes;
 } tud_mtp_cb_data_t;
@@ -117,9 +108,13 @@ bool tud_mtp_response_send(mtp_container_info_t* p_container);
 //--------------------------------------------------------------------+
 
 // Invoked when new command is received. Application fill the out_block with either DATA or RESPONSE container
+// and call tud_mtp_data_send() or tud_mtp_response_send().
 // return MTP response code
 int32_t tud_mtp_command_received_cb(tud_mtp_cb_data_t * cb_data);
 
+// Invoked when a data packet is received/sent, and more data is expected
+int32_t tud_mtp_data_more_cb(tud_mtp_cb_data_t* cb_data);
+
 // Invoked when data phase is complete
 int32_t tud_mtp_data_complete_cb(tud_mtp_cb_data_t* cb_data);