1 files changed, 22 insertions, 6 deletions

diff --git a/firmware/target/arm/as3525/sd-as3525.c b/firmware/target/arm/as3525/sd-as3525.c
index 7e61fa509f..423181ab7a 100644
--- a/firmware/target/arm/as3525/sd-as3525.c
+++ b/firmware/target/arm/as3525/sd-as3525.c
@@ -686,6 +686,7 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 #endif
     int ret = 0;
     unsigned loops = 0;
+    bool aligned = !((uintptr_t)buf & (CACHEALIGN_SIZE - 1));
 
     mutex_lock(&sd_mtx);
     sd_enable(true);
@@ -717,6 +718,14 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 
     dma_retain();
 
+    if(aligned)
+    {
+        if(write)
+            clean_dcache_range(buf, count * SECTOR_SIZE);
+        else
+            dump_dcache_range(buf, count * SECTOR_SIZE);
+    }
+
     while(count)
     {
         /* 128 * 512 = 2^16, and doesn't fit in the 16 bits of DATA_LENGTH
@@ -758,12 +767,19 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
         if(!(card_info[drive].ocr & (1<<30)))   /* not SDHC */
             bank_start *= SD_BLOCK_SIZE;
 
-        dma_buf = aligned_buffer;
-        if(transfer > UNALIGNED_NUM_SECTORS)
-            transfer = UNALIGNED_NUM_SECTORS;
+        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);
+            if(write)
+                memcpy(uncached_buffer, buf, transfer * SD_BLOCK_SIZE);
+        }
 
         ret = sd_wait_for_tran_state(drive);
         if (ret < 0)
@@ -822,7 +838,7 @@ static int sd_transfer_sectors(IF_MD2(int drive,) unsigned long start,
 
         if(!transfer_error[drive])
         {
-            if(!write)
+            if(!write && !aligned)
                 memcpy(buf, uncached_buffer, transfer * SD_BLOCK_SIZE);
             buf += transfer * SD_BLOCK_SIZE;
             start += transfer;