11 files changed, 488 insertions, 156 deletions

diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index 76909ee2d9..316feba29d 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -22,20 +22,30 @@ config TEGRA_IVC
 
 config TEGRA_COMMON
 	bool "Tegra common options"
+	select CLK
 	select DM
 	select DM_ETH
 	select DM_GPIO
 	select DM_I2C
 	select DM_KEYBOARD
+	select DM_MMC
 	select DM_PCI
 	select DM_PCI_COMPAT
 	select DM_PWM
+	select DM_RESET
 	select DM_SERIAL
 	select DM_SPI
 	select DM_SPI_FLASH
+	select MISC
 	select OF_CONTROL
 	select VIDCONSOLE_AS_LCD if DM_VIDEO
 
+config TEGRA_NO_BPMP
+	bool "Tegra common options for SoCs without BPMP"
+	select TEGRA_CAR
+	select TEGRA_CAR_CLOCK
+	select TEGRA_CAR_RESET
+
 config TEGRA_ARMV7_COMMON
 	bool "Tegra 32-bit common options"
 	select CPU_V7
@@ -43,6 +53,7 @@ config TEGRA_ARMV7_COMMON
 	select SUPPORT_SPL
 	select TEGRA_COMMON
 	select TEGRA_GPIO
+	select TEGRA_NO_BPMP
 
 config TEGRA_ARMV8_COMMON
 	bool "Tegra 64-bit common options"
@@ -73,13 +84,11 @@ config TEGRA210
 	bool "Tegra210 family"
 	select TEGRA_GPIO
 	select TEGRA_ARMV8_COMMON
+	select TEGRA_NO_BPMP
 
 config TEGRA186
 	bool "Tegra186 family"
-	select CLK
 	select DM_MAILBOX
-	select DM_RESET
-	select MISC
 	select TEGRA186_BPMP
 	select TEGRA186_CLOCK
 	select TEGRA186_GPIO
diff --git a/arch/arm/mach-tegra/board186.c b/arch/arm/mach-tegra/board186.c
index 1b9799fd80..a071758afe 100644
--- a/arch/arm/mach-tegra/board186.c
+++ b/arch/arm/mach-tegra/board186.c
@@ -6,8 +6,6 @@
 
 #include <common.h>
 #include <asm/arch/tegra.h>
-#include <asm/arch-tegra/mmc.h>
-#include <asm/arch-tegra/tegra_mmc.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -30,14 +28,3 @@ int board_late_init(void)
 {
 	return 0;
 }
-
-void pad_init_mmc(struct mmc_host *host)
-{
-}
-
-int board_mmc_init(bd_t *bd)
-{
-	tegra_mmc_init();
-
-	return 0;
-}
diff --git a/arch/arm/mach-tegra/board2.c b/arch/arm/mach-tegra/board2.c
index 9158ace44c..cb9503f8e6 100644
--- a/arch/arm/mach-tegra/board2.c
+++ b/arch/arm/mach-tegra/board2.c
@@ -32,10 +32,6 @@
 #ifdef CONFIG_USB_EHCI_TEGRA
 #include <usb.h>
 #endif
-#ifdef CONFIG_TEGRA_MMC
-#include <asm/arch-tegra/tegra_mmc.h>
-#include <asm/arch-tegra/mmc.h>
-#endif
 #include <asm/arch-tegra/xusb-padctl.h>
 #include <power/as3722.h>
 #include <i2c.h>
@@ -54,6 +50,7 @@ U_BOOT_DEVICE(tegra_gpios) = {
 __weak void pinmux_init(void) {}
 __weak void pin_mux_usb(void) {}
 __weak void pin_mux_spi(void) {}
+__weak void pin_mux_mmc(void) {}
 __weak void gpio_early_init_uart(void) {}
 __weak void pin_mux_display(void) {}
 __weak void start_cpu_fan(void) {}
@@ -128,6 +125,10 @@ int board_init(void)
 	pin_mux_spi();
 #endif
 
+#ifdef CONFIG_TEGRA_MMC
+	pin_mux_mmc();
+#endif
+
 	/* Init is handled automatically in the driver-model case */
 #if defined(CONFIG_DM_VIDEO)
 	pin_mux_display();
@@ -230,54 +231,6 @@ int board_late_init(void)
 	return 0;
 }
 
