diff --git a/drivers/mmc/am654_sdhci.c b/drivers/mmc/am654_sdhci.c
index a8c9227..b9a7924 100644
--- a/drivers/mmc/am654_sdhci.c
+++ b/drivers/mmc/am654_sdhci.c
@@ -10,21 +10,189 @@
 #include <dm.h>
 #include <malloc.h>
 #include <power-domain.h>
+#include <regmap.h>
 #include <sdhci.h>
 
+/* CTL_CFG Registers */
+#define CTL_CFG_2		0x14
+
+#define SLOTTYPE_MASK		GENMASK(31, 30)
+#define SLOTTYPE_EMBEDDED	BIT(30)
+
+/* PHY Registers */
+#define PHY_CTRL1	0x100
+#define PHY_CTRL2	0x104
+#define PHY_CTRL3	0x108
+#define PHY_CTRL4	0x10C
+#define PHY_CTRL5	0x110
+#define PHY_CTRL6	0x114
+#define PHY_STAT1	0x130
+#define PHY_STAT2	0x134
+
+#define IOMUX_ENABLE_SHIFT	31
+#define IOMUX_ENABLE_MASK	BIT(IOMUX_ENABLE_SHIFT)
+#define OTAPDLYENA_SHIFT	20
+#define OTAPDLYENA_MASK		BIT(OTAPDLYENA_SHIFT)
+#define OTAPDLYSEL_SHIFT	12
+#define OTAPDLYSEL_MASK		GENMASK(15, 12)
+#define STRBSEL_SHIFT		24
+#define STRBSEL_MASK		GENMASK(27, 24)
+#define SEL50_SHIFT		8
+#define SEL50_MASK		BIT(SEL50_SHIFT)
+#define SEL100_SHIFT		9
+#define SEL100_MASK		BIT(SEL100_SHIFT)
+#define DLL_TRIM_ICP_SHIFT	4
+#define DLL_TRIM_ICP_MASK	GENMASK(7, 4)
+#define DR_TY_SHIFT		20
+#define DR_TY_MASK		GENMASK(22, 20)
+#define ENDLL_SHIFT		1
+#define ENDLL_MASK		BIT(ENDLL_SHIFT)
+#define DLLRDY_SHIFT		0
+#define DLLRDY_MASK		BIT(DLLRDY_SHIFT)
+#define PDB_SHIFT		0
+#define PDB_MASK		BIT(PDB_SHIFT)
+#define CALDONE_SHIFT		1
+#define CALDONE_MASK		BIT(CALDONE_SHIFT)
+#define RETRIM_SHIFT		17
+#define RETRIM_MASK		BIT(RETRIM_SHIFT)
+
+#define DRIVER_STRENGTH_50_OHM	0x0
+#define DRIVER_STRENGTH_33_OHM	0x1
+#define DRIVER_STRENGTH_66_OHM	0x2
+#define DRIVER_STRENGTH_100_OHM	0x3
+#define DRIVER_STRENGTH_40_OHM	0x4
+
 #define AM654_SDHCI_MIN_FREQ	400000
 
 struct am654_sdhci_plat {
 	struct mmc_config cfg;
 	struct mmc mmc;
-	unsigned int f_max;
+	struct regmap *base;
+	bool non_removable;
+	u32 otap_del_sel;
+	u32 trm_icp;
+	u32 drv_strength;
+	bool dll_on;
 };
 
