mmc: stm32_sdmmc2: correctly manage block size

DBLOCKSIZE should be filled such as the data size is 2^DBLOCKSIZE.
Hence it is calculated with __builtin_ctz.

Change-Id: Id6b5ff9b594afc4fc523a388011beed307e6abd1
Signed-off-by: Yann Gautier <[email protected]>

diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index 1be88502dd484c79c41847c9bf4d8c2e0af259d4..24e6efe98296a58a2ccbd674c26b88a799d5dbe9 100644 (file)
--- a/drivers/st/mmc/stm32_sdmmc2.c
+++ b/drivers/st/mmc/stm32_sdmmc2.c
@@ -71,20 +71,14 @@
 #define SDMMC_DCTRLR_DTEN              BIT(0)
 #define SDMMC_DCTRLR_DTDIR             BIT(1)
 #define SDMMC_DCTRLR_DTMODE            GENMASK(3, 2)
-#define SDMMC_DCTRLR_DBLOCKSIZE_0      BIT(4)
-#define SDMMC_DCTRLR_DBLOCKSIZE_1      BIT(5)
-#define SDMMC_DCTRLR_DBLOCKSIZE_3      BIT(7)
 #define SDMMC_DCTRLR_DBLOCKSIZE                GENMASK(7, 4)
+#define SDMMC_DCTRLR_DBLOCKSIZE_SHIFT  4
 #define SDMMC_DCTRLR_FIFORST           BIT(13)
 
 #define SDMMC_DCTRLR_CLEAR_MASK                (SDMMC_DCTRLR_DTEN | \
                                         SDMMC_DCTRLR_DTDIR | \
                                         SDMMC_DCTRLR_DTMODE | \
                                         SDMMC_DCTRLR_DBLOCKSIZE)
-#define SDMMC_DBLOCKSIZE_8             (SDMMC_DCTRLR_DBLOCKSIZE_0 | \
-                                        SDMMC_DCTRLR_DBLOCKSIZE_1)
-#define SDMMC_DBLOCKSIZE_512           (SDMMC_DCTRLR_DBLOCKSIZE_0 | \
-                                        SDMMC_DCTRLR_DBLOCKSIZE_3)
 
 /* SDMMC status register */
 #define SDMMC_STAR_CCRCFAIL            BIT(0)
@@ -464,11 +458,14 @@ static int stm32_sdmmc2_prepare(int lba, uintptr_t buf, size_t size)
        int ret;
        uintptr_t base = sdmmc2_params.reg_base;
        uint32_t data_ctrl = SDMMC_DCTRLR_DTDIR;
+       uint32_t arg_size;
 
-       if (size == 8U) {
-               data_ctrl |= SDMMC_DBLOCKSIZE_8;
+       assert(size != 0U);
+
+       if (size > MMC_BLOCK_SIZE) {
+               arg_size = MMC_BLOCK_SIZE;
        } else {
-               data_ctrl |= SDMMC_DBLOCKSIZE_512;
+               arg_size = size;
        }
 
        sdmmc2_params.use_dma = plat_sdmmc2_use_dma(base, buf);
@@ -487,12 +484,7 @@ static int stm32_sdmmc2_prepare(int lba, uintptr_t buf, size_t size)
        zeromem(&cmd, sizeof(struct mmc_cmd));
 
        cmd.cmd_idx = MMC_CMD(16);
-       if (size > MMC_BLOCK_SIZE) {
-               cmd.cmd_arg = MMC_BLOCK_SIZE;
-       } else {
-               cmd.cmd_arg = size;
-       }
-
+       cmd.cmd_arg = arg_size;
        cmd.resp_type = MMC_RESPONSE_R1;
 
        ret = stm32_sdmmc2_send_cmd(&cmd);
@@ -514,6 +506,8 @@ static int stm32_sdmmc2_prepare(int lba, uintptr_t buf, size_t size)
                flush_dcache_range(buf, size);
        }
 
+       data_ctrl |= __builtin_ctz(arg_size) << SDMMC_DCTRLR_DBLOCKSIZE_SHIFT;
+
        mmio_clrsetbits_32(base + SDMMC_DCTRLR,
                           SDMMC_DCTRLR_CLEAR_MASK,
                           data_ctrl);