14 files changed, 1033 insertions, 138 deletions

diff --git a/arch/arm/cpu/arm1136/cpu.c b/arch/arm/cpu/arm1136/cpu.c
index b98e3d9..32a4c24 100644
--- a/arch/arm/cpu/arm1136/cpu.c
+++ b/arch/arm/cpu/arm1136/cpu.c
@@ -141,16 +141,6 @@ void flush_cache(unsigned long start, unsigned long size)
 	flush_dcache_range(start, start + size);
 }
 
-void enable_caches(void)
-{
-#ifndef CONFIG_SYS_ICACHE_OFF
-	icache_enable();
-#endif
-#ifndef CONFIG_SYS_DCACHE_OFF
-	dcache_enable();
-#endif
-}
-
 #else /* #ifndef CONFIG_SYS_DCACHE_OFF */
 void invalidate_dcache_all(void)
 {
@@ -172,3 +162,15 @@ void flush_cache(unsigned long start, unsigned long size)
 {
 }
 #endif /* #ifndef CONFIG_SYS_DCACHE_OFF */
+
+#if !defined(CONFIG_SYS_ICACHE_OFF) || !defined(CONFIG_SYS_DCACHE_OFF)
+void enable_caches(void)
+{
+#ifndef CONFIG_SYS_ICACHE_OFF
+	icache_enable();
+#endif
+#ifndef CONFIG_SYS_DCACHE_OFF
+	dcache_enable();
+#endif
+}
+#endif
diff --git a/arch/arm/cpu/arm1176/s3c64xx/Makefile b/arch/arm/cpu/arm1176/s3c64xx/Makefile
index 0785b19..266a073 100644
--- a/arch/arm/cpu/arm1176/s3c64xx/Makefile
+++ b/arch/arm/cpu/arm1176/s3c64xx/Makefile
@@ -31,7 +31,7 @@ LIB	= $(obj)lib$(SOC).o
 SOBJS	= reset.o
 
 COBJS-$(CONFIG_S3C6400)	+= cpu_init.o speed.o
-COBJS-y	+= timer.o
+COBJS-y	+= timer.o init.o
 
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS-y))
 
diff --git a/arch/arm/cpu/arm1176/s3c64xx/init.c b/arch/arm/cpu/arm1176/s3c64xx/init.c
new file mode 100644
index 0000000..f113d8e
--- /dev/null
+++ b/arch/arm/cpu/arm1176/s3c64xx/init.c
@@ -0,0 +1,26 @@
+/*
+ * (C) Copyright 2012 Ashok Kumar Reddy Kourla
+ * ashokkourla2000@gmail.com
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include<common.h>
+
+int arch_cpu_init(void)
+{
+	icache_enable();
+
+	return 0;
+}
diff --git a/arch/arm/cpu/arm926ejs/mxs/clock.c b/arch/arm/cpu/arm926ejs/mxs/clock.c
index bfea6ab..4ff19c3 100644
--- a/arch/arm/cpu/arm926ejs/mxs/clock.c
+++ b/arch/arm/cpu/arm926ejs/mxs/clock.c
@@ -333,6 +333,8 @@ uint32_t mxc_get_clock(enum mxc_clock clk)
 		return mx28_get_sspclk(MXC_SSPCLK2);
 	case MXC_SSP3_CLK:
 		return mx28_get_sspclk(MXC_SSPCLK3);
+	case MXC_XTAL_CLK:
+		return XTAL_FREQ_KHZ * 1000;
 	}
 
 	return 0;
diff --git a/arch/arm/cpu/armv7/cache_v7.c b/arch/arm/cpu/armv7/cache_v7.c
index 1b4e808..5f6d039 100644
--- a/arch/arm/cpu/armv7/cache_v7.c
+++ b/arch/arm/cpu/armv7/cache_v7.c
@@ -297,6 +297,12 @@ void arm_init_before_mmu(void)
 	v7_inval_tlb();
 }
 
+void mmu_page_table_flush(unsigned long start, unsigned long stop)
+{
+	flush_dcache_range(start, stop);
+	v7_inval_tlb();
+}
+
 /*
  * Flush range from all levels of d-cache/unified-cache used:
  * Affects the range [start, start + size - 1]
@@ -329,6 +335,11 @@ void arm_init_before_mmu(void)
 void  flush_cache(unsigned long start, unsigned long size)
 {
 }
+
+void mmu_page_table_flush(unsigned long start, unsigned long stop)
+{
+}
+
 #endif /* #ifndef CONFIG_SYS_DCACHE_OFF */
 
 #ifndef CONFIG_SYS_ICACHE_OFF
diff --git a/arch/arm/cpu/armv7/exynos/clock.c b/arch/arm/cpu/armv7/exynos/clock.c
index 4f3b451..7459979 100644
--- a/arch/arm/cpu/armv7/exynos/clock.c
+++ b/arch/arm/cpu/armv7/exynos/clock.c
@@ -25,42 +25,32 @@
 #include <asm/io.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/clk.h>
-
-/* exynos4: return pll clock frequency */
-static unsigned long exynos4_get_pll_clk(int pllreg)
+#include <asm/arch/periph.h>
+
+/* Epll Clock division values to achive different frequency output */
+static struct set_epll_con_val exynos5_epll_div[] = {
+	{ 192000000, 0, 48, 3, 1, 0 },
+	{ 180000000, 0, 45, 3, 1, 0 },
+	{  73728000, 1, 73, 3, 3, 47710 },
+	{  67737600, 1, 90, 4, 3, 20762 },
+	{  49152000, 0, 49, 3, 3, 9961 },
+	{  45158400, 0, 45, 3, 3, 10381 },
+	{ 180633600, 0, 45, 3, 1, 10381 }
+};
+
+/* exynos: return pll clock frequency */
+static int exynos_get_pll_clk(int pllreg, unsigned int r, unsigned int k)
 {
-	struct exynos4_clock *clk =
-		(struct exynos4_clock *)samsung_get_base_clock();
-	unsigned long r, m, p, s, k = 0, mask, fout;
+	unsigned long m, p, s = 0, mask, fout;
 	unsigned int freq;
-
-	switch (pllreg) {
-	case APLL:
-		r = readl(&clk->apll_con0);
-		break;
-	case MPLL:
-		r = readl(&clk->mpll_con0);
-		break;
-	case EPLL:
-		r = readl(&clk->epll_con0);
-		k = readl(&clk->epll_con1);
-		break;
-	case VPLL:
-		r = readl(&clk->vpll_con0);
-		k = readl(&clk->vpll_con1);
-		break;
-	default:
-		printf("Unsupported PLL (%d)\n", pllreg);
-		return 0;
-	}
-
 	/*
 	 * APLL_CON: MIDV [25:16]
 	 * MPLL_CON: MIDV [25:16]
 	 * EPLL_CON: MIDV [24:16]
 	 * VPLL_CON: MIDV [24:16]
+	 * BPLL_CON: MIDV [25:16]: Exynos5
 	 */
-	if (pllreg == APLL || pllreg == MPLL)
+	if (pllreg == APLL || pllreg == MPLL || pllreg == BPLL)
 		mask = 0x3ff;
 	else
 		mask = 0x1ff;
@@ -92,13 +82,43 @@ static unsigned long exynos4_get_pll_clk(int pllreg)
 	return fout;
 }
 