-#if defined(CONFIG_TEGRA_MMC)
-__weak void pin_mux_mmc(void)
-{
-}
-
-/* this is a weak define that we are overriding */
-int board_mmc_init(bd_t *bd)
-{
-	debug("%s called\n", __func__);
-
-	/* Enable muxes, etc. for SDMMC controllers */
-	pin_mux_mmc();
-
-	debug("%s: init MMC\n", __func__);
-	tegra_mmc_init();
-
-	return 0;
-}
-
-void pad_init_mmc(struct mmc_host *host)
-{
-#if defined(CONFIG_TEGRA30)
-	enum periph_id id = host->mmc_id;
-	u32 val;
-
-	debug("%s: sdmmc address = %08x, id = %d\n", __func__,
-		(unsigned int)host->reg, id);
-
-	/* Set the pad drive strength for SDMMC1 or 3 only */
-	if (id != PERIPH_ID_SDMMC1 && id != PERIPH_ID_SDMMC3) {
-		debug("%s: settings are only valid for SDMMC1/SDMMC3!\n",
-			__func__);
-		return;
-	}
-
-	val = readl(&host->reg->sdmemcmppadctl);
-	val &= 0xFFFFFFF0;
-	val |= MEMCOMP_PADCTRL_VREF;
-	writel(val, &host->reg->sdmemcmppadctl);
-
-	val = readl(&host->reg->autocalcfg);
-	val &= 0xFFFF0000;
-	val |= AUTO_CAL_PU_OFFSET | AUTO_CAL_PD_OFFSET | AUTO_CAL_ENABLED;
-	writel(val, &host->reg->autocalcfg);
-#endif	/* T30 */
-}
-#endif	/* MMC */
-
 /*
  * In some SW environments, a memory carve-out exists to house a secure
  * monitor, a trusted OS, and/or various statically allocated media buffers.
diff --git a/arch/arm/mach-tegra/clock.c b/arch/arm/mach-tegra/clock.c
index 36eabc8f57..3bb72331a4 100644
--- a/arch/arm/mach-tegra/clock.c
+++ b/arch/arm/mach-tegra/clock.c
@@ -206,6 +206,29 @@ int clock_ll_set_source_bits(enum periph_id periph_id, int mux_bits,
 	return 0;
 }
 
+static int clock_ll_get_source_bits(enum periph_id periph_id, int mux_bits)
+{
+	u32 *reg = get_periph_source_reg(periph_id);
+	u32 val = readl(reg);
+
+	switch (mux_bits) {
+	case MASK_BITS_31_30:
+		val >>= OUT_CLK_SOURCE_31_30_SHIFT;
+		val &= OUT_CLK_SOURCE_31_30_MASK;
+		return val;
+	case MASK_BITS_31_29:
+		val >>= OUT_CLK_SOURCE_31_29_SHIFT;
+		val &= OUT_CLK_SOURCE_31_29_MASK;
+		return val;
+	case MASK_BITS_31_28:
+		val >>= OUT_CLK_SOURCE_31_28_SHIFT;
+		val &= OUT_CLK_SOURCE_31_28_MASK;
+		return val;
+	default:
+		return -1;
+	}
+}
+
 void clock_ll_set_source(enum periph_id periph_id, unsigned source)
 {
 	clock_ll_set_source_bits(periph_id, MASK_BITS_31_30, source);
@@ -288,9 +311,43 @@ unsigned long clock_get_periph_rate(enum periph_id periph_id,
 		enum clock_id parent)
 {
 	u32 *reg = get_periph_source_reg(periph_id);
+	unsigned parent_rate = pll_rate[parent];
+	int div = (readl(reg) & OUT_CLK_DIVISOR_MASK) >> OUT_CLK_DIVISOR_SHIFT;
+
+	switch (periph_id) {
+	case PERIPH_ID_UART1:
+	case PERIPH_ID_UART2:
+	case PERIPH_ID_UART3:
+	case PERIPH_ID_UART4:
+	case PERIPH_ID_UART5:
+#ifdef CONFIG_TEGRA20
+		/* There's no divider for these clocks in this SoC. */
+		return parent_rate;
+#else
+		/*
+		 * This undoes the +2 in get_rate_from_divider() which I
+		 * believe is incorrect. Ideally we would fix
+		 * get_rate_from_divider(), but... Removing the +2 from
+		 * get_rate_from_divider() would probably require remove the -2
+		 * from the tail of clk_get_divider() since I believe that's
+		 * only there to invert get_rate_from_divider()'s +2. Observe
+		 * how find_best_divider() uses those two functions together.
+		 * However, doing so breaks other stuff, such as Seaboard's
+		 * display, likely due to clock_set_pllout()'s call to
+		 * clk_get_divider(). Attempting to fix that by making
+		 * clock_set_pllout() subtract 2 from clk_get_divider()'s
+		 * return value doesn't help. In summary this clock driver is
+		 * quite broken but I'm afraid I have no idea how to fix it
+		 * without completely replacing it.
+		 */
+		div -= 2;
+		break;
+#endif
+	default:
+		break;
+	}
 
