tegra124/nyan: rougly stable code base

nyan: Clock setup.
Reviewed-on: https://chromium-review.googlesource.com/172106
(cherry picked from commit 3697b6454c0aceebcf735436de90ba2441c9b7b1)

tegra124: Call into the mainboard bootblock init if one exists.
Reviewed-on: https://chromium-review.googlesource.com/172581
(cherry picked from commit 3a0cd48a0d1a9ce6b32ed614cd81fb81f5f82aec)

nyan: Add a mainboard specific bootblock.
Reviewed-on: https://chromium-review.googlesource.com/172582
(cherry picked from commit a83d065d660a26fe71ed79879c25f84a1b669f69)

nyan: tegra124: Redestribute the clock code between the mainboard and soc.
Reviewed-on: https://chromium-review.googlesource.com/172583
(cherry picked from commit ea703137fc37befa7d5a65afc982e298a0daca1b)

nyan: Initialize the i2c pins and controllers.
Reviewed-on: https://chromium-review.googlesource.com/172584
(cherry picked from commit 9c10a3074ef834688fea46c03551c2e3e54e44a8)

nyan: Initialize the PMIC.
Reviewed-on: https://chromium-review.googlesource.com/172585
(cherry picked from commit f6be8b0e607e05b73b5e4a84afcf04c879eee88a)

tegra124: add a chip.h and use it in NYAN
Reviewed-on: https://chromium-review.googlesource.com/172773
(cherry picked from commit 4dd5f1f091f2dcae5ce38203bb86c62994609f8f)

tegra: Reorder GPIO register accesses to avoid glitching
Reviewed-on: https://chromium-review.googlesource.com/172730
(cherry picked from commit 61bedbf0f839e19b284d21af2ad10f2ff15e17d5)

tegra: Turn GPIO wrappers into macros to make them easier to write
Reviewed-on: https://chromium-review.googlesource.com/172731
(cherry picked from commit 94550fdfa5a8005d2e6a313041de212ab7ac470c)

tegra: Change GPIO functions to allow variable arguments
Reviewed-on: https://chromium-review.googlesource.com/172916
(cherry picked from commit e95ccd984f718a04b6067ff6ad5049a2cd74466d)

tegra124: Implement starting up the main CPUs.
Reviewed-on: https://chromium-review.googlesource.com/172917
(cherry picked from commit 7c5169a197310e18a3df0f176c499669e3c2bda3)

tegra: Simplify the I2C constants.
Reviewed-on: https://chromium-review.googlesource.com/172953
(cherry picked from commit 130a07c86dfa5ba5ac4580f29db927c91f045c76)

tegra124: Fix SPI base addresses
Reviewed-on: https://chromium-review.googlesource.com/173322
(cherry picked from commit da808e46919ebd3b9f2377a5889f0d5f10b92357)

tegra124: Scrub the clock constants.
Reviewed-on: https://chromium-review.googlesource.com/172954
(cherry picked from commit 9305ff0696a6d556a97f928b8683770833a309a4)

tegra124: add DMA support
Reviewed-on: https://chromium-review.googlesource.com/172951
(cherry picked from commit 4d2a5a56b922ac37d2326d7b139697567aac37b8)

tegra124: add basic SPI driver
Reviewed-on: https://chromium-review.googlesource.com/172952
(cherry picked from commit 5f861f13c7fd2dd881f3cbd0f1b4d4a9994ce429)

tegra124: Add an assembly stub which is run first on the main CPUs.
Reviewed-on: https://chromium-review.googlesource.com/173541
(cherry picked from commit e142b9572a89f43fe984c4fc87e3203f380ff4de)

nyan: tegra124: Set up dynamic cbmem.
Reviewed-on: https://chromium-review.googlesource.com/173542
(cherry picked from commit b6e1a70103446abb5c3440f145617e6566879c6f)

tegra124: Add an soc.c which sets up the chip operations and memory resource.
Reviewed-on: https://chromium-review.googlesource.com/173543
(cherry picked from commit af49a5bd1f589cf053c4808510138aae26e20db4)

