When writing/reading blocks via WRITE/READ_MULTIPLE_BLOCK we try to
directly pass this request down to the block layer. This can only be done
for properly sized and aligned accesses, other access still use a bounce
buffer but still benefit from copying as much data in one memcpy as
possible.

RPMB is limited to the slow path using a bounce buffer.

Signed-off-by: Christian Speich <c.speich@avm.de>
---
 hw/sd/sd.c | 220 ++++++++++++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 168 insertions(+), 52 deletions(-)

diff --git a/hw/sd/sd.c b/hw/sd/sd.c
index 2c81776df316feda75f97e15cc9bbd1538f1a21c..c9cfd3620b563fbe7520cfe361e14b57cd8f7472 100644
--- a/hw/sd/sd.c
+++ b/hw/sd/sd.c
@@ -1112,24 +1112,36 @@ static const VMStateDescription sd_vmstate = {
     },
 };
 
-static void sd_blk_read(SDState *sd, uint64_t addr, uint32_t len)
+static void sd_blk_read_direct(SDState *sd, void* buf, uint64_t addr,
+                               uint32_t len)
 {
     trace_sdcard_read_block(addr, len);
     addr += sd_part_offset(sd);
-    if (!sd->blk || blk_pread(sd->blk, addr, len, sd->data, 0) < 0) {
+    if (!sd->blk || blk_pread(sd->blk, addr, len, buf, 0) < 0) {
         fprintf(stderr, "sd_blk_read: read error on host side\n");
     }
 }
 
-static void sd_blk_write(SDState *sd, uint64_t addr, uint32_t len)
+static void sd_blk_read(SDState *sd, uint64_t addr, uint32_t len)
+{
+    sd_blk_read_direct(sd, sd->data, addr, len);
+}
+
+static void sd_blk_write_direct(SDState *sd, const void *buf, uint64_t addr,
+                                uint32_t len)
 {
     trace_sdcard_write_block(addr, len);
     addr += sd_part_offset(sd);
-    if (!sd->blk || blk_pwrite(sd->blk, addr, len, sd->data, 0) < 0) {
+    if (!sd->blk || blk_pwrite(sd->blk, addr, len, buf, 0) < 0) {
         fprintf(stderr, "sd_blk_write: write error on host side\n");
     }
 }
 
+static void sd_blk_write(SDState *sd, uint64_t addr, uint32_t len)
+{
+    sd_blk_write_direct(sd, sd->data, addr, len);
+}
+
 static bool rpmb_calc_hmac(SDState *sd, const RPMBDataFrame *frame,
                            unsigned int num_blocks, uint8_t *mac)
 {
@@ -2682,44 +2694,84 @@ static size_t sd_write_data(SDState *sd, const void *buf, size_t length)
         break;
 
     case 25:  /* CMD25:  WRITE_MULTIPLE_BLOCK */
-        /*
-         * Only read one byte at a time. We will be called again with the
-         * remaining.
-         */
-        length = 1;
-
-        if (sd->data_offset == 0) {
-            /* Start of the block - let's check the address is valid */
-            if (!address_in_range(sd, "WRITE_MULTIPLE_BLOCK",
-                                  sd->data_start, sd->blk_len)) {
-                break;
+        if (!address_in_range(sd, "WRITE_MULTIPLE_BLOCK",
+                            sd->data_start + sd->data_offset, length)) {
+            /* Limit writing data to our device size */
+            length = sd->size - sd->data_start - sd->data_offset;
+
+            /* We've read past the end, return a dummy write. */
+            if (length == 0) {
+                return 1;
             }
-            if (sd->size <= SDSC_MAX_CAPACITY) {
-                if (sd_wp_addr(sd, sd->data_start)) {
+        }
+
+        if (sd->size <= SDSC_MAX_CAPACITY) {
+            uint64_t start = sd->data_start + sd->data_offset;
+
+            /*
+             * Check if any covered address violates WP. If so, limit our write
+             * up to the allowed address.
+             */
+            for (uint64_t addr = start; addr < start + length;
+                 addr = ROUND_UP(addr + 1, WPGROUP_SIZE)) {
+                if (sd_wp_addr(sd, addr)) {
                     sd->card_status |= WP_VIOLATION;
+
+                    length = addr - start - 1;
                     break;
                 }
             }
         }
-        sd->data[sd->data_offset++] = value[0];
-        if (sd->data_offset >= sd->blk_len) {
-            /* TODO: Check CRC before committing */
-            sd->state = sd_programming_state;
-            partition_access = sd->ext_csd[EXT_CSD_PART_CONFIG]
-                    & EXT_CSD_PART_CONFIG_ACC_MASK;
-            if (partition_access == EXT_CSD_PART_CONFIG_ACC_RPMB) {
-                emmc_rpmb_blk_write(sd, sd->data_start, sd->data_offset);
-            } else {
-                sd_blk_write(sd, sd->data_start, sd->data_offset);
+
+        partition_access = sd->ext_csd[EXT_CSD_PART_CONFIG]
+                            & EXT_CSD_PART_CONFIG_ACC_MASK;
+
+        /* Partial write or RPMB (single block only for now) */
+        if (sd->data_offset > 0
+            || partition_access == EXT_CSD_PART_CONFIG_ACC_RPMB) {
+            length = MIN(sd->blk_len - sd->data_offset, length);
+
+            memcpy(sd->data + sd->data_offset, buf, length);
+            sd->data_offset += length;
+
+            if (sd->data_offset >= sd->blk_len) {
+                sd->state = sd_programming_state;
+                if (partition_access == EXT_CSD_PART_CONFIG_ACC_RPMB) {
+                    emmc_rpmb_blk_write(sd, sd->data_start, sd->data_offset);
+                } else {
+                    sd_blk_write(sd, sd->data_start, sd->data_offset);
+                }
+                sd->blk_written++;
+                sd->data_start += sd->blk_len;
+                sd->data_offset = 0;
+                sd->csd[14] |= 0x40;
+
+                /* Bzzzzzzztt .... Operation complete.  */
+                if (sd->multi_blk_cnt != 0) {
+                    if (--sd->multi_blk_cnt == 0) {
+                        /* Stop! */
+                        sd->state = sd_transfer_state;
+                        break;
+                    }
+                }
+
+                sd->state = sd_receivingdata_state;
             }
-            sd->blk_written++;
-            sd->data_start += sd->blk_len;
-            sd->data_offset = 0;
+        }
+        /* Try to write multiple of block sizes */
+        else if (length >= sd->blk_len) {
+            length = QEMU_ALIGN_DOWN(length, sd->blk_len);
+
+            sd->state = sd_programming_state;
+            sd_blk_write_direct(sd, buf, sd->data_start, length);
+            sd->blk_written += length / sd->blk_len;
+            sd->data_start += length;
             sd->csd[14] |= 0x40;
 
-            /* Bzzzzzzztt .... Operation complete.  */
             if (sd->multi_blk_cnt != 0) {
-                if (--sd->multi_blk_cnt == 0) {
+                sd->multi_blk_cnt -= length / sd->blk_len;
+
+                if (sd->multi_blk_cnt == 0) {
                     /* Stop! */
                     sd->state = sd_transfer_state;
                     break;
@@ -2728,6 +2780,12 @@ static size_t sd_write_data(SDState *sd, const void *buf, size_t length)
 
             sd->state = sd_receivingdata_state;
         }
+        /* Partial write */
+        else if (length > 0) {
+            memcpy(sd->data, buf, length);
+            sd->data_offset = length;
+        }
+
         break;
 
     case 26:  /* CMD26:  PROGRAM_CID */
@@ -2798,7 +2856,6 @@ static size_t sd_read_data(SDState *sd, void *buf, size_t length)
     const uint8_t dummy_byte = 0x00;
     unsigned int partition_access;
     uint32_t io_len;
-    uint8_t *value = buf;
 
     if (!sd->blk || !blk_is_inserted(sd->blk)) {
         memset(buf, dummy_byte, length);
@@ -2838,36 +2895,95 @@ static size_t sd_read_data(SDState *sd, void *buf, size_t length)
         break;
 
     case 18:  /* CMD18:  READ_MULTIPLE_BLOCK */
+        if (!address_in_range(sd, "READ_MULTIPLE_BLOCK",
+                              sd->data_start + sd->data_offset, length)) {
+            /* Limit reading data to our device size */
+            length = sd->size - sd->data_start - sd->data_offset;
+
+            /* We read past the end, return a dummy read. */
+            if (length == 0) {
+                memset(buf, dummy_byte, 1);
+                return 1;
+            }
+        }
+
+        partition_access = sd->ext_csd[EXT_CSD_PART_CONFIG]
+                & EXT_CSD_PART_CONFIG_ACC_MASK;
+
+        /* We have a partially read block. */
+        if (sd->data_offset > 0) {
+            length = MIN(sd->data_size - sd->data_offset, length);
+
+            memcpy(buf, sd->data + sd->data_offset, length);
+
+            sd->data_offset += length;
+
+            /* Partial read is complete, clear state. */
+            if (sd->data_offset >= sd->data_size) {
+                sd->data_start += io_len;
+                sd->data_size = 0;
+                sd->data_offset = 0;
+
+                if (sd->multi_blk_cnt != 0) {
+                    if (--sd->multi_blk_cnt == 0) {
+                        sd->state = sd_transfer_state;
+                    }
+                }
+            }
+        }
         /*
-         * We will only read one byte at a time. We will be called again with
-         * the remaining buffer.
+         * Try to read multiples of the block size directly bypassing the local
+         * bounce buffer.
+         * Not for RPMB.
          */
-        length = 1;
+        else if (length >= io_len
+                 && partition_access != EXT_CSD_PART_CONFIG_ACC_RPMB) {
+            length = QEMU_ALIGN_DOWN(length, io_len);
 
-        if (sd->data_offset == 0) {
-            if (!address_in_range(sd, "READ_MULTIPLE_BLOCK",
-                                  sd->data_start, io_len)) {
-                return dummy_byte;
+            /* For limited reads, only read the requested block count. */
+            if (sd->multi_blk_cnt != 0) {
+                length = MIN(length, sd->multi_blk_cnt * io_len);
             }
-            partition_access = sd->ext_csd[EXT_CSD_PART_CONFIG]
-                    & EXT_CSD_PART_CONFIG_ACC_MASK;
+
+            sd_blk_read_direct(sd, buf, sd->data_start,
+                               length);
+
+            sd->data_start += length;
+
+            if (sd->multi_blk_cnt != 0) {
+                sd->multi_blk_cnt -= length / io_len;
+
+                if (sd->multi_blk_cnt == 0) {
+                    sd->state = sd_transfer_state;
+                }
+            }
+        }
+        /* Read partial at the end or sinlge-block RPMB */
+        else if (length > 0) {
+            length = MIN(length, io_len);
+
+            /* Fill the buffer */
             if (partition_access == EXT_CSD_PART_CONFIG_ACC_RPMB) {
                 emmc_rpmb_blk_read(sd, sd->data_start, io_len);
             } else {
                 sd_blk_read(sd, sd->data_start, io_len);
             }
-        }
-        *value = sd->data[sd->data_offset++];
 
-        if (sd->data_offset >= io_len) {
-            sd->data_start += io_len;
-            sd->data_offset = 0;
+            memcpy(buf, sd->data, length);
 
-            if (sd->multi_blk_cnt != 0) {
-                if (--sd->multi_blk_cnt == 0) {
-                    /* Stop! */
-                    sd->state = sd_transfer_state;
-                    break;
+            sd->data_size = io_len;
+            sd->data_offset = length;
+
+            if (sd->data_offset >= io_len) {
+                sd->data_start += io_len;
+                sd->data_offset = 0;
+
+                if (sd->multi_blk_cnt != 0) {
+                    if (--sd->multi_blk_cnt == 0) {
+                        /* Stop! */
+                        sd->state = sd_transfer_state;
+                        break;
+                    }
                 }
             }
         }

-- 
2.43.0