Merge branch 'for-next/rpi'

4 files changed, 394 insertions, 230 deletions

diff --git a/arch/arm/boards/raspberry-pi/rpi-common.c b/arch/arm/boards/raspberry-pi/rpi-common.c
index 2684bd5ed7..9aa150de56 100644
--- a/arch/arm/boards/raspberry-pi/rpi-common.c
+++ b/arch/arm/boards/raspberry-pi/rpi-common.c
@@ -2,9 +2,11 @@
 // SPDX-FileCopyrightText: 2009 Carlo Caione <carlo@carlocaione.org>
 
 #include <common.h>
+#include <deep-probe.h>
 #include <init.h>
 #include <fs.h>
 #include <of.h>
+#include <of_device.h>
 #include <linux/stat.h>
 #include <linux/clk.h>
 #include <linux/clkdev.h>
@@ -28,16 +30,20 @@
 
 #include "lowlevel.h"
 
-struct rpi_model {
-	const char *name;
-	void (*init)(void);
+struct rpi_priv;
+struct rpi_machine_data {
+	int (*init)(struct rpi_priv *priv);
+	u8 hw_id;
+#define RPI_OLD_SCHEMA			BIT(0)
+	u8 flags;
 };
 
-#define RPI_MODEL(_id, _name, _init)	\
-	[_id] = {				\
-		.name			= _name,\
-		.init			= _init,\
-	}
+struct rpi_priv {
+	struct device_d *dev;
+	const struct rpi_machine_data *dcfg;
+	unsigned int hw_id;
+	const char *name;
+};
 
 struct msg_get_arm_mem {
 	struct bcm2835_mbox_hdr hdr;
@@ -45,12 +51,6 @@ struct msg_get_arm_mem {
 	u32 end_tag;
 };
 
-struct msg_get_clock_rate {
-	struct bcm2835_mbox_hdr hdr;
-	struct bcm2835_mbox_tag_get_clock_rate get_clock_rate;
-	u32 end_tag;
-};
-
 struct msg_get_board_rev {
 	struct bcm2835_mbox_hdr hdr;
 	struct bcm2835_mbox_tag_get_board_rev get_board_rev;
@@ -80,22 +80,6 @@ static int rpi_get_arm_mem(u32 *size)
 	return 0;
 }
 
-static struct clk *rpi_register_firmware_clock(u32 clock_id, const char *name)
-{
-	BCM2835_MBOX_STACK_ALIGN(struct msg_get_clock_rate, msg);
-	int ret;
-
-	BCM2835_MBOX_INIT_HDR(msg);
-	BCM2835_MBOX_INIT_TAG(&msg->get_clock_rate, GET_CLOCK_RATE);
-	msg->get_clock_rate.body.req.clock_id = clock_id;
-
-	ret = bcm2835_mbox_call_prop(BCM2835_MBOX_PROP_CHAN, &msg->hdr);
-	if (ret)
-		return ERR_PTR(ret);
-
-	return clk_fixed(name, msg->get_clock_rate.body.resp.rate_hz);
-}
-
 static void rpi_set_usbethaddr(void)
 {
 	BCM2835_MBOX_STACK_ALIGN(struct msg_get_mac_address, msg);
@@ -154,98 +138,56 @@ static void rpi_add_led(void)
 		led_set_trigger(LED_TRIGGER_HEARTBEAT, &l->led);
 }
 
-static void rpi_b_init(void)
+static int rpi_b_init(struct rpi_priv *priv)
 {
 	rpi_leds[0].gpio = 16;
 	rpi_leds[0].active_low = 1;
 	rpi_set_usbethaddr();
+
+	return 0;
 }
 
-static void rpi_b_plus_init(void)
+static int rpi_b_plus_init(struct rpi_priv *priv)
 {
 	rpi_leds[0].gpio = 47;
 	rpi_leds[1].gpio = 35;
 	rpi_set_usbethaddr();
+
+	return 0;
 }
 
-static void rpi_0_init(void)
+static int rpi_0_init(struct rpi_priv *priv)
 {
 	rpi_leds[0].gpio = 47;
 	rpi_set_usbotg("usb0");
+
+	return 0;
 }
 
-static void rpi_0_w_init(void)
+static int rpi_0_w_init(struct rpi_priv *priv)
 {
 	struct device_node *np;
 	int ret;
 
-	rpi_0_init();
+	rpi_0_init(priv);
 
 	np = of_find_node_by_path("/chosen");
 	if (!np)
-		return;
+		return -ENODEV;
 
 	if (!of_device_enable_and_register_by_alias("serial1"))
-		return;
+		return -ENODEV;
 
 	ret = of_property_write_string(np, "stdout-path", "serial1:115200n8");
 	if (ret)
-		return;
+		return ret;
 
-	of_device_disable_by_alias("serial0");
+	return of_device_disable_by_alias("serial0");
 }
 
-/* See comments in mbox.h for data source */
-static const struct rpi_model rpi_models_old_scheme[] = {
-	RPI_MODEL(0, "Unknown model", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_B_I2C0_2, "Model B (no P5)", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_B_I2C0_3, "Model B (no P5)", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_B_I2C1_4, "Model B", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_B_I2C1_5, "Model B", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_B_I2C1_6, "Model B", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_A_7, "Model A", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_A_8, "Model A", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_A_9, "Model A", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_B_REV2_d, "Model B rev2", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_B_REV2_e, "Model B rev2", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_B_REV2_f, "Model B rev2", rpi_b_init),
-	RPI_MODEL(BCM2835_BOARD_REV_B_PLUS_10, "Model B+", rpi_b_plus_init),
-	RPI_MODEL(BCM2835_BOARD_REV_CM_11, "Compute Module", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_A_PLUS_12, "Model A+", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_B_PLUS_13, "Model B+", rpi_b_plus_init),
-	RPI_MODEL(BCM2835_BOARD_REV_CM_14, "Compute Module", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_A_PLUS_15, "Model A+", NULL),
-};
-
-static const struct rpi_model rpi_models_new_scheme[] = {
-	RPI_MODEL(BCM2835_BOARD_REV_A, 		"Model A",	NULL ),
-	RPI_MODEL(BCM2835_BOARD_REV_B, 		"Model B", 	rpi_b_init ),
-	RPI_MODEL(BCM2835_BOARD_REV_A_PLUS, 	"Model A+", 	NULL ),
-	RPI_MODEL(BCM2835_BOARD_REV_B_PLUS, 	"Model B+", 	rpi_b_plus_init ),
-	RPI_MODEL(BCM2836_BOARD_REV_2_B, 	"Model 2B", 	rpi_b_plus_init),
-	RPI_MODEL(BCM283x_BOARD_REV_Alpha, 	"Alpha", 	NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_CM1, 	"Compute Module", NULL ),
-	RPI_MODEL(0x7, "Unknown model", NULL),
-	RPI_MODEL(BCM2837_BOARD_REV_3_B, 	"Model 3B", 	rpi_b_init ),
-	RPI_MODEL(BCM2835_BOARD_REV_ZERO, 	"Zero", 	rpi_0_init),
-	RPI_MODEL(BCM2837_BOARD_REV_CM3, 	"Compute Module 3", NULL ),
-	RPI_MODEL(0xb, "Unknown model", NULL),
-	RPI_MODEL(BCM2835_BOARD_REV_ZERO_W, 	"Zero W", 	rpi_0_w_init),
-	RPI_MODEL(BCM2837B0_BOARD_REV_3B_PLUS, 	"Model 3B+", 	rpi_b_plus_init ),
-	RPI_MODEL(BCM2837B0_BOARD_REV_3A_PLUS, 	"Model 3A+", 	rpi_b_plus_init),
-	RPI_MODEL(0xf, "Unknown model", NULL),
-	RPI_MODEL(BCM2837B0_BOARD_REV_CM3_PLUS, "Compute Module 3+", NULL),
-};
-
-static int rpi_board_rev = 0;
-static const struct rpi_model *model = NULL;
-
-static void rpi_get_board_rev(void)
+static int rpi_get_board_rev(struct rpi_priv *priv)
 {
 	int ret;
-	char *name;
-	const struct rpi_model *rpi_models;
-	size_t rpi_models_size;
 
 	BCM2835_MBOX_STACK_ALIGN(struct msg_get_board_rev, msg);
 	BCM2835_MBOX_INIT_HDR(msg);
@@ -253,9 +195,8 @@ static void rpi_get_board_rev(void)
 
 	ret = bcm2835_mbox_call_prop(BCM2835_MBOX_PROP_CHAN, &msg->hdr);
 	if (ret) {
-		printf("bcm2835: Could not query board revision\n");
-		/* Ignore error; not critical */
-		return;
+		dev_err(priv->dev, "Could not query board revision\n");
+		return ret;
 	}
 
 	/* Comments from u-boot:
@@ -269,53 +210,9 @@ static void rpi_get_board_rev(void)
 	 * http://www.raspberrypi.org/forums/viewtopic.php?f=63&t=98367&start=250
 	 * http://www.raspberrypi.org/forums/viewtopic.php?f=31&t=20594
 	 */
-	rpi_board_rev = msg->get_board_rev.body.resp.rev;
-	if (rpi_board_rev & 0x800000) {
-		rpi_board_rev = (rpi_board_rev >> 4) & 0xff;
-		rpi_models = rpi_models_new_scheme;
-		rpi_models_size = ARRAY_SIZE(rpi_models_new_scheme);
-
-	} else {
-		rpi_board_rev &= 0xff;
-		rpi_models = rpi_models_old_scheme;
-		rpi_models_size = ARRAY_SIZE(rpi_models_old_scheme);
-	}
-
-	if (rpi_board_rev >= rpi_models_size) {
-		printf("RPI: Board rev %u outside known range\n",
-		       rpi_board_rev);
-		goto unknown_rev;
-	}
-
-	if (!rpi_models[rpi_board_rev].name) {
-		printf("RPI: Board rev %u unknown\n", rpi_board_rev);
-		goto unknown_rev;
-	}
-
-	if (!rpi_board_rev)
-		goto unknown_rev;
-
-	model = &rpi_models[rpi_board_rev];
-	name = basprintf("RaspberryPi %s", model->name);
-	barebox_set_model(name);
-	free(name);
-
-	return;
+	priv->hw_id = msg->get_board_rev.body.resp.rev;
 
-unknown_rev:
-	rpi_board_rev = 0;
-	barebox_set_model("RaspberryPi (unknown rev)");
-}
-
-static void rpi_model_init(void)
-{
-	if (!model)
-		return;
-
-	if (!model->init)
-		return;
-
-	model->init();
+	return 0;
 }
 
 static int rpi_mem_init(void)
@@ -333,67 +230,6 @@ static int rpi_mem_init(void)
 }
 mem_initcall(rpi_mem_init);
 
-static int rpi_postcore_init(void)
-{
-	rpi_get_board_rev();
-	barebox_set_hostname("rpi");
-	rpi_model_init();
-
-	return 0;
-}
-postcore_initcall(rpi_postcore_init);
-
-static int rpi_clock_init(void)
-{
-	struct clk *clk;
-
-	clk = rpi_register_firmware_clock(BCM2835_MBOX_CLOCK_ID_EMMC,
-					 "bcm2835_mci0");
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
-
-	clkdev_add_physbase(clk, 0x20300000, NULL);
-	clkdev_add_physbase(clk, 0x3f300000, NULL);
-
-	clk = rpi_register_firmware_clock(BCM2835_MBOX_CLOCK_ID_CORE,
-					  "bcm2835_sdhost");
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
-
-	clkdev_add_physbase(clk, 0x20202000, NULL);
-	clkdev_add_physbase(clk, 0x3f202000, NULL);
-
-	return 0;
-}
-postconsole_initcall(rpi_clock_init);
-
-static int rpi_console_clock_init(void)
-{
-	struct clk *clk;
-
-	clk = clk_fixed("apb_pclk", 0);
-	clk_register_clkdev(clk, "apb_pclk", NULL);
-
-	clk = clk_fixed("uart0-pl0110", 48 * 1000 * 1000);
-	clk_register_clkdev(clk, NULL, "uart0-pl0110");
-	clkdev_add_physbase(clk, BCM2835_PL011_BASE, NULL);
-	clkdev_add_physbase(clk, BCM2836_PL011_BASE, NULL);
-
-	clk = rpi_register_firmware_clock(BCM2835_MBOX_CLOCK_ID_CORE,
-					  "uart1-8250");
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
-
-	clkdev_add_physbase(clk, BCM2835_MINIUART_BASE, NULL);
-	clkdev_add_physbase(clk, BCM2836_MINIUART_BASE, NULL);
-
-	clk = clk_fixed("bcm2835-cs", 1 * 1000 * 1000);
-	clk_register_clkdev(clk, NULL, "bcm2835-cs");
-
-	return 0;
-}
-postcore_initcall(rpi_console_clock_init);
-
 static int rpi_env_init(void)
 {
 	struct stat s;
@@ -511,9 +347,59 @@ static void rpi_vc_fdt(void)
 	}
 }
 