tegra124: extend chip.h to include video settings
Reviewed-on: https://chromium-review.googlesource.com/173600
(cherry picked from commit 87687633a2116f58fad7333b3b639cee9089ad29)

tegra124 and nyan: fill in the devicetree a bit more, add defines
Reviewed-on: https://chromium-review.googlesource.com/173684
(cherry picked from commit c107eaca3dea42be89f61690d0d6cb2181acb147)

tegra124: clean-ups for SPI driver
Reviewed-on: https://chromium-review.googlesource.com/173599
(cherry picked from commit 1e2f9fd442ea336bf0663c3c8ea51f771e21beb7)

tegra124: add a #define for DMA alignment size
Reviewed-on: https://chromium-review.googlesource.com/173638
(cherry picked from commit f9dc2a8d8016fa7db974fb6cb01c3275e26832af)

tegra124: Add FIFO transmit functions to SPI driver
Reviewed-on: https://chromium-review.googlesource.com/173639
(cherry picked from commit 97e61f36ad96ce2f9b12a7ef765ee73d3f4285f7)

tegra124: clean-ups for DMA driver
Reviewed-on: https://chromium-review.googlesource.com/173598
(cherry picked from commit 750c0a5d6942748dd21f3a3f884ad94a561e86e0)

tegra124: early display and display code.
Reviewed-on: https://chromium-review.googlesource.com/173622
(cherry picked from commit 651c7ab96b1f136865e4673a120de7afc1218558)

tegra124: Move transfer size handling to spi_xfer()
Reviewed-on: https://chromium-review.googlesource.com/173680
(cherry picked from commit 4a9b7b47b3c09d70063ea843054ffef98f554621)

tegra124: strict error detection and reporting for SPI
Reviewed-on: https://chromium-review.googlesource.com/173681
(cherry picked from commit c056fa954e1dab40a56faec6c50385763a2eb010)

tegra124: add thread-friendly delays to SPI driver
Reviewed-on: https://chromium-review.googlesource.com/173648
(cherry picked from commit c1a321c8f61942801627f895c5db74c518e2aa8e)

Tegra124: Take the SPI1 controller out of reset and enable its clock.
Reviewed-on: https://chromium-review.googlesource.com/173787
(cherry picked from commit c026a3fb861e157f1e17a121fc2ef70b903f36f2)

tegra124: add two more clock setting values
Reviewed-on: https://chromium-review.googlesource.com/173772
(cherry picked from commit 7d79d7dd9f0c1fd7127a7ba41652d809ccff7a57)

nyan: Set up the ChromeOS related GPIOs and SPI bus 1 which goes to the EC.
Reviewed-on: https://chromium-review.googlesource.com/173788
(cherry picked from commit ff172bfe30f75983a1e8efa2ead0a4519583d0a8)

tegra124: Add some stub functions to the Tegra SPI driver.
Reviewed-on: https://chromium-review.googlesource.com/173789
(cherry picked from commit 8bc527aa4afd301c046b0e844c7fa400630af0d2)

tegra124: Build source files into the various stges needed by CONFIG_CHROMEOS.
Reviewed-on: https://chromium-review.googlesource.com/173790
(cherry picked from commit 86a6423b668ca912295c47d8c6e3ef6c6f8c6084)

nyan: Implement the code which reads GPIOs for ChromeOS.
Reviewed-on: https://chromium-review.googlesource.com/173791
(cherry picked from commit 4c394dfbce762574fc79edcb6e4ac6bf346e48a3)

nyan: Enable the CHROMEOS and ChromeOS EC related kconfig options.
Reviewed-on: https://chromium-review.googlesource.com/173792
(cherry picked from commit 2845a4487159aa4b1dba58d977f52c449574fc8e)

Tegra124: SDMMC: Take the SDMMC 3 and 4 out of reset and ungate their clocks.
Reviewed-on: https://chromium-review.googlesource.com/173793
(cherry picked from commit c238b87bcd9d35afd828476d6ee88322ac5d0f88)

tegra124: fix clear_fifo_status() in SPI driver
Reviewed-on: https://chromium-review.googlesource.com/173738
(cherry picked from commit f415d2c0aaffc0f1a3592551a2db782d538f8f4f)

