@@ -607,6 +607,32 @@ static u16 calc_divisor(struct mmc_ios *ios)
return dsor;
}
+static void omap_hsmmc_set_clock(struct omap_hsmmc_host *host)
+{
+ struct mmc_ios *ios = &host->mmc->ios;
+ unsigned long regval;
+ unsigned long timeout;
+
+ dev_dbg(mmc_dev(host->mmc), "Set clock to %uHz\n", ios->clock);
+
+ omap_hsmmc_stop_clock(host);
+
+ regval = OMAP_HSMMC_READ(host->base, SYSCTL);
+ regval = regval & ~(CLKD_MASK | DTO_MASK);
+ regval = regval | (calc_divisor(ios) << 6) | (DTO << 16);
+ OMAP_HSMMC_WRITE(host->base, SYSCTL, regval);
+ OMAP_HSMMC_WRITE(host->base, SYSCTL,
+ OMAP_HSMMC_READ(host->base, SYSCTL) | ICE);
+
+ /* Wait till the ICS bit is set */
+ timeout = jiffies + msecs_to_jiffies(MMC_TIMEOUT_MS);
+ while ((OMAP_HSMMC_READ(host->base, SYSCTL) & ICS) != ICS
+ && time_before(jiffies, timeout))
+ cpu_relax();
+
+ omap_hsmmc_start_clock(host);
+}
+
#ifdef CONFIG_PM
/*
@@ -697,19 +723,7 @@ static int omap_hsmmc_context_restore(struct omap_hsmmc_host *host)
break;
}
- omap_hsmmc_stop_clock(host);
-
- OMAP_HSMMC_WRITE(host->base, SYSCTL,
- (calc_divisor(ios) << 6) | (DTO << 16));
- OMAP_HSMMC_WRITE(host->base, SYSCTL,
- OMAP_HSMMC_READ(host->base, SYSCTL) | ICE);
-
- timeout = jiffies + msecs_to_jiffies(MMC_TIMEOUT_MS);
- while ((OMAP_HSMMC_READ(host->base, SYSCTL) & ICS) != ICS
- && time_before(jiffies, timeout))
- ;
-
- omap_hsmmc_start_clock(host);
+ omap_hsmmc_set_clock(host);
con = OMAP_HSMMC_READ(host->base, CON);
if (ios->bus_mode == MMC_BUSMODE_OPENDRAIN)
@@ -1540,8 +1554,6 @@ static void omap_hsmmc_request(struct mmc_host *mmc, struct mmc_request *req)
static void omap_hsmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
{
struct omap_hsmmc_host *host = mmc_priv(mmc);
- unsigned long regval;
- unsigned long timeout;
u32 con;
int do_send_init_stream = 0;
@@ -1603,22 +1615,7 @@ static void omap_hsmmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
}
}
- omap_hsmmc_stop_clock(host);
-
- regval = OMAP_HSMMC_READ(host->base, SYSCTL);
- regval = regval & ~(CLKD_MASK);
- regval = regval | (calc_divisor(ios) << 6) | (DTO << 16);
- OMAP_HSMMC_WRITE(host->base, SYSCTL, regval);
- OMAP_HSMMC_WRITE(host->base, SYSCTL,
- OMAP_HSMMC_READ(host->base, SYSCTL) | ICE);
-
- /* Wait till the ICS bit is set */
- timeout = jiffies + msecs_to_jiffies(MMC_TIMEOUT_MS);
- while ((OMAP_HSMMC_READ(host->base, SYSCTL) & ICS) != ICS
- && time_before(jiffies, timeout))
- msleep(1);
-
- omap_hsmmc_start_clock(host);
+ omap_hsmmc_set_clock(host);
if (do_send_init_stream)
send_init_stream(host);