1 files changed, 68 insertions, 27 deletions

diff --git a/drivers/mmc/sunxi_mmc.c b/drivers/mmc/sunxi_mmc.c
index 231f0a0315..20d18d0184 100644
--- a/drivers/mmc/sunxi_mmc.c
+++ b/drivers/mmc/sunxi_mmc.c
@@ -22,7 +22,6 @@ struct sunxi_mmc_host {
 	unsigned mmc_no;
 	uint32_t *mclkreg;
 	unsigned fatal_err;
-	unsigned mod_clk;
 	struct sunxi_mmc *reg;
 	struct mmc_config cfg;
 };
@@ -79,10 +78,63 @@ static int mmc_resource_init(int sdc_no)
 	return ret;
 }
 
+static int mmc_set_mod_clk(struct sunxi_mmc_host *mmchost, unsigned int hz)
+{
+	unsigned int pll, pll_hz, div, n, oclk_dly, sclk_dly;
+
+	if (hz <= 24000000) {
+		pll = CCM_MMC_CTRL_OSCM24;
+		pll_hz = 24000000;
+	} else {
+		pll = CCM_MMC_CTRL_PLL6;
+		pll_hz = clock_get_pll6();
+	}
+
+	div = pll_hz / hz;
+	if (pll_hz % hz)
+		div++;
+
+	n = 0;
+	while (div > 16) {
+		n++;
+		div = (div + 1) / 2;
+	}
+
+	if (n > 3) {
+		printf("mmc %u error cannot set clock to %u\n",
+		       mmchost->mmc_no, hz);
+		return -1;
+	}
+
+	/* determine delays */
+	if (hz <= 400000) {
+		oclk_dly = 0;
+		sclk_dly = 7;
+	} else if (hz <= 25000000) {
+		oclk_dly = 0;
+		sclk_dly = 5;
+	} else if (hz <= 50000000) {
+		oclk_dly = 3;
+		sclk_dly = 5;
+	} else {
+		/* hz > 50000000 */
+		oclk_dly = 2;
+		sclk_dly = 4;
+	}
+
+	writel(CCM_MMC_CTRL_ENABLE | pll | CCM_MMC_CTRL_SCLK_DLY(sclk_dly) |
+	       CCM_MMC_CTRL_N(n) | CCM_MMC_CTRL_OCLK_DLY(oclk_dly) |
+	       CCM_MMC_CTRL_M(div), mmchost->mclkreg);
+
+	debug("mmc %u set mod-clk req %u parent %u n %u m %u rate %u\n",
+	      mmchost->mmc_no, hz, pll_hz, 1u << n, div,
+	      pll_hz / (1u << n) / div);
+
+	return 0;
+}
+
 static int mmc_clk_io_on(int sdc_no)
 {
-	unsigned int pll_clk;
-	unsigned int divider;
 	struct sunxi_mmc_host *mmchost = &mmc_host[sdc_no];
 	struct sunxi_ccm_reg *ccm = (struct sunxi_ccm_reg *)SUNXI_CCM_BASE;
 
@@ -96,15 +148,7 @@ static int mmc_clk_io_on(int sdc_no)
 	setbits_le32(&ccm->ahb_reset0_cfg, 1 << AHB_RESET_OFFSET_MMC(sdc_no));
 #endif
 
-	/* config mod clock */
-	pll_clk = clock_get_pll6();
-	/* should be close to 100 MHz but no more, so round up */
-	divider = ((pll_clk + 99999999) / 100000000) - 1;
-	writel(CCM_MMC_CTRL_ENABLE | CCM_MMC_CTRL_PLL6 | divider,
-	       mmchost->mclkreg);
-	mmchost->mod_clk = pll_clk / (divider + 1);
-
-	return 0;
+	return mmc_set_mod_clk(mmchost, 24000000);
 }
 
 static int mmc_update_clk(struct mmc *mmc)
@@ -129,7 +173,7 @@ static int mmc_update_clk(struct mmc *mmc)
 	return 0;
 }
 
-static int mmc_config_clock(struct mmc *mmc, unsigned div)
+static int mmc_config_clock(struct mmc *mmc)
 {
 	struct sunxi_mmc_host *mmchost = mmc->priv;
 	unsigned rval = readl(&mmchost->reg->clkcr);
@@ -140,16 +184,17 @@ static int mmc_config_clock(struct mmc *mmc, unsigned div)
 	if (mmc_update_clk(mmc))
 		return -1;
 
-	/* Change Divider Factor */
+	/* Set mod_clk to new rate */
+	if (mmc_set_mod_clk(mmchost, mmc->clock))
+		return -1;
+
+	/* Clear internal divider */
 	rval &= ~SUNXI_MMC_CLK_DIVIDER_MASK;
-	rval |= div;
 	writel(rval, &mmchost->reg->clkcr);
-	if (mmc_update_clk(mmc))
-		return -1;
+
 	/* Re-enable Clock */
 	rval |= SUNXI_MMC_CLK_ENABLE;
 	writel(rval, &mmchost->reg->clkcr);
-
 	if (mmc_update_clk(mmc))
 		return -1;
 
@@ -159,18 +204,14 @@ static int mmc_config_clock(struct mmc *mmc, unsigned div)
 static void mmc_set_ios(struct mmc *mmc)
 {
 	struct sunxi_mmc_host *mmchost = mmc->priv;
-	unsigned int clkdiv = 0;
 
-	debug("set ios: bus_width: %x, clock: %d, mod_clk: %d\n",
-	      mmc->bus_width, mmc->clock, mmchost->mod_clk);
+	debug("set ios: bus_width: %x, clock: %d\n",
+	      mmc->bus_width, mmc->clock);
 
 	/* Change clock first */
-	clkdiv = (mmchost->mod_clk + (mmc->clock >> 1)) / mmc->clock / 2;
-	if (mmc->clock) {
-		if (mmc_config_clock(mmc, clkdiv)) {
-			mmchost->fatal_err = 1;
-			return;
-		}
+	if (mmc->clock && mmc_config_clock(mmc) != 0) {
+		mmchost->fatal_err = 1;
+		return;
 	}
 
 	/* Change bus width */