+/* exynos4: return pll clock frequency */
+static unsigned long exynos4_get_pll_clk(int pllreg)
+{
+	struct exynos4_clock *clk =
+		(struct exynos4_clock *)samsung_get_base_clock();
+	unsigned long r, k = 0;
+
+	switch (pllreg) {
+	case APLL:
+		r = readl(&clk->apll_con0);
+		break;
+	case MPLL:
+		r = readl(&clk->mpll_con0);
+		break;
+	case EPLL:
+		r = readl(&clk->epll_con0);
+		k = readl(&clk->epll_con1);
+		break;
+	case VPLL:
+		r = readl(&clk->vpll_con0);
+		k = readl(&clk->vpll_con1);
+		break;
+	default:
+		printf("Unsupported PLL (%d)\n", pllreg);
+		return 0;
+	}
+
+	return exynos_get_pll_clk(pllreg, r, k);
+}
+
 /* exynos5: return pll clock frequency */
 static unsigned long exynos5_get_pll_clk(int pllreg)
 {
 	struct exynos5_clock *clk =
 		(struct exynos5_clock *)samsung_get_base_clock();
-	unsigned long r, m, p, s, k = 0, mask, fout;
-	unsigned int freq, pll_div2_sel, fout_sel;
+	unsigned long r, k = 0, fout;
+	unsigned int pll_div2_sel, fout_sel;
 
 	switch (pllreg) {
 	case APLL:
@@ -123,41 +143,7 @@ static unsigned long exynos5_get_pll_clk(int pllreg)
 		return 0;
 	}
 
-	/*
-	 * APLL_CON: MIDV [25:16]
-	 * MPLL_CON: MIDV [25:16]
-	 * EPLL_CON: MIDV [24:16]
-	 * VPLL_CON: MIDV [24:16]
-	 * BPLL_CON: MIDV [25:16]
-	 */
-	if (pllreg == APLL || pllreg == MPLL || pllreg == BPLL)
-		mask = 0x3ff;
-	else
-		mask = 0x1ff;
-
-	m = (r >> 16) & mask;
-
-	/* PDIV [13:8] */
-	p = (r >> 8) & 0x3f;
-	/* SDIV [2:0] */
-	s = r & 0x7;
-
-	freq = CONFIG_SYS_CLK_FREQ;
-
-	if (pllreg == EPLL) {
-		k = k & 0xffff;
-		/* FOUT = (MDIV + K / 65536) * FIN / (PDIV * 2^SDIV) */
-		fout = (m + k / 65536) * (freq / (p * (1 << s)));
-	} else if (pllreg == VPLL) {
-		k = k & 0xfff;
-		/* FOUT = (MDIV + K / 1024) * FIN / (PDIV * 2^SDIV) */
-		fout = (m + k / 1024) * (freq / (p * (1 << s)));
-	} else {
-		if (s < 1)
-			s = 1;
-		/* FOUT = MDIV * FIN / (PDIV * 2^(SDIV - 1)) */
-		fout = m * (freq / (p * (1 << (s - 1))));
-	}
+	fout = exynos_get_pll_clk(pllreg, r, k);
 
 	/* According to the user manual, in EVT1 MPLL and BPLL always gives
 	 * 1.6GHz clock, so divide by 2 to get 800MHz MPLL clock.*/
@@ -732,6 +718,224 @@ static unsigned long exynos5_get_i2c_clk(void)
 	return aclk_66;
 }
 