ARM: Include stdint.h in cpu.h.
Reviewed-on: https://chromium-review.googlesource.com/173774
(cherry picked from commit f1930faea3f14b2a2560a6c4058ef38532b6f1a6)

tegra124: When setting up the main CPU, set its CPSR appropriately.
Reviewed-on: https://chromium-review.googlesource.com/173775
(cherry picked from commit bc2ba9c15cfd22aeaca4f80b1d13a8b5e0178ead)

tegra124: fix wrong names in clk_rst.h
Reviewed-on: https://chromium-review.googlesource.com/173955
(cherry picked from commit 19dd9c85e4a3d1f77b23828bcbdd4bd8c2688b8d)

tegra124: Fix up the PLLX divider table.
Reviewed-on: https://chromium-review.googlesource.com/173778
(cherry picked from commit 3362cf3a7d6f5eaec879dda42323345922f6df17)

tegra124: clock: Get rid of cpcon and dccon.
Reviewed-on: https://chromium-review.googlesource.com/173779
(cherry picked from commit 08626ffac4a7e9ea3d4738af87e9e4cced7be2c7)

Tegra124: SPI: Set and unset CS in spi_claim_bus and spi_release_bus.
Reviewed-on: https://chromium-review.googlesource.com/173953
(cherry picked from commit a2df8f3a9c9c54c62d6ff37d3baff1d30ee6d355)

armv7: expose dcache_line_bytes() in cache API
Reviewed-on: https://chromium-review.googlesource.com/173975
(cherry picked from commit 6727f65702c7668fcb33848b4113bc3d3cc04e12)

libpayload: expose dcache_line_bytes() in ARM cache API
Reviewed-on: https://chromium-review.googlesource.com/174099
(cherry picked from commit 9387b02dff85b42944d95c3bccf59059c93fb4a9)

armv4: add a stub for dcache_line_bytes()
Reviewed-on: https://chromium-review.googlesource.com/173976
(cherry picked from commit 924f61ea895b9268c716791466637009bbac6469)

tegra124: Base early UART on CLK_M to enable debugging of PLL init code
Reviewed-on: https://chromium-review.googlesource.com/174339
(cherry picked from commit 8d9387432f0a0d9b257b040304238e543cced1aa)

tegra124: Add additional PLLs and redesign the divisor table
Reviewed-on: https://chromium-review.googlesource.com/174380
(cherry picked from commit f6a5f5c4562f1ca733505717c175be00413f2384)

Squashed 49 commits for tegra124/nyan that included a lot of churn on
different pieces.

Change-Id: I00e8f5b74e835e01b28ca2e9c4af3709c9363d56
Signed-off-by: Isaac Christensen <isaac.christensen@se-eng.com>
Reviewed-on: http://review.coreboot.org/6869
Tested-by: build bot (Jenkins)
Reviewed-by: David Hendricks <dhendrix@chromium.org>

1 files changed, 349 insertions, 12 deletions

diff --git a/src/soc/nvidia/tegra124/clock.c b/src/soc/nvidia/tegra124/clock.c
index af01b56e08..9dce86754e 100644
--- a/src/soc/nvidia/tegra124/clock.c
+++ b/src/soc/nvidia/tegra124/clock.c
@@ -13,31 +13,368 @@
  * You should have received a copy of the GNU General Public License
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-
+#include <console/console.h>
 #include <delay.h>
 #include <arch/io.h>
 #include <soc/addressmap.h>
-
+#include <soc/clock.h>
 #include "clk_rst.h"
-#include "clock.h"
+#include "cpug.h"
+#include "flow.h"
+#include "pmc.h"
 
 static struct clk_rst_ctlr *clk_rst = (void *)TEGRA_CLK_RST_BASE;