-static int rpi_devices_init(void)
+static const struct rpi_machine_data *rpi_get_dcfg(struct rpi_priv *priv)
 {
+	const struct rpi_machine_data *dcfg;
+
+	dcfg = of_device_get_match_data(priv->dev);
+	if (!dcfg)
+		return ERR_PTR(-EINVAL);
+
+	for (; dcfg->hw_id != U8_MAX; dcfg++) {
+		if (priv->hw_id & 0x800000) {
+			if (dcfg->hw_id != ((priv->hw_id >> 4) & 0xff))
+				continue;
+		} else {
+			if (!(dcfg->flags & RPI_OLD_SCHEMA))
+				continue;
+			if (dcfg->hw_id != (priv->hw_id & 0xff))
+				continue;
+		}
+
+		return dcfg;
+	}
+
+	dev_err(priv->dev, "Failed to get dcfg for board_id: 0x%x.\n",
+		priv->hw_id);
+	return ERR_PTR(-ENODEV);
+}
+
+static int rpi_devices_probe(struct device_d *dev)
+{
+	const struct rpi_machine_data *dcfg;
 	struct regulator *reg;
+	struct rpi_priv *priv;
+	const char *name, *ptr;
+	char *hostname;
+	int ret;
+
+	priv = xzalloc(sizeof(*priv));
+	priv->dev = dev;
+
+	ret = rpi_get_board_rev(priv);
+	if (ret)
+		goto free_priv;
+
+	dcfg = rpi_get_dcfg(priv);
+	if (IS_ERR(dcfg))
+		goto free_priv;
+
+	/* construct short recognizable host name */
+	name = of_device_get_match_compatible(priv->dev);
+	ptr = strchr(name, ',');
+	hostname = basprintf("rpi-%s", ptr ? ptr + 1 : name);
+	barebox_set_hostname(hostname);
+	free(hostname);
 
 	rpi_add_led();
 	bcm2835_register_fb();
@@ -521,6 +407,9 @@ static int rpi_devices_init(void)
 	rpi_env_init();
 	rpi_vc_fdt();
 
+	if (dcfg->init)
+		dcfg->init(priv);
+
 	reg = regulator_get_name("bcm2835_usb");
 	if (IS_ERR(reg))
 		return PTR_ERR(reg);
@@ -528,5 +417,202 @@ static int rpi_devices_init(void)
 	regulator_enable(reg);
 
 	return 0;
+
+free_priv:
+	kfree(priv);
+	return ret;
 }