+int exynos5_set_epll_clk(unsigned long rate)
+{
+	unsigned int epll_con, epll_con_k;
+	unsigned int i;
+	unsigned int lockcnt;
+	unsigned int start;
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)samsung_get_base_clock();
+
+	epll_con = readl(&clk->epll_con0);
+	epll_con &= ~((EPLL_CON0_LOCK_DET_EN_MASK <<
+			EPLL_CON0_LOCK_DET_EN_SHIFT) |
+		EPLL_CON0_MDIV_MASK << EPLL_CON0_MDIV_SHIFT |
+		EPLL_CON0_PDIV_MASK << EPLL_CON0_PDIV_SHIFT |
+		EPLL_CON0_SDIV_MASK << EPLL_CON0_SDIV_SHIFT);
+
+	for (i = 0; i < ARRAY_SIZE(exynos5_epll_div); i++) {
+		if (exynos5_epll_div[i].freq_out == rate)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(exynos5_epll_div))
+		return -1;
+
+	epll_con_k = exynos5_epll_div[i].k_dsm << 0;
+	epll_con |= exynos5_epll_div[i].en_lock_det <<
+				EPLL_CON0_LOCK_DET_EN_SHIFT;
+	epll_con |= exynos5_epll_div[i].m_div << EPLL_CON0_MDIV_SHIFT;
+	epll_con |= exynos5_epll_div[i].p_div << EPLL_CON0_PDIV_SHIFT;
+	epll_con |= exynos5_epll_div[i].s_div << EPLL_CON0_SDIV_SHIFT;
+
+	/*
+	 * Required period ( in cycles) to genarate a stable clock output.
+	 * The maximum clock time can be up to 3000 * PDIV cycles of PLLs
+	 * frequency input (as per spec)
+	 */
+	lockcnt = 3000 * exynos5_epll_div[i].p_div;
+
+	writel(lockcnt, &clk->epll_lock);
+	writel(epll_con, &clk->epll_con0);
+	writel(epll_con_k, &clk->epll_con1);
+
+	start = get_timer(0);
+
+	 while (!(readl(&clk->epll_con0) &
+			(0x1 << EXYNOS5_EPLLCON0_LOCKED_SHIFT))) {
+		if (get_timer(start) > TIMEOUT_EPLL_LOCK) {
+			debug("%s: Timeout waiting for EPLL lock\n", __func__);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+void exynos5_set_i2s_clk_source(void)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)samsung_get_base_clock();
+
+	clrsetbits_le32(&clk->src_peric1, AUDIO1_SEL_MASK,
+			(CLK_SRC_SCLK_EPLL));
+}
+
+int exynos5_set_i2s_clk_prescaler(unsigned int src_frq,
+					unsigned int dst_frq)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)samsung_get_base_clock();
+	unsigned int div;
+
+	if ((dst_frq == 0) || (src_frq == 0)) {
+		debug("%s: Invalid requency input for prescaler\n", __func__);
+		debug("src frq = %d des frq = %d ", src_frq, dst_frq);
+		return -1;
+	}
+
+	div = (src_frq / dst_frq);
+	if (div > AUDIO_1_RATIO_MASK) {
+		debug("%s: Frequency ratio is out of range\n", __func__);
+		debug("src frq = %d des frq = %d ", src_frq, dst_frq);
+		return -1;
+	}
+	clrsetbits_le32(&clk->div_peric4, AUDIO_1_RATIO_MASK,
+				(div & AUDIO_1_RATIO_MASK));
+	return 0;
+}
+
+/**
+ * Linearly searches for the most accurate main and fine stage clock scalars
+ * (divisors) for a specified target frequency and scalar bit sizes by checking
+ * all multiples of main_scalar_bits values. Will always return scalars up to or
+ * slower than target.
+ *
+ * @param main_scalar_bits	Number of main scalar bits, must be > 0 and < 32
+ * @param fine_scalar_bits	Number of fine scalar bits, must be > 0 and < 32
+ * @param input_freq		Clock frequency to be scaled in Hz
+ * @param target_freq		Desired clock frequency in Hz
+ * @param best_fine_scalar	Pointer to store the fine stage divisor
+ *
+ * @return best_main_scalar	Main scalar for desired frequency or -1 if none
+ * found
+ */
+static int clock_calc_best_scalar(unsigned int main_scaler_bits,
+	unsigned int fine_scalar_bits, unsigned int input_rate,
+	unsigned int target_rate, unsigned int *best_fine_scalar)
+{
+	int i;
+	int best_main_scalar = -1;
+	unsigned int best_error = target_rate;
+	const unsigned int cap = (1 << fine_scalar_bits) - 1;
+	const unsigned int loops = 1 << main_scaler_bits;
+
+	debug("Input Rate is %u, Target is %u, Cap is %u\n", input_rate,
+			target_rate, cap);
+
+	assert(best_fine_scalar != NULL);
+	assert(main_scaler_bits <= fine_scalar_bits);
+
+	*best_fine_scalar = 1;
+
+	if (input_rate == 0 || target_rate == 0)
+		return -1;
+
+	if (target_rate >= input_rate)
+		return 1;
+
+	for (i = 1; i <= loops; i++) {
+		const unsigned int effective_div = max(min(input_rate / i /
+							target_rate, cap), 1);
+		const unsigned int effective_rate = input_rate / i /
+							effective_div;
+		const int error = target_rate - effective_rate;
+
+		debug("%d|effdiv:%u, effrate:%u, error:%d\n", i, effective_div,
+				effective_rate, error);
+
+		if (error >= 0 && error <= best_error) {
+			best_error = error;
+			best_main_scalar = i;
+			*best_fine_scalar = effective_div;
+		}
+	}
+
+	return best_main_scalar;
+}
+
+static int exynos5_set_spi_clk(enum periph_id periph_id,
+					unsigned int rate)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)samsung_get_base_clock();
+	int main;
+	unsigned int fine;
+	unsigned shift, pre_shift;
+	unsigned mask = 0xff;
+	u32 *reg;
+
+	main = clock_calc_best_scalar(4, 8, 400000000, rate, &fine);
+	if (main < 0) {
+		debug("%s: Cannot set clock rate for periph %d",
+				__func__, periph_id);
+		return -1;
+	}
+	main = main - 1;
+	fine = fine - 1;
+
+	switch (periph_id) {
+	case PERIPH_ID_SPI0:
+		reg = &clk->div_peric1;
+		shift = 0;
+		pre_shift = 8;
+		break;
+	case PERIPH_ID_SPI1:
+		reg = &clk->div_peric1;
+		shift = 16;
+		pre_shift = 24;
+		break;
+	case PERIPH_ID_SPI2:
+		reg = &clk->div_peric2;
+		shift = 0;
+		pre_shift = 8;
+		break;
+	case PERIPH_ID_SPI3:
+		reg = &clk->sclk_div_isp;
+		shift = 0;
+		pre_shift = 4;
+		break;
+	case PERIPH_ID_SPI4:
+		reg = &clk->sclk_div_isp;
+		shift = 12;
+		pre_shift = 16;
+		break;
+	default:
+		debug("%s: Unsupported peripheral ID %d\n", __func__,
+		      periph_id);
+		return -1;
+	}
+	clrsetbits_le32(reg, mask << shift, (main & mask) << shift);
+	clrsetbits_le32(reg, mask << pre_shift, (fine & mask) << pre_shift);
+
+	return 0;
+}
+
+static unsigned long exynos4_get_i2c_clk(void)
+{
+	struct exynos4_clock *clk =
+		(struct exynos4_clock *)samsung_get_base_clock();
+	unsigned long sclk, aclk_100;
+	unsigned int ratio;
+
+	sclk = get_pll_clk(APLL);
+
+	ratio = (readl(&clk->div_top)) >> 4;
+	ratio &= 0xf;
+	aclk_100 = sclk / (ratio + 1);
+	return aclk_100;
+}
+
 unsigned long get_pll_clk(int pllreg)
 {
 	if (cpu_is_exynos5())
@@ -752,6 +956,8 @@ unsigned long get_i2c_clk(void)
 {
 	if (cpu_is_exynos5()) {
 		return exynos5_get_i2c_clk();
+	} else if (cpu_is_exynos4()) {
+		return exynos4_get_i2c_clk();
 	} else {
 		debug("I2C clock is not set for this CPU\n");
 		return 0;
@@ -803,3 +1009,34 @@ void set_mipi_clk(void)
 	if (cpu_is_exynos4())
 		exynos4_set_mipi_clk();
 }
+
+int set_spi_clk(int periph_id, unsigned int rate)
+{
+	if (cpu_is_exynos5())
+		return exynos5_set_spi_clk(periph_id, rate);
+	else
+		return 0;
+}
+
+int set_i2s_clk_prescaler(unsigned int src_frq, unsigned int dst_frq)
+{
+
+	if (cpu_is_exynos5())
+		return exynos5_set_i2s_clk_prescaler(src_frq, dst_frq);
+	else
+		return 0;
+}
+
+void set_i2s_clk_source(void)
+{
+	if (cpu_is_exynos5())
+		exynos5_set_i2s_clk_source();
+}
+
+int set_epll_clk(unsigned long rate)
+{
+	if (cpu_is_exynos5())
+		return exynos5_set_epll_clk(rate);
+	else
+		return 0;
+}
diff --git a/arch/arm/cpu/armv7/exynos/pinmux.c b/arch/arm/cpu/armv7/exynos/pinmux.c
index 7776add..20a4b84 100644
--- a/arch/arm/cpu/armv7/exynos/pinmux.c
+++ b/arch/arm/cpu/armv7/exynos/pinmux.c
@@ -112,6 +112,7 @@ static int exynos5_mmc_config(int peripheral, int flags)
 		s5p_gpio_set_pull(bank, i, GPIO_PULL_UP);
 		s5p_gpio_set_drv(bank, i, GPIO_DRV_4X);
 	}
+
 	return 0;
 }
 
