From f4bf48e76a089aca197dd1e4df61362725d4875a Mon Sep 17 00:00:00 2001
From: CTCaer <ctcaer@gmail.com>
Date: Fri, 31 Mar 2023 09:04:10 +0300
Subject: [PATCH] bdk: sdmmc: add driver type set support

---
 bdk/storage/sdmmc.c        | 22 ++++++++++++++++++++++
 bdk/storage/sdmmc_driver.c |  7 +++++++
 bdk/storage/sdmmc_driver.h |  3 +++
 3 files changed, 32 insertions(+)

diff --git a/bdk/storage/sdmmc.c b/bdk/storage/sdmmc.c
index 1fd7dea..f7ab417 100644
--- a/bdk/storage/sdmmc.c
+++ b/bdk/storage/sdmmc.c
@@ -1030,6 +1030,28 @@ static void _sd_storage_set_power_limit(sdmmc_storage_t *storage, u16 power_limi
 	}
 }
 
+int _sd_storage_set_driver_type(sdmmc_storage_t *storage, u32 driver, u8 *buf)
+{
+	if (!_sd_storage_switch(storage, buf, SD_SWITCH_CHECK, SD_SWITCH_GRP_DRVSTR, driver))
+		return 0;
+
+	u32 driver_out = buf[15] & 0xF;
+	if (driver_out != driver)
+		return 0;
+	DPRINTF("[SD] supports Driver Strength %d\n", driver);
+
+	if (!_sd_storage_switch(storage, buf, SD_SWITCH_SET, SD_SWITCH_GRP_DRVSTR, driver))
+		return 0;
+
+	if (driver_out != (buf[15] & 0xF))
+		return 0;
+	DPRINTF("[SD] card accepted Driver Strength %d\n", driver);
+
+	sdmmc_setup_drv_type(storage->sdmmc, driver);
+
+	return 1;
+}
+
 /*
  * SD Card DDR200 (DDR208) support
  *
diff --git a/bdk/storage/sdmmc_driver.c b/bdk/storage/sdmmc_driver.c
index b705091..27d25fe 100644
--- a/bdk/storage/sdmmc_driver.c
+++ b/bdk/storage/sdmmc_driver.c
@@ -297,6 +297,13 @@ static void _sdmmc_reset_all(sdmmc_t *sdmmc)
 		;
 }
 
+void sdmmc_setup_drv_type(sdmmc_t *sdmmc, u32 type)
+{
+	sdmmc->regs->hostctl2  = (sdmmc->regs->hostctl2 & (~SDHCI_CTRL_DRV_TYPE_MASK)) | SDHCI_CTRL_DRV_TYPE(type);
+
+	_sdmmc_commit_changes(sdmmc);
+}
+
 int sdmmc_setup_clock(sdmmc_t *sdmmc, u32 type)
 {
 	// Disable the SD clock if it was enabled, and reenable it later.
diff --git a/bdk/storage/sdmmc_driver.h b/bdk/storage/sdmmc_driver.h
index a3c7bf5..bbef04e 100644
--- a/bdk/storage/sdmmc_driver.h
+++ b/bdk/storage/sdmmc_driver.h
@@ -120,10 +120,12 @@
 /*! SDMMC host control 2. 0x3E. */
 #define SDHCI_CTRL_UHS_MASK        0x7
 #define SDHCI_CTRL_VDD_180         BIT(3)
+#define SDHCI_CTRL_DRV_TYPE_MASK   (3U << 4)
 #define SDHCI_CTRL_DRV_TYPE_B      (0U << 4)
 #define SDHCI_CTRL_DRV_TYPE_A      (1U << 4)
 #define SDHCI_CTRL_DRV_TYPE_C      (2U << 4)
 #define SDHCI_CTRL_DRV_TYPE_D      (3U << 4)
+#define SDHCI_CTRL_DRV_TYPE(type)  ((type) << 4)
 #define SDHCI_CTRL_EXEC_TUNING     BIT(6)
 #define SDHCI_CTRL_TUNED_CLK_SHIFT 7
 #define SDHCI_CTRL_TUNED_CLK       BIT(7)
@@ -317,6 +319,7 @@ int  sdmmc_get_io_power(sdmmc_t *sdmmc);
 u32  sdmmc_get_bus_width(sdmmc_t *sdmmc);
 void sdmmc_set_bus_width(sdmmc_t *sdmmc, u32 bus_width);
 void sdmmc_save_tap_value(sdmmc_t *sdmmc);
+void sdmmc_setup_drv_type(sdmmc_t *sdmmc, u32 type);
 int  sdmmc_setup_clock(sdmmc_t *sdmmc, u32 type);
 void sdmmc_card_clock_powersave(sdmmc_t *sdmmc, int powersave_enable);
 int  sdmmc_get_rsp(sdmmc_t *sdmmc, u32 *rsp, u32 size, u32 type);