1 files changed, 25 insertions, 6 deletions

diff --git a/firmware/target/arm/as3525/sd-as3525v2.c b/firmware/target/arm/as3525/sd-as3525v2.c
index 386b76e758..238cd7a5eb 100644
--- a/firmware/target/arm/as3525/sd-as3525v2.c
+++ b/firmware/target/arm/as3525/sd-as3525v2.c
@@ -789,6 +789,8 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 #ifndef HAVE_MULTIDRIVE
     const int drive = 0;
 #endif
+    bool aligned = !((uintptr_t)buf & (CACHEALIGN_SIZE - 1));
+
 
     mutex_lock(&sd_mtx);
 #ifndef BOOTLOADER
@@ -828,17 +830,34 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
     last_disk_activity = current_tick;
     dma_retain();
 
+    if(aligned)
+    {
+        if(write)
+            clean_dcache_range(buf, count * SECTOR_SIZE);
+        else
+            dump_dcache_range(buf, count * SECTOR_SIZE);
+    }
+
     const int cmd = write ? SD_WRITE_MULTIPLE_BLOCK : SD_READ_MULTIPLE_BLOCK;
 
     do
     {
-        void *dma_buf = aligned_buffer;
+        void *dma_buf;
         unsigned int transfer = count;
-        if(transfer > UNALIGNED_NUM_SECTORS)
-            transfer = UNALIGNED_NUM_SECTORS;
 
-        if(write)
-            memcpy(uncached_buffer, buf, transfer * SD_BLOCK_SIZE);
+        if(aligned)
+        {
+            dma_buf = AS3525_PHYSICAL_ADDR(buf);
+        }
+        else
+        {
+            dma_buf = aligned_buffer;
+            if(transfer > UNALIGNED_NUM_SECTORS)
+                transfer = UNALIGNED_NUM_SECTORS;
+
+            if(write)
+                memcpy(uncached_buffer, buf, transfer * SD_BLOCK_SIZE);
+        }
 
         /* Interrupt handler might set this to true during transfer */
         retry = false;
@@ -893,7 +912,7 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 
         if(!retry)
         {
-            if(!write)
+            if(!write && !aligned)
                 memcpy(buf, uncached_buffer, transfer * SD_BLOCK_SIZE);
             buf += transfer * SD_BLOCK_SIZE;
             start += transfer;