-/*
- * On poweron, AVP clock source (also called system clock) is set to PLLP_out0
- * with frequency set at 1MHz. Before initializing PLLP, we need to move the
- * system clock's source to CLK_M temporarily. And then switch it to PLLP_out4
- * (204MHz) at a later time.
+static struct flow_ctlr *flow = (void *)TEGRA_FLOW_BASE;
+static struct tegra_pmc_regs *pmc = (void*)TEGRA_PMC_BASE;
+
+struct pll_dividers {
+	u32	n : 10;
+	u32	m : 8;
+	u32	p : 4;
+	u32	cpcon: 4;
+	u32	: 6;
+};
+
+/* Some PLLs have more restrictive divider bit lengths or are missing some
+ * fields. Make sure to use the right struct in the osc_table definition to get
+ * compile-time checking, but keep the bits aligned with struct pll_dividers so
+ * they can be used interchangeably at run time. Add new formats as required. */
+struct pllcx_dividers {
+	u32	n : 8;
+	u32	: 2;
+	u32	m : 8;
+	u32	p : 4;
+	u32	: 10;
+};
+struct pllpad_dividers {
+	u32	n : 10;
+	u32	m : 5;
+	u32	: 3;
+	u32	p : 3;
+	u32	: 1;
+	u32	cpcon : 4;
+	u32	: 6;
+};
+struct pllu_dividers {
+	u32	n : 10;
+	u32	m : 5;
+	u32	: 3;
+	u32	p : 1;
+	u32	: 3;
+	u32	cpcon : 4;
+	u32	: 6;
+};
+
+union __attribute__((transparent_union)) pll_fields {
+	u32 raw;
+	struct pll_dividers div;
+	struct pllcx_dividers cx;
+	struct pllpad_dividers pad;
+	struct pllu_dividers u;
+};
+
+/* This table defines the frequency dividers for every PLL to turn the external
+ * OSC clock into the frequencies defined by TEGRA_PLL*_KHZ in soc/clock.h.
+ * All PLLs have three dividers (N, M and P), with the governing formula for
+ * the output frequency being OUT = (IN / m) * N / (2^P). */
+struct {
+	int khz;
+	struct pllcx_dividers	pllx;	/* target: 1900 MHz */
+	struct pllpad_dividers	pllp;	/* target:  408 MHz */
+	struct pllcx_dividers	pllc;	/* target:  600 MHz */
+	struct pllpad_dividers	plld;	/* target:  925 MHz */
+	struct pllu_dividers	pllu;	/* target;  960 MHz */
+} static const osc_table[16] = {
+	[OSC_FREQ_OSC12]{
+		.khz = 12000,
+		.pllx = {.n = 158, .m =  1, .p = 0},		  /* 1896 MHz */
+		.pllp = {.n =  34, .m =  1, .p = 0, .cpcon = 2},
+		.pllc = {.n =  50, .m =  1, .p = 0},
+		.plld = {.n = 925, .m = 12, .p = 0, .cpcon = 12},
+		.pllu = {.n =  80, .m =  1, .p = 0, .cpcon = 3},
+	},
+	[OSC_FREQ_OSC13]{
+		.khz = 13000,
+		.pllx = {.n = 146, .m =  1, .p = 0},		  /* 1898 MHz */
+		.pllp = {.n = 408, .m = 13, .p = 0, .cpcon = 8},
+		.pllc = {.n = 231, .m =  5, .p = 0},		 /* 600.6 MHz */
+		.plld = {.n = 925, .m = 13, .p = 0, .cpcon = 12},
+		.pllu = {.n = 960, .m = 13, .p = 0, .cpcon = 12},
+	},
+	[OSC_FREQ_OSC16P8]{
+		.khz = 16800,
+		.pllx = {.n = 113, .m =  1, .p = 0},		/* 1898.4 MHz */
+		.pllp = {.n = 170, .m =  7, .p = 0, .cpcon = 4},
+		.pllc = {.n = 250, .m =  7, .p = 0},
+		.plld = {.n = 936, .m = 17, .p = 0, .cpcon = 12},/* 924.9 MHz */
+		.pllu = {.n = 400, .m =  7, .p = 0, .cpcon = 8},
+	},
+	[OSC_FREQ_OSC19P2]{
+		.khz = 19200,
+		.pllx = {.n =  98, .m =  1, .p = 0},		/* 1881.6 MHz */
+		.pllp = {.n =  85, .m =  4, .p = 0, .cpcon = 3},
+		.pllc = {.n = 125, .m =  4, .p = 0},
+		.plld = {.n = 819, .m = 17, .p = 0, .cpcon = 12},/* 924.9 MHz */
+		.pllu = {.n =  50, .m =  1, .p = 0, .cpcon = 2},
+	},
+	[OSC_FREQ_OSC26]{
+		.khz = 26000,
+		.pllx = {.n =  73, .m =  1, .p = 0},		  /* 1898 MHz */
+		.pllp = {.n = 204, .m = 13, .p = 0, .cpcon = 5},
+		.pllc = {.n =  23, .m =  1, .p = 0},		   /* 598 MHz */
+		.plld = {.n = 925, .m = 26, .p = 0, .cpcon = 12},
+		.pllu = {.n = 480, .m = 13, .p = 0, .cpcon = 8},
+	},
+	[OSC_FREQ_OSC38P4]{
+		.khz = 38400,
+		.pllx = {.n =  98, .m =  1, .p = 0},		/* 1881.6 MHz */
+		.pllp = {.n =  85, .m =  4, .p = 0, .cpcon = 3},
+		.pllc = {.n = 125, .m =  4, .p = 0},
+		.plld = {.n = 819, .m = 17, .p = 0, .cpcon = 12},/* 924.9 MHz */
+		.pllu = {.n =  50, .m =  1, .p = 0, .cpcon = 2},
+	},
+	[OSC_FREQ_OSC48]{
+		.khz = 48000,
+		.pllx = {.n = 158, .m =  1, .p = 0},		  /* 1896 MHz */
+		.pllp = {.n =  24, .m =  1, .p = 0, .cpcon = 2},
+		.pllc = {.n =  50, .m =  1, .p = 0},
+		.plld = {.n = 925, .m = 12, .p = 0, .cpcon = 12},
+		.pllu = {.n =  80, .m =  1, .p = 0, .cpcon = 3},
+	},
+};
+
+void clock_ll_set_source_divisor(u32 *reg, u32 source, u32 divisor)
+{
+        u32 value;
+
+        value = readl(reg);
+
+        value &= ~CLK_SOURCE_MASK;
+        value |= source << CLK_SOURCE_SHIFT;
+
+        value &= ~CLK_DIVISOR_MASK;
+        value |= divisor << CLK_DIVISOR_SHIFT;
+
+        writel(value, reg);
+}
+
+/* Get the oscillator frequency, from the corresponding hardware
+ * configuration field. This is actually a per-soc thing. Avoid the
+ * temptation to make it common.
  */
