3 files changed, 49 insertions, 12 deletions

diff --git a/firmware/target/arm/as3525/sd-as3525.c b/firmware/target/arm/as3525/sd-as3525.c
index 9e55c16be6..52bf6932ec 100644
--- a/firmware/target/arm/as3525/sd-as3525.c
+++ b/firmware/target/arm/as3525/sd-as3525.c
@@ -669,6 +669,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);
@@ -700,6 +701,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
@@ -740,12 +749,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 = 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)
@@ -804,7 +820,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;
diff --git a/firmware/target/arm/as3525/sd-as3525v2.c b/firmware/target/arm/as3525/sd-as3525v2.c
index 386b76e758..d268bc779c 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 = 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;
diff --git a/firmware/target/arm/as3525/system-target.h b/firmware/target/arm/as3525/system-target.h
index f7dc1ac312..5d11ecb26c 100644
--- a/firmware/target/arm/as3525/system-target.h
+++ b/firmware/target/arm/as3525/system-target.h
@@ -27,6 +27,8 @@
 
 #include "clock-target.h" /* CPUFREQ_* are defined here */
 
+#define STORAGE_WANTS_ALIGN
+
 /* We can use a interrupt-based mechanism on the fuzev2 */
 #define INCREASED_SCROLLWHEEL_POLLING \
     (defined(HAVE_SCROLLWHEEL) && (CONFIG_CPU == AS3525))