Snow bootblock (bloated/debug version)

This is the bloated Snow bootblock which includes:
- SPI driver
- UART, including requisite I2C, Maxim PMIC, and clock config code.
- Adjustments for magic offsets (id section, stack pointer address)

This is just a temporary solution until we have romstage loading.
Once that happens, we'll rip out all but the code necessary for
copying SPI ROM content into SRAM.

Change-Id: I2a11e272eb9b6f626b5d9783eabb4a720a1d06be
Signed-off-by: David Hendricks <dhendrix@chromium.org>
Reviewed-on: http://review.coreboot.org/2170
Tested-by: build bot (Jenkins)
Reviewed-by: Ronald G. Minnich <rminnich@gmail.com>

8 files changed, 2173 insertions, 14 deletions

diff --git a/src/arch/armv7/Makefile.inc b/src/arch/armv7/Makefile.inc
index ca64f7873f..ecb5aee290 100644
--- a/src/arch/armv7/Makefile.inc
+++ b/src/arch/armv7/Makefile.inc
@@ -259,7 +259,7 @@ $(objcbfs)/bootblock.debug: $(objgenerated)/bootblock.o $(objgenerated)/bootbloc
 ifeq ($(CONFIG_COMPILER_LLVM_CLANG),y)
 	$(LD) -m armelf_linux_eabi -static -o $@.tmp -L$(obj) $< -T $(objgenerated)/bootblock.ld
 else
-	$(CC) -nostdlib -nostartfiles -static -o $@ -L$(obj) -T $(objgenerated)/bootblock.ld $<
+	$(CC) -nostdlib -nostartfiles -static -o $@ -L$(obj) -T $(objgenerated)/bootblock.ld -Wl,--start-group $< $(LIBGCC_FILE_NAME) -Wl,--end-group
 endif
 
 ################################################################################
diff --git a/src/arch/armv7/bootblock_simple.c b/src/arch/armv7/bootblock_simple.c
index c10ee1f033..6897e15fc6 100644
--- a/src/arch/armv7/bootblock_simple.c
+++ b/src/arch/armv7/bootblock_simple.c
@@ -40,6 +40,7 @@ void main(unsigned long bist)
 
 	if (boot_cpu()) {
 		bootblock_mainboard_init();
+		bootblock_cpu_init();
 	}
 
 	entry = findstage(target1);