-late_initcall(rpi_devices_init);
+
+static const struct rpi_machine_data rpi_model_a[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_A_7,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_A_8,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_A_9,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_A,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_model_a_plus[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_A_PLUS_12,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_A_PLUS_15,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_A_PLUS,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_model_b[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_B_I2C1_4,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B_I2C1_5,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B_I2C1_6,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_model_b_i2c0[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_B_I2C0_2,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B_I2C0_3,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_model_b_rev2[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_B_REV2_d,
+		.flags = RPI_OLD_SCHEMA,
+		.init = rpi_b_init,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B_REV2_e,
+		.flags = RPI_OLD_SCHEMA,
+		.init = rpi_b_init,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B_REV2_f,
+		.flags = RPI_OLD_SCHEMA,
+		.init = rpi_b_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_model_b_plus[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_B_PLUS_10,
+		.flags = RPI_OLD_SCHEMA,
+		.init = rpi_b_plus_init,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B_PLUS_13,
+		.flags = RPI_OLD_SCHEMA,
+		.init = rpi_b_plus_init,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_B_PLUS,
+		.init = rpi_b_plus_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_compute_module[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_CM_11,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_CM_14,
+		.flags = RPI_OLD_SCHEMA,
+	}, {
+		.hw_id = BCM2835_BOARD_REV_CM1,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_model_zero[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_ZERO,
+		.init = rpi_0_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_model_zero_w[] = {
+	{
+		.hw_id = BCM2835_BOARD_REV_ZERO_W,
+		.init = rpi_0_w_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_2_model_b[] = {
+	{
+		.hw_id = BCM2836_BOARD_REV_2_B,
+		.init = rpi_b_plus_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_3_model_a_plus[] = {
+	{
+		.hw_id = BCM2837B0_BOARD_REV_3A_PLUS,
+		.init = rpi_b_plus_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_3_model_b[] = {
+	{
+		.hw_id = BCM2837_BOARD_REV_3_B,
+		.init = rpi_b_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct rpi_machine_data rpi_3_model_b_plus[] = {
+	{
+		.hw_id = BCM2837B0_BOARD_REV_3B_PLUS,
+		.init = rpi_b_plus_init,
+	}, {
+		.hw_id = U8_MAX
+	},
+};
+
+static const struct of_device_id rpi_of_match[] = {
+	/* BCM2835 based Boards */
+	{ .compatible = "raspberrypi,model-a", .data = rpi_model_a },
+	{ .compatible = "raspberrypi,model-a-plus", .data = rpi_model_a_plus },
+	{ .compatible = "raspberrypi,model-b", .data = rpi_model_b },
+	/* Raspberry Pi Model B (no P5) */
+	{ .compatible = "raspberrypi,model-b-i2c0", .data = rpi_model_b_i2c0 },
+	{ .compatible = "raspberrypi,model-b-rev2", .data = rpi_model_b_rev2 },
+	{ .compatible = "raspberrypi,model-b-plus", .data = rpi_model_b_plus },
+	{ .compatible = "raspberrypi,compute-module", .data = rpi_compute_module },
+	{ .compatible = "raspberrypi,model-zero", .data = rpi_model_zero },
+	{ .compatible = "raspberrypi,model-zero-w", .data = rpi_model_zero_w },
+
+	/* BCM2836 based Boards */
+	{ .compatible = "raspberrypi,2-model-b", .data = rpi_2_model_b },
+
+	/* BCM2837 based Boards */
+	{ .compatible = "raspberrypi,3-model-a-plus", .data = rpi_3_model_a_plus },
+	{ .compatible = "raspberrypi,3-model-b", .data = rpi_3_model_b },
+	{ .compatible = "raspberrypi,3-model-b-plus", .data = rpi_3_model_b_plus },
+	{ /* sentinel */ },
+};
+BAREBOX_DEEP_PROBE_ENABLE(rpi_of_match);
+
+static struct driver_d rpi_board_driver = {
+	.name = "board-rpi",
+	.probe = rpi_devices_probe,
+	.of_compatible = DRV_OF_COMPAT(rpi_of_match),
+};
+late_platform_driver(rpi_board_driver);
diff --git a/arch/arm/mach-bcm283x/mbox.c b/arch/arm/mach-bcm283x/mbox.c
index 9839683d03..4b14afcfe4 100644
--- a/arch/arm/mach-bcm283x/mbox.c
+++ b/arch/arm/mach-bcm283x/mbox.c
@@ -13,6 +13,7 @@
 #include <dma.h>
 #include <init.h>
 #include <io.h>
+#include <of_address.h>
 
 #include <mach/mbox.h>
 
@@ -108,6 +109,21 @@ static void dump_buf(struct bcm2835_mbox_hdr *buffer)
 }
 #endif
 
+static int bcm2835_mbox_probe(void)
+{
+	struct device_node *mbox_node;
+
+	mbox_node = of_find_compatible_node(NULL, NULL, "brcm,bcm2835-mbox");
+	if (!mbox_node) {
+		pr_err("Missing mbox node\n");
+		return -ENOENT;
+	}
+
+	mbox_base = of_iomap(mbox_node, 0);
+
+	return 0;
+}
+
 int bcm2835_mbox_call_prop(u32 chan, struct bcm2835_mbox_hdr *buffer)
 {
 	int ret;
@@ -115,6 +131,12 @@ int bcm2835_mbox_call_prop(u32 chan, struct bcm2835_mbox_hdr *buffer)
 	struct bcm2835_mbox_tag_hdr *tag;
 	int tag_index;
 
+	if (!mbox_base) {
+		ret = bcm2835_mbox_probe();
+		if (ret)
+			return ret;
+	}
+
 	pr_debug("mbox: TX buffer\n");
 	dump_buf(buffer);
 
@@ -150,33 +172,3 @@ int bcm2835_mbox_call_prop(u32 chan, struct bcm2835_mbox_hdr *buffer)
 
 	return 0;
 }
-
-static int bcm2835_mbox_probe(struct device_d *dev)
-{
-	struct resource *iores;
-
-	iores = dev_request_mem_resource(dev, 0);
-	if (IS_ERR(iores)) {
-		dev_err(dev, "could not get memory region\n");
-		return PTR_ERR(iores);
-	}
-	mbox_base = IOMEM(iores->start);
-
-	return 0;
-}
-
-static __maybe_unused struct of_device_id bcm2835_mbox_dt_ids[] = {
-	{
-		.compatible = "brcm,bcm2835-mbox",
-	}, {
-		/* sentinel */
-	},
-};
-
-static struct driver_d bcm2835_mbox_driver = {
-	.name		= "bcm2835_mbox",
-	.of_compatible	= DRV_OF_COMPAT(bcm2835_mbox_dt_ids),
-	.probe		= bcm2835_mbox_probe,
-};
-
-core_platform_driver(bcm2835_mbox_driver);
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 499df2fe39..9675d45dc9 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -24,3 +24,4 @@ obj-$(CONFIG_ARCH_LAYERSCAPE)	+= clk-qoric.o
 obj-y				+= analogbits/
 obj-$(CONFIG_CLK_SIFIVE)	+= sifive/
 obj-$(CONFIG_SOC_STARFIVE)	+= starfive/
+obj-$(CONFIG_MACH_RPI_COMMON)	+= clk-rpi.o
diff --git a/drivers/clk/clk-rpi.c b/drivers/clk/clk-rpi.c
new file mode 100644
index 0000000000..59ae8e59ba
--- /dev/null
+++ b/drivers/clk/clk-rpi.c
@@ -0,0 +1,85 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include <common.h>
+#include <init.h>
+#include <driver.h>
+#include <linux/clk.h>
+#include <io.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+
+#include <mach/core.h>
+#include <mach/mbox.h>
+#include <mach/platform.h>
+#include <dt-bindings/clock/bcm2835.h>
+
+#define BCM2711_CLOCK_END			(BCM2711_CLOCK_EMMC2 + 1)
+
+static struct clk *clks[BCM2711_CLOCK_END];
+static struct clk_onecell_data clk_data;
+
+struct msg_get_clock_rate {
+	struct bcm2835_mbox_hdr hdr;
+	struct bcm2835_mbox_tag_get_clock_rate get_clock_rate;
+	u32 end_tag;
+};
+
+static struct clk *rpi_register_firmware_clock(u32 clock_id, const char *name)
+{
+	BCM2835_MBOX_STACK_ALIGN(struct msg_get_clock_rate, msg);
+	int ret;
+
+	BCM2835_MBOX_INIT_HDR(msg);
+	BCM2835_MBOX_INIT_TAG(&msg->get_clock_rate, GET_CLOCK_RATE);
+	msg->get_clock_rate.body.req.clock_id = clock_id;
+
+	ret = bcm2835_mbox_call_prop(BCM2835_MBOX_PROP_CHAN, &msg->hdr);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return clk_fixed(name, msg->get_clock_rate.body.resp.rate_hz);
+}
+
+static int bcm2835_cprman_probe(struct device_d *dev)
+{
+	struct clk *clk_cs;
+
+	clks[BCM2835_CLOCK_EMMC] =
+		rpi_register_firmware_clock(BCM2835_MBOX_CLOCK_ID_EMMC,
+					 "bcm2835_mci0");
+	if (IS_ERR(clks[BCM2835_CLOCK_EMMC]))
+		return PTR_ERR(clks[BCM2835_CLOCK_EMMC]);
+
+	clks[BCM2835_CLOCK_VPU] =
+		rpi_register_firmware_clock(BCM2835_MBOX_CLOCK_ID_CORE,
+					    "vpu");
+	if (IS_ERR(clks[BCM2835_CLOCK_VPU]))
+		return PTR_ERR(clks[BCM2835_CLOCK_VPU]);
+
+	clks[BCM2835_CLOCK_UART] = clk_fixed("uart0-pl0110", 48 * 1000 * 1000);
+	clk_register_clkdev(clks[BCM2835_CLOCK_UART], NULL, "uart0-pl0110");
+
+	clk_cs = clk_fixed("bcm2835-cs", 1 * 1000 * 1000);
+	clk_register_clkdev(clk_cs, NULL, "bcm2835-cs");
+
+	clk_data.clks = clks;
+	clk_data.clk_num = BCM2711_CLOCK_END;
+	of_clk_add_provider(dev->device_node, of_clk_src_onecell_get, &clk_data);
+
+	return 0;
+}
+
+static __maybe_unused struct of_device_id bcm2835_cprman_dt_ids[] = {
+	{
+		.compatible = "brcm,bcm2835-cprman",
+	}, {
+		/* sentinel */
+	}
+};
+
+static struct driver_d bcm2835_cprman_driver = {
+	.probe	= bcm2835_cprman_probe,
+	.name	= "bcm2835-cprman",
+	.of_compatible = DRV_OF_COMPAT(bcm2835_cprman_dt_ids),
+};
+core_platform_driver(bcm2835_cprman_driver);