+static int am654_sdhci_set_ios_post(struct sdhci_host *host)
+{
+	struct udevice *dev = host->mmc->dev;
+	struct am654_sdhci_plat *plat = dev_get_platdata(dev);
+	unsigned int speed = host->mmc->clock;
+	int sel50, sel100;
+	u32 mask, val;
+	int ret;
+
+	/* Reset SD Clock Enable */
+	val = sdhci_readw(host, SDHCI_CLOCK_CONTROL);
+	val &= ~SDHCI_CLOCK_CARD_EN;
+	sdhci_writew(host, val, SDHCI_CLOCK_CONTROL);
+
+	/* power off phy */
+	if (plat->dll_on) {
+		regmap_update_bits(plat->base, PHY_CTRL1, ENDLL_MASK, 0);
+
+		plat->dll_on = false;
+	}
+
+	/* restart clock */
+	sdhci_set_clock(host->mmc, speed);
+
+	/* switch phy back on */
+	if (speed > AM654_SDHCI_MIN_FREQ) {
+		mask = OTAPDLYENA_MASK | OTAPDLYSEL_MASK;
+		val = (1 << OTAPDLYENA_SHIFT) |
+		      (plat->otap_del_sel << OTAPDLYSEL_SHIFT);
+		regmap_update_bits(plat->base, PHY_CTRL4, mask, val);
+		switch (speed) {
+		case 200000000:
+			sel50 = 0;
+			sel100 = 0;
+			break;
+		case 100000000:
+			sel50 = 0;
+			sel100 = 1;
+			break;
+		default:
+			sel50 = 1;
+			sel100 = 0;
+		}
+
+		/* Configure PHY DLL frequency */
+		mask = SEL50_MASK | SEL100_MASK;
+		val = (sel50 << SEL50_SHIFT) | (sel100 << SEL100_SHIFT);
+		regmap_update_bits(plat->base, PHY_CTRL5, mask, val);
+
+		/* Enable DLL */
+		regmap_update_bits(plat->base, PHY_CTRL1, ENDLL_MASK,
+				   0x1 << ENDLL_SHIFT);
+		/*
+		 * Poll for DLL ready. Use a one second timeout.
+		 * Works in all experiments done so far
+		 */
+		ret = regmap_read_poll_timeout(plat->base, PHY_STAT1, val,
+					 val & DLLRDY_MASK, 1000, 1000000);
+		if (ret)
+			return ret;
+
+		plat->dll_on = true;
+	}
+
+	return 0;
+}
+
+const struct sdhci_ops am654_sdhci_ops = {
+	.set_ios_post = &am654_sdhci_set_ios_post,
+};
+
+int am654_sdhci_init(struct am654_sdhci_plat *plat)
+{
+	u32 ctl_cfg_2 = 0;
+	u32 mask, val;
+	int ret;
+
+	/* Reset OTAP to default value */
+	mask = OTAPDLYENA_MASK | OTAPDLYSEL_MASK;
+	regmap_update_bits(plat->base, PHY_CTRL4, mask, 0x0);
+
+	regmap_read(plat->base, PHY_STAT1, &val);
+	if (~val & CALDONE_MASK) {
+		/* Calibrate IO lines */
+		regmap_update_bits(plat->base, PHY_CTRL1, PDB_MASK, PDB_MASK);
+		ret = regmap_read_poll_timeout(plat->base, PHY_STAT1, val,
+					       val & CALDONE_MASK, 1, 20);
+		if (ret)
+			return ret;
+	}
+
+	/* Configure DLL TRIM */
+	mask = DLL_TRIM_ICP_MASK;
+	val = plat->trm_icp << DLL_TRIM_ICP_SHIFT;
+
+	/* Configure DLL driver strength */
+	mask |= DR_TY_MASK;
+	val |= plat->drv_strength << DR_TY_SHIFT;
+	regmap_update_bits(plat->base, PHY_CTRL1, mask, val);
+
+	/* Enable pins by setting IO mux to 0 */
+	regmap_update_bits(plat->base, PHY_CTRL1, IOMUX_ENABLE_MASK, 0);
+
+	/* Set slot type based on SD or eMMC */
+	if (plat->non_removable)
+		ctl_cfg_2 = SLOTTYPE_EMBEDDED;
+
+	regmap_update_bits(plat->base, CTL_CFG_2, SLOTTYPE_MASK, ctl_cfg_2);
+
+	return 0;
+}
+
 static int am654_sdhci_probe(struct udevice *dev)
 {
 	struct am654_sdhci_plat *plat = dev_get_platdata(dev);
 	struct mmc_uclass_priv *upriv = dev_get_uclass_priv(dev);
 	struct sdhci_host *host = dev_get_priv(dev);
+	struct mmc_config *cfg = &plat->cfg;
 	struct power_domain sdhci_pwrdmn;
 	struct clk clk;
 	unsigned long clock;
@@ -55,16 +223,20 @@
 	}
 
 	host->max_clk = clock;
-
-	ret = sdhci_setup_cfg(&plat->cfg, host, plat->f_max,
-			      AM654_SDHCI_MIN_FREQ);
 	host->mmc = &plat->mmc;
+	host->mmc->dev = dev;
+	ret = sdhci_setup_cfg(cfg, host, cfg->f_max,
+			      AM654_SDHCI_MIN_FREQ);
 	if (ret)
 		return ret;
+	host->ops = &am654_sdhci_ops;
 	host->mmc->priv = host;
-	host->mmc->dev = dev;
 	upriv->mmc = host->mmc;
 
+	regmap_init_mem_index(dev_ofnode(dev), &plat->base, 1);
+
+	am654_sdhci_init(plat);
+
 	return sdhci_probe(dev);
 }
 
@@ -72,11 +244,50 @@
 {
 	struct am654_sdhci_plat *plat = dev_get_platdata(dev);
 	struct sdhci_host *host = dev_get_priv(dev);
+	struct mmc_config *cfg = &plat->cfg;
+	u32 drv_strength;
+	int ret;
 
 	host->name = dev->name;
 	host->ioaddr = (void *)dev_read_addr(dev);
-	host->bus_width = dev_read_u32_default(dev, "bus-width", 4);
-	plat->f_max = dev_read_u32_default(dev, "max-frequency", 0);
+	plat->non_removable = dev_read_bool(dev, "non-removable");
+
+	ret = dev_read_u32(dev, "ti,trm-icp", &plat->trm_icp);
+	if (ret)
+		return ret;
+
+	ret = dev_read_u32(dev, "ti,otap-del-sel", &plat->otap_del_sel);
+	if (ret)
+		return ret;
+
+	ret = dev_read_u32(dev, "ti,driver-strength-ohm", &drv_strength);
+	if (ret)
+		return ret;
+
+	switch (drv_strength) {
+	case 50:
+		plat->drv_strength = DRIVER_STRENGTH_50_OHM;
+		break;
+	case 33:
+		plat->drv_strength = DRIVER_STRENGTH_33_OHM;
+		break;
+	case 66:
+		plat->drv_strength = DRIVER_STRENGTH_66_OHM;
+		break;
+	case 100:
+		plat->drv_strength = DRIVER_STRENGTH_100_OHM;
+		break;
+	case 40:
+		plat->drv_strength = DRIVER_STRENGTH_40_OHM;
+		break;
+	default:
+		dev_err(dev, "Invalid driver strength\n");
+		return -EINVAL;
+	}
+
+	ret = mmc_of_parse(dev, cfg);
+	if (ret)
+		return ret;
 
 	return 0;
 }