-	return get_rate_from_divider(pll_rate[parent],
-		(readl(reg) & OUT_CLK_DIVISOR_MASK) >> OUT_CLK_DIVISOR_SHIFT);
+	return get_rate_from_divider(parent_rate, div);
 }
 
 /**
@@ -363,6 +420,20 @@ static int adjust_periph_pll(enum periph_id periph_id, int source,
 	return 0;
 }
 
+enum clock_id clock_get_periph_parent(enum periph_id periph_id)
+{
+	int err, mux_bits, divider_bits, type;
+	int source;
+
+	err = get_periph_clock_info(periph_id, &mux_bits, &divider_bits, &type);
+	if (err)
+		return CLOCK_ID_NONE;
+
+	source = clock_ll_get_source_bits(periph_id, mux_bits);
+
+	return get_periph_clock_id(periph_id, source);
+}
+
 unsigned clock_adjust_periph_pll_div(enum periph_id periph_id,
 		enum clock_id parent, unsigned rate, int *extra_div)
 {
@@ -612,6 +683,8 @@ int clock_verify(void)
 
 void clock_init(void)
 {
+	int i;
+
 	pll_rate[CLOCK_ID_CGENERAL] = clock_get_rate(CLOCK_ID_CGENERAL);
 	pll_rate[CLOCK_ID_MEMORY] = clock_get_rate(CLOCK_ID_MEMORY);
 	pll_rate[CLOCK_ID_PERIPH] = clock_get_rate(CLOCK_ID_PERIPH);
@@ -630,6 +703,19 @@ void clock_init(void)
 	debug("PLLU = %d\n", pll_rate[CLOCK_ID_USB]);
 	debug("PLLD = %d\n", pll_rate[CLOCK_ID_DISPLAY]);
 	debug("PLLX = %d\n", pll_rate[CLOCK_ID_XCPU]);
+
+	for (i = 0; periph_clk_init_table[i].periph_id != -1; i++) {
+		enum periph_id periph_id;
+		enum clock_id parent;
+		int source, mux_bits, divider_bits;
+
+		periph_id = periph_clk_init_table[i].periph_id;
+		parent = periph_clk_init_table[i].parent_clock_id;
+
+		source = get_periph_clock_source(periph_id, parent, &mux_bits,
+						 &divider_bits);
+		clock_ll_set_source_bits(periph_id, mux_bits, source);
+	}
 }
 
 static void set_avp_clock_source(u32 src)
diff --git a/arch/arm/mach-tegra/tegra114/clock.c b/arch/arm/mach-tegra/tegra114/clock.c
index e6ef873c8d..177ab6b55b 100644
--- a/arch/arm/mach-tegra/tegra114/clock.c
+++ b/arch/arm/mach-tegra/tegra114/clock.c
@@ -496,6 +496,51 @@ u32 *get_periph_source_reg(enum periph_id periph_id)
 		return &clkrst->crc_clk_src[internal_id];
 }
 
+int get_periph_clock_info(enum periph_id periph_id, int *mux_bits,
+			  int *divider_bits, int *type)
+{
+	enum periphc_internal_id internal_id;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return -1;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return -1;
+
+	*type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(*type))
+		return -1;
+
+	*mux_bits = clock_source[*type][CLOCK_MAX_MUX];
+
+	if (*type == CLOCK_TYPE_PCMT16)
+		*divider_bits = 16;
+	else
+		*divider_bits = 8;
+
+	return 0;
+}
+
+enum clock_id get_periph_clock_id(enum periph_id periph_id, int source)
+{
+	enum periphc_internal_id internal_id;
+	int type;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return CLOCK_ID_NONE;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return CLOCK_ID_NONE;
+
+	type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(type))
+		return CLOCK_ID_NONE;
+
+	return clock_source[type][source];
+}
+
 /**
  * Given a peripheral ID and the required source clock, this returns which
  * value should be programmed into the source mux for that peripheral.
@@ -512,23 +557,10 @@ int get_periph_clock_source(enum periph_id periph_id,
 	enum clock_id parent, int *mux_bits, int *divider_bits)
 {
 	enum clock_type_id type;
-	enum periphc_internal_id internal_id;
-	int mux;
-
-	assert(clock_periph_id_isvalid(periph_id));
-
-	internal_id = periph_id_to_internal_id[periph_id];
-	assert(periphc_internal_id_isvalid(internal_id));
-
-	type = clock_periph_type[internal_id];
-	assert(clock_type_id_isvalid(type));
+	int mux, err;
 
-	*mux_bits = clock_source[type][CLOCK_MAX_MUX];
-
-	if (type == CLOCK_TYPE_PCMT16)
-		*divider_bits = 16;
-	else
-		*divider_bits = 8;
+	err = get_periph_clock_info(periph_id, mux_bits, divider_bits, &type);
+	assert(!err);
 
 	for (mux = 0; mux < CLOCK_MAX_MUX; mux++)
 		if (clock_source[type][mux] == parent)
@@ -699,3 +731,26 @@ void arch_timer_init(void)
 	writel(val, &sysctr->cntcr);
 	debug("%s: TSC CNTCR = 0x%08X\n", __func__, val);
 }
+
+struct periph_clk_init periph_clk_init_table[] = {
+	{ PERIPH_ID_SBC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC5, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC6, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_HOST1X, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_DISP1, CLOCK_ID_CGENERAL },
+	{ PERIPH_ID_NDFLASH, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_PWM, CLOCK_ID_SFROM32KHZ },
+	{ PERIPH_ID_I2C1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C5, CLOCK_ID_PERIPH },
+	{ -1, },
+};
diff --git a/arch/arm/mach-tegra/tegra124/clock.c b/arch/arm/mach-tegra/tegra124/clock.c
index 1e71146236..5e4406102f 100644
--- a/arch/arm/mach-tegra/tegra124/clock.c
+++ b/arch/arm/mach-tegra/tegra124/clock.c
@@ -642,6 +642,51 @@ u32 *get_periph_source_reg(enum periph_id periph_id)
 	}
 }
 
+int get_periph_clock_info(enum periph_id periph_id, int *mux_bits,
+			  int *divider_bits, int *type)
+{
+	enum periphc_internal_id internal_id;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return -1;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return -1;
+
+	*type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(*type))
+		return -1;
+
+	*mux_bits = clock_source[*type][CLOCK_MAX_MUX];
+
+	if (*type == CLOCK_TYPE_PC2CC3M_T16)
+		*divider_bits = 16;
+	else
+		*divider_bits = 8;
+
+	return 0;
+}
+
+enum clock_id get_periph_clock_id(enum periph_id periph_id, int source)
+{
+	enum periphc_internal_id internal_id;
+	int type;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return CLOCK_ID_NONE;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return CLOCK_ID_NONE;
+
+	type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(type))
+		return CLOCK_ID_NONE;
+
+	return clock_source[type][source];
+}
+
 /**
  * Given a peripheral ID and the required source clock, this returns which
  * value should be programmed into the source mux for that peripheral.
@@ -658,23 +703,10 @@ int get_periph_clock_source(enum periph_id periph_id,
 	enum clock_id parent, int *mux_bits, int *divider_bits)
 {
 	enum clock_type_id type;
-	enum periphc_internal_id internal_id;
-	int mux;
+	int mux, err;
 
-	assert(clock_periph_id_isvalid(periph_id));
-
-	internal_id = periph_id_to_internal_id[periph_id];
-	assert(periphc_internal_id_isvalid(internal_id));
-
-	type = clock_periph_type[internal_id];
-	assert(clock_type_id_isvalid(type));
-
-	*mux_bits = clock_source[type][CLOCK_MAX_MUX];
-
-	if (type == CLOCK_TYPE_PC2CC3M_T16)
-		*divider_bits = 16;
-	else
-		*divider_bits = 8;
+	err = get_periph_clock_info(periph_id, mux_bits, divider_bits, &type);
+	assert(!err);
 
 	for (mux = 0; mux < CLOCK_MAX_MUX; mux++)
 		if (clock_source[type][mux] == parent)
@@ -1107,3 +1139,26 @@ struct clk_pll_simple *clock_get_simple_pll(enum clock_id clkid)
 
 	return NULL;
 }
+
+struct periph_clk_init periph_clk_init_table[] = {
+	{ PERIPH_ID_SBC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC5, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC6, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_HOST1X, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_DISP1, CLOCK_ID_CGENERAL },
+	{ PERIPH_ID_SDMMC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_PWM, CLOCK_ID_SFROM32KHZ },
+	{ PERIPH_ID_I2C1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C5, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C6, CLOCK_ID_PERIPH },
+	{ -1, },
+};
diff --git a/arch/arm/mach-tegra/tegra186/Makefile b/arch/arm/mach-tegra/tegra186/Makefile
index 033d6005fb..7f46a057bc 100644
--- a/arch/arm/mach-tegra/tegra186/Makefile
+++ b/arch/arm/mach-tegra/tegra186/Makefile
@@ -3,5 +3,6 @@
 # SPDX-License-Identifier: GPL-2.0
 
 obj-y += ../board186.o
+obj-y += cache.o
 obj-y += nvtboot_ll.o
 obj-y += nvtboot_mem.o
diff --git a/arch/arm/mach-tegra/tegra186/cache.c b/arch/arm/mach-tegra/tegra186/cache.c
new file mode 100644
index 0000000000..adaed8968e
--- /dev/null
+++ b/arch/arm/mach-tegra/tegra186/cache.c
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2016, NVIDIA CORPORATION.
+ *
+ * SPDX-License-Identifier: GPL-2.0
+ */
+
+#include <common.h>
+#include <asm/system.h>
+
+#define SMC_SIP_INVOKE_MCE	0x82FFFF00
+#define MCE_SMC_ROC_FLUSH_CACHE	11
+
+int __asm_flush_l3_cache(void)
+{
+	struct pt_regs regs = {0};
+
+	isb();
+
+	regs.regs[0] = SMC_SIP_INVOKE_MCE | MCE_SMC_ROC_FLUSH_CACHE;
+	smc_call(&regs);
+
+	return 0;
+}
diff --git a/arch/arm/mach-tegra/tegra20/clock.c b/arch/arm/mach-tegra/tegra20/clock.c
index cd9fb9a553..ec04cf5261 100644
--- a/arch/arm/mach-tegra/tegra20/clock.c
+++ b/arch/arm/mach-tegra/tegra20/clock.c
@@ -413,46 +413,78 @@ u32 *get_periph_source_reg(enum periph_id periph_id)
 	return &clkrst->crc_clk_src[internal_id];
 }
 
