diff --git a/arch/arm/cpu/armv7/tegra2/Makefile b/arch/arm/cpu/armv7/tegra2/Makefile
index f1ea915..b35764c 100644
--- a/arch/arm/cpu/armv7/tegra2/Makefile
+++ b/arch/arm/cpu/armv7/tegra2/Makefile
@@ -28,7 +28,7 @@
 LIB	=  $(obj)lib$(SOC).o
 
 SOBJS	:= lowlevel_init.o
-COBJS	:= ap20.o board.o sys_info.o timer.o
+COBJS	:= ap20.o board.o clock.o sys_info.o timer.o
 
 SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(COBJS) $(SOBJS))
diff --git a/arch/arm/cpu/armv7/tegra2/ap20.c b/arch/arm/cpu/armv7/tegra2/ap20.c
index 60dd5df..e3832e2 100644
--- a/arch/arm/cpu/armv7/tegra2/ap20.c
+++ b/arch/arm/cpu/armv7/tegra2/ap20.c
@@ -25,6 +25,7 @@
 #include <asm/io.h>
 #include <asm/arch/tegra2.h>
 #include <asm/arch/clk_rst.h>
+#include <asm/arch/clock.h>
 #include <asm/arch/pmc.h>
 #include <asm/arch/pinmux.h>
 #include <asm/arch/scu.h>