-void set_avp_clock_to_clkm(void)
+static u32 clock_get_osc_bits(void)
+{
+	return readl(&clk_rst->osc_ctrl) >> OSC_CTRL_OSC_FREQ_SHIFT;
+}
+
+int clock_get_osc_khz(void)
+{
+	return osc_table[clock_get_osc_bits()].khz;
+}
+
+static void adjust_pllp_out_freqs(void)
+{
+	u32 reg;
+	/* Set T30 PLLP_OUT1, 2, 3 & 4 freqs to 9.6, 48, 102 & 204MHz */
+	reg = readl(&clk_rst->pllp_outa); // OUTA contains OUT2 / OUT1
+	reg |= (IN_408_OUT_48_DIVISOR << PLLP_OUT2_RATIO) | PLLP_OUT2_OVR
+		| (IN_408_OUT_9_6_DIVISOR << PLLP_OUT1_RATIO) | PLLP_OUT1_OVR;
+	writel(reg, &clk_rst->pllp_outa);
+
+	reg = readl(&clk_rst->pllp_outb);   // OUTB, contains OUT4 / OUT3
+	reg |= (IN_408_OUT_204_DIVISOR << PLLP_OUT4_RATIO) | PLLP_OUT4_OVR
+		| (IN_408_OUT_102_DIVISOR << PLLP_OUT3_RATIO) | PLLP_OUT3_OVR;
+	writel(reg, &clk_rst->pllp_outb);
+}
+
+static void init_pll(u32 *base, u32 *misc, const union pll_fields pll)
+{
+	u32 dividers =  pll.div.n << PLL_BASE_DIVN_SHIFT |
+			pll.div.m << PLL_BASE_DIVM_SHIFT |
+			pll.div.p << PLL_BASE_DIVP_SHIFT;
+
+	/* Write dividers but BYPASS the PLL while we're messing with it. */
+	writel(dividers | PLL_BASE_BYPASS, base);
+
+	/* Set CPCON field (defaults to 0 if it doesn't exist for this PLL) */
+	writel(pll.div.cpcon << PLL_MISC_CPCON_SHIFT, misc);
+
+	/* enable PLL and take it back out of BYPASS (we don't wait for lock
+	 * because we assume that to be done by the time we start using it). */
+	writel(dividers | PLL_BASE_ENABLE, base);
+}
+
+/* Initialize the UART and put it on CLK_M so we can use it during clock_init().
+ * Will later move it to PLLP in clock_config(). The divisor must be very small
+ * to accomodate 12KHz OSCs, so we override the 16.0 UART divider with the 15.1
+ * CLK_SOURCE divider to get more precision. (This might still not be enough for
+ * some OSCs... if you use 13KHz, be prepared to have a bad time.) The 1800 has
+ * been determined through trial and error (must lead to div 13 at 24MHz). */
+void clock_early_uart(void)
+{
+	clock_ll_set_source_divisor(&clk_rst->clk_src_uarta, 3,
+		CLK_UART_DIV_OVERRIDE | CLK_DIVIDER(clock_get_osc_khz(), 1800));
+	setbits_le32(&clk_rst->clk_out_enb_l, CLK_L_UARTA);
+	udelay(2);
+	clrbits_le32(&clk_rst->rst_dev_l, CLK_L_UARTA);
+}
+
+void clock_cpu0_config_and_reset(void *entry)
+{
+	void * const evp_cpu_reset = (uint8_t *)TEGRA_EVP_BASE + 0x100;
+
+	write32(CONFIG_STACK_TOP, &cpug_stack_pointer);
+	write32((uintptr_t)entry, &cpug_entry_point);
+	write32((uintptr_t)&cpug_setup, evp_cpu_reset);
+
+	// Set up cclk_brst and divider.
+	write32((CRC_CCLK_BRST_POL_PLLX_OUT0 << 0) |
+		(CRC_CCLK_BRST_POL_PLLX_OUT0 << 4) |
+		(CRC_CCLK_BRST_POL_PLLX_OUT0 << 8) |
+		(CRC_CCLK_BRST_POL_PLLX_OUT0 << 12) |
+		(CRC_CCLK_BRST_POL_CPU_STATE_RUN << 28),
+		&clk_rst->cclk_brst_pol);
+	write32(CRC_SUPER_CCLK_DIVIDER_SUPER_CDIV_ENB,
+		&clk_rst->super_cclk_div);
+
+	// Enable the clocks for CPUs 0-3.
+	uint32_t cpu_cmplx_clr = read32(&clk_rst->clk_cpu_cmplx_clr);
+	cpu_cmplx_clr |= CRC_CLK_CLR_CPU0_STP | CRC_CLK_CLR_CPU1_STP |
+			 CRC_CLK_CLR_CPU2_STP | CRC_CLK_CLR_CPU3_STP;
+	write32(cpu_cmplx_clr, &clk_rst->clk_cpu_cmplx_clr);
+
+	// Enable other CPU related clocks.
+	setbits_le32(&clk_rst->clk_out_enb_l, CLK_L_CPU);
+	setbits_le32(&clk_rst->clk_out_enb_v, CLK_V_CPUG);
+
+	// Disable the reset on the non-CPU parts of the fast cluster.
+	write32(CRC_RST_CPUG_CLR_NONCPU,
+		&clk_rst->rst_cpug_cmplx_clr);
+	// Disable the various resets on the CPUs.
+	write32(CRC_RST_CPUG_CLR_CPU0 | CRC_RST_CPUG_CLR_CPU1 |
+		CRC_RST_CPUG_CLR_CPU2 | CRC_RST_CPUG_CLR_CPU3 |
+		CRC_RST_CPUG_CLR_DBG0 | CRC_RST_CPUG_CLR_DBG1 |
+		CRC_RST_CPUG_CLR_DBG2 | CRC_RST_CPUG_CLR_DBG3 |
+		CRC_RST_CPUG_CLR_CORE0 | CRC_RST_CPUG_CLR_CORE1 |
+		CRC_RST_CPUG_CLR_CORE2 | CRC_RST_CPUG_CLR_CORE3 |
+		CRC_RST_CPUG_CLR_CX0 | CRC_RST_CPUG_CLR_CX1 |
+		CRC_RST_CPUG_CLR_CX2 | CRC_RST_CPUG_CLR_CX3 |
+		CRC_RST_CPUG_CLR_L2 | CRC_RST_CPUG_CLR_PDBG,
+		&clk_rst->rst_cpug_cmplx_clr);
+}
+
+/**
+ * The T124 requires some special clock initialization, including setting up
+ * the DVC I2C, turning on MSELECT and selecting the G CPU cluster
+ */
+void clock_init(void)
 {
 	u32 val;
+	u32 osc = clock_get_osc_bits();
 
+	/*
+	 * On poweron, AVP clock source (also called system clock) is set to
+	 * PLLP_out0 with frequency set at 1MHz. Before initializing PLLP, we
+	 * need to move the system clock's source to CLK_M temporarily. And
+	 * then switch it to PLLP_out4 (204MHz) at a later time.
+	 */
 	val = (SCLK_SOURCE_CLKM << SCLK_SWAKEUP_FIQ_SOURCE_SHIFT) |
 		(SCLK_SOURCE_CLKM << SCLK_SWAKEUP_IRQ_SOURCE_SHIFT) |
 		(SCLK_SOURCE_CLKM << SCLK_SWAKEUP_RUN_SOURCE_SHIFT) |
 		(SCLK_SOURCE_CLKM << SCLK_SWAKEUP_IDLE_SOURCE_SHIFT) |
 		(SCLK_SYS_STATE_RUN << SCLK_SYS_STATE_SHIFT);
-	writel(val, &clk_rst->crc_sclk_brst_pol);
-	/* Wait 2-3us for the clock to flush thru the logic as per the TRM */
-	udelay(3);
+	writel(val, &clk_rst->sclk_brst_pol);
+	udelay(2);
+
+	/* Set active CPU cluster to G */
+	clrbits_le32(&flow->cluster_control, 1);
+
+	/* Change the oscillator drive strength */
+	val = readl(&clk_rst->osc_ctrl);
+	val &= ~OSC_XOFS_MASK;
+	val |= (OSC_DRIVE_STRENGTH << OSC_XOFS_SHIFT);
+	writel(val, &clk_rst->osc_ctrl);
+
+	/* Ambiguous quote from u-boot. TODO: what's this mean?
+	 * "should update same value in PMC_OSC_EDPD_OVER XOFS
+	   field for warmboot "*/
+	val = readl(&pmc->osc_edpd_over);
+	val &= ~PMC_OSC_EDPD_OVER_XOFS_MASK;
+	val |= (OSC_DRIVE_STRENGTH << PMC_OSC_EDPD_OVER_XOFS_SHIFT);
+	writel(val, &pmc->osc_edpd_over);
+
+	/* Disable IDDQ for PLLX before we set it up (from U-Boot -- why?) */
+	val = readl(&clk_rst->pllx_misc3);
+	val &= ~PLLX_IDDQ_MASK;
+	writel(val, &clk_rst->pllx_misc3);
+	udelay(2);
+
+	/* Set PLLC dynramp_step A to 0x2b and B to 0xb (from U-Boot -- why? */
+	writel(0x2b << 17 | 0xb << 9, &clk_rst->pllc_misc2);
+
+	adjust_pllp_out_freqs();
+
+	init_pll(&clk_rst->pllx_base, &clk_rst->pllx_misc, osc_table[osc].pllx);
+	init_pll(&clk_rst->pllp_base, &clk_rst->pllp_misc, osc_table[osc].pllp);
+	init_pll(&clk_rst->pllc_base, &clk_rst->pllc_misc, osc_table[osc].pllc);
+	init_pll(&clk_rst->plld_base, &clk_rst->plld_misc, osc_table[osc].plld);
+	init_pll(&clk_rst->pllu_base, &clk_rst->pllu_misc, osc_table[osc].pllu);
+
+	val = (1 << CLK_SYS_RATE_AHB_RATE_SHIFT);
+	writel(val, &clk_rst->clk_sys_rate);
+}
+
+void clock_config(void)
+{
+	/* Enable clocks for the required peripherals. */
+	/* TODO: can (should?) we use the _SET and _CLR registers here? */
+	setbits_le32(&clk_rst->clk_out_enb_l,
+		     CLK_L_CACHE2 | CLK_L_GPIO | CLK_L_TMR | CLK_L_I2C1 |
+		     CLK_L_SDMMC4);
+	setbits_le32(&clk_rst->clk_out_enb_h,
+		     CLK_H_EMC | CLK_H_I2C2 | CLK_H_I2C5 | CLK_H_SBC1 |
+		     CLK_H_PMC | CLK_H_APBDMA | CLK_H_MEM);
+	setbits_le32(&clk_rst->clk_out_enb_u,
+		     CLK_U_I2C3 | CLK_U_CSITE | CLK_U_SDMMC3);
+	setbits_le32(&clk_rst->clk_out_enb_v, CLK_V_MSELECT);
+	setbits_le32(&clk_rst->clk_out_enb_w, CLK_W_DVFS);
+
+	/*
+	 * Set MSELECT clock source as PLLP (00)_REG, and ask for a clock
+	 * divider that would set the MSELECT clock at 102MHz for a
+	 * PLLP base of 408MHz.
+	 */
+	clock_ll_set_source_divisor(&clk_rst->clk_src_mselect, 0,
+		CLK_DIVIDER(TEGRA_PLLP_KHZ, 102000));
+
+	/* Give clock time to stabilize */
+	udelay(IO_STABILIZATION_DELAY);
+
+	/* I2C1 gets CLK_M and a divisor of 17 */
+	clock_ll_set_source_divisor(&clk_rst->clk_src_i2c1, 3, 16);
+	/* I2C2 gets CLK_M and a divisor of 17 */
+	clock_ll_set_source_divisor(&clk_rst->clk_src_i2c2, 3, 16);
+	/* I2C3 (cam) gets CLK_M and a divisor of 17 */
+	clock_ll_set_source_divisor(&clk_rst->clk_src_i2c3, 3, 16);
+	/* I2C5 (PMU) gets CLK_M and a divisor of 17 */
+	clock_ll_set_source_divisor(&clk_rst->clk_src_i2c5, 3, 16);
+
+	/* UARTA gets PLLP, deactivate CLK_UART_DIV_OVERRIDE */
+	writel(0 << CLK_SOURCE_SHIFT, &clk_rst->clk_src_uarta);
+
+	/* Give clock time to stabilize. */
+	udelay(IO_STABILIZATION_DELAY);
+
+	/* Take required peripherals out of reset. */
+
+	clrbits_le32(&clk_rst->rst_dev_l,
+		     CLK_L_CACHE2 | CLK_L_GPIO | CLK_L_TMR | CLK_L_I2C1 |
+		     CLK_L_SDMMC4);
+	clrbits_le32(&clk_rst->rst_dev_h,
+		     CLK_H_EMC | CLK_H_I2C2 | CLK_H_I2C5 | CLK_H_SBC1 |
+		     CLK_H_PMC | CLK_H_APBDMA | CLK_H_MEM);
+	clrbits_le32(&clk_rst->rst_dev_u,
+		     CLK_U_I2C3 | CLK_U_CSITE | CLK_U_SDMMC3);
+	clrbits_le32(&clk_rst->rst_dev_v, CLK_V_MSELECT);
+	clrbits_le32(&clk_rst->rst_dev_w, CLK_W_DVFS);
 }