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 <yann.gautier@st.com>
diff --git a/drivers/st/mmc/stm32_sdmmc2.c b/drivers/st/mmc/stm32_sdmmc2.c
index 1be8850..24e6efe 100644
--- 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 @@
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 @@
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 @@
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);