-/**
- * Given a peripheral ID and the required source clock, this returns which
- * value should be programmed into the source mux for that peripheral.
- *
- * There is special code here to handle the one source type with 5 sources.
- *
- * @param periph_id	peripheral to start
- * @param source	PLL id of required parent clock
- * @param mux_bits	Set to number of bits in mux register: 2 or 4
- * @param divider_bits	Set to number of divider bits (8 or 16)
- * @return mux value (0-4, or -1 if not found)
- */
-int get_periph_clock_source(enum periph_id periph_id,
-		enum clock_id parent, int *mux_bits, int *divider_bits)
+int get_periph_clock_info(enum periph_id periph_id, int *mux_bits,
+			  int *divider_bits, int *type)
 {
-	enum clock_type_id type;
 	enum periphc_internal_id internal_id;
-	int mux;
 
-	assert(clock_periph_id_isvalid(periph_id));
+	if (!clock_periph_id_isvalid(periph_id))
+		return -1;
 
 	internal_id = periph_id_to_internal_id[periph_id];
-	assert(periphc_internal_id_isvalid(internal_id));
+	if (!periphc_internal_id_isvalid(internal_id))
+		return -1;
 
-	type = clock_periph_type[internal_id];
-	assert(clock_type_id_isvalid(type));
+	*type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(*type))
+		return -1;
 
 	/*
 	 * Special cases here for the clock with a 4-bit source mux and I2C
 	 * with its 16-bit divisor
 	 */