@@ -230,6 +231,59 @@ static void exynos5_i2c_config(int peripheral, int flags)
 	}
 }
 
+static void exynos5_i2s_config(int peripheral)
+{
+	int i;
+	struct exynos5_gpio_part1 *gpio1 =
+		(struct exynos5_gpio_part1 *) samsung_get_base_gpio_part1();
+
+	for (i = 0; i < 5; i++)
+		s5p_gpio_cfg_pin(&gpio1->b0, i, GPIO_FUNC(0x02));
+}
+
+void exynos5_spi_config(int peripheral)
+{
+	int cfg = 0, pin = 0, i;
+	struct s5p_gpio_bank *bank = NULL;
+	struct exynos5_gpio_part1 *gpio1 =
+		(struct exynos5_gpio_part1 *) samsung_get_base_gpio_part1();
+	struct exynos5_gpio_part2 *gpio2 =
+		(struct exynos5_gpio_part2 *) samsung_get_base_gpio_part2();
+
+	switch (peripheral) {
+	case PERIPH_ID_SPI0:
+		bank = &gpio1->a2;
+		cfg = GPIO_FUNC(0x2);
+		pin = 0;
+		break;
+	case PERIPH_ID_SPI1:
+		bank = &gpio1->a2;
+		cfg = GPIO_FUNC(0x2);
+		pin = 4;
+		break;
+	case PERIPH_ID_SPI2:
+		bank = &gpio1->b1;
+		cfg = GPIO_FUNC(0x5);
+		pin = 1;
+		break;
+	case PERIPH_ID_SPI3:
+		bank = &gpio2->f1;
+		cfg = GPIO_FUNC(0x2);
+		pin = 0;
+		break;
+	case PERIPH_ID_SPI4:
+		for (i = 0; i < 2; i++) {
+			s5p_gpio_cfg_pin(&gpio2->f0, i + 2, GPIO_FUNC(0x4));
+			s5p_gpio_cfg_pin(&gpio2->e0, i + 4, GPIO_FUNC(0x4));
+		}
+		break;
+	}
+	if (peripheral != PERIPH_ID_SPI4) {
+		for (i = pin; i < pin + 4; i++)
+			s5p_gpio_cfg_pin(bank, i, cfg);
+	}
+}
+
 static int exynos5_pinmux_config(int peripheral, int flags)
 {
 	switch (peripheral) {
@@ -257,6 +311,78 @@ static int exynos5_pinmux_config(int peripheral, int flags)
 	case PERIPH_ID_I2C7:
 		exynos5_i2c_config(peripheral, flags);
 		break;
+	case PERIPH_ID_I2S1:
+		exynos5_i2s_config(peripheral);
+		break;
+	case PERIPH_ID_SPI0:
+	case PERIPH_ID_SPI1:
+	case PERIPH_ID_SPI2:
+	case PERIPH_ID_SPI3:
+	case PERIPH_ID_SPI4:
+		exynos5_spi_config(peripheral);
+		break;
+	default:
+		debug("%s: invalid peripheral %d", __func__, peripheral);
+		return -1;
+	}
+
+	return 0;
+}
+
+static void exynos4_i2c_config(int peripheral, int flags)
+{
+	struct exynos4_gpio_part1 *gpio1 =
+		(struct exynos4_gpio_part1 *) samsung_get_base_gpio_part1();
+
+	switch (peripheral) {
+	case PERIPH_ID_I2C0:
+		s5p_gpio_cfg_pin(&gpio1->d1, 0, GPIO_FUNC(0x2));
+		s5p_gpio_cfg_pin(&gpio1->d1, 1, GPIO_FUNC(0x2));
+		break;
+	case PERIPH_ID_I2C1:
+		s5p_gpio_cfg_pin(&gpio1->d1, 2, GPIO_FUNC(0x2));
+		s5p_gpio_cfg_pin(&gpio1->d1, 3, GPIO_FUNC(0x2));
+		break;
+	case PERIPH_ID_I2C2:
+		s5p_gpio_cfg_pin(&gpio1->a0, 6, GPIO_FUNC(0x3));
+		s5p_gpio_cfg_pin(&gpio1->a0, 7, GPIO_FUNC(0x3));
+		break;
+	case PERIPH_ID_I2C3:
+		s5p_gpio_cfg_pin(&gpio1->a1, 2, GPIO_FUNC(0x3));
+		s5p_gpio_cfg_pin(&gpio1->a1, 3, GPIO_FUNC(0x3));
+		break;
+	case PERIPH_ID_I2C4:
+		s5p_gpio_cfg_pin(&gpio1->b, 2, GPIO_FUNC(0x3));
+		s5p_gpio_cfg_pin(&gpio1->b, 3, GPIO_FUNC(0x3));
+		break;
+	case PERIPH_ID_I2C5:
+		s5p_gpio_cfg_pin(&gpio1->b, 6, GPIO_FUNC(0x3));
+		s5p_gpio_cfg_pin(&gpio1->b, 7, GPIO_FUNC(0x3));
+		break;
+	case PERIPH_ID_I2C6:
+		s5p_gpio_cfg_pin(&gpio1->c1, 3, GPIO_FUNC(0x4));
+		s5p_gpio_cfg_pin(&gpio1->c1, 4, GPIO_FUNC(0x4));
+		break;
+	case PERIPH_ID_I2C7:
+		s5p_gpio_cfg_pin(&gpio1->d0, 2, GPIO_FUNC(0x3));
+		s5p_gpio_cfg_pin(&gpio1->d0, 3, GPIO_FUNC(0x3));
+		break;
+	}
+}
+
+static int exynos4_pinmux_config(int peripheral, int flags)
+{
+	switch (peripheral) {
+	case PERIPH_ID_I2C0:
+	case PERIPH_ID_I2C1:
+	case PERIPH_ID_I2C2:
+	case PERIPH_ID_I2C3:
+	case PERIPH_ID_I2C4:
+	case PERIPH_ID_I2C5:
+	case PERIPH_ID_I2C6:
+	case PERIPH_ID_I2C7:
+		exynos4_i2c_config(peripheral, flags);
+		break;
 	default:
 		debug("%s: invalid peripheral %d", __func__, peripheral);
 		return -1;
@@ -269,6 +395,8 @@ int exynos_pinmux_config(int peripheral, int flags)
 {
 	if (cpu_is_exynos5())
 		return exynos5_pinmux_config(peripheral, flags);
+	else if (cpu_is_exynos4())
+		return exynos4_pinmux_config(peripheral, flags);
 	else {
 		debug("pinmux functionality not supported\n");
 		return -1;
diff --git a/arch/arm/cpu/armv7/s5p-common/Makefile b/arch/arm/cpu/armv7/s5p-common/Makefile
index f975f3f..1705399 100644
--- a/arch/arm/cpu/armv7/s5p-common/Makefile
+++ b/arch/arm/cpu/armv7/s5p-common/Makefile
@@ -28,7 +28,6 @@ LIB	= $(obj)libs5p-common.o
 COBJS-y		+= cpu_info.o
 COBJS-y		+= timer.o
 COBJS-y		+= sromc.o
-COBJS-y		+= wdt.o
 COBJS-$(CONFIG_PWM)	+= pwm.o
 
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
diff --git a/arch/arm/cpu/armv7/s5p-common/wdt.c b/arch/arm/cpu/armv7/s5p-common/wdt.c
deleted file mode 100644
index 94acc1e..0000000
--- a/arch/arm/cpu/armv7/s5p-common/wdt.c
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * Copyright (C) 2012 Samsung Electronics
- * Minkyu Kang <mk7.kang@samsung.com>
- *
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- */
-
-#include <common.h>
-#include <asm/io.h>
-#include <asm/arch/watchdog.h>
-
-#define PRESCALER_VAL 255
-
-void wdt_stop(void)
-{
-	struct s5p_watchdog *wdt =
-		(struct s5p_watchdog *)samsung_get_base_watchdog();
-	unsigned int wtcon;
-
-	wtcon = readl(&wdt->wtcon);
-	wtcon &= ~(WTCON_EN | WTCON_INT | WTCON_RESET);
-
-	writel(wtcon, &wdt->wtcon);
-}
-
-void wdt_start(unsigned int timeout)
-{
-	struct s5p_watchdog *wdt =
-		(struct s5p_watchdog *)samsung_get_base_watchdog();
-	unsigned int wtcon;
-
-	wdt_stop();
-
-	wtcon = readl(&wdt->wtcon);
-	wtcon |= (WTCON_EN | WTCON_CLK(WTCON_CLK_128));
-	wtcon &= ~WTCON_INT;
-	wtcon |= WTCON_RESET;
-	wtcon |= WTCON_PRESCALER(PRESCALER_VAL);
-
-	writel(timeout, &wdt->wtdat);
-	writel(timeout, &wdt->wtcnt);
-	writel(wtcon, &wdt->wtcon);
-}
diff --git a/arch/arm/cpu/armv7/tegra20/Makefile b/arch/arm/cpu/armv7/tegra20/Makefile
index 09a0314..54ed8c4 100644
--- a/arch/arm/cpu/armv7/tegra20/Makefile
+++ b/arch/arm/cpu/armv7/tegra20/Makefile
@@ -28,6 +28,8 @@ include $(TOPDIR)/config.mk
 LIB	=  $(obj)lib$(SOC).o
 
 COBJS-$(CONFIG_USB_EHCI_TEGRA) += usb.o
+COBJS-$(CONFIG_PWM_TEGRA) += pwm.o
+COBJS-$(CONFIG_VIDEO_TEGRA) += display.o
 
 COBJS	:= $(COBJS-y)
 SRCS	:= $(COBJS:.o=.c)
diff --git a/arch/arm/cpu/armv7/tegra20/display.c b/arch/arm/cpu/armv7/tegra20/display.c
new file mode 100644
index 0000000..031f9a8
--- /dev/null
+++ b/arch/arm/cpu/armv7/tegra20/display.c
@@ -0,0 +1,409 @@
+/*
+ *  (C) Copyright 2010
+ *  NVIDIA Corporation <www.nvidia.com>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/tegra.h>
+#include <asm/arch/display.h>
+#include <asm/arch/dc.h>
+#include <asm/arch-tegra/clk_rst.h>
+#include <asm/arch-tegra/timer.h>
+
+static struct fdt_disp_config config;
+
+static void update_window(struct dc_ctlr *dc, struct disp_ctl_win *win)
+{
+	unsigned h_dda, v_dda;
+	unsigned long val;
+
+	val = readl(&dc->cmd.disp_win_header);
+	val |= WINDOW_A_SELECT;
+	writel(val, &dc->cmd.disp_win_header);
+
+	writel(win->fmt, &dc->win.color_depth);
+
+	clrsetbits_le32(&dc->win.byte_swap, BYTE_SWAP_MASK,
+			BYTE_SWAP_NOSWAP << BYTE_SWAP_SHIFT);
+
+	val = win->out_x << H_POSITION_SHIFT;
+	val |= win->out_y << V_POSITION_SHIFT;
+	writel(val, &dc->win.pos);
+
+	val = win->out_w << H_SIZE_SHIFT;
+	val |= win->out_h << V_SIZE_SHIFT;
+	writel(val, &dc->win.size);
+
+	val = (win->w * win->bpp / 8) << H_PRESCALED_SIZE_SHIFT;
+	val |= win->h << V_PRESCALED_SIZE_SHIFT;
+	writel(val, &dc->win.prescaled_size);
+
+	writel(0, &dc->win.h_initial_dda);
+	writel(0, &dc->win.v_initial_dda);
+
+	h_dda = (win->w * 0x1000) / max(win->out_w - 1, 1);
+	v_dda = (win->h * 0x1000) / max(win->out_h - 1, 1);
+
+	val = h_dda << H_DDA_INC_SHIFT;
+	val |= v_dda << V_DDA_INC_SHIFT;
+	writel(val, &dc->win.dda_increment);
+
+	writel(win->stride, &dc->win.line_stride);
+	writel(0, &dc->win.buf_stride);
+
+	val = WIN_ENABLE;
+	if (win->bpp < 24)
+		val |= COLOR_EXPAND;
+	writel(val, &dc->win.win_opt);
+
+	writel((unsigned long)win->phys_addr, &dc->winbuf.start_addr);
+	writel(win->x, &dc->winbuf.addr_h_offset);
+	writel(win->y, &dc->winbuf.addr_v_offset);
+
+	writel(0xff00, &dc->win.blend_nokey);
+	writel(0xff00, &dc->win.blend_1win);
+
+	val = GENERAL_ACT_REQ | WIN_A_ACT_REQ;
+	val |= GENERAL_UPDATE | WIN_A_UPDATE;
+	writel(val, &dc->cmd.state_ctrl);
+}
+
+static void write_pair(struct fdt_disp_config *config, int item, u32 *reg)
+{
+	writel(config->horiz_timing[item] |
+			(config->vert_timing[item] << 16), reg);
+}
+
+static int update_display_mode(struct dc_disp_reg *disp,
+		struct fdt_disp_config *config)
+{
+	unsigned long val;
+	unsigned long rate;
+	unsigned long div;
+
+	writel(0x0, &disp->disp_timing_opt);
+	write_pair(config, FDT_LCD_TIMING_REF_TO_SYNC, &disp->ref_to_sync);
+	write_pair(config, FDT_LCD_TIMING_SYNC_WIDTH, &disp->sync_width);
+	write_pair(config, FDT_LCD_TIMING_BACK_PORCH, &disp->back_porch);
+	write_pair(config, FDT_LCD_TIMING_FRONT_PORCH, &disp->front_porch);
+
+	writel(config->width | (config->height << 16), &disp->disp_active);
+
+	val = DE_SELECT_ACTIVE << DE_SELECT_SHIFT;
+	val |= DE_CONTROL_NORMAL << DE_CONTROL_SHIFT;
+	writel(val, &disp->data_enable_opt);
+
+	val = DATA_FORMAT_DF1P1C << DATA_FORMAT_SHIFT;
+	val |= DATA_ALIGNMENT_MSB << DATA_ALIGNMENT_SHIFT;
+	val |= DATA_ORDER_RED_BLUE << DATA_ORDER_SHIFT;
+	writel(val, &disp->disp_interface_ctrl);
+
+	/*
+	 * The pixel clock divider is in 7.1 format (where the bottom bit
+	 * represents 0.5). Here we calculate the divider needed to get from
+	 * the display clock (typically 600MHz) to the pixel clock. We round
+	 * up or down as requried.
+	 */
+	rate = clock_get_periph_rate(PERIPH_ID_DISP1, CLOCK_ID_CGENERAL);
+	div = ((rate * 2 + config->pixel_clock / 2) / config->pixel_clock) - 2;
+	debug("Display clock %lu, divider %lu\n", rate, div);
+
+	writel(0x00010001, &disp->shift_clk_opt);
+
+	val = PIXEL_CLK_DIVIDER_PCD1 << PIXEL_CLK_DIVIDER_SHIFT;
+	val |= div << SHIFT_CLK_DIVIDER_SHIFT;
+	writel(val, &disp->disp_clk_ctrl);
+
+	return 0;
+}
+
+/* Start up the display and turn on power to PWMs */
+static void basic_init(struct dc_cmd_reg *cmd)
+{
+	u32 val;
+
+	writel(0x00000100, &cmd->gen_incr_syncpt_ctrl);
+	writel(0x0000011a, &cmd->cont_syncpt_vsync);
+	writel(0x00000000, &cmd->int_type);
+	writel(0x00000000, &cmd->int_polarity);
+	writel(0x00000000, &cmd->int_mask);
+	writel(0x00000000, &cmd->int_enb);
+
+	val = PW0_ENABLE | PW1_ENABLE | PW2_ENABLE;
+	val |= PW3_ENABLE | PW4_ENABLE | PM0_ENABLE;
+	val |= PM1_ENABLE;
+	writel(val, &cmd->disp_pow_ctrl);
+
+	val = readl(&cmd->disp_cmd);
+	val |= CTRL_MODE_C_DISPLAY << CTRL_MODE_SHIFT;
+	writel(val, &cmd->disp_cmd);
+}
+
+static void basic_init_timer(struct dc_disp_reg *disp)
+{
+	writel(0x00000020, &disp->mem_high_pri);
+	writel(0x00000001, &disp->mem_high_pri_timer);
+}
+
+static const u32 rgb_enb_tab[PIN_REG_COUNT] = {
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+};
+
+static const u32 rgb_polarity_tab[PIN_REG_COUNT] = {
+	0x00000000,
+	0x01000000,
+	0x00000000,
+	0x00000000,
+};
+
+static const u32 rgb_data_tab[PIN_REG_COUNT] = {
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+};
+
+static const u32 rgb_sel_tab[PIN_OUTPUT_SEL_COUNT] = {
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00210222,
+	0x00002200,
+	0x00020000,
+};
+
+static void rgb_enable(struct dc_com_reg *com)
+{
+	int i;
+
+	for (i = 0; i < PIN_REG_COUNT; i++) {
+		writel(rgb_enb_tab[i], &com->pin_output_enb[i]);
+		writel(rgb_polarity_tab[i], &com->pin_output_polarity[i]);
+		writel(rgb_data_tab[i], &com->pin_output_data[i]);
+	}
+
+	for (i = 0; i < PIN_OUTPUT_SEL_COUNT; i++)
+		writel(rgb_sel_tab[i], &com->pin_output_sel[i]);
+}
+
+int setup_window(struct disp_ctl_win *win, struct fdt_disp_config *config)
+{
+	win->x = 0;
+	win->y = 0;
+	win->w = config->width;
+	win->h = config->height;
+	win->out_x = 0;
+	win->out_y = 0;
+	win->out_w = config->width;
+	win->out_h = config->height;
+	win->phys_addr = config->frame_buffer;
+	win->stride = config->width * (1 << config->log2_bpp) / 8;
+	debug("%s: depth = %d\n", __func__, config->log2_bpp);
+	switch (config->log2_bpp) {
+	case 5:
+	case 24:
+		win->fmt = COLOR_DEPTH_R8G8B8A8;
+		win->bpp = 32;
+		break;
+	case 4:
+		win->fmt = COLOR_DEPTH_B5G6R5;
+		win->bpp = 16;
+		break;
+
+	default:
+		debug("Unsupported LCD bit depth");
+		return -1;
+	}
+
+	return 0;
+}
+
+struct fdt_disp_config *tegra_display_get_config(void)
+{
+	return config.valid ? &config : NULL;
+}
+
+static void debug_timing(const char *name, unsigned int timing[])
+{
+#ifdef DEBUG
+	int i;
+
+	debug("%s timing: ", name);
+	for (i = 0; i < FDT_LCD_TIMING_COUNT; i++)
+		debug("%d ", timing[i]);
+	debug("\n");
+#endif
+}
+
+/**
+ * Decode panel information from the fdt, according to a standard binding
+ *
+ * @param blob		fdt blob
+ * @param node		offset of fdt node to read from
+ * @param config	structure to store fdt config into
+ * @return 0 if ok, -ve on error
+ */
+static int tegra_decode_panel(const void *blob, int node,
+			      struct fdt_disp_config *config)
+{
+	int front, back, ref;
+
+	config->width = fdtdec_get_int(blob, node, "xres", -1);
+	config->height = fdtdec_get_int(blob, node, "yres", -1);
+	config->pixel_clock = fdtdec_get_int(blob, node, "clock", 0);
+	if (!config->pixel_clock || config->width == -1 ||
+			config->height == -1) {
+		debug("%s: Pixel parameters missing\n", __func__);
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	back = fdtdec_get_int(blob, node, "left-margin", -1);
+	front = fdtdec_get_int(blob, node, "right-margin", -1);
+	ref = fdtdec_get_int(blob, node, "hsync-len", -1);
+	if ((back | front | ref) == -1) {
+		debug("%s: Horizontal parameters missing\n", __func__);
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	/* Use a ref-to-sync of 1 always, and take this from the front porch */
+	config->horiz_timing[FDT_LCD_TIMING_REF_TO_SYNC] = 1;
+	config->horiz_timing[FDT_LCD_TIMING_SYNC_WIDTH] = ref;
+	config->horiz_timing[FDT_LCD_TIMING_BACK_PORCH] = back;
+	config->horiz_timing[FDT_LCD_TIMING_FRONT_PORCH] = front -
+		config->horiz_timing[FDT_LCD_TIMING_REF_TO_SYNC];
+	debug_timing("horiz", config->horiz_timing);
+
+	back = fdtdec_get_int(blob, node, "upper-margin", -1);
+	front = fdtdec_get_int(blob, node, "lower-margin", -1);
+	ref = fdtdec_get_int(blob, node, "vsync-len", -1);
+	if ((back | front | ref) == -1) {
+		debug("%s: Vertical parameters missing\n", __func__);
+		return -FDT_ERR_NOTFOUND;
+	}
+
+	config->vert_timing[FDT_LCD_TIMING_REF_TO_SYNC] = 1;
+	config->vert_timing[FDT_LCD_TIMING_SYNC_WIDTH] = ref;
+	config->vert_timing[FDT_LCD_TIMING_BACK_PORCH] = back;
+	config->vert_timing[FDT_LCD_TIMING_FRONT_PORCH] = front -
+		config->vert_timing[FDT_LCD_TIMING_REF_TO_SYNC];
+	debug_timing("vert", config->vert_timing);
+
+	return 0;
+}
+
+/**
+ * Decode the display controller information from the fdt.
+ *
+ * @param blob		fdt blob
+ * @param config	structure to store fdt config into
+ * @return 0 if ok, -ve on error
+ */
+static int tegra_display_decode_config(const void *blob,
+				       struct fdt_disp_config *config)
+{
+	int node, rgb;
+	int bpp, bit;
+
+	/* TODO: Support multiple controllers */
+	node = fdtdec_next_compatible(blob, 0, COMPAT_NVIDIA_TEGRA20_DC);
+	if (node < 0) {
+		debug("%s: Cannot find display controller node in fdt\n",
+		      __func__);
+		return node;
+	}
+	config->disp = (struct disp_ctlr *)fdtdec_get_addr(blob, node, "reg");
+	if (!config->disp) {
+		debug("%s: No display controller address\n", __func__);
+		return -1;
+	}
+
+	rgb = fdt_subnode_offset(blob, node, "rgb");
+
+	config->panel_node = fdtdec_lookup_phandle(blob, rgb, "nvidia,panel");
+	if (!config->panel_node < 0) {
+		debug("%s: Cannot find panel information\n", __func__);
+		return -1;
+	}
+
+	if (tegra_decode_panel(blob, config->panel_node, config)) {
+		debug("%s: Failed to decode panel information\n", __func__);
+		return -1;
+	}
+
+	bpp = fdtdec_get_int(blob, config->panel_node, "nvidia,bits-per-pixel",
+			     -1);
+	bit = ffs(bpp) - 1;
+	if (bpp == (1 << bit))
+		config->log2_bpp = bit;
+	else
+		config->log2_bpp = bpp;
+	if (bpp == -1) {
+		debug("%s: Pixel bpp parameters missing\n", __func__);
+		return -FDT_ERR_NOTFOUND;
+	}
+	config->bpp = bpp;
+
+	config->valid = 1;	/* we have a valid configuration */
+
+	return 0;
+}
+
+int tegra_display_probe(const void *blob, void *default_lcd_base)
+{
+	struct disp_ctl_win window;
+	struct dc_ctlr *dc;
+
+	if (tegra_display_decode_config(blob, &config))
+		return -1;
+
+	config.frame_buffer = (u32)default_lcd_base;
+
+	dc = (struct dc_ctlr *)config.disp;
+
+	/*
+	 * A header file for clock constants was NAKed upstream.
+	 * TODO: Put this into the FDT and fdt_lcd struct when we have clock
+	 * support there
+	 */
+	clock_start_periph_pll(PERIPH_ID_HOST1X, CLOCK_ID_PERIPH,
+			       144 * 1000000);
+	clock_start_periph_pll(PERIPH_ID_DISP1, CLOCK_ID_CGENERAL,
+			       600 * 1000000);
+	basic_init(&dc->cmd);
+	basic_init_timer(&dc->disp);
+	rgb_enable(&dc->com);
+
+	if (config.pixel_clock)
+		update_display_mode(&dc->disp, &config);
+
+	if (setup_window(&window, &config))
+		return -1;
+
+	update_window(dc, &window);
+
+	return 0;
+}
diff --git a/arch/arm/cpu/armv7/tegra20/pwm.c b/arch/arm/cpu/armv7/tegra20/pwm.c
new file mode 100644
index 0000000..b655c5c
--- /dev/null
+++ b/arch/arm/cpu/armv7/tegra20/pwm.c
@@ -0,0 +1,101 @@
+/*
+ * Tegra2 pulse width frequency modulator definitions
+ *
+ * Copyright (c) 2011 The Chromium OS Authors.
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <fdtdec.h>
+#include <asm/io.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/pwm.h>
+
+struct pwm_info {
+	struct pwm_ctlr *pwm;		/* Registers for our pwm controller */
+	int pwm_node;			/* PWM device tree node */
+} local;
+
+void pwm_enable(unsigned channel, int rate, int pulse_width, int freq_divider)
+{
+	u32 reg;
+
+	assert(channel < PWM_NUM_CHANNELS);
+
+	/* TODO: Can we use clock_adjust_periph_pll_div() here? */
+	clock_start_periph_pll(PERIPH_ID_PWM, CLOCK_ID_SFROM32KHZ, rate);
+
+	reg = PWM_ENABLE_MASK;
+	reg |= pulse_width << PWM_WIDTH_SHIFT;
+	reg |= freq_divider << PWM_DIVIDER_SHIFT;
+	writel(reg, &local.pwm[channel].control);
+	debug("%s: channel=%d, rate=%d\n", __func__, channel, rate);
+}
+
+int pwm_request(const void *blob, int node, const char *prop_name)
+{
+	int pwm_node;
+	u32 data[3];
+
+	if (fdtdec_get_int_array(blob, node, prop_name, data,
+			ARRAY_SIZE(data))) {
+		debug("%s: Cannot decode PWM property '%s'\n", __func__,
+		      prop_name);
+		return -1;
+	}
+
+	pwm_node = fdt_node_offset_by_phandle(blob, data[0]);
+	if (pwm_node != local.pwm_node) {
+		debug("%s: PWM property '%s' phandle %d not recognised"
+		      "- expecting %d\n", __func__, prop_name, data[0],
+		      local.pwm_node);
+		return -1;
+	}
+	if (data[1] >= PWM_NUM_CHANNELS) {
+		debug("%s: PWM property '%s': invalid channel %u\n", __func__,
+		      prop_name, data[1]);
+		return -1;
+	}
+
+	/*
+	 * TODO: We could maintain a list of requests, but it might not be
+	 * worth it for U-Boot.
+	 */
+	return data[1];
+}
+
+int pwm_init(const void *blob)
+{
+	local.pwm_node = fdtdec_next_compatible(blob, 0,
+						COMPAT_NVIDIA_TEGRA20_PWM);
+	if (local.pwm_node < 0) {
+		debug("%s: Cannot find device tree node\n", __func__);
+		return -1;
+	}
+
+	local.pwm = (struct pwm_ctlr *)fdtdec_get_addr(blob, local.pwm_node,
+						       "reg");
+	if (local.pwm == (struct pwm_ctlr *)FDT_ADDR_T_NONE) {
+		debug("%s: Cannot find pwm reg address\n", __func__);
+		return -1;
+	}
+	debug("Tegra PWM at %p, node %d\n", local.pwm, local.pwm_node);
+
+	return 0;
+}
diff --git a/arch/arm/cpu/tegra20-common/funcmux.c b/arch/arm/cpu/tegra20-common/funcmux.c
index 00b8029..ece7ad9 100644
--- a/arch/arm/cpu/tegra20-common/funcmux.c
+++ b/arch/arm/cpu/tegra20-common/funcmux.c
@@ -25,6 +25,30 @@
 #include <asm/arch/funcmux.h>
 #include <asm/arch/pinmux.h>
 
+/*
+ * The PINMUX macro is used to set up pinmux tables.
+ */
+#define PINMUX(grp, mux, pupd, tri)                   \
+	{PINGRP_##grp, PMUX_FUNC_##mux, PMUX_PULL_##pupd, PMUX_TRI_##tri}
+
+static const struct pingroup_config disp1_default[] = {
+	PINMUX(LDI,   DISPA,      NORMAL,    NORMAL),
+	PINMUX(LHP0,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LHP1,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LHP2,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LHS,   DISPA,      NORMAL,    NORMAL),
+	PINMUX(LM0,   RSVD4,      NORMAL,    NORMAL),
+	PINMUX(LPP,   DISPA,      NORMAL,    NORMAL),
+	PINMUX(LPW0,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LPW2,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LSC0,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LSPI,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LVP1,  DISPA,      NORMAL,    NORMAL),
+	PINMUX(LVS,   DISPA,      NORMAL,    NORMAL),
+	PINMUX(SLXD,  SPDIF,      NORMAL,    NORMAL),
+};
+
+
 int funcmux_select(enum periph_id id, int config)
 {
 	int bad_config = config != FUNCMUX_DEFAULT;
@@ -257,6 +281,19 @@ int funcmux_select(enum periph_id id, int config)
 			break;
 		}
 		break;
+	case PERIPH_ID_DISP1:
+		if (config == FUNCMUX_DEFAULT) {
+			int i;
+
+			for (i = PINGRP_LD0; i <= PINGRP_LD17; i++) {
+				pinmux_set_func(i, PMUX_FUNC_DISPA);
+				pinmux_tristate_disable(i);
+				pinmux_set_pullupdown(i, PMUX_PULL_NORMAL);
+			}
+			pinmux_config_table(disp1_default,
+					    ARRAY_SIZE(disp1_default));
+		}
+		break;
 
 	default:
 		debug("%s: invalid periph_id %d", __func__, id);
diff --git a/arch/arm/cpu/tegra20-common/pinmux.c b/arch/arm/cpu/tegra20-common/pinmux.c
index 08b8305..a2a0916 100644
--- a/arch/arm/cpu/tegra20-common/pinmux.c
+++ b/arch/arm/cpu/tegra20-common/pinmux.c
@@ -554,7 +554,7 @@ void pinmux_set_func(enum pmux_pingrp pin, enum pmux_func func)
 	writel(reg, muxctl);
 }
 
-void pinmux_config_pingroup(struct pingroup_config *config)
+void pinmux_config_pingroup(const struct pingroup_config *config)
 {
 	enum pmux_pingrp pin = config->pingroup;
 
@@ -563,7 +563,7 @@ void pinmux_config_pingroup(struct pingroup_config *config)
 	pinmux_set_tristate(pin, config->tristate);
 }
 
-void pinmux_config_table(struct pingroup_config *config, int len)
+void pinmux_config_table(const struct pingroup_config *config, int len)
 {
 	int i;