@@ -35,33 +36,34 @@
 void init_pllx(void)
 {
 	struct clk_rst_ctlr *clkrst = (struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
+	struct clk_pll *pll = &clkrst->crc_pll[CLOCK_PLL_ID_XCPU];
 	u32 reg;
 
 	/* If PLLX is already enabled, just return */
-	reg = readl(&clkrst->crc_pllx_base);
+	reg = readl(&pll->pll_base);
 	if (reg & PLL_ENABLE)
 		return;
 
 	/* Set PLLX_MISC */
 	reg = CPCON;				/* CPCON[11:8]  = 0001 */
-	writel(reg, &clkrst->crc_pllx_misc);
+	writel(reg, &pll->pll_misc);
 
 	/* Use 12MHz clock here */
-	reg = (PLL_BYPASS | PLL_DIVM);
+	reg = (PLL_BYPASS | PLL_DIVM_VALUE);
 	reg |= (1000 << 8);			/* DIVN = 0x3E8 */
-	writel(reg, &clkrst->crc_pllx_base);
+	writel(reg, &pll->pll_base);
 
 	reg |= PLL_ENABLE;
-	writel(reg, &clkrst->crc_pllx_base);
+	writel(reg, &pll->pll_base);
 
 	reg &= ~PLL_BYPASS;
-	writel(reg, &clkrst->crc_pllx_base);
+	writel(reg, &pll->pll_base);
 }
 
 static void enable_cpu_clock(int enable)
 {
 	struct clk_rst_ctlr *clkrst = (struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
-	u32 reg, clk;
+	u32 clk;
 
 	/*
 	 * NOTE:
@@ -83,10 +85,6 @@
 		writel(SUPER_CCLK_DIVIDER, &clkrst->crc_super_cclk_div);
 	}
 
-	/* Fetch the register containing the main CPU complex clock enable */
-	reg = readl(&clkrst->crc_clk_out_enb_l);
-	reg |= CLK_ENB_CPU;
-
 	/*
 	 * Read the register containing the individual CPU clock enables and
 	 * always stop the clock to CPU 1.
@@ -103,7 +101,8 @@
 	}
 
 	writel(clk, &clkrst->crc_clk_cpu_cmplx);
-	writel(reg, &clkrst->crc_clk_out_enb_l);
+
+	clock_enable(PERIPH_ID_CPU);
 }
 
 static int is_cpu_powered(void)
@@ -179,7 +178,7 @@
 static void reset_A9_cpu(int reset)
 {
 	struct clk_rst_ctlr *clkrst = (struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
-	u32 reg, cpu;
+	u32 cpu;
 
 	/*
 	* NOTE:  Regardless of whether the request is to hold the CPU in reset
@@ -193,44 +192,27 @@
 	cpu = SET_DBGRESET1 | SET_DERESET1 | SET_CPURESET1;
 	writel(cpu, &clkrst->crc_cpu_cmplx_set);
 
-	reg = readl(&clkrst->crc_rst_dev_l);
 	if (reset) {
 		/* Now place CPU0 into reset */
 		cpu |= SET_DBGRESET0 | SET_DERESET0 | SET_CPURESET0;
 		writel(cpu, &clkrst->crc_cpu_cmplx_set);
-
-		/* Enable master CPU reset */
-		reg |= SWR_CPU_RST;
 	} else {
 		/* Take CPU0 out of reset */
 		cpu = CLR_DBGRESET0 | CLR_DERESET0 | CLR_CPURESET0;
 		writel(cpu, &clkrst->crc_cpu_cmplx_clr);
-
-		/* Disable master CPU reset */
-		reg &= ~SWR_CPU_RST;
 	}
 
-	writel(reg, &clkrst->crc_rst_dev_l);
+	/* Enable/Disable master CPU reset */
+	reset_set_enable(PERIPH_ID_CPU, reset);
 }
 
 static void clock_enable_coresight(int enable)
 {
 	struct clk_rst_ctlr *clkrst = (struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
-	u32 rst, clk, src;
-
-	rst = readl(&clkrst->crc_rst_dev_u);
-	clk = readl(&clkrst->crc_clk_out_enb_u);
-
-	if (enable) {
-		rst &= ~SWR_CSITE_RST;
-		clk |= CLK_ENB_CSITE;
-	} else {
-		rst |= SWR_CSITE_RST;
-		clk &= ~CLK_ENB_CSITE;
-	}
+	u32 rst, src;
 
-	writel(clk, &clkrst->crc_clk_out_enb_u);
-	writel(rst, &clkrst->crc_rst_dev_u);
+	clock_set_enable(PERIPH_ID_CORESIGHT, enable);
+	reset_set_enable(PERIPH_ID_CORESIGHT, !enable);
 
 	if (enable) {
 		/*
diff --git a/arch/arm/cpu/armv7/tegra2/clock.c b/arch/arm/cpu/armv7/tegra2/clock.c
new file mode 100644
index 0000000..67eed14
--- /dev/null
+++ b/arch/arm/cpu/armv7/tegra2/clock.c
@@ -0,0 +1,158 @@
+/*
+ * 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
+ */
+
+/* Tegra2 Clock control functions */
+
+#include <asm/io.h>
+#include <asm/arch/clk_rst.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/timer.h>
+#include <asm/arch/tegra2.h>
+#include <common.h>
+
+#ifdef DEBUG
+#define assert(x)	\
+	({ if (!(x)) printf("Assertion failure '%s' %s line %d\n", \
+		#x, __FILE__, __LINE__); })
+#else
+#define assert(x)
+#endif
+
+/*
+ * Get the oscillator frequency, from the corresponding hardware configuration
+ * field.
+ */
+enum clock_osc_freq clock_get_osc_freq(void)
+{
+	struct clk_rst_ctlr *clkrst =
+			(struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
+	u32 reg;
+
+	reg = readl(&clkrst->crc_osc_ctrl);
+	return (reg & OSC_FREQ_MASK) >> OSC_FREQ_SHIFT;
+}
+
+unsigned long clock_start_pll(enum clock_pll_id clkid, u32 divm, u32 divn,
+		u32 divp, u32 cpcon, u32 lfcon)
+{
+	struct clk_rst_ctlr *clkrst =
+			(struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
+	u32 data;
+	struct clk_pll *pll;
+
+	assert(clock_pll_id_isvalid(clkid));
+	pll = &clkrst->crc_pll[clkid];
+
+	/*
+	 * We cheat by treating all PLL (except PLLU) in the same fashion.
+	 * This works only because:
+	 * - same fields are always mapped at same offsets, except DCCON
+	 * - DCCON is always 0, doesn't conflict
+	 * - M,N, P of PLLP values are ignored for PLLP
+	 */
+	data = (cpcon << PLL_CPCON_SHIFT) | (lfcon << PLL_LFCON_SHIFT);
+	writel(data, &pll->pll_misc);
+
+	data = (divm << PLL_DIVM_SHIFT) | (divn << PLL_DIVN_SHIFT) |
+			(0 << PLL_BYPASS_SHIFT) | (1 << PLL_ENABLE_SHIFT);
+
+	if (clkid == CLOCK_PLL_ID_USB)
+		data |= divp << PLLU_VCO_FREQ_SHIFT;
+	else
+		data |= divp << PLL_DIVP_SHIFT;
+	writel(data, &pll->pll_base);
+
+	/* calculate the stable time */
+	return timer_get_us() + CLOCK_PLL_STABLE_DELAY_US;
+}
+
+void clock_set_enable(enum periph_id periph_id, int enable)
+{
+	struct clk_rst_ctlr *clkrst =
+			(struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
+	u32 *clk = &clkrst->crc_clk_out_enb[PERIPH_REG(periph_id)];
+	u32 reg;
+
+	/* Enable/disable the clock to this peripheral */
+	assert(clock_periph_id_isvalid(periph_id));
+	reg = readl(clk);
+	if (enable)
+		reg |= PERIPH_MASK(periph_id);
+	else
+		reg &= ~PERIPH_MASK(periph_id);
+	writel(reg, clk);
+}
+
+void clock_enable(enum periph_id clkid)
+{
+	clock_set_enable(clkid, 1);
+}
+
+void clock_disable(enum periph_id clkid)
+{
+	clock_set_enable(clkid, 0);
+}
+
+void reset_set_enable(enum periph_id periph_id, int enable)
+{
+	struct clk_rst_ctlr *clkrst =
+			(struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
+	u32 *reset = &clkrst->crc_rst_dev[PERIPH_REG(periph_id)];
+	u32 reg;
+
+	/* Enable/disable reset to the peripheral */
+	assert(clock_periph_id_isvalid(periph_id));
+	reg = readl(reset);
+	if (enable)
+		reg |= PERIPH_MASK(periph_id);
+	else
+		reg &= ~PERIPH_MASK(periph_id);
+	writel(reg, reset);
+}
+
+void reset_periph(enum periph_id periph_id, int us_delay)
+{
+	/* Put peripheral into reset */
+	reset_set_enable(periph_id, 1);
+	udelay(us_delay);
+
+	/* Remove reset */
+	reset_set_enable(periph_id, 0);
+
+	udelay(us_delay);
+}
+
+void reset_cmplx_set_enable(int cpu, int which, int reset)
+{
+	struct clk_rst_ctlr *clkrst =
+			(struct clk_rst_ctlr *)NV_PA_CLK_RST_BASE;
+	u32 mask;
+
+	/* Form the mask, which depends on the cpu chosen. Tegra2 has 2 */
+	assert(cpu >= 0 && cpu < 2);
+	mask = which << cpu;
+
+	/* either enable or disable those reset for that CPU */
+	if (reset)
+		writel(mask, &clkrst->crc_cpu_cmplx_set);
+	else
+		writel(mask, &clkrst->crc_cpu_cmplx_clr);
+}