-	if (type == CLOCK_TYPE_PCXTS)
+	if (*type == CLOCK_TYPE_PCXTS)
 		*mux_bits = MASK_BITS_31_28;
 	else
 		*mux_bits = MASK_BITS_31_30;
-	if (type == CLOCK_TYPE_PCMT16)
+	if (*type == CLOCK_TYPE_PCMT16)
 		*divider_bits = 16;
 	else
 		*divider_bits = 8;
 
+	return 0;
+}
+
+enum clock_id get_periph_clock_id(enum periph_id periph_id, int source)
+{
+	enum periphc_internal_id internal_id;
+	int type;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return CLOCK_ID_NONE;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return CLOCK_ID_NONE;
+
+	type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(type))
+		return CLOCK_ID_NONE;
+
+	return clock_source[type][source];
+}
+
+/**
+ * Given a peripheral ID and the required source clock, this returns which
+ * value should be programmed into the source mux for that peripheral.
+ *
+ * There is special code here to handle the one source type with 5 sources.
+ *
+ * @param periph_id	peripheral to start
+ * @param source	PLL id of required parent clock
+ * @param mux_bits	Set to number of bits in mux register: 2 or 4
+ * @param divider_bits	Set to number of divider bits (8 or 16)
+ * @return mux value (0-4, or -1 if not found)
+ */
+int get_periph_clock_source(enum periph_id periph_id,
+		enum clock_id parent, int *mux_bits, int *divider_bits)
+{
+	enum clock_type_id type;
+	int mux, err;
+
+	err = get_periph_clock_info(periph_id, mux_bits, divider_bits, &type);
+	assert(!err);
+
 	for (mux = 0; mux < CLOCK_MAX_MUX; mux++)
 		if (clock_source[type][mux] == parent)
 			return mux;
@@ -717,3 +749,24 @@ int tegra_plle_enable(void)
 
 	return 0;
 }
