From 9a94168e781adc5fdb27d07ebe21c754559200b2 Mon Sep 17 00:00:00 2001
From: Francois Ramu <francois.ramu@st.com>
Date: Fri, 4 Sep 2020 09:18:07 +0200
Subject: [PATCH] disk: sdhc: use the multiple block transfer for writing

The sdhc_spi_write_multi function is sending the CMD25
for multiple block write operation on the SD card
instead of looping the CMD24. This is improving
the transfer speed compared to the sdhc_spi_write function
when the nb of blocks to write is significant (more than 2).

Signed-off-by: Francois Ramu <francois.ramu@st.com>
---
 subsys/disk/disk_access_spi_sdhc.c | 99 ++++++++++++++++++++++++++++--
 1 file changed, 94 insertions(+), 5 deletions(-)

diff --git a/subsys/disk/disk_access_spi_sdhc.c b/subsys/disk/disk_access_spi_sdhc.c
index 5d59225735b..44684d23729 100644
--- a/subsys/disk/disk_access_spi_sdhc.c
+++ b/subsys/disk/disk_access_spi_sdhc.c
@@ -748,6 +748,85 @@ error:
 	return err;
 }
 
+/* this function is optimized to write multiple blocks */
+static int sdhc_spi_write_multi(struct sdhc_spi_data *data,
+	const uint8_t *buf, uint32_t sector, uint32_t count)
+{
+	int err;
+	uint32_t addr;
+	uint8_t block[SDHC_CRC16_SIZE];
+
+	err = sdhc_map_disk_status(data->status);
+	if (err != 0) {
+		return err;
+	}
+
+	if (data->high_capacity) {
+		addr = sector;
+	} else {
+		addr = sector * SDMMC_DEFAULT_BLOCK_SIZE;
+	}
+
+	err = sdhc_spi_cmd_r1(data, SDHC_WRITE_MULTIPLE_BLOCK, addr);
+	if (err < 0) {
+		goto exit;
+	}
+
+	/* Write the blocks */
+	for (; count != 0U; count--) {
+		/* Start the block */
+		block[0] = SDHC_TOKEN_MULTI_WRITE;
+		err = sdhc_spi_tx(data, block, 1);
+		if (err != 0) {
+			goto exit;
+		}
+
+		/* Write the payload */
+		err = sdhc_spi_tx(data, buf, SDMMC_DEFAULT_BLOCK_SIZE);
+		if (err != 0) {
+			goto exit;
+		}
+
+		/* Build and write the trailing CRC */
+		sys_put_be16(crc16_itu_t(0, buf, SDMMC_DEFAULT_BLOCK_SIZE),
+			     block);
+
+		err = sdhc_spi_tx(data, block, sizeof(block));
+		if (err != 0) {
+			goto exit;
+		}
+
+		err = sdhc_map_data_status(sdhc_spi_rx_u8(data));
+		if (err != 0) {
+			goto exit;
+		}
+
+		/* Wait for the card to finish programming */
+		err = sdhc_spi_skip_until_ready(data);
+		if (err != 0) {
+			goto exit;
+		}
+
+		buf += SDMMC_DEFAULT_BLOCK_SIZE;
+		sector++;
+	}
+
+	/* Stop the transmission */
+	sdhc_spi_tx_cmd(data, SDHC_STOP_TRANSMISSION, 0);
+
+	/* Wait for the card to finish operation */
+	err = sdhc_spi_skip_until_ready(data);
+	if (err != 0) {
+		goto exit;
+	}
+
+	err = 0;
+exit:
+	spi_release(data->spi, &data->cfg);
+
+	return err;
+}
+
 static int disk_spi_sdhc_init(const struct device *dev);
 
 static int sdhc_spi_init(const struct device *dev)
@@ -808,14 +887,24 @@ static int disk_spi_sdhc_access_write(struct disk_info *disk,
 	struct sdhc_spi_data *data = dev->data;
 	int err;
 
-	LOG_DBG("sector=%u count=%u", sector, count);
+	/* for more than 2 blocks the multiple block is preferred */
+	if (count > 2) {
+		LOG_DBG("multi block sector=%u count=%u", sector, count);
+
+		err = sdhc_spi_write_multi(data, buf, sector, count);
+		if (err != 0 && sdhc_is_retryable(err)) {
+			sdhc_spi_recover(data);
+			err = sdhc_spi_write_multi(data, buf, sector, count);
+		}
+	} else {
+		LOG_DBG("sector=%u count=%u", sector, count);
 
-	err = sdhc_spi_write(data, buf, sector, count);
-	if (err != 0 && sdhc_is_retryable(err)) {
-		sdhc_spi_recover(data);
 		err = sdhc_spi_write(data, buf, sector, count);
+		if (err != 0 && sdhc_is_retryable(err)) {
+			sdhc_spi_recover(data);
+			err = sdhc_spi_write(data, buf, sector, count);
+		}
 	}
-
 	return err;
 }