From e104409d525e64f23d5a087f02e564485e84845b Mon Sep 17 00:00:00 2001
From: UncleRus <unclerus@gmail.com>
Date: Thu, 2 Aug 2018 19:05:42 +0500
Subject: [PATCH] CMD25 workaround for SDIO

---
 extras/sdio/component.mk |  5 +++++
 extras/sdio/sdio.c       | 24 ++++++++++++++++++++----
 2 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/extras/sdio/component.mk b/extras/sdio/component.mk
index 5ed3ce0..f8fd0ec 100644
--- a/extras/sdio/component.mk
+++ b/extras/sdio/component.mk
@@ -4,4 +4,9 @@ INC_DIRS += $(sdio_ROOT)..
 # args for passing into compile rule generation
 sdio_SRC_DIR = $(sdio_ROOT)
 
+# Workaround unsupported CMD25 for very old SD cards
+SDIO_CMD25_WORKAROUND ?= 0
+
+sdio_CFLAGS = $(CFLAGS) -DSDIO_CMD25_WORKAROUND=$(SDIO_CMD25_WORKAROUND)
+
 $(eval $(call component_compile_rules,sdio))
\ No newline at end of file
diff --git a/extras/sdio/sdio.c b/extras/sdio/sdio.c
index a721442..48a05da 100644
--- a/extras/sdio/sdio.c
+++ b/extras/sdio/sdio.c
@@ -49,6 +49,11 @@
 #define WRITE_RES_OK       0x05
 
 
+#ifndef SDIO_CMD25_WORKAROUND
+    #define SDIO_CMD25_WORKAROUND 0
+#endif
+
+
 #define CMD0   0x00 // GO_IDLE_STATE - Resets the SD Memory Card
 #define CMD1   0x01 // SEND_OP_COND - Sends host capacity support information
                     // and activates the card's initialization process.
@@ -420,19 +425,30 @@ sdio_error_t sdio_write_sectors(sdio_card_t *card, uint32_t sector, uint8_t *src
         return set_error(card, SDIO_ERR_IO);
     }
 
+#if SDIO_CMD25_WORKAROUND
+    // Workaround for very old cards that don't support CMD25
+    while (count--)
+    {
+        // single block
+        if (command(card, CMD24, sector))
+            return set_error(card, SDIO_ERR_IO);
+        if (write_data_block(card, TOKEN_SINGLE_TRAN, src) != SDIO_ERR_NONE)
+            return card->error;
+        src += SDIO_BLOCK_SIZE;
+    }
+#else
     if (command(card, multi ? CMD25 : CMD24, sector))
         return set_error(card, SDIO_ERR_IO);
 
     while (count--)
     {
-        if (write_data_block(card, multi ? TOKEN_MULTI_TRAN : TOKEN_SINGLE_TRAN, src) != SDIO_ERR_NONE){
-          return card->error;
-        }
+        if (write_data_block(card, multi ? TOKEN_MULTI_TRAN : TOKEN_SINGLE_TRAN, src) != SDIO_ERR_NONE)
+            return card->error;
         src += SDIO_BLOCK_SIZE;
     }
-
     if (multi && command(card, CMD12, 0))
         return set_error(card, SDIO_ERR_IO);
+#endif
 
     return set_error(card, SDIO_ERR_NONE);
 }