+
+struct periph_clk_init periph_clk_init_table[] = {
+	{ PERIPH_ID_SPI1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_HOST1X, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_DISP1, CLOCK_ID_CGENERAL },
+	{ PERIPH_ID_NDFLASH, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_PWM, CLOCK_ID_SFROM32KHZ },
+	{ PERIPH_ID_DVC_I2C, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C3, CLOCK_ID_PERIPH },
+	{ -1, },
+};
diff --git a/arch/arm/mach-tegra/tegra210/clock.c b/arch/arm/mach-tegra/tegra210/clock.c
index f0052e7934..c8bb94626e 100644
--- a/arch/arm/mach-tegra/tegra210/clock.c
+++ b/arch/arm/mach-tegra/tegra210/clock.c
@@ -732,6 +732,51 @@ u32 *get_periph_source_reg(enum periph_id periph_id)
 	return &clkrst->crc_clk_src_y[internal_id];
 }
 
+int get_periph_clock_info(enum periph_id periph_id, int *mux_bits,
+			  int *divider_bits, int *type)
+{
+	enum periphc_internal_id internal_id;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return -1;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return -1;
+
+	*type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(*type))
+		return -1;
+
+	*mux_bits = clock_source[*type][CLOCK_MAX_MUX];
+
+	if (*type == CLOCK_TYPE_PC2CC3M_T16)
+		*divider_bits = 16;
+	else
+		*divider_bits = 8;
+
+	return 0;
+}
+
+enum clock_id get_periph_clock_id(enum periph_id periph_id, int source)
+{
+	enum periphc_internal_id internal_id;
+	int type;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return CLOCK_ID_NONE;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return CLOCK_ID_NONE;
+
+	type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(type))
+		return CLOCK_ID_NONE;
+
+	return clock_source[type][source];
+}
+
 /**
  * Given a peripheral ID and the required source clock, this returns which
  * value should be programmed into the source mux for that peripheral.
@@ -748,23 +793,10 @@ int get_periph_clock_source(enum periph_id periph_id,
 	enum clock_id parent, int *mux_bits, int *divider_bits)
 {
 	enum clock_type_id type;
-	enum periphc_internal_id internal_id;
-	int mux;
-
-	assert(clock_periph_id_isvalid(periph_id));
-
-	internal_id = INTERNAL_ID(periph_id_to_internal_id[periph_id]);
-	assert(periphc_internal_id_isvalid(internal_id));
-
-	type = clock_periph_type[internal_id];
-	assert(clock_type_id_isvalid(type));
+	int mux, err;
 
-	*mux_bits = clock_source[type][CLOCK_MAX_MUX];
-
-	if (type == CLOCK_TYPE_PC2CC3M_T16)
-		*divider_bits = 16;
-	else
-		*divider_bits = 8;
+	err = get_periph_clock_info(periph_id, mux_bits, divider_bits, &type);
+	assert(!err);
 
 	for (mux = 0; mux < CLOCK_MAX_MUX; mux++)
 		if (clock_source[type][mux] == parent)
@@ -1225,3 +1257,26 @@ int tegra_plle_enable(void)
 
 	return 0;
 }
+
+struct periph_clk_init periph_clk_init_table[] = {
+	{ PERIPH_ID_SBC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC5, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC6, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_HOST1X, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_DISP1, CLOCK_ID_CGENERAL },
+	{ PERIPH_ID_SDMMC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_PWM, CLOCK_ID_SFROM32KHZ },
+	{ PERIPH_ID_I2C1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C5, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C6, CLOCK_ID_PERIPH },
+	{ -1, },
+};
diff --git a/arch/arm/mach-tegra/tegra30/clock.c b/arch/arm/mach-tegra/tegra30/clock.c
index 1f104f288e..4fd8b8a3b1 100644
--- a/arch/arm/mach-tegra/tegra30/clock.c
+++ b/arch/arm/mach-tegra/tegra30/clock.c
@@ -476,6 +476,51 @@ u32 *get_periph_source_reg(enum periph_id periph_id)
 		return &clkrst->crc_clk_src[internal_id];
 }
 
+int get_periph_clock_info(enum periph_id periph_id, int *mux_bits,
+			  int *divider_bits, int *type)
+{
+	enum periphc_internal_id internal_id;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return -1;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return -1;
+
+	*type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(*type))
+		return -1;
+
+	*mux_bits = clock_source[*type][CLOCK_MAX_MUX];
+
+	if (*type == CLOCK_TYPE_PCMT16)
+		*divider_bits = 16;
+	else
+		*divider_bits = 8;
+
+	return 0;
+}
+
+enum clock_id get_periph_clock_id(enum periph_id periph_id, int source)
+{
+	enum periphc_internal_id internal_id;
+	int type;
+
+	if (!clock_periph_id_isvalid(periph_id))
+		return CLOCK_ID_NONE;
+
+	internal_id = periph_id_to_internal_id[periph_id];
+	if (!periphc_internal_id_isvalid(internal_id))
+		return CLOCK_ID_NONE;
+
+	type = clock_periph_type[internal_id];
+	if (!clock_type_id_isvalid(type))
+		return CLOCK_ID_NONE;
+
+	return clock_source[type][source];
+}
+
 /**
  * Given a peripheral ID and the required source clock, this returns which
  * value should be programmed into the source mux for that peripheral.
@@ -492,23 +537,10 @@ int get_periph_clock_source(enum periph_id periph_id,
 	enum clock_id parent, int *mux_bits, int *divider_bits)
 {
 	enum clock_type_id type;
-	enum periphc_internal_id internal_id;
-	int mux;
+	int mux, err;
 
-	assert(clock_periph_id_isvalid(periph_id));
-
-	internal_id = periph_id_to_internal_id[periph_id];
-	assert(periphc_internal_id_isvalid(internal_id));
-
-	type = clock_periph_type[internal_id];
-	assert(clock_type_id_isvalid(type));
-
-	*mux_bits = clock_source[type][CLOCK_MAX_MUX];
-
-	if (type == CLOCK_TYPE_PCMT16)
-		*divider_bits = 16;
-	else
-		*divider_bits = 8;
+	err = get_periph_clock_info(periph_id, mux_bits, divider_bits, &type);
+	assert(!err);
 
 	for (mux = 0; mux < CLOCK_MAX_MUX; mux++)
 		if (clock_source[type][mux] == parent)
@@ -763,3 +795,26 @@ int tegra_plle_enable(void)
 
 	return 0;
 }
+
+struct periph_clk_init periph_clk_init_table[] = {
+	{ PERIPH_ID_SBC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC5, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SBC6, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_HOST1X, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_DISP1, CLOCK_ID_CGENERAL },
+	{ PERIPH_ID_NDFLASH, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_SDMMC4, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_PWM, CLOCK_ID_SFROM32KHZ },
+	{ PERIPH_ID_DVC_I2C, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C1, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C2, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C3, CLOCK_ID_PERIPH },
+	{ PERIPH_ID_I2C4, CLOCK_ID_PERIPH },
+	{ -1, },
+};