From f3a870bfa075fb880f5e018a0a5ae6f27ca8be49 Mon Sep 17 00:00:00 2001
From: Vincent Fazio <vfazio@xes-inc.com>
Date: Mon, 13 Sep 2021 13:34:45 -0500
Subject: [PATCH] mmc: bcm2835-host: let firmware manage the clock

Newer firmware supports managing the sdhost clock divisor, so leverage
this feature if it is available.

SET_SDHOST_CLOCK is largely undocumented except for its usage within the
Linux kernel, which this change is based on.

https://github.com/raspberrypi/linux/commit/3cd16c39c718e2dda7885c4ed7a20118aed12524

Signed-off-by: Vincent Fazio <vfazio@xes-inc.com>
---
 arch/arm/mach-bcm283x/include/mach/mbox.h | 18 ++++++++
 arch/arm/mach-bcm283x/include/mach/msg.h  | 10 +++++
 arch/arm/mach-bcm283x/msg.c               | 30 +++++++++++++
 drivers/mmc/bcm2835_sdhost.c              | 53 ++++++++++++++---------
 4 files changed, 90 insertions(+), 21 deletions(-)

diff --git a/arch/arm/mach-bcm283x/include/mach/mbox.h b/arch/arm/mach-bcm283x/include/mach/mbox.h
index 7dcac583cc4..9b1943fcfc4 100644
--- a/arch/arm/mach-bcm283x/include/mach/mbox.h
+++ b/arch/arm/mach-bcm283x/include/mach/mbox.h
@@ -252,6 +252,24 @@ struct bcm2835_mbox_tag_get_clock_rate {
 	} body;
 };
 
+#define BCM2835_MBOX_TAG_SET_SDHOST_CLOCK	0x00038042
+
+struct bcm2835_mbox_tag_set_sdhost_clock {
+	struct bcm2835_mbox_tag_hdr tag_hdr;
+	union {
+		struct {
+			u32 rate_hz;
+			u32 rate_1;
+			u32 rate_2;
+		} req;
+		struct {
+			u32 rate_hz;
+			u32 rate_1;
+			u32 rate_2;
+		} resp;
+	} body;
+};
+
 #define BCM2835_MBOX_TAG_ALLOCATE_BUFFER	0x00040001
 
 struct bcm2835_mbox_tag_allocate_buffer {
diff --git a/arch/arm/mach-bcm283x/include/mach/msg.h b/arch/arm/mach-bcm283x/include/mach/msg.h
index e45c1bf010f..ab37abdb6c6 100644
--- a/arch/arm/mach-bcm283x/include/mach/msg.h
+++ b/arch/arm/mach-bcm283x/include/mach/msg.h
@@ -22,6 +22,16 @@ int bcm2835_power_on_module(u32 module);
  */
 int bcm2835_get_mmc_clock(u32 clock_id);
 
+/**
+ * bcm2835_set_sdhost_clock() - determine if firmware controls sdhost cdiv
+ *
+ * @rate_hz: Input clock frequency
+ * @rate_1: Returns a clock frequency
+ * @rate_2: Returns a clock frequency
+ * @return 0 of OK, -EIO on error
+ */
+int bcm2835_set_sdhost_clock(u32 rate_hz, u32 *rate_1, u32 *rate_2);
+
 /**
  * bcm2835_get_video_size() - get the current display size
  *
diff --git a/arch/arm/mach-bcm283x/msg.c b/arch/arm/mach-bcm283x/msg.c
index e2badfecb09..8c1c36a5f15 100644
--- a/arch/arm/mach-bcm283x/msg.c
+++ b/arch/arm/mach-bcm283x/msg.c
@@ -21,6 +21,12 @@ struct msg_get_clock_rate {
 	u32 end_tag;
 };
 
+struct msg_set_sdhost_clock {
+	struct bcm2835_mbox_hdr hdr;
+	struct bcm2835_mbox_tag_set_sdhost_clock set_sdhost_clock;
+	u32 end_tag;
+};
+
 struct msg_query {
 	struct bcm2835_mbox_hdr hdr;
 	struct bcm2835_mbox_tag_physical_w_h physical_w_h;
@@ -111,6 +117,30 @@ int bcm2835_get_mmc_clock(u32 clock_id)
 	return clock_rate;
 }
 
+int bcm2835_set_sdhost_clock(u32 rate_hz, u32 *rate_1, u32 *rate_2)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct msg_set_sdhost_clock, msg_sdhost_clk, 1);
+	int ret;
+
+	BCM2835_MBOX_INIT_HDR(msg_sdhost_clk);
+	BCM2835_MBOX_INIT_TAG(&msg_sdhost_clk->set_sdhost_clock, SET_SDHOST_CLOCK);
+
+	msg_sdhost_clk->set_sdhost_clock.body.req.rate_hz = rate_hz;
+	msg_sdhost_clk->set_sdhost_clock.body.req.rate_1 = *rate_1;
+	msg_sdhost_clk->set_sdhost_clock.body.req.rate_2 = *rate_2;
+
+	ret = bcm2835_mbox_call_prop(BCM2835_MBOX_PROP_CHAN, &msg_sdhost_clk->hdr);
+	if (ret) {
+		printf("bcm2835: Could not query sdhost clock rate\n");
+		return -EIO;
+	}
+
+	*rate_1 = msg_sdhost_clk->set_sdhost_clock.body.resp.rate_1;
+	*rate_2 = msg_sdhost_clk->set_sdhost_clock.body.resp.rate_2;
+
+	return 0;
+}
+
 int bcm2835_get_video_size(int *widthp, int *heightp)
 {
 	ALLOC_CACHE_ALIGN_BUFFER(struct msg_query, msg_query, 1);
diff --git a/drivers/mmc/bcm2835_sdhost.c b/drivers/mmc/bcm2835_sdhost.c
index 894dbdd6861..3a9cd6f1eb2 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__);