mmc: bcm2835-host: let firmware manage the clock divisor

Newer firmware can manage the SDCDIV clock divisor register, allowing
the divisor to scale with the core as necessary.

Leverage this ability if the firmware supports it.

Adapted from the following raspberrypi Linux kernel commit:

  bcm2835-sdhost: Firmware manages the clock divisor
  https://github.com/raspberrypi/linux/commit/08532d242d7702ae0add95096aa49c5e96e066e2

Signed-off-by: Vincent Fazio <vfazio@xes-inc.com>
Signed-off-by: Peter Robinson <pbrobinson@gmail.com>

1 files changed, 32 insertions, 21 deletions

diff --git a/drivers/mmc/bcm2835_sdhost.c b/drivers/mmc/bcm2835_sdhost.c
index 894dbdd6861..5c23c03d10d 100644
--- a/drivers/mmc/bcm2835_sdhost.c
+++ b/drivers/mmc/bcm2835_sdhost.c
@@ -181,6 +181,7 @@ struct bcm2835_host {
 	struct udevice		*dev;
 	struct mmc		*mmc;
 	struct bcm2835_plat	*plat;
+	unsigned int		firmware_sets_cdiv:1;
 };
 
 static void bcm2835_dumpregs(struct bcm2835_host *host)
@@ -233,7 +234,7 @@ static void bcm2835_reset_internal(struct bcm2835_host *host)
 	msleep(20);
 	host->clock = 0;
 	writel(host->hcfg, host->ioaddr + SDHCFG);
-	writel(host->cdiv, host->ioaddr + SDCDIV);
+	writel(SDCDIV_MAX_CDIV, host->ioaddr + SDCDIV);
 }
 
 static int bcm2835_wait_transfer_complete(struct bcm2835_host *host)
@@ -598,6 +599,7 @@ static int bcm2835_transmit(struct bcm2835_host *host)
 static void bcm2835_set_clock(struct bcm2835_host *host, unsigned int clock)
 {
 	int div;
+	u32 clock_rate[2] = { 0 };
 
 	/* The SDCDIV register has 11 bits, and holds (div - 2).  But
 	 * in data mode the max is 50MHz wihout a minimum, and only
@@ -620,26 +622,34 @@ static void bcm2835_set_clock(struct bcm2835_host *host, unsigned int clock)
 	 * clock divisor at all times.
 	 */
 
-	if (clock < 100000) {
-		/* Can't stop the clock, but make it as slow as possible
-		 * to show willing
-		 */
-		host->cdiv = SDCDIV_MAX_CDIV;
-		writel(host->cdiv, host->ioaddr + SDCDIV);
-		return;
-	}
+	if (host->firmware_sets_cdiv) {
+		bcm2835_set_sdhost_clock(clock, &clock_rate[0], &clock_rate[1]);
+		clock = max(clock_rate[0], clock_rate[1]);
+	} else {
+		if (clock < 100000) {
+			/* Can't stop the clock, but make it as slow as possible
+			* to show willing
+			*/
+			host->cdiv = SDCDIV_MAX_CDIV;
+			writel(host->cdiv, host->ioaddr + SDCDIV);
+			return;
+		}
 
-	div = host->max_clk / clock;
-	if (div < 2)
-		div = 2;
-	if ((host->max_clk / div) > clock)
-		div++;
-	div -= 2;
+		div = host->max_clk / clock;
+		if (div < 2)
+			div = 2;
+		if ((host->max_clk / div) > clock)
+			div++;
+		div -= 2;
 
-	if (div > SDCDIV_MAX_CDIV)
-		div = SDCDIV_MAX_CDIV;
+		if (div > SDCDIV_MAX_CDIV)
+			div = SDCDIV_MAX_CDIV;
+
+		clock = host->max_clk / (div + 2);
+		host->cdiv = div;
+		writel(host->cdiv, host->ioaddr + SDCDIV);
+	}
 
-	clock = host->max_clk / (div + 2);
 	host->mmc->clock = clock;
 
 	/* Calibrate some delays */
@@ -647,9 +657,6 @@ static void bcm2835_set_clock(struct bcm2835_host *host, unsigned int clock)
 	host->ns_per_fifo_word = (1000000000 / clock) *
 		((host->mmc->card_caps & MMC_MODE_4BIT) ? 8 : 32);
 
-	host->cdiv = div;
-	writel(host->cdiv, host->ioaddr + SDCDIV);
-
 	/* Set the timeout to 500ms */
 	writel(host->mmc->clock / 2, host->ioaddr + SDTOUT);
 }
@@ -759,6 +766,7 @@ static int bcm2835_probe(struct udevice *dev)
 	struct bcm2835_host *host = dev_get_priv(dev);
 	struct mmc *mmc = mmc_get_mmc_dev(dev);
 	struct mmc_uclass_priv *upriv = dev_get_uclass_priv(dev);
+	u32 clock_rate[2] = { ~0 };
 
 	host->dev = dev;
 	host->mmc = mmc;
@@ -776,6 +784,9 @@ static int bcm2835_probe(struct udevice *dev)
 
 	host->max_clk = bcm2835_get_mmc_clock(BCM2835_MBOX_CLOCK_ID_CORE);
 
+	bcm2835_set_sdhost_clock(0, &clock_rate[0], &clock_rate[1]);
+	host->firmware_sets_cdiv = (clock_rate[0] != ~0);
+
 	bcm2835_add_host(host);
 
 	dev_dbg(dev, "%s -> OK\n", __func__);