diff --git a/src/arch/armv7/lib/id.lds b/src/arch/armv7/lib/id.lds
index f2b794a194..b521f3956b 100644
--- a/src/arch/armv7/lib/id.lds
+++ b/src/arch/armv7/lib/id.lds
@@ -1,6 +1,6 @@
 SECTIONS {
 	/* FIXME: determine a sensible location... */
-	. = (0x2024000) - (__id_end - __id_start);
+	. = (0x2026400);
 	.id (.): {
 		*(.id)
 	}
diff --git a/src/arch/armv7/romstage.ld b/src/arch/armv7/romstage.ld
index 61e3479cae..d40d2ef44a 100644
--- a/src/arch/armv7/romstage.ld
+++ b/src/arch/armv7/romstage.ld
@@ -35,11 +35,11 @@
 OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm")
 OUTPUT_ARCH(arm)
 
-ENTRY(_start)
+/* ENTRY(_start) */
 
 SECTIONS
 {
-	. = CONFIG_ROMSTAGE_BASE;
+	. = 0x02023400 + 0x4000;
 
 	.romtext . : {
 		_rom = .;
diff --git a/src/cpu/samsung/exynos5250/Kconfig b/src/cpu/samsung/exynos5250/Kconfig
index 3d66c77440..206df9260b 100644
--- a/src/cpu/samsung/exynos5250/Kconfig
+++ b/src/cpu/samsung/exynos5250/Kconfig
@@ -45,7 +45,7 @@ config IRAM_TOP
 
 config SYS_INIT_SP_ADDR
 	hex
-	default 0x0204F800
+	default 0x02058000
 
 config IRAM_STACK
 	hex
diff --git a/src/cpu/samsung/exynos5250/bootblock.c b/src/cpu/samsung/exynos5250/bootblock.c
index 58d0919f02..0d65676bb5 100644
--- a/src/cpu/samsung/exynos5250/bootblock.c
+++ b/src/cpu/samsung/exynos5250/bootblock.c
@@ -17,16 +17,24 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
  */
 
+#if 0
+/*
+ * Set/clear program flow prediction and return the previous state.
+ */
+static int config_branch_prediction(int set_cr_z)
+{
+	unsigned int cr;
+
+	/* System Control Register: 11th bit Z Branch prediction enable */
+	cr = get_cr();
+	set_cr(set_cr_z ? cr | CR_Z : cr & ~CR_Z);
+
+	return cr & CR_Z;
+}
+#endif
+
 void bootblock_cpu_init(void);
 void bootblock_cpu_init(void)
 {
-	/*
-	 * FIXME: this is a stub for now. It should eventually copy
-	 * romstage data (and maybe more) from SPI to SRAM.
-	 */
-#if 0
-	volatile unsigned long *addr = (unsigned long *)0x1004330c;
-	*addr |= 0x100;
-	while (1) ;
-#endif
+	/* FIXME: this is a stub for now */
 }
diff --git a/src/mainboard/google/snow/Kconfig b/src/mainboard/google/snow/Kconfig
index 21cdfa5fd2..8f41c64a40 100644
--- a/src/mainboard/google/snow/Kconfig
+++ b/src/mainboard/google/snow/Kconfig
@@ -56,6 +56,10 @@ config MAINBOARD_VENDOR
 	string
 	default "Samsung"
 
+config BOOTBLOCK_MAINBOARD_INIT
+	string
+	default "mainboard/google/snow/bootblock.c"
+
 # SPL (second-phase loader) stuff
 config SPL_TEXT_BASE
 	hex
diff --git a/src/mainboard/google/snow/bootblock.c b/src/mainboard/google/snow/bootblock.c
new file mode 100644
index 0000000000..a025b28740
--- /dev/null
+++ b/src/mainboard/google/snow/bootblock.c
@@ -0,0 +1,2146 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2013 The ChromiumOS Authors.  All rights reserved.
+ *
+ * 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; version 2 of the License.
+ *
+ * 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., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#define uchar unsigned char
+#define uint  unsigned int
+
+#include <stdlib.h>
+#include <types.h>
+#include <arch/io.h>
+#include "cpu/samsung/exynos5250/clk.h"
+#include "cpu/samsung/exynos5250/cpu.h"
+#include "cpu/samsung/exynos5250/dmc.h"
+#include "cpu/samsung/exynos5250/gpio.h"
+#include "cpu/samsung/exynos5250/periph.h"
+#include "cpu/samsung/exynos5250/setup.h"
+#include "cpu/samsung/exynos5250/clock_init.h"
+#include "cpu/samsung/s5p-common/gpio.h"
+#include "cpu/samsung/s5p-common/s3c24x0_i2c.h"
+#include "cpu/samsung/exynos5-common/spi.h"
+#include "cpu/samsung/exynos5-common/uart.h"
+
+#include <device/i2c.h>
+#include <drivers/maxim/max77686/max77686.h>
+
+#include <console/console.h>
+
+#define EXYNOS5_CLOCK_BASE		0x10010000
+
+volatile unsigned long *pshold = (unsigned long *)0x1004330c;
+
+/* FIXME(dhendrix): Can we move this SPI stuff elsewhere? */
+static void spi_rx_tx(struct exynos_spi *regs, int todo,
+			void *dinp, void const *doutp, int i)
+{
+	unsigned int *rxp = (unsigned int *)(dinp + (i * (32 * 1024)));
+	int rx_lvl, tx_lvl;
+	unsigned int out_bytes, in_bytes;
+
+	out_bytes = in_bytes = todo;
+	setbits_le32(&regs->ch_cfg, SPI_CH_RST);
+	clrbits_le32(&regs->ch_cfg, SPI_CH_RST);
+	writel(((todo * 8) / 32) | SPI_PACKET_CNT_EN, &regs->pkt_cnt);
+
+	while (in_bytes) {
+		uint32_t spi_sts;
+		int temp;
+
+		spi_sts = readl(&regs->spi_sts);
+		rx_lvl = ((spi_sts >> 15) & 0x7f);
+		tx_lvl = ((spi_sts >> 6) & 0x7f);
+		while (tx_lvl < 32 && out_bytes) {
+			temp = 0xffffffff;
+			writel(temp, &regs->tx_data);
+			out_bytes -= 4;
+			tx_lvl += 4;
+		}
+		while (rx_lvl >= 4 && in_bytes) {
+			temp = readl(&regs->rx_data);
+			if (rxp)
+				*rxp++ = temp;
+			in_bytes -= 4;
+			rx_lvl -= 4;
+		}
+	}
+}
+
+#if 0
+void clock_ll_set_pre_ratio(enum periph_id periph_id, unsigned divisor)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)samsung_get_base_clock();
+	unsigned shift;
+	unsigned mask = 0xff;
+	u32 *reg;
+
+	/*
+	 * For now we only handle a very small subset of peipherals here.
+	 * Others will need to (and do) mangle the clock registers
+	 * themselves, At some point it is hoped that this function can work
+	 * from a table or calculated register offset / mask. For now this
+	 * is at least better than spreading clock control code around
+	 * U-Boot.
+	 */
+	switch (periph_id) {
+	case PERIPH_ID_SPI0:
+		reg = &clk->div_peric1;
+		shift = 8;
+		break;
+	case PERIPH_ID_SPI1:
+		reg = &clk->div_peric1;
+		shift = 24;
+		break;
+	case PERIPH_ID_SPI2:
+		reg = &clk->div_peric2;
+		shift = 8;
+		break;
+	case PERIPH_ID_SPI3:
+		reg = &clk->sclk_div_isp;
+		shift = 4;
+		break;
+	case PERIPH_ID_SPI4:
+		reg = &clk->sclk_div_isp;
+		shift = 16;
+		break;
+	default:
+		debug("%s: Unsupported peripheral ID %d\n", __func__,
+		      periph_id);
+		return;
+	}
+}
+#endif
+void clock_ll_set_pre_ratio(enum periph_id periph_id, unsigned divisor)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)EXYNOS5_CLOCK_BASE;
+	unsigned shift;
+	unsigned mask = 0xff;
+	u32 *reg;
+
+	reg = &clk->div_peric1;
+	shift = 24;
+	clrsetbits_le32(reg, mask << shift, (divisor & mask) << shift);
+}
+
+#if 0
+void clock_ll_set_ratio(enum periph_id periph_id, unsigned divisor)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)samsung_get_base_clock();
+	unsigned shift;
+	unsigned mask = 0xff;
+	u32 *reg;
+
+	switch (periph_id) {
+	case PERIPH_ID_SPI0:
+		reg = &clk->div_peric1;
+		shift = 0;
+		break;
+	case PERIPH_ID_SPI1:
+		reg = &clk->div_peric1;
+		shift = 16;
+		break;
+	case PERIPH_ID_SPI2:
+		reg = &clk->div_peric2;
+		shift = 0;
+		break;
+	case PERIPH_ID_SPI3:
+		reg = &clk->sclk_div_isp;
+		shift = 0;
+		break;
+	case PERIPH_ID_SPI4:
+		reg = &clk->sclk_div_isp;
+		shift = 12;
+		break;
+	default:
+		debug("%s: Unsupported peripheral ID %d\n", __func__,
+		      periph_id);
+		return;
+	}
+}
+#endif
+void clock_ll_set_ratio(enum periph_id periph_id, unsigned divisor)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)EXYNOS5_CLOCK_BASE;
+	unsigned shift;
+	unsigned mask = 0xff;
+	u32 *reg;
+
+	reg = &clk->div_peric1;
+	shift = 16;
+	clrsetbits_le32(reg, mask << shift, (divisor & mask) << shift);
+}
+
+/**
+ * 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;
+
+#if 0
+	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);
+#endif
+
+	*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;
+
+#if 0
+		debug("%d|effdiv:%u, effrate:%u, error:%d\n", i, effective_div,
+				effective_rate, error);
+#endif
+
+		if (error >= 0 && error <= best_error) {
+			best_error = error;
+			best_main_scalar = i;
+			*best_fine_scalar = effective_div;
+		}
+	}
+
+	return best_main_scalar;
+}
+
+#if 0
+int clock_set_rate(enum periph_id periph_id, unsigned int rate)
+{
+	int main;
+	unsigned int fine;
+
+	switch (periph_id) {
+	case PERIPH_ID_SPI0:
+	case PERIPH_ID_SPI1:
+	case PERIPH_ID_SPI2:
+	case PERIPH_ID_SPI3:
+	case PERIPH_ID_SPI4:
+		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;
+		}
+		clock_ll_set_ratio(periph_id, main - 1);
+		clock_ll_set_pre_ratio(periph_id, fine - 1);
+		break;
+	default:
+		debug("%s: Unsupported peripheral ID %d\n", __func__,
+		      periph_id);
+		return -1;
+	}
+	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;
+	}
+	clock_ll_set_ratio(PERIPH_ID_SPI1, main - 1);
+	clock_ll_set_pre_ratio(PERIPH_ID_SPI1, fine - 1);
+
+	return 0;
+}
+#endif
+int clock_set_rate(enum periph_id periph_id, unsigned int rate)
+{
+	int main;
+	unsigned int fine;
+
+	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;
+	}
+	clock_ll_set_ratio(-1, main - 1);
+	clock_ll_set_pre_ratio(-1, fine - 1);
+
+	return 0;
+}
+
+struct gpio_info {
+	unsigned int reg_addr;	/* Address of register for this part */
+	unsigned int max_gpio;	/* Maximum GPIO in this part */
+};
+
+static const struct gpio_info gpio_data[EXYNOS_GPIO_NUM_PARTS] = {
+	{ EXYNOS5_GPIO_PART1_BASE, GPIO_MAX_PORT_PART_1 },
+	{ EXYNOS5_GPIO_PART2_BASE, GPIO_MAX_PORT_PART_2 },
+	{ EXYNOS5_GPIO_PART3_BASE, GPIO_MAX_PORT_PART_3 },
+	{ EXYNOS5_GPIO_PART4_BASE, GPIO_MAX_PORT_PART_4 },
+	{ EXYNOS5_GPIO_PART5_BASE, GPIO_MAX_PORT_PART_5 },
+	{ EXYNOS5_GPIO_PART6_BASE, GPIO_MAX_PORT },
+};
+
+static struct s5p_gpio_bank *gpio_get_bank(unsigned int gpio)
+{
+	const struct gpio_info *data;
+	unsigned int upto;
+	int i;
+
+	for (i = upto = 0, data = gpio_data; i < EXYNOS_GPIO_NUM_PARTS;
+			i++, upto = data->max_gpio, data++) {
+		if (gpio < data->max_gpio) {
+			struct s5p_gpio_bank *bank;
+
+			bank = (struct s5p_gpio_bank *)data->reg_addr;
+			bank += (gpio - upto) / GPIO_PER_BANK;
+			return bank;
+		}
+	}
+
+#ifndef CONFIG_SPL_BUILD
+	assert(gpio < GPIO_MAX_PORT);	/* ...which it will not be */
+#endif
+	return NULL;
+}
+
+#define CON_MASK(x)		(0xf << ((x) << 2))
+#define CON_SFR(x, v)		((v) << ((x) << 2))
+
+/* This macro gets gpio pin offset from 0..7 */
+#define GPIO_BIT(x)     ((x) & 0x7)
+
+void gpio_cfg_pin(int gpio, int cfg)
+{
+	unsigned int value;
+	struct s5p_gpio_bank *bank = gpio_get_bank(gpio);
+
+	value = readl(&bank->con);
+	value &= ~CON_MASK(GPIO_BIT(gpio));
+	value |= CON_SFR(GPIO_BIT(gpio), cfg);
+	writel(value, &bank->con);
+}
+
+//static void exynos_spi_copy(unsigned int uboot_size)
+static void copy_romstage(uint32_t spi_addr, uint32_t sram_addr, unsigned int len)
+{
+	int upto, todo;
+	int i;
+//	struct exynos_spi *regs = (struct exynos_spi *)samsung_get_base_spi1();
+	struct exynos_spi *regs = (struct exynos_spi *)0x12d30000;
+
+	clock_set_rate(PERIPH_ID_SPI1, 50000000); /* set spi clock to 50Mhz */
+	/* set the spi1 GPIO */
+//	exynos_pinmux_config(PERIPH_ID_SPI1, PINMUX_FLAG_NONE);
+	gpio_cfg_pin(GPIO_A24, 0x2);
+	gpio_cfg_pin(GPIO_A25, 0x2);
+	gpio_cfg_pin(GPIO_A26, 0x2);
+	gpio_cfg_pin(GPIO_A27, 0x2);
+
+	/* set pktcnt and enable it */
+	writel(4 | SPI_PACKET_CNT_EN, &regs->pkt_cnt);
+	/* set FB_CLK_SEL */
+	writel(SPI_FB_DELAY_180, &regs->fb_clk);
+	/* set CH_WIDTH and BUS_WIDTH as word */
+	setbits_le32(&regs->mode_cfg, SPI_MODE_CH_WIDTH_WORD |
+					SPI_MODE_BUS_WIDTH_WORD);
+	clrbits_le32(&regs->ch_cfg, SPI_CH_CPOL_L); /* CPOL: active high */
+
+	/* clear rx and tx channel if set priveously */
+	clrbits_le32(&regs->ch_cfg, SPI_RX_CH_ON | SPI_TX_CH_ON);
+
+	setbits_le32(&regs->swap_cfg, SPI_RX_SWAP_EN |
+		SPI_RX_BYTE_SWAP |
+		SPI_RX_HWORD_SWAP);
+
+	/* do a soft reset */
+	setbits_le32(&regs->ch_cfg, SPI_CH_RST);
+	clrbits_le32(&regs->ch_cfg, SPI_CH_RST);
+
+	/* now set rx and tx channel ON */
+	setbits_le32(&regs->ch_cfg, SPI_RX_CH_ON | SPI_TX_CH_ON | SPI_CH_HS_EN);
+	clrbits_le32(&regs->cs_reg, SPI_SLAVE_SIG_INACT); /* CS low */
+
+	/* Send read instruction (0x3h) followed by a 24 bit addr */
+	writel((SF_READ_DATA_CMD << 24) | spi_addr, &regs->tx_data);
+
+	/* waiting for TX done */
+	while (!(readl(&regs->spi_sts) & SPI_ST_TX_DONE));
+
+	for (upto = 0, i = 0; upto < len; upto += todo, i++) {
+		todo = MIN(len - upto, (1 << 15));
+		spi_rx_tx(regs, todo, (void *)(sram_addr),
+					(void *)(spi_addr), i);
+	}
+
+	setbits_le32(&regs->cs_reg, SPI_SLAVE_SIG_INACT);/* make the CS high */
+
+	/*
+	 * Let put controller mode to BYTE as
+	 * SPI driver does not support WORD mode yet
+	 */
+	clrbits_le32(&regs->mode_cfg, SPI_MODE_CH_WIDTH_WORD |
+					SPI_MODE_BUS_WIDTH_WORD);
+	writel(0, &regs->swap_cfg);
+
+	/*
+	 * Flush spi tx, rx fifos and reset the SPI controller
+	 * and clear rx/tx channel
+	 */
+	clrsetbits_le32(&regs->ch_cfg, SPI_CH_HS_EN, SPI_CH_RST);
+	clrbits_le32(&regs->ch_cfg, SPI_CH_RST);
+	clrbits_le32(&regs->ch_cfg, SPI_TX_CH_ON | SPI_RX_CH_ON);
+}
+
+/* Pull mode */
+#define EXYNOS_GPIO_PULL_NONE	0x0
+#define EXYNOS_GPIO_PULL_DOWN	0x1
+#define EXYNOS_GPIO_PULL_UP	0x3
+
+#define PULL_MASK(x)		(0x3 << ((x) << 1))
+#define PULL_MODE(x, v)		((v) << ((x) << 1))
+
+void gpio_set_pull(int gpio, int mode)
+{
+	unsigned int value;
+	struct s5p_gpio_bank *bank = gpio_get_bank(gpio);
+
+	value = readl(&bank->pull);
+	value &= ~PULL_MASK(GPIO_BIT(gpio));
+
+	switch (mode) {
+	case EXYNOS_GPIO_PULL_DOWN:
+	case EXYNOS_GPIO_PULL_UP:
+		value |= PULL_MODE(GPIO_BIT(gpio), mode);
+		break;
+	default:
+		break;
+	}
+
+	writel(value, &bank->pull);
+}
+
+static uint32_t uart3_base = CONFIG_CONSOLE_SERIAL_UART_ADDRESS;
+
+#define CONFIG_SYS_CLK_FREQ            24000000
+
+/* exynos5: return pll clock frequency */
+unsigned long get_pll_clk(int pllreg);
+unsigned long get_pll_clk(int pllreg)
+{
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)EXYNOS5_CLOCK_BASE;
+	unsigned long r, m, p, s, k = 0, mask, fout;
+	unsigned int freq;
+
+	switch (pllreg) {
+	case APLL:
+		r = readl(&clk->apll_con0);
+		break;
+	case BPLL:
+		r = readl(&clk->bpll_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:
+//		printk(BIOS_DEBUG, "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]
+	 */
+	if (pllreg == APLL || pllreg == BPLL || pllreg == MPLL)
+		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 {
+		/* FOUT = MDIV * FIN / (PDIV * 2^SDIV) */
+		fout = m * (freq / (p * (1 << s)));
+	}
+
+	return fout;
+}
+
+/* src_bit div_bit prediv_bit */
+static struct clk_bit_info clk_bit_info[PERIPH_ID_COUNT] = {
+	{0,	4,	0,	-1},
+	{4,	4,	4,	-1},
+	{8,	4,	8,	-1},
+	{12,	4,	12,	-1},
+	{0,	4,	0,	8},
+	{4,	4,	16,	24},
+	{8,	4,	0,	8},
+	{12,	4,	16,	24},
+	{-1,	-1,	-1,	-1},
+	{16,	4,	0,	8}, /* PERIPH_ID_SROMC */
+	{20,	4,	16,	24},
+	{24,	4,	0,	8},
+	{0,	4,	0,	4},
+	{4,	4,	12,	16},
+	{-1,	4,	-1,	-1},
+	{-1,	4,	-1,	-1},
+	{-1,	4,	24,	0},
+	{-1,	4,	24,	0},
+	{-1,	4,	24,	0},
+	{-1,	4,	24,	0},
+	{-1,	4,	24,	0},
+	{-1,	4,	24,	0},
+	{-1,	4,	24,	0},
+	{-1,	4,	24,	0},
+	{24,	4,	0,	-1},
+	{24,	4,	0,	-1},
+	{24,	4,	0,	-1},
+	{24,	4,	0,	-1},
+	{24,	4,	0,	-1},
+	{-1,	-1,	-1,	-1},
+	{-1,	-1,	-1,	-1},
+	{-1,	-1,	-1,	-1}, /* PERIPH_ID_I2S1 */
+	{24,	1,	20,	-1}, /* PERIPH_ID_SATA */
+};
+
+static unsigned long my_clock_get_periph_rate(enum periph_id peripheral)
+{
+//	struct exynos5_clock *clk =
+//		(struct exynos5_clock *)samsung_get_base_clock();
+	struct exynos5_clock *clk =
+		(struct exynos5_clock *)EXYNOS5_CLOCK_BASE;
+	struct clk_bit_info *bit_info = &clk_bit_info[peripheral];
+//	struct clk_bit_info bit_info = { 12, 4, 12, -1 };
+	unsigned long sclk, sub_clk;
+	unsigned int src, div, sub_div;
+
+	switch (peripheral) {
+	case PERIPH_ID_UART0:
+	case PERIPH_ID_UART1:
+	case PERIPH_ID_UART2:
+	case PERIPH_ID_UART3:
+		src = readl(&clk->src_peric0);
+		div = readl(&clk->div_peric0);
+		break;
+	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:
+		src = 0;
+		sclk = get_pll_clk(MPLL);
+		sub_div = ((readl(&clk->div_top1) >> bit_info->div_bit) & 0x7) + 1;
+		div = ((readl(&clk->div_top0) >> bit_info->prediv_bit) & 0x7) + 1;
+		return (sclk / sub_div) / div;
+	default:
+		return -1;
+	};
+
+	src = (src >> bit_info->src_bit) & ((1 << bit_info->n_src_bits) - 1);
+	if (src == SRC_MPLL)
+		sclk = get_pll_clk(MPLL);
+	else if (src == SRC_EPLL)
+		sclk = get_pll_clk(EPLL);
+	else if (src == SRC_VPLL)
+		sclk = get_pll_clk(VPLL);
+	else
+		return 0;
+
+	sub_div = (div >> bit_info->div_bit) & 0xf;
+	sub_clk = sclk / (sub_div + 1);
+
+	return sub_clk;
+}
+
+static void serial_setbrg_dev(void)
+{
+//	struct s5p_uart *const uart = s5p_get_base_uart(dev_index);
+	struct s5p_uart *uart = (struct s5p_uart *)uart3_base;
+	u32 uclk;
+	u32 baudrate = CONFIG_TTYS0_BAUD;
+	u32 val;
+//	enum periph_id periph;
+
+//	periph = exynos5_get_periph_id(base_port);
+	uclk = my_clock_get_periph_rate(PERIPH_ID_UART3);
+	val = uclk / baudrate;
+
+	writel(val / 16 - 1, &uart->ubrdiv);
+
+	/*
+	 * FIXME(dhendrix): the original uart.h had a "br_rest" value which
+	 * does not seem relevant to the exynos5250... not entirely sure
+	 * where/if we need to worry about it here
+	 */
+#if 0
+	if (s5p_uart_divslot())
+		writew(udivslot[val % 16], &uart->rest.slot);
+	else
+		writeb(val % 16, &uart->rest.value);
+#endif
+}
+
+/*
+ * Initialise the serial port with the given baudrate. The settings
+ * are always 8 data bits, no parity, 1 stop bit, no start bits.
+ */
+static void exynos5_init_dev(void)
+{
+//	struct s5p_uart *const uart = s5p_get_base_uart(dev_index);
+	struct s5p_uart *uart = (struct s5p_uart *)uart3_base;
+
+	/* enable FIFOs */
+	writel(0x1, &uart->ufcon);
+	writel(0, &uart->umcon);
+	/* 8N1 */
+	writel(0x3, &uart->ulcon);
+	/* No interrupts, no DMA, pure polling */
+	writel(0x245, &uart->ucon);
+
+	serial_setbrg_dev();
+}
+
+static int exynos5_uart_err_check(int op)
+{
+	//struct s5p_uart *const uart = s5p_get_base_uart(dev_index);
+	struct s5p_uart *uart = (struct s5p_uart *)uart3_base;
+	unsigned int mask;
+
+	/*
+	 * UERSTAT
+	 * Break Detect	[3]
+	 * Frame Err	[2] : receive operation
+	 * Parity Err	[1] : receive operation
+	 * Overrun Err	[0] : receive operation
+	 */
+	if (op)
+		mask = 0x8;
+	else
+		mask = 0xf;
+
+	return readl(&uart->uerstat) & mask;
+}
+
+#define RX_FIFO_COUNT_MASK	0xff
+#define RX_FIFO_FULL_MASK	(1 << 8)
+#define TX_FIFO_FULL_MASK	(1 << 24)
+
+#if 0
+/*
+ * Read a single byte from the serial port. Returns 1 on success, 0
+ * otherwise. When the function is succesfull, the character read is
+ * written into its argument c.
+ */
+static unsigned char exynos5_uart_rx_byte(void)
+{
+//	struct s5p_uart *const uart = s5p_get_base_uart(dev_index);
+	struct s5p_uart *uart = (struct s5p_uart *)uart3_base;
+
+	/* wait for character to arrive */
+	while (!(readl(&uart->ufstat) & (RX_FIFO_COUNT_MASK |
+					 RX_FIFO_FULL_MASK))) {
+		if (exynos5_uart_err_check(0))
+			return 0;
+	}
+
+	return readb(&uart->urxh) & 0xff;
+}
+#endif
+
+/*
+ * Output a single byte to the serial port.
+ */
+static void exynos5_uart_tx_byte(unsigned char data)
+{
+//	struct s5p_uart *const uart = s5p_get_base_uart(dev_index);
+	struct s5p_uart *uart = (struct s5p_uart *)uart3_base;
+
+	/* wait for room in the tx FIFO */
+	while ((readl(uart->ufstat) & TX_FIFO_FULL_MASK)) {
+		if (exynos5_uart_err_check(1))
+			return;
+	}
+
+	writeb(data, &uart->utxh);
+}
+
+void puts(const char *s);
+void puts(const char *s)
+{
+	int n = 0;
+
+	while (*s) {
+		exynos5_uart_tx_byte(*s++);
+		n++;
+	}
+
+	exynos5_uart_tx_byte(0xd);	/* CR */
+	exynos5_uart_tx_byte(0xa);	/* LF */
+}
+
+static void do_serial(void)
+{
+	//exynos_pinmux_config(PERIPH_ID_UART3, PINMUX_FLAG_NONE);
+	gpio_set_pull(GPIO_A14, EXYNOS_GPIO_PULL_NONE);
+	gpio_cfg_pin(GPIO_A15, EXYNOS_GPIO_FUNC(0x2));
+
+	exynos5_init_dev();
+}
+
+#define I2C_WRITE	0
+#define I2C_READ	1
+
+#define I2C_OK		0
+#define I2C_NOK		1
+#define I2C_NACK	2
+#define I2C_NOK_LA	3	/* Lost arbitration */
+#define I2C_NOK_TOUT	4	/* time out */
+
+#define I2CSTAT_BSY	0x20	/* Busy bit */
+#define I2CSTAT_NACK	0x01	/* Nack bit */
+#define I2CCON_ACKGEN	0x80	/* Acknowledge generation */
+#define I2CCON_IRPND	0x10	/* Interrupt pending bit */
+#define I2C_MODE_MT	0xC0	/* Master Transmit Mode */
+#define I2C_MODE_MR	0x80	/* Master Receive Mode */
+#define I2C_START_STOP	0x20	/* START / STOP */
+#define I2C_TXRX_ENA	0x10	/* I2C Tx/Rx enable */
+
+
+/* The timeouts we live by */
+enum {
+	I2C_XFER_TIMEOUT_MS	= 35,	/* xfer to complete */
+	I2C_INIT_TIMEOUT_MS	= 1000,	/* bus free on init */
+	I2C_IDLE_TIMEOUT_MS	= 100,	/* waiting for bus idle */
+	I2C_STOP_TIMEOUT_US	= 200,	/* waiting for stop events */
+};
+
+#define I2C0_BASE	0x12c60000
+struct s3c24x0_i2c_bus i2c0 = {
+	.node = 0,
+	.bus_num = 0,
+	.regs = (struct s3c24x0_i2c *)I2C0_BASE,
+	.id = PERIPH_ID_I2C0,
+};
+
+static void i2c_ch_init(struct s3c24x0_i2c *i2c, int speed, int slaveadd)
+{
+	unsigned long freq, pres = 16, div;
+
+	freq = my_clock_get_periph_rate(PERIPH_ID_I2C0);
+	/* calculate prescaler and divisor values */
+	if ((freq / pres / (16 + 1)) > speed)
+		/* set prescaler to 512 */
+		pres = 512;
+
+	div = 0;
+
+	while ((freq / pres / (div + 1)) > speed)
+		div++;
+
+	/* set prescaler, divisor according to freq, also set ACKGEN, IRQ */
+	writel((div & 0x0F) | 0xA0 | ((pres == 512) ? 0x40 : 0), &i2c->iiccon);
+
+	/* init to SLAVE REVEIVE and set slaveaddr */
+	writel(0, &i2c->iicstat);
+	writel(slaveadd, &i2c->iicadd);
+	/* program Master Transmit (and implicit STOP) */
+	writel(I2C_MODE_MT | I2C_TXRX_ENA, &i2c->iicstat);
+}
+
+static void i2c_bus_init(struct s3c24x0_i2c_bus *i2c, unsigned int bus)
+{
+//	exynos_pinmux_config(i2c->id, 0);
+	gpio_cfg_pin(GPIO_B30, EXYNOS_GPIO_FUNC(0x2));
+	gpio_cfg_pin(GPIO_B31, EXYNOS_GPIO_FUNC(0x2));
+	gpio_set_pull(GPIO_B30, EXYNOS_GPIO_PULL_NONE);
+	gpio_set_pull(GPIO_B31, EXYNOS_GPIO_PULL_NONE);
+
+	i2c_ch_init(i2c->regs, CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE);
+}
+
+void do_barriers(void);
+void do_barriers(void)
+{
+	/*
+	 * The reason we don't write out the instructions dsb/isb/sev:
+	 * While ARM Cortex-A8 supports ARM v7 instruction set (-march=armv7a),
+	 * we compile with -march=armv5 to allow more compilers to work.
+	 * For U-Boot code this has no performance impact.
+	 */
+	__asm__ __volatile__(
+#if defined(__thumb__)
+	".hword 0xF3BF, 0x8F4F\n"  /* dsb; darn -march=armv5 */
+	".hword 0xF3BF, 0x8F6F\n"  /* isb; darn -march=armv5 */
+	".hword 0xBF40\n"          /* sev; darn -march=armv5 */
+#else
+	".word  0xF57FF04F\n"      /* dsb; darn -march=armv5 */
+	".word  0xF57FF06F\n"      /* isb; darn -march=armv5 */
+	".word  0xE320F004\n"      /* sev; darn -march=armv5 */
+#endif
+	);
+}
+
+void sdelay(unsigned long loops);
+void sdelay(unsigned long loops)
+{
+	__asm__ volatile ("1:\n" "subs %0, %1, #1\n"
+			  "bne 1b":"=r" (loops):"0"(loops));
+}
+
+/* is this right? meh, it seems to work well enough... */
+void my_udelay(unsigned int n);
+void my_udelay(unsigned int n)
+{
+	sdelay(n * 1000);
+}
+
+
+void i2c_init(int speed, int slaveadd)
+{
+	struct s3c24x0_i2c_bus *i2c = &i2c0;
+	struct exynos5_gpio_part1 *gpio;
+	int i;
+	uint32_t x;
+
+#if 0
+	/* By default i2c channel 0 is the current bus */
+	g_current_bus = 0;
+
+	i2c = get_bus(g_current_bus);
+	if (!i2c)
+		return;
+#endif
+
+	i2c_bus_init(i2c, 0);
+
+	/* wait for some time to give previous transfer a chance to finish */
+	i = I2C_INIT_TIMEOUT_MS * 20;
+	while ((readl(&i2c->regs->iicstat) & I2CSTAT_BSY) && (i > 0)) {
+		my_udelay(50);
+		i--;
+	}
+
+	gpio = (struct exynos5_gpio_part1 *)(EXYNOS5_GPIO_PART1_BASE);
+	/* FIXME(dhendrix): cannot use nested macro (compilation failure) */
+//	writel((readl(&gpio->b3.con) & ~0x00FF) | 0x0022, &gpio->b3.con);
+	x = readl(&gpio->b3.con);
+	writel((x & ~0x00FF) | 0x0022, &gpio->b3.con);
+
+	i2c_ch_init(i2c->regs, speed, slaveadd);
+}
+
+static int WaitForXfer(struct s3c24x0_i2c *i2c)
+{
+	int i;
+
+	i = I2C_XFER_TIMEOUT_MS * 20;
+	while (!(readl(&i2c->iiccon) & I2CCON_IRPND)) {
+		if (i == 0) {
+			//debug("%s: i2c xfer timeout\n", __func__);
+			return I2C_NOK_TOUT;
+		}
+		my_udelay(50);
+		i--;
+	}
+
+	return I2C_OK;
+}
+
+static int IsACK(struct s3c24x0_i2c *i2c)
+{
+	return !(readl(&i2c->iicstat) & I2CSTAT_NACK);
+}
+
+static void ReadWriteByte(struct s3c24x0_i2c *i2c)
+{
+	uint32_t x;
+
+	x = readl(&i2c->iiccon);
+	writel(x & ~I2CCON_IRPND, &i2c->iiccon);
+	/* FIXME(dhendrix): cannot use nested macro (compilation failure) */
+//	writel(readl(&i2c->iiccon) & ~I2CCON_IRPND, &i2c->iiccon);
+}
+
+/*
+ * Verify the whether I2C ACK was received or not
+ *
+ * @param i2c	pointer to I2C register base
+ * @param buf	array of data
+ * @param len	length of data
+ * return	I2C_OK when transmission done
+ *		I2C_NACK otherwise
+ */
+static int i2c_send_verify(struct s3c24x0_i2c *i2c, unsigned char buf[],
+			   unsigned char len)
+{
+	int i, result = I2C_OK;
+
+	if (IsACK(i2c)) {
+		for (i = 0; (i < len) && (result == I2C_OK); i++) {
+			writel(buf[i], &i2c->iicds);
+			ReadWriteByte(i2c);
+			result = WaitForXfer(i2c);
+			if (result == I2C_OK && !IsACK(i2c))
+				result = I2C_NACK;
+		}
+	} else {
+		result = I2C_NACK;
+	}
+
+	return result;
+}
+
+/*
+ * Send a STOP event and wait for it to have completed
+ *
+ * @param mode	If it is a master transmitter or receiver
+ * @return I2C_OK if the line became idle before timeout I2C_NOK_TOUT otherwise
+ */
+static int i2c_send_stop(struct s3c24x0_i2c *i2c, int mode)
+{
+	int timeout;
+
+	/* Setting the STOP event to fire */
+	writel(mode | I2C_TXRX_ENA, &i2c->iicstat);
+	ReadWriteByte(i2c);
+
+	/* Wait for the STOP to send and the bus to go idle */
+	for (timeout = I2C_STOP_TIMEOUT_US; timeout > 0; timeout -= 5) {
+		if (!(readl(&i2c->iicstat) & I2CSTAT_BSY))
+			return I2C_OK;
+		my_udelay(5);
+	}
+
+	return I2C_NOK_TOUT;
+}
+
+/*
+ * cmd_type is 0 for write, 1 for read.
+ *
+ * addr_len can take any value from 0-255, it is only limited
+ * by the char, we could make it larger if needed. If it is
+ * 0 we skip the address write cycle.
+ */
+static int i2c_transfer(struct s3c24x0_i2c *i2c,
+			unsigned char cmd_type,
+			unsigned char chip,
+			unsigned char addr[],
+			unsigned char addr_len,
+			unsigned char data[],
+			unsigned short data_len)
+{
+	int i, result, stop_bit_result;
+	uint32_t x;
+
+	if (data == 0 || data_len == 0) {
+		/* Don't support data transfer of no length or to address 0 */
+		//debug("i2c_transfer: bad call\n");
+		return I2C_NOK;
+	}
+
+	/* Check I2C bus idle */
+	i = I2C_IDLE_TIMEOUT_MS * 20;
+	while ((readl(&i2c->iicstat) & I2CSTAT_BSY) && (i > 0)) {
+		my_udelay(50);
+		i--;
+	}
+
+	if (readl(&i2c->iicstat) & I2CSTAT_BSY) {
+		//debug("%s: bus busy\n", __func__);
+		return I2C_NOK_TOUT;
+	}
+
+	/* FIXME(dhendrix): cannot use nested macro (compilation failure) */
+	//writel(readl(&i2c->iiccon) | I2CCON_ACKGEN, &i2c->iiccon);
+	x = readl(&i2c->iiccon);
+	writel(x | I2CCON_ACKGEN, &i2c->iiccon);
+
+	if (addr && addr_len) {
+		writel(chip, &i2c->iicds);
+		/* send START */
+		writel(I2C_MODE_MT | I2C_TXRX_ENA | I2C_START_STOP,
+			&i2c->iicstat);
+		if (WaitForXfer(i2c) == I2C_OK)
+			result = i2c_send_verify(i2c, addr, addr_len);
+		else
+			result = I2C_NACK;
+	} else
+		result = I2C_NACK;
+
+	switch (cmd_type) {
+	case I2C_WRITE:
+		if (result == I2C_OK)
+			result = i2c_send_verify(i2c, data, data_len);
+		else {
+			writel(chip, &i2c->iicds);
+			/* send START */
+			writel(I2C_MODE_MT | I2C_TXRX_ENA | I2C_START_STOP,
+				&i2c->iicstat);
+			if (WaitForXfer(i2c) == I2C_OK)
+				result = i2c_send_verify(i2c, data, data_len);
+		}
+
+		if (result == I2C_OK)
+			result = WaitForXfer(i2c);
+
+		stop_bit_result = i2c_send_stop(i2c, I2C_MODE_MT);
+		break;
+
+	case I2C_READ:
+	{
+		int was_ok = (result == I2C_OK);
+
+		writel(chip, &i2c->iicds);
+		/* resend START */
+		writel(I2C_MODE_MR | I2C_TXRX_ENA |
+					I2C_START_STOP, &i2c->iicstat);
+		ReadWriteByte(i2c);
+		result = WaitForXfer(i2c);
+
+		if (was_ok || IsACK(i2c)) {
+			i = 0;
+			while ((i < data_len) && (result == I2C_OK)) {
+				/* disable ACK for final READ */
+				if (i == data_len - 1) {
+					/* FIXME(dhendrix): nested macro */
+#if 0
+					writel(readl(&i2c->iiccon) &
+					       ~I2CCON_ACKGEN,
+					       &i2c->iiccon);
+#endif
+					x = readl(&i2c->iiccon) & ~I2CCON_ACKGEN;
+					writel(x, &i2c->iiccon);
+				}
+				ReadWriteByte(i2c);
+				result = WaitForXfer(i2c);
+				data[i] = readl(&i2c->iicds);
+				i++;
+			}
+		} else {
+			result = I2C_NACK;
+		}
+
+		stop_bit_result = i2c_send_stop(i2c, I2C_MODE_MR);
+		break;
+	}
+
+	default:
+		//debug("i2c_transfer: bad call\n");
+		result = stop_bit_result = I2C_NOK;
+		break;
+	}
+
+	/*
+	 * If the transmission went fine, then only the stop bit was left to
+	 * fail.  Otherwise, the real failure we're interested in came before
+	 * that, during the actual transmission.
+	 */
+	return (result == I2C_OK) ? stop_bit_result : result;
+}
+
+
+int i2c_read(uchar chip, uint addr, int alen, uchar *buffer, int len)
+{
+	struct s3c24x0_i2c_bus *i2c = &i2c0;
+	uchar xaddr[4];
+	int ret;
+
+	if (alen > 4) {
+		//debug("I2C read: addr len %d not supported\n", alen);
+		return 1;
+	}
+
+	if (alen > 0) {
+		xaddr[0] = (addr >> 24) & 0xFF;
+		xaddr[1] = (addr >> 16) & 0xFF;
+		xaddr[2] = (addr >> 8) & 0xFF;
+		xaddr[3] = addr & 0xFF;
+	}
+
+#ifdef CONFIG_SYS_I2C_EEPROM_ADDR_OVERFLOW
+	/*
+	 * EEPROM chips that implement "address overflow" are ones
+	 * like Catalyst 24WC04/08/16 which has 9/10/11 bits of
+	 * address and the extra bits end up in the "chip address"
+	 * bit slots. This makes a 24WC08 (1Kbyte) chip look like
+	 * four 256 byte chips.
+	 *
+	 * Note that we consider the length of the address field to
+	 * still be one byte because the extra address bits are
+	 * hidden in the chip address.
+	 */
+	if (alen > 0)
+		chip |= ((addr >> (alen * 8)) &
+			 CONFIG_SYS_I2C_EEPROM_ADDR_OVERFLOW);
+#endif
+	if (!i2c)
+		return -1;
+#if 0
+	if (board_i2c_claim_bus(i2c->node)) {
+		debug("I2C cannot claim bus %d\n", i2c->bus_num);
+		return -1;
+	}
+#endif
+	ret = i2c_transfer(i2c->regs, I2C_READ, chip << 1, &xaddr[4 - alen],
+			   alen, buffer, len);
+	//board_i2c_release_bus(i2c->node);
+	if (ret) {
+		//debug("I2c read: failed %d\n", ret);
+		return 1;
+	}
+	return 0;
+}
+
+int i2c_write(uchar chip, uint addr, int alen, uchar *buffer, int len)
+{
+	struct s3c24x0_i2c_bus *i2c;
+	uchar xaddr[4];
+	int ret;
+
+	if (alen > 4) {
+		//debug("I2C write: addr len %d not supported\n", alen);
+		return 1;
+	}
+
+	if (alen > 0) {
+		xaddr[0] = (addr >> 24) & 0xFF;
+		xaddr[1] = (addr >> 16) & 0xFF;
+		xaddr[2] = (addr >> 8) & 0xFF;
+		xaddr[3] = addr & 0xFF;
+	}
+#ifdef CONFIG_SYS_I2C_EEPROM_ADDR_OVERFLOW
+	/*
+	 * EEPROM chips that implement "address overflow" are ones
+	 * like Catalyst 24WC04/08/16 which has 9/10/11 bits of
+	 * address and the extra bits end up in the "chip address"
+	 * bit slots. This makes a 24WC08 (1Kbyte) chip look like
+	 * four 256 byte chips.
+	 *
+	 * Note that we consider the length of the address field to
+	 * still be one byte because the extra address bits are
+	 * hidden in the chip address.
+	 */
+	if (alen > 0)
+		chip |= ((addr >> (alen * 8)) &
+			 CONFIG_SYS_I2C_EEPROM_ADDR_OVERFLOW);
+#endif
+	//i2c = get_bus(g_current_bus);
+	i2c = &i2c0;
+	if (!i2c)
+		return -1;
+#if 0
+	if (board_i2c_claim_bus(i2c->node)) {
+		//debug("I2C cannot claim bus %d\n", i2c->bus_num);
+		return -1;
+	}
+#endif
+	ret = i2c_transfer(i2c->regs, I2C_WRITE, chip << 1, &xaddr[4 - alen],
+			   alen, buffer, len);
+	//board_i2c_release_bus(i2c->node);
+
+	return ret != 0;
+}
+
+/*
+ * Max77686 parameters values
+ * see max77686.h for parameters details
+ */
+struct max77686_para max77686_param[] = {/*{vol_addr, vol_bitpos,
+	vol_bitmask, reg_enaddr, reg_enbitpos, reg_enbitmask, reg_enbiton,
+	reg_enbitoff, vol_min, vol_div}*/
+	{/* PMIC_BUCK1 */ 0x11, 0x0, 0x3F, 0x10, 0x0, 0x3, 0x3, 0x0, 750, 50000},
+	{/* PMIC_BUCK2 */ 0x14, 0x0, 0xFF, 0x12, 0x4, 0x3, 0x1, 0x0, 600, 12500},
+	{/* PMIC_BUCK3 */ 0x1E, 0x0, 0xFF, 0x1C, 0x4, 0x3, 0x1, 0x0, 600, 12500},
+	{/* PMIC_BUCK4 */ 0x28, 0x0, 0xFF, 0x26, 0x4, 0x3, 0x1, 0x0, 600, 12500},
+	{/* PMIC_BUCK5 */ 0x31, 0x0, 0x3F, 0x30, 0x0, 0x3, 0x3, 0x0, 750, 50000},
+	{/* PMIC_BUCK6 */ 0x33, 0x0, 0x3F, 0x32, 0x0, 0x3, 0x3, 0x0, 750, 50000},
+	{/* PMIC_BUCK7 */ 0x35, 0x0, 0x3F, 0x34, 0x0, 0x3, 0x3, 0x0, 750, 50000},
+	{/* PMIC_BUCK8 */ 0x37, 0x0, 0x3F, 0x36, 0x0, 0x3, 0x3, 0x0, 750, 50000},
+	{/* PMIC_BUCK9 */ 0x39, 0x0, 0x3F, 0x38, 0x0, 0x3, 0x3, 0x0, 750, 50000},
+	{/* PMIC_LDO1 */  0x40, 0x0, 0x3F, 0x40, 0x6, 0x3, 0x3, 0x0, 800, 25000},
+	{/* PMIC_LDO2 */  0x41, 0x0, 0x3F, 0x41, 0x6, 0x3, 0x1, 0x0, 800, 25000},
+	{/* PMIC_LDO3 */  0x42, 0x0, 0x3F, 0x42, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO4 */  0x43, 0x0, 0x3F, 0x43, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO5 */  0x44, 0x0, 0x3F, 0x44, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO6 */  0x45, 0x0, 0x3F, 0x45, 0x6, 0x3, 0x1, 0x0, 800, 25000},
+	{/* PMIC_LDO7 */  0x46, 0x0, 0x3F, 0x46, 0x6, 0x3, 0x1, 0x0, 800, 25000},
+	{/* PMIC_LDO8 */  0x47, 0x0, 0x3F, 0x47, 0x6, 0x3, 0x1, 0x0, 800, 25000},
+	{/* PMIC_LDO9 */  0x48, 0x0, 0x3F, 0x48, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO10 */ 0x49, 0x0, 0x3F, 0x49, 0x6, 0x3, 0x1, 0x0, 800, 50000},
+	{/* PMIC_LDO11 */ 0x4A, 0x0, 0x3F, 0x4A, 0x6, 0x3, 0x1, 0x0, 800, 50000},
+	{/* PMIC_LDO12 */ 0x4B, 0x0, 0x3F, 0x4B, 0x6, 0x3, 0x1, 0x0, 800, 50000},
+	{/* PMIC_LDO13 */ 0x4C, 0x0, 0x3F, 0x4C, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO14 */ 0x4D, 0x0, 0x3F, 0x4D, 0x6, 0x3, 0x1, 0x0, 800, 50000},
+	{/* PMIC_LDO15 */ 0x4E, 0x0, 0x3F, 0x4E, 0x6, 0x3, 0x1, 0x0, 800, 25000},
+	{/* PMIC_LDO16 */ 0x4F, 0x0, 0x3F, 0x4F, 0x6, 0x3, 0x1, 0x0, 800, 50000},
+	{/* PMIC_LDO17 */ 0x50, 0x0, 0x3F, 0x50, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO18 */ 0x51, 0x0, 0x3F, 0x51, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO19 */ 0x52, 0x0, 0x3F, 0x52, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO20 */ 0x53, 0x0, 0x3F, 0x53, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO21 */ 0x54, 0x0, 0x3F, 0x54, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO22 */ 0x55, 0x0, 0x3F, 0x55, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO23 */ 0x56, 0x0, 0x3F, 0x56, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO24 */ 0x57, 0x0, 0x3F, 0x57, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO25 */ 0x58, 0x0, 0x3F, 0x58, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_LDO26 */ 0x59, 0x0, 0x3F, 0x59, 0x6, 0x3, 0x3, 0x0, 800, 50000},
+	{/* PMIC_EN32KHZ_CP */ 0x0, 0x0, 0x0, 0x7F, 0x1, 0x1, 0x1, 0x0, 0x0, 0x0},
+};
+
+/*
+ * Write a value to a register
+ *
+ * @param chip_addr	i2c addr for max77686
+ * @param reg		reg number to write
+ * @param val		value to be written
+ *
+ */
+static inline int max77686_i2c_write(unsigned char chip_addr,
+					unsigned int reg, unsigned char val)
+{
+	return i2c_write(chip_addr, reg, 1, &val, 1);
+}
+
+/*
+ * Read a value from a register
+ *
+ * @param chip_addr	i2c addr for max77686
+ * @param reg		reg number to write
+ * @param val		value to be written
+ *
+ */
+static inline int max77686_i2c_read(unsigned char chip_addr,
+					unsigned int reg, unsigned char *val)
+{
+	return i2c_read(chip_addr, reg, 1, val, 1);
+}
+
+/* Chip register numbers (not exported from this module) */
+enum {
+	REG_BBAT		= 0x7e,
+
+	/* Bits for BBAT */
+	BBAT_BBCHOSTEN_MASK	= 1 << 0,
+	BBAT_BBCVS_SHIFT	= 3,
+	BBAT_BBCVS_MASK		= 3 << BBAT_BBCVS_SHIFT,
+};
+
+int max77686_disable_backup_batt(void)
+{
+	unsigned char val;
+	int ret;
+
+	//i2c_set_bus_num(0);
+	ret = max77686_i2c_read(MAX77686_I2C_ADDR, REG_BBAT, &val);
+	if (ret) {
+//		debug("max77686 i2c read failed\n");
+		return ret;
+	}
+
+	/* If we already have the correct values, exit */
+	if ((val & (BBAT_BBCVS_MASK | BBAT_BBCHOSTEN_MASK)) ==
+			BBAT_BBCVS_MASK)
+		return 0;
+
+	/* First disable charging */
+	val &= ~BBAT_BBCHOSTEN_MASK;
+	ret = max77686_i2c_write(MAX77686_I2C_ADDR, REG_BBAT, val);
+	if (ret) {
+		//debug("max77686 i2c write failed\n");
+		return -1;
+	}
+
+	/* Finally select 3.5V to minimize power consumption */
+	val |= BBAT_BBCVS_MASK;
+	ret = max77686_i2c_write(MAX77686_I2C_ADDR, REG_BBAT, val);
+	if (ret) {
+		//debug("max77686 i2c write failed\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int max77686_enablereg(enum max77686_regnum reg, int enable)
+{
+	struct max77686_para *pmic;
+	unsigned char read_data;
+	int ret;
+
+	pmic = &max77686_param[reg];
+
+	ret = max77686_i2c_read(MAX77686_I2C_ADDR, pmic->reg_enaddr,
+				&read_data);
+	if (ret != 0) {
+		//debug("max77686 i2c read failed.\n");
+		return -1;
+	}
+
+	if (enable == REG_DISABLE) {
+		clrbits_8(&read_data,
+				pmic->reg_enbitmask << pmic->reg_enbitpos);
+	} else {
+		clrsetbits_8(&read_data,
+				pmic->reg_enbitmask << pmic->reg_enbitpos,
+				pmic->reg_enbiton << pmic->reg_enbitpos);
+	}
+
+	ret = max77686_i2c_write(MAX77686_I2C_ADDR,
+				 pmic->reg_enaddr, read_data);
+	if (ret != 0) {
+		//debug("max77686 i2c write failed.\n");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int max77686_do_volsetting(enum max77686_regnum reg, unsigned int volt,
+				  int enable, int volt_units)
+{
+	struct max77686_para *pmic;
+	unsigned char read_data;
+	int vol_level = 0;
+	int ret;
+
+	pmic = &max77686_param[reg];
+
+	if (pmic->vol_addr == 0) {
+		//debug("not a voltage register.\n");
+		return -1;
+	}
+
+	ret = max77686_i2c_read(MAX77686_I2C_ADDR, pmic->vol_addr, &read_data);
+	if (ret != 0) {
+		//debug("max77686 i2c read failed.\n");
+		return -1;
+	}
+
+	if (volt_units == MAX77686_UV)
+		vol_level = volt - (u32)pmic->vol_min * 1000;
+	else
+		vol_level = (volt - (u32)pmic->vol_min) * 1000;
+
+	if (vol_level < 0) {
+		//debug("Not a valid voltage level to set\n");
+		return -1;
+	}
+	vol_level /= (u32)pmic->vol_div;
+
+	clrsetbits_8(&read_data, pmic->vol_bitmask << pmic->vol_bitpos,
+			vol_level << pmic->vol_bitpos);
+
+	ret = max77686_i2c_write(MAX77686_I2C_ADDR, pmic->vol_addr, read_data);
+	if (ret != 0) {
+		//debug("max77686 i2c write failed.\n");
+		return -1;
+	}
+
+	ret = max77686_enablereg(reg, enable);
+	if (ret != 0) {
+		//debug("Failed to enable buck/ldo.\n");
+		return -1;
+	}
+	return 0;
+}
+
+int max77686_volsetting(enum max77686_regnum reg, unsigned int volt,
+			int enable, int volt_units)
+{
+//	int old_bus = i2c_get_bus_num();
+	int ret;
+
+//	i2c_set_bus_num(0);
+	ret = max77686_do_volsetting(reg, volt, enable, volt_units);
+//	i2c_set_bus_num(old_bus);
+	return ret;
+}
+
+/* power_init() stuff */
+static void power_init(void)
+{
+	max77686_disable_backup_batt();
+
+#if 0
+	/* FIXME: for debugging... */
+	volatile unsigned long *addr = (unsigned long *)0x1004330c;
+	*addr |= 0x100;
+	while (1);
+#endif
+
+	max77686_volsetting(PMIC_BUCK2, CONFIG_VDD_ARM_MV,
+						REG_ENABLE, MAX77686_MV);
+	max77686_volsetting(PMIC_BUCK3, CONFIG_VDD_INT_UV,
+						REG_ENABLE, MAX77686_UV);
+	max77686_volsetting(PMIC_BUCK1, CONFIG_VDD_MIF_MV,
+						REG_ENABLE, MAX77686_MV);
+	max77686_volsetting(PMIC_BUCK4, CONFIG_VDD_G3D_MV,
+						REG_ENABLE, MAX77686_MV);
+	max77686_volsetting(PMIC_LDO2, CONFIG_VDD_LDO2_MV,
+						REG_ENABLE, MAX77686_MV);
+	max77686_volsetting(PMIC_LDO3, CONFIG_VDD_LDO3_MV,
+						REG_ENABLE, MAX77686_MV);
+	max77686_volsetting(PMIC_LDO5, CONFIG_VDD_LDO5_MV,
+						REG_ENABLE, MAX77686_MV);
+	max77686_volsetting(PMIC_LDO10, CONFIG_VDD_LDO10_MV,
+						REG_ENABLE, MAX77686_MV);
+}
+
+struct mem_timings mem_timings[] = {
+	{
+		.mem_manuf = MEM_MANUF_ELPIDA,
+		.mem_type = DDR_MODE_DDR3,
+		.frequency_mhz = 800,
+		.mpll_mdiv = 0x64,
+		.mpll_pdiv = 0x3,
+		.mpll_sdiv = 0x0,
+		.cpll_mdiv = 0xde,
+		.cpll_pdiv = 0x4,
+		.cpll_sdiv = 0x2,
+		.gpll_mdiv = 0x215,
+		.gpll_pdiv = 0xc,
+		.gpll_sdiv = 0x1,
+		.epll_mdiv = 0x60,
+		.epll_pdiv = 0x3,
+		.epll_sdiv = 0x3,
+		.vpll_mdiv = 0x96,
+		.vpll_pdiv = 0x3,
+		.vpll_sdiv = 0x2,
+
+		.bpll_mdiv = 0x64,
+		.bpll_pdiv = 0x3,
+		.bpll_sdiv = 0x0,
+		.use_bpll = 0,
+		.pclk_cdrex_ratio = 0x5,
+		.direct_cmd_msr = {
+			0x00020018, 0x00030000, 0x00010042, 0x00000d70
+		},
+		.timing_ref = 0x000000bb,
+		.timing_row = 0x8c36660f,
+		.timing_data = 0x3630580b,
+		.timing_power = 0x41000a44,
+		.phy0_dqs = 0x08080808,
+		.phy1_dqs = 0x08080808,
+		.phy0_dq = 0x08080808,
+		.phy1_dq = 0x08080808,
+		.phy0_tFS = 0x4,
+		.phy1_tFS = 0x4,
+		.phy0_pulld_dqs = 0xf,
+		.phy1_pulld_dqs = 0xf,
+
+		.lpddr3_ctrl_phy_reset = 0x1,
+		.ctrl_start_point = 0x10,
+		.ctrl_inc = 0x10,
+		.ctrl_start = 0x1,
+		.ctrl_dll_on = 0x1,
+		.ctrl_ref = 0x8,
+
+		.ctrl_force = 0x1a,
+		.ctrl_rdlat = 0x0b,
+		.ctrl_bstlen = 0x08,
+
+		.fp_resync = 0x8,
+		.iv_size = 0x7,
+		.dfi_init_start = 1,
+		.aref_en = 1,
+
+		.rd_fetch = 0x3,
+
+		.zq_mode_dds = 0x7,
+		.zq_mode_term = 0x1,
+		.zq_mode_noterm = 0,
+
+		/*
+		* Dynamic Clock: Always Running
+		* Memory Burst length: 8
+		* Number of chips: 1
+		* Memory Bus width: 32 bit
+		* Memory Type: DDR3
+		* Additional Latancy for PLL: 0 Cycle
+		*/
+		.memcontrol = DMC_MEMCONTROL_CLK_STOP_DISABLE |
+			DMC_MEMCONTROL_DPWRDN_DISABLE |
+			DMC_MEMCONTROL_DPWRDN_ACTIVE_PRECHARGE |
+			DMC_MEMCONTROL_TP_DISABLE |
+			DMC_MEMCONTROL_DSREF_ENABLE |
+			DMC_MEMCONTROL_ADD_LAT_PALL_CYCLE(0) |
+			DMC_MEMCONTROL_MEM_TYPE_DDR3 |
+			DMC_MEMCONTROL_MEM_WIDTH_32BIT |
+			DMC_MEMCONTROL_NUM_CHIP_1 |
+			DMC_MEMCONTROL_BL_8 |
+			DMC_MEMCONTROL_PZQ_DISABLE |
+			DMC_MEMCONTROL_MRR_BYTE_7_0,
+		.memconfig = DMC_MEMCONFIGx_CHIP_MAP_INTERLEAVED |
+			DMC_MEMCONFIGx_CHIP_COL_10 |
+			DMC_MEMCONFIGx_CHIP_ROW_15 |
+			DMC_MEMCONFIGx_CHIP_BANK_8,
+		.membaseconfig0 = DMC_MEMBASECONFIG_VAL(0x40),
+		.membaseconfig1 = DMC_MEMBASECONFIG_VAL(0x80),
+		.prechconfig_tp_cnt = 0xff,
+		.dpwrdn_cyc = 0xff,
+		.dsref_cyc = 0xffff,
+		.concontrol = DMC_CONCONTROL_DFI_INIT_START_DISABLE |
+			DMC_CONCONTROL_TIMEOUT_LEVEL0 |
+			DMC_CONCONTROL_RD_FETCH_DISABLE |
+			DMC_CONCONTROL_EMPTY_DISABLE |
+			DMC_CONCONTROL_AREF_EN_DISABLE |
+			DMC_CONCONTROL_IO_PD_CON_DISABLE,
+		.dmc_channels = 2,
+		.chips_per_channel = 2,
+		.chips_to_configure = 1,
+		.send_zq_init = 1,
+		.impedance = IMP_OUTPUT_DRV_30_OHM,
+		.gate_leveling_enable = 0,
+	},
+};
+
+struct arm_clk_ratios arm_clk_ratios[] = {
+	{
+		.arm_freq_mhz = 1700,
+
+		.apll_mdiv = 0x1a9,
+		.apll_pdiv = 0x6,
+		.apll_sdiv = 0x0,
+
+		.arm2_ratio = 0x0,
+		.apll_ratio = 0x3,
+		.pclk_dbg_ratio = 0x1,
+		.atb_ratio = 0x6,
+		.periph_ratio = 0x7,
+		.acp_ratio = 0x7,
+		.cpud_ratio = 0x3,
+		.arm_ratio = 0x0,
+	}
+};
+
+static void clock_init(void)
+{
+	struct exynos5_clock *clk = (struct exynos5_clock *)EXYNOS5_CLOCK_BASE;
+	struct exynos5_mct_regs *mct_regs =
+		(struct exynos5_mct_regs *)EXYNOS5_MULTI_CORE_TIMER_BASE;
+	struct mem_timings *mem = &mem_timings[0];
+	struct arm_clk_ratios *arm_clk_ratio = &arm_clk_ratios[0];
+	u32 val, tmp;
+
+	/* Turn on the MCT as early as possible. */
+	mct_regs->g_tcon |= (1 << 8);
+
+//	mem = clock_get_mem_timings();
+//	arm_clk_ratio = get_arm_ratios();
+
+	clrbits_le32(&clk->src_cpu, MUX_APLL_SEL_MASK);
+	do {
+		val = readl(&clk->mux_stat_cpu);
+	} while ((val | MUX_APLL_SEL_MASK) != val);
+
+	clrbits_le32(&clk->src_core1, MUX_MPLL_SEL_MASK);
+	do {
+		val = readl(&clk->mux_stat_core1);
+	} while ((val | MUX_MPLL_SEL_MASK) != val);
+
+	clrbits_le32(&clk->src_top2, MUX_CPLL_SEL_MASK);
+	clrbits_le32(&clk->src_top2, MUX_EPLL_SEL_MASK);
+	clrbits_le32(&clk->src_top2, MUX_VPLL_SEL_MASK);
+	clrbits_le32(&clk->src_top2, MUX_GPLL_SEL_MASK);
+	tmp = MUX_CPLL_SEL_MASK | MUX_EPLL_SEL_MASK | MUX_VPLL_SEL_MASK
+		| MUX_GPLL_SEL_MASK;
+	do {
+		val = readl(&clk->mux_stat_top2);
+	} while ((val | tmp) != val);
+
+	clrbits_le32(&clk->src_cdrex, MUX_BPLL_SEL_MASK);
+	do {
+		val = readl(&clk->mux_stat_cdrex);
+	} while ((val | MUX_BPLL_SEL_MASK) != val);
+
+	/* PLL locktime */
+	writel(APLL_LOCK_VAL, &clk->apll_lock);
+
+	writel(MPLL_LOCK_VAL, &clk->mpll_lock);
+
+	writel(BPLL_LOCK_VAL, &clk->bpll_lock);
+
+	writel(CPLL_LOCK_VAL, &clk->cpll_lock);
+
+	writel(GPLL_LOCK_VAL, &clk->gpll_lock);
+
+	writel(EPLL_LOCK_VAL, &clk->epll_lock);
+
+	writel(VPLL_LOCK_VAL, &clk->vpll_lock);
+
+	writel(CLK_REG_DISABLE, &clk->pll_div2_sel);
+
+	writel(MUX_HPM_SEL_MASK, &clk->src_cpu);
+	do {
+		val = readl(&clk->mux_stat_cpu);
+	} while ((val | HPM_SEL_SCLK_MPLL) != val);
+
+	val = arm_clk_ratio->arm2_ratio << 28
+		| arm_clk_ratio->apll_ratio << 24
+		| arm_clk_ratio->pclk_dbg_ratio << 20
+		| arm_clk_ratio->atb_ratio << 16
+		| arm_clk_ratio->periph_ratio << 12
+		| arm_clk_ratio->acp_ratio << 8
+		| arm_clk_ratio->cpud_ratio << 4
+		| arm_clk_ratio->arm_ratio;
+	writel(val, &clk->div_cpu0);
+	do {
+		val = readl(&clk->div_stat_cpu0);
+	} while (0 != val);
+
+	writel(CLK_DIV_CPU1_VAL, &clk->div_cpu1);
+	do {
+		val = readl(&clk->div_stat_cpu1);
+	} while (0 != val);
+
+	/* Set APLL */
+	writel(APLL_CON1_VAL, &clk->apll_con1);
+	val = set_pll(arm_clk_ratio->apll_mdiv, arm_clk_ratio->apll_pdiv,
+			arm_clk_ratio->apll_sdiv);
+	writel(val, &clk->apll_con0);
+	while ((readl(&clk->apll_con0) & APLL_CON0_LOCKED) == 0)
+		;
+
+	/* Set MPLL */
+	writel(MPLL_CON1_VAL, &clk->mpll_con1);
+	val = set_pll(mem->mpll_mdiv, mem->mpll_pdiv, mem->mpll_sdiv);
+	writel(val, &clk->mpll_con0);
+	while ((readl(&clk->mpll_con0) & MPLL_CON0_LOCKED) == 0)
+		;
+
+	/*
+	 * Configure MUX_MPLL_FOUT to choose the direct clock source
+	 * path and avoid the fixed DIV/2 block to save power
+	 */
+	setbits_le32(&clk->pll_div2_sel, MUX_MPLL_FOUT_SEL);
+
+	/* Set BPLL */
+	if (mem->use_bpll) {
+		writel(BPLL_CON1_VAL, &clk->bpll_con1);
+		val = set_pll(mem->bpll_mdiv, mem->bpll_pdiv, mem->bpll_sdiv);
+		writel(val, &clk->bpll_con0);
+		while ((readl(&clk->bpll_con0) & BPLL_CON0_LOCKED) == 0)
+			;
+
+		setbits_le32(&clk->pll_div2_sel, MUX_BPLL_FOUT_SEL);
+	}
+
+	/* Set CPLL */
+	writel(CPLL_CON1_VAL, &clk->cpll_con1);
+	val = set_pll(mem->cpll_mdiv, mem->cpll_pdiv, mem->cpll_sdiv);
+	writel(val, &clk->cpll_con0);
+	while ((readl(&clk->cpll_con0) & CPLL_CON0_LOCKED) == 0)
+		;
+
+	/* Set GPLL */
+	writel(GPLL_CON1_VAL, &clk->gpll_con1);
+	val = set_pll(mem->gpll_mdiv, mem->gpll_pdiv, mem->gpll_sdiv);
+	writel(val, &clk->gpll_con0);
+	while ((readl(&clk->gpll_con0) & GPLL_CON0_LOCKED) == 0)
+		;
+
+	/* Set EPLL */
+	writel(EPLL_CON2_VAL, &clk->epll_con2);
+	writel(EPLL_CON1_VAL, &clk->epll_con1);
+	val = set_pll(mem->epll_mdiv, mem->epll_pdiv, mem->epll_sdiv);
+	writel(val, &clk->epll_con0);
+	while ((readl(&clk->epll_con0) & EPLL_CON0_LOCKED) == 0)
+		;
+
+	/* Set VPLL */
+	writel(VPLL_CON2_VAL, &clk->vpll_con2);
+	writel(VPLL_CON1_VAL, &clk->vpll_con1);
+	val = set_pll(mem->vpll_mdiv, mem->vpll_pdiv, mem->vpll_sdiv);
+	writel(val, &clk->vpll_con0);
+	while ((readl(&clk->vpll_con0) & VPLL_CON0_LOCKED) == 0)
+		;
+
+	writel(CLK_SRC_CORE0_VAL, &clk->src_core0);
+	writel(CLK_DIV_CORE0_VAL, &clk->div_core0);
+	while (readl(&clk->div_stat_core0) != 0)
+		;
+
+	writel(CLK_DIV_CORE1_VAL, &clk->div_core1);
+	while (readl(&clk->div_stat_core1) != 0)
+		;
+
+	writel(CLK_DIV_SYSRGT_VAL, &clk->div_sysrgt);
+	while (readl(&clk->div_stat_sysrgt) != 0)
+		;
+
+	writel(CLK_DIV_ACP_VAL, &clk->div_acp);
+	while (readl(&clk->div_stat_acp) != 0)
+		;
+
+	writel(CLK_DIV_SYSLFT_VAL, &clk->div_syslft);
+	while (readl(&clk->div_stat_syslft) != 0)
+		;
+
+	writel(CLK_SRC_TOP0_VAL, &clk->src_top0);
+	writel(CLK_SRC_TOP1_VAL, &clk->src_top1);
+	writel(TOP2_VAL, &clk->src_top2);
+	writel(CLK_SRC_TOP3_VAL, &clk->src_top3);
+
+	writel(CLK_DIV_TOP0_VAL, &clk->div_top0);
+	while (readl(&clk->div_stat_top0))
+		;
+
+	writel(CLK_DIV_TOP1_VAL, &clk->div_top1);
+	while (readl(&clk->div_stat_top1))
+		;
+
+	writel(CLK_SRC_LEX_VAL, &clk->src_lex);
+	while (1) {
+		val = readl(&clk->mux_stat_lex);
+		if (val == (val | 1))
+			break;
+	}
+
+	writel(CLK_DIV_LEX_VAL, &clk->div_lex);
+	while (readl(&clk->div_stat_lex))
+		;
+
+	writel(CLK_DIV_R0X_VAL, &clk->div_r0x);
+	while (readl(&clk->div_stat_r0x))
+		;
+
+	writel(CLK_DIV_R0X_VAL, &clk->div_r0x);
+	while (readl(&clk->div_stat_r0x))
+		;
+
+	writel(CLK_DIV_R1X_VAL, &clk->div_r1x);
+	while (readl(&clk->div_stat_r1x))
+		;
+
+	if (mem->use_bpll) {
+		writel(MUX_BPLL_SEL_MASK | MUX_MCLK_CDREX_SEL |
+			MUX_MCLK_DPHY_SEL, &clk->src_cdrex);
+	} else {
+		writel(CLK_REG_DISABLE, &clk->src_cdrex);
+	}
+
+	writel(CLK_DIV_CDREX_VAL, &clk->div_cdrex);
+	while (readl(&clk->div_stat_cdrex))
+		;
+
+	val = readl(&clk->src_cpu);
+	val |= CLK_SRC_CPU_VAL;
+	writel(val, &clk->src_cpu);
+
+	val = readl(&clk->src_top2);
+	val |= CLK_SRC_TOP2_VAL;
+	writel(val, &clk->src_top2);
+
+	val = readl(&clk->src_core1);
+	val |= CLK_SRC_CORE1_VAL;
+	writel(val, &clk->src_core1);
+
+	writel(CLK_SRC_FSYS0_VAL, &clk->src_fsys);
+	writel(CLK_DIV_FSYS0_VAL, &clk->div_fsys0);
+	while (readl(&clk->div_stat_fsys0))
+		;
+
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_cpu);
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_core);
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_acp);
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_top);
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_lex);
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_r0x);
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_r1x);
+	writel(CLK_REG_DISABLE, &clk->clkout_cmu_cdrex);
+
+	writel(CLK_SRC_PERIC0_VAL, &clk->src_peric0);
+	writel(CLK_DIV_PERIC0_VAL, &clk->div_peric0);
+
+	writel(CLK_SRC_PERIC1_VAL, &clk->src_peric1);
+	writel(CLK_DIV_PERIC1_VAL, &clk->div_peric1);
+	writel(CLK_DIV_PERIC2_VAL, &clk->div_peric2);
+	writel(SCLK_SRC_ISP_VAL, &clk->sclk_src_isp);
+	writel(SCLK_DIV_ISP_VAL, &clk->sclk_div_isp);
+	writel(CLK_DIV_ISP0_VAL, &clk->div_isp0);
+	writel(CLK_DIV_ISP1_VAL, &clk->div_isp1);
+	writel(CLK_DIV_ISP2_VAL, &clk->div_isp2);
+
+	/* FIMD1 SRC CLK SELECTION */
+	writel(CLK_SRC_DISP1_0_VAL, &clk->src_disp1_0);
+
+	val = MMC2_PRE_RATIO_VAL << MMC2_PRE_RATIO_OFFSET
+		| MMC2_RATIO_VAL << MMC2_RATIO_OFFSET
+		| MMC3_PRE_RATIO_VAL << MMC3_PRE_RATIO_OFFSET
+		| MMC3_RATIO_VAL << MMC3_RATIO_OFFSET;
+	writel(val, &clk->div_fsys2);
+}
+
+#include <console/vtxprintf.h>
+#include <string.h>
+#define ZEROPAD	1		/* pad with zero */
+#define SIGN	2		/* unsigned/signed long */
+#define PLUS	4		/* show plus */
+#define SPACE	8		/* space if plus */
+#define LEFT	16		/* left justified */
+#define SPECIAL	32		/* 0x */
+#define LARGE	64		/* use 'ABCDEF' instead of 'abcdef' */
+/* haha, don't need ctype.c */
+#define isdigit(c)	((c) >= '0' && (c) <= '9')
+#define is_digit isdigit
+#define isxdigit(c)	(((c) >= '0' && (c) <= '9') || ((c) >= 'a' && (c) <= 'f') || ((c) >= 'A' && (c) <= 'F'))
+
+void __div0 (void);
+void __div0 (void)
+{
+	puts("divide by zero detected");
+	while(1) ;
+}
+
+static int skip_atoi(const char **s)
+{
+	int i=0;
+
+	while (is_digit(**s))
+		i = i*10 + *((*s)++) - '0';
+	return i;
+}
+
+#include <div64.h>
+static int number(void (*tx_byte)(unsigned char byte),
+	unsigned long long num, int base, int size, int precision, int type)
+{
+	char c,sign,tmp[66];
+	const char *digits="0123456789abcdefghijklmnopqrstuvwxyz";
+	int i;
+	int count = 0;
+
+	if (type & LARGE)
+		digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
+	if (type & LEFT)
+		type &= ~ZEROPAD;
+	if (base < 2 || base > 36)
+		return 0;
+	c = (type & ZEROPAD) ? '0' : ' ';
+	sign = 0;
+	if (type & SIGN) {
+		if ((signed long long)num < 0) {
+			sign = '-';
+			num = -num;
+			size--;
+		} else if (type & PLUS) {
+			sign = '+';
+			size--;
+		} else if (type & SPACE) {
+			sign = ' ';
+			size--;
+		}
+	}
+	if (type & SPECIAL) {
+		if (base == 16)
+			size -= 2;
+		else if (base == 8)
+			size--;
+	}
+	i = 0;
+	if (num == 0) {
+		tmp[i++]='0';
+	} else while (num != 0) {
+		/* FIXME: do_div was broken */
+//		tmp[i++] = digits[do_div(num,base)];
+		tmp[i++] = digits[num & 0xf];
+		num = num >> 4;
+	}
+	if (i > precision)
+		precision = i;
+	size -= precision;
+	if (!(type&(ZEROPAD+LEFT)))
+		while(size-->0)
+			tx_byte(' '), count++;
+	if (sign)
+		tx_byte(sign), count++;
+	if (type & SPECIAL) {
+		if (base==8)
+			tx_byte('0'), count++;
+		else if (base==16) {
+			tx_byte('0'), count++;
+			tx_byte(digits[33]), count++;
+		}
+	}
+	if (!(type & LEFT))
+		while (size-- > 0)
+			tx_byte(c), count++;
+	while (i < precision--)
+		tx_byte('0'), count++;
+	while (i-- > 0)
+		tx_byte(tmp[i]), count++;
+	while (size-- > 0)
+		tx_byte(' '), count++;
+	return count;
+}
+
+int vtxprintf(void (*tx_byte)(unsigned char byte), const char *fmt, va_list args)
+{
+	int len;
+	unsigned long long num;
+	int i, base;
+	const char *s;
+
+	int flags;		/* flags to number() */
+
+	int field_width;	/* width of output field */
+	int precision;		/* min. # of digits for integers; max
+				   number of chars for from string */
+	int qualifier;		/* 'h', 'l', or 'L' for integer fields */
+
+	int count;
+
+	for (count=0; *fmt ; ++fmt) {
+		if (*fmt != '%') {
+			tx_byte(*fmt), count++;
+			continue;
+		}
+
+		/* process flags */
+		flags = 0;
+repeat:
+			++fmt;		/* this also skips first '%' */
+			switch (*fmt) {
+				case '-': flags |= LEFT; goto repeat;
+				case '+': flags |= PLUS; goto repeat;
+				case ' ': flags |= SPACE; goto repeat;
+				case '#': flags |= SPECIAL; goto repeat;
+				case '0': flags |= ZEROPAD; goto repeat;
+				}
+
+		/* get field width */
+		field_width = -1;
+		if (is_digit(*fmt))
+			field_width = skip_atoi(&fmt);
+		else if (*fmt == '*') {
+			++fmt;
+			/* it's the next argument */
+			field_width = va_arg(args, int);
+			if (field_width < 0) {
+				field_width = -field_width;
+				flags |= LEFT;
+			}
+		}
+
+		/* get the precision */
+		precision = -1;
+		if (*fmt == '.') {
+			++fmt;
+			if (is_digit(*fmt))
+				precision = skip_atoi(&fmt);
+			else if (*fmt == '*') {
+				++fmt;
+				/* it's the next argument */
+				precision = va_arg(args, int);
+			}
+			if (precision < 0)
+				precision = 0;
+		}
+
+		/* get the conversion qualifier */
+		qualifier = -1;
+		if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L' || *fmt == 'z') {
+			qualifier = *fmt;
+			++fmt;
+			if (*fmt == 'l') {
+				qualifier = 'L';
+				++fmt;
+			}
+		}
+
+		/* default base */
+		base = 10;
+
+		switch (*fmt) {
+		case 'c':
+			if (!(flags & LEFT))
+				while (--field_width > 0)
+					tx_byte(' '), count++;
+			tx_byte((unsigned char) va_arg(args, int)), count++;
+			while (--field_width > 0)
+				tx_byte(' '), count++;
+			continue;
+
+		case 's':
+			s = va_arg(args, char *);
+			if (!s)
+				s = "<NULL>";
+
+			len = strnlen(s, precision);
+
+			if (!(flags & LEFT))
+				while (len < field_width--)
+					tx_byte(' '), count++;
+			for (i = 0; i < len; ++i)
+				tx_byte(*s++), count++;
+			while (len < field_width--)
+				tx_byte(' '), count++;
+			continue;
+
+		case 'p':
+			if (field_width == -1) {
+				field_width = 2*sizeof(void *);
+				flags |= ZEROPAD;
+			}
+			count += number(tx_byte,
+				(unsigned long) va_arg(args, void *), 16,
+				field_width, precision, flags);
+			continue;
+
+		case 'n':
+			if (qualifier == 'L') {
+				long long *ip = va_arg(args, long long *);
+				*ip = count;
+			} else if (qualifier == 'l') {
+				long * ip = va_arg(args, long *);
+				*ip = count;
+			} else {
+				int * ip = va_arg(args, int *);
+				*ip = count;
+			}
+			continue;
+
+		case '%':
+			tx_byte('%'), count++;
+			continue;
+
+		/* integer number formats - set up the flags and "break" */
+		case 'o':
+			base = 8;
+			break;
+
+		case 'X':
+			flags |= LARGE;
+		case 'x':
+			base = 16;
+			break;
+
+		case 'd':
+		case 'i':
+			flags |= SIGN;
+		case 'u':
+			break;
+
+		default:
+			tx_byte('%'), count++;
+			if (*fmt)
+				tx_byte(*fmt), count++;
+			else
+				--fmt;
+			continue;
+		}
+		if (qualifier == 'L') {
+			num = va_arg(args, unsigned long long);
+		} else if (qualifier == 'l') {
+			num = va_arg(args, unsigned long);
+		} else if (qualifier == 'z') {
+			num = va_arg(args, size_t);
+		} else if (qualifier == 'h') {
+			num = (unsigned short) va_arg(args, int);
+			if (flags & SIGN)
+				num = (short) num;
+		} else if (flags & SIGN) {
+			num = va_arg(args, int);
+		} else {
+			num = va_arg(args, unsigned int);
+		}
+		count += number(tx_byte, num, base, field_width, precision, flags);
+	}
+	return count;
+}
+
+int do_printk(int msg_level, const char *fmt, ...)
+{
+	va_list args;
+	int i;
+
+	va_start(args, fmt);
+	i = vtxprintf(exynos5_uart_tx_byte, fmt, args);
+	va_end(args);
+
+	return i;
+}
+
+void bootblock_mainboard_init(void);
+void bootblock_mainboard_init(void)
+{
+	/* FIXME: we should not need UART in bootblock, this is only
+	   done for testing purposes */
+	i2c_init(CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE);
+	power_init();
+	clock_init();
+	do_serial();
+	printk(BIOS_INFO, "%s: hello world\n", __func__);
+
+	/* Copy romstage data from SPI ROM to SRAM */
+	/* FIXME: test with something benign, then fix the offsets once
+	   we're more confident in this */
+	copy_romstage(0x2000, 0x2060000, 0x800);
+}