1 files changed, 301 insertions, 134 deletions

diff --git a/arch/arm/mach-at91/clock.c b/arch/arm/mach-at91/clock.c
index 7525cee3983..034529d801b 100644
--- a/arch/arm/mach-at91/clock.c
+++ b/arch/arm/mach-at91/clock.c
@@ -23,14 +23,19 @@
 #include <linux/delay.h>
 #include <linux/clk.h>
 #include <linux/io.h>
+#include <linux/of_address.h>
+#include <linux/clk/at91_pmc.h>
 
 #include <mach/hardware.h>
-#include <mach/at91_pmc.h>
 #include <mach/cpu.h>
 
+#include <asm/proc-fns.h>
+
 #include "clock.h"
 #include "generic.h"
 
+void __iomem *at91_pmc_base;
+EXPORT_SYMBOL_GPL(at91_pmc_base);
 
 /*
  * There's a lot more which can be done with clocks, including cpufreq
@@ -47,26 +52,57 @@
 /*
  * Chips have some kind of clocks : group them by functionality
  */
-#define cpu_has_utmi()		(  cpu_is_at91cap9() \
-				|| cpu_is_at91sam9rl() \
-				|| cpu_is_at91sam9g45())
+#define cpu_has_utmi()		(  cpu_is_at91sam9rl() \
+				|| cpu_is_at91sam9g45() \
+				|| cpu_is_at91sam9x5() \
+				|| cpu_is_sama5d3())
+
+#define cpu_has_1056M_plla()	(cpu_is_sama5d3())
 
 #define cpu_has_800M_plla()	(  cpu_is_at91sam9g20() \
-				|| cpu_is_at91sam9g45())
+				|| cpu_is_at91sam9g45() \
+				|| cpu_is_at91sam9x5() \
+				|| cpu_is_at91sam9n12())
 
 #define cpu_has_300M_plla()	(cpu_is_at91sam9g10())
 
+#define cpu_has_240M_plla()	(cpu_is_at91sam9261() \
+				|| cpu_is_at91sam9263() \
+				|| cpu_is_at91sam9rl())
+
+#define cpu_has_210M_plla()	(cpu_is_at91sam9260())
+
 #define cpu_has_pllb()		(!(cpu_is_at91sam9rl() \
-				|| cpu_is_at91sam9g45()))
+				|| cpu_is_at91sam9g45() \
+				|| cpu_is_at91sam9x5() \
+				|| cpu_is_sama5d3()))
 
-#define cpu_has_upll()		(cpu_is_at91sam9g45())
+#define cpu_has_upll()		(cpu_is_at91sam9g45() \
+				|| cpu_is_at91sam9x5() \
+				|| cpu_is_sama5d3())
 
 /* USB host HS & FS */
 #define cpu_has_uhp()		(!cpu_is_at91sam9rl())
 
 /* USB device FS only */
 #define cpu_has_udpfs()		(!(cpu_is_at91sam9rl() \
-				|| cpu_is_at91sam9g45()))
+				|| cpu_is_at91sam9g45() \
+				|| cpu_is_at91sam9x5() \
+				|| cpu_is_sama5d3()))
+
+#define cpu_has_plladiv2()	(cpu_is_at91sam9g45() \
+				|| cpu_is_at91sam9x5() \
+				|| cpu_is_at91sam9n12() \
+				|| cpu_is_sama5d3())
+
+#define cpu_has_mdiv3()		(cpu_is_at91sam9g45() \
+				|| cpu_is_at91sam9x5() \
+				|| cpu_is_at91sam9n12() \
+				|| cpu_is_sama5d3())
+
+#define cpu_has_alt_prescaler()	(cpu_is_at91sam9x5() \
+				|| cpu_is_at91sam9n12() \
+				|| cpu_is_sama5d3())
 
 static LIST_HEAD(clocks);
 static DEFINE_SPINLOCK(clk_lock);
@@ -111,11 +147,11 @@ static void pllb_mode(struct clk *clk, int is_on)
 		value = 0;
 
 	// REVISIT: Add work-around for AT91RM9200 Errata #26 ?
-	at91_sys_write(AT91_CKGR_PLLBR, value);
+	at91_pmc_write(AT91_CKGR_PLLBR, value);
 
 	do {
 		cpu_relax();
-	} while ((at91_sys_read(AT91_PMC_SR) & AT91_PMC_LOCKB) != is_on);
+	} while ((at91_pmc_read(AT91_PMC_SR) & AT91_PMC_LOCKB) != is_on);
 }
 
 static struct clk pllb = {
@@ -130,31 +166,24 @@ static struct clk pllb = {
 static void pmc_sys_mode(struct clk *clk, int is_on)
 {
 	if (is_on)
-		at91_sys_write(AT91_PMC_SCER, clk->pmc_mask);
+		at91_pmc_write(AT91_PMC_SCER, clk->pmc_mask);
 	else
-		at91_sys_write(AT91_PMC_SCDR, clk->pmc_mask);
+		at91_pmc_write(AT91_PMC_SCDR, clk->pmc_mask);
 }
 
 static void pmc_uckr_mode(struct clk *clk, int is_on)
 {
-	unsigned int uckr = at91_sys_read(AT91_CKGR_UCKR);
-
-	if (cpu_is_at91sam9g45()) {
-		if (is_on)
-			uckr |= AT91_PMC_BIASEN;
-		else
-			uckr &= ~AT91_PMC_BIASEN;
-	}
+	unsigned int uckr = at91_pmc_read(AT91_CKGR_UCKR);
 
 	if (is_on) {
 		is_on = AT91_PMC_LOCKU;
-		at91_sys_write(AT91_CKGR_UCKR, uckr | clk->pmc_mask);
+		at91_pmc_write(AT91_CKGR_UCKR, uckr | clk->pmc_mask);
 	} else
-		at91_sys_write(AT91_CKGR_UCKR, uckr & ~(clk->pmc_mask));
+		at91_pmc_write(AT91_CKGR_UCKR, uckr & ~(clk->pmc_mask));
 
 	do {
 		cpu_relax();
-	} while ((at91_sys_read(AT91_PMC_SR) & AT91_PMC_LOCKU) != is_on);
+	} while ((at91_pmc_read(AT91_PMC_SR) & AT91_PMC_LOCKU) != is_on);
 }
 
 /* USB function clocks (PLLB must be 48 MHz) */
@@ -163,7 +192,7 @@ static struct clk udpck = {
 	.parent		= &pllb,
 	.mode		= pmc_sys_mode,
 };
-static struct clk utmi_clk = {
+struct clk utmi_clk = {
 	.name		= "utmi_clk",
 	.parent		= &main_clk,
 	.pmc_mask	= AT91_PMC_UPLLEN,	/* in CKGR_UCKR */
@@ -182,17 +211,33 @@ static struct clk uhpck = {
  * memory, interfaces to on-chip peripherals, the AIC, and sometimes more
  * (e.g baud rate generation).  It's sourced from one of the primary clocks.
  */
-static struct clk mck = {
+struct clk mck = {
 	.name		= "mck",
 	.pmc_mask	= AT91_PMC_MCKRDY,	/* in PMC_SR */
 };
 
 static void pmc_periph_mode(struct clk *clk, int is_on)
 {
-	if (is_on)
-		at91_sys_write(AT91_PMC_PCER, clk->pmc_mask);
-	else
-		at91_sys_write(AT91_PMC_PCDR, clk->pmc_mask);
+	u32 regval = 0;
+
+	/*
+	 * With sama5d3 devices, we are managing clock division so we have to
+	 * use the Peripheral Control Register introduced from at91sam9x5
+	 * devices.
+	 */
+	if (cpu_is_sama5d3()) {
+		regval |= AT91_PMC_PCR_CMD; /* write command */
+		regval |= clk->pid & AT91_PMC_PCR_PID; /* peripheral selection */
+		regval |= AT91_PMC_PCR_DIV(clk->div);
+		if (is_on)
+			regval |= AT91_PMC_PCR_EN; /* enable clock */
+		at91_pmc_write(AT91_PMC_PCR, regval);
+	} else {
+		if (is_on)
+			at91_pmc_write(AT91_PMC_PCER, clk->pmc_mask);
+		else
+			at91_pmc_write(AT91_PMC_PCDR, clk->pmc_mask);
+	}
 }
 
 static struct clk __init *at91_css_to_clk(unsigned long css)
@@ -210,47 +255,23 @@ static struct clk __init *at91_css_to_clk(unsigned long css)
 				return &utmi_clk;
 			else if (cpu_has_pllb())
 				return &pllb;
+			break;
+		/* alternate PMC: can use master clock */
+		case AT91_PMC_CSS_MASTER:
+			return &mck;
 	}
 
 	return NULL;
 }
 
-/*
- * Associate a particular clock with a function (eg, "uart") and device.
- * The drivers can then request the same 'function' with several different
- * devices and not care about which clock name to use.
- */
-void __init at91_clock_associate(const char *id, struct device *dev, const char *func)
+static int pmc_prescaler_divider(u32 reg)
 {
-	struct clk *clk = clk_get(NULL, id);
-
-	if (!dev || !clk || !IS_ERR(clk_get(dev, func)))
-		return;
-
-	clk->function = func;
-	clk->dev = dev;
-}
-
-/* clocks cannot be de-registered no refcounting necessary */
-struct clk *clk_get(struct device *dev, const char *id)
-{
-	struct clk *clk;
-
-	list_for_each_entry(clk, &clocks, node) {
-		if (strcmp(id, clk->name) == 0)
-			return clk;
-		if (clk->function && (dev == clk->dev) && strcmp(id, clk->function) == 0)
-			return clk;
+	if (cpu_has_alt_prescaler()) {
+		return 1 << ((reg & AT91_PMC_ALT_PRES) >> PMC_ALT_PRES_OFFSET);
+	} else {
+		return 1 << ((reg & AT91_PMC_PRES) >> PMC_PRES_OFFSET);
 	}
-
-	return ERR_PTR(-ENOENT);
-}
-EXPORT_SYMBOL(clk_get);
-
-void clk_put(struct clk *clk)
-{
 }
-EXPORT_SYMBOL(clk_put);
 
 static void __clk_enable(struct clk *clk)
 {
@@ -309,8 +330,6 @@ EXPORT_SYMBOL(clk_get_rate);
 
 /*------------------------------------------------------------------------*/
 
-#ifdef CONFIG_AT91_PROGRAMMABLE_CLOCKS
-
 /*
  * For now, only the programmable clocks support reparenting (MCK could
  * do this too, with care) or rate changing (the PLLs could do this too,
@@ -353,12 +372,22 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 {
 	unsigned long	flags;
 	unsigned	prescale;
+	unsigned long	prescale_offset, css_mask;
 	unsigned long	actual;
 
 	if (!clk_is_programmable(clk))
 		return -EINVAL;
 	if (clk->users)
 		return -EBUSY;
+
+	if (cpu_has_alt_prescaler()) {
+		prescale_offset = PMC_ALT_PRES_OFFSET;
+		css_mask = AT91_PMC_ALT_PCKR_CSS;
+	} else {
+		prescale_offset = PMC_PRES_OFFSET;
+		css_mask = AT91_PMC_CSS;
+	}
+
 	spin_lock_irqsave(&clk_lock, flags);
 
 	actual = clk->parent->rate_hz;
@@ -366,10 +395,10 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 		if (actual && actual <= rate) {
 			u32	pckr;
 
-			pckr = at91_sys_read(AT91_PMC_PCKR(clk->id));
-			pckr &= AT91_PMC_CSS;	/* clock selection */
-			pckr |= prescale << 2;
-			at91_sys_write(AT91_PMC_PCKR(clk->id), pckr);
+			pckr = at91_pmc_read(AT91_PMC_PCKR(clk->id));
+			pckr &= css_mask;	/* keep clock selection */
+			pckr |= prescale << prescale_offset;
+			at91_pmc_write(AT91_PMC_PCKR(clk->id), pckr);
 			clk->rate_hz = actual;
 			break;
 		}
@@ -403,7 +432,7 @@ int clk_set_parent(struct clk *clk, struct clk *parent)
 
 	clk->rate_hz = parent->rate_hz;
 	clk->parent = parent;
-	at91_sys_write(AT91_PMC_PCKR(clk->id), parent->id);
+	at91_pmc_write(AT91_PMC_PCKR(clk->id), parent->id);
 
 	spin_unlock_irqrestore(&clk_lock, flags);
 	return 0;
@@ -415,57 +444,80 @@ static void __init init_programmable_clock(struct clk *clk)
 {
 	struct clk	*parent;
 	u32		pckr;
+	unsigned int	css_mask;
 
-	pckr = at91_sys_read(AT91_PMC_PCKR(clk->id));
-	parent = at91_css_to_clk(pckr & AT91_PMC_CSS);
+	if (cpu_has_alt_prescaler())
+		css_mask = AT91_PMC_ALT_PCKR_CSS;
+	else
+		css_mask = AT91_PMC_CSS;
+
+	pckr = at91_pmc_read(AT91_PMC_PCKR(clk->id));
+	parent = at91_css_to_clk(pckr & css_mask);
 	clk->parent = parent;
-	clk->rate_hz = parent->rate_hz / (1 << ((pckr & AT91_PMC_PRES) >> 2));
+	clk->rate_hz = parent->rate_hz / pmc_prescaler_divider(pckr);
 }
 
-#endif	/* CONFIG_AT91_PROGRAMMABLE_CLOCKS */
-
 /*------------------------------------------------------------------------*/
 
 #ifdef CONFIG_DEBUG_FS
 
 static int at91_clk_show(struct seq_file *s, void *unused)
 {
-	u32		scsr, pcsr, uckr = 0, sr;
+	u32		scsr, pcsr, pcsr1 = 0, uckr = 0, sr;
 	struct clk	*clk;
 
-	seq_printf(s, "SCSR = %8x\n", scsr = at91_sys_read(AT91_PMC_SCSR));
-	seq_printf(s, "PCSR = %8x\n", pcsr = at91_sys_read(AT91_PMC_PCSR));
-	seq_printf(s, "MOR  = %8x\n", at91_sys_read(AT91_CKGR_MOR));
-	seq_printf(s, "MCFR = %8x\n", at91_sys_read(AT91_CKGR_MCFR));
-	seq_printf(s, "PLLA = %8x\n", at91_sys_read(AT91_CKGR_PLLAR));
+	scsr = at91_pmc_read(AT91_PMC_SCSR);
+	pcsr = at91_pmc_read(AT91_PMC_PCSR);
+	if (cpu_is_sama5d3())
+		pcsr1 = at91_pmc_read(AT91_PMC_PCSR1);
+	sr = at91_pmc_read(AT91_PMC_SR);
+	seq_printf(s, "SCSR = %8x\n", scsr);
+	seq_printf(s, "PCSR = %8x\n", pcsr);
+	if (cpu_is_sama5d3())
+		seq_printf(s, "PCSR1 = %8x\n", pcsr1);
+	seq_printf(s, "MOR  = %8x\n", at91_pmc_read(AT91_CKGR_MOR));
+	seq_printf(s, "MCFR = %8x\n", at91_pmc_read(AT91_CKGR_MCFR));
+	seq_printf(s, "PLLA = %8x\n", at91_pmc_read(AT91_CKGR_PLLAR));
 	if (cpu_has_pllb())
-		seq_printf(s, "PLLB = %8x\n", at91_sys_read(AT91_CKGR_PLLBR));
-	if (cpu_has_utmi())
-		seq_printf(s, "UCKR = %8x\n", uckr = at91_sys_read(AT91_CKGR_UCKR));
-	seq_printf(s, "MCKR = %8x\n", at91_sys_read(AT91_PMC_MCKR));
-	if (cpu_has_upll())
-		seq_printf(s, "USB  = %8x\n", at91_sys_read(AT91_PMC_USB));
-	seq_printf(s, "SR   = %8x\n", sr = at91_sys_read(AT91_PMC_SR));
+		seq_printf(s, "PLLB = %8x\n", at91_pmc_read(AT91_CKGR_PLLBR));
+	if (cpu_has_utmi()) {
+		uckr = at91_pmc_read(AT91_CKGR_UCKR);
+		seq_printf(s, "UCKR = %8x\n", uckr);
+	}
+	seq_printf(s, "MCKR = %8x\n", at91_pmc_read(AT91_PMC_MCKR));
+	if (cpu_has_upll() || cpu_is_at91sam9n12())
+		seq_printf(s, "USB  = %8x\n", at91_pmc_read(AT91_PMC_USB));
+	seq_printf(s, "SR   = %8x\n", sr);
 
 	seq_printf(s, "\n");
 
 	list_for_each_entry(clk, &clocks, node) {
 		char	*state;
 
-		if (clk->mode == pmc_sys_mode)
+		if (clk->mode == pmc_sys_mode) {
 			state = (scsr & clk->pmc_mask) ? "on" : "off";
-		else if (clk->mode == pmc_periph_mode)
-			state = (pcsr & clk->pmc_mask) ? "on" : "off";
-		else if (clk->mode == pmc_uckr_mode)
+		} else if (clk->mode == pmc_periph_mode) {
+			if (cpu_is_sama5d3()) {
+				u32 pmc_mask = 1 << (clk->pid % 32);
+
+				if (clk->pid > 31)
+					state = (pcsr1 & pmc_mask) ? "on" : "off";
+				else
+					state = (pcsr & pmc_mask) ? "on" : "off";
+			} else {
+				state = (pcsr & clk->pmc_mask) ? "on" : "off";
+			}
+		} else if (clk->mode == pmc_uckr_mode) {
 			state = (uckr & clk->pmc_mask) ? "on" : "off";
-		else if (clk->pmc_mask)
+		} else if (clk->pmc_mask) {
 			state = (sr & clk->pmc_mask) ? "on" : "off";
-		else if (clk == &clk32k || clk == &main_clk)
+		} else if (clk == &clk32k || clk == &main_clk) {
 			state = "on";
-		else
+		} else {
 			state = "";
+		}
 
-		seq_printf(s, "%-10s users=%2d %-3s %9ld Hz %s\n",
+		seq_printf(s, "%-10s users=%2d %-3s %9lu Hz %s\n",
 			clk->name, clk->users, state, clk_get_rate(clk),
 			clk->parent ? clk->parent->name : "");
 	}
@@ -498,32 +550,39 @@ postcore_initcall(at91_clk_debugfs_init);
 /*------------------------------------------------------------------------*/
 
 /* Register a new clock */
+static void __init at91_clk_add(struct clk *clk)
+{
+	list_add_tail(&clk->node, &clocks);
+
+	clk->cl.con_id = clk->name;
+	clk->cl.clk = clk;
+	clkdev_add(&clk->cl);
+}
+
 int __init clk_register(struct clk *clk)
 {
 	if (clk_is_peripheral(clk)) {
 		if (!clk->parent)
 			clk->parent = &mck;
+		if (cpu_is_sama5d3())
+			clk->rate_hz = DIV_ROUND_UP(clk->parent->rate_hz,
+						    1 << clk->div);
 		clk->mode = pmc_periph_mode;
-		list_add_tail(&clk->node, &clocks);
 	}
 	else if (clk_is_sys(clk)) {
 		clk->parent = &mck;
 		clk->mode = pmc_sys_mode;
-
-		list_add_tail(&clk->node, &clocks);
 	}
-#ifdef CONFIG_AT91_PROGRAMMABLE_CLOCKS
 	else if (clk_is_programmable(clk)) {
 		clk->mode = pmc_sys_mode;
 		init_programmable_clock(clk);
-		list_add_tail(&clk->node, &clocks);
 	}
-#endif
+
+	at91_clk_add(clk);
 
 	return 0;
 }
 
-
 /*------------------------------------------------------------------------*/
 
 static u32 __init at91_pll_rate(struct clk *pll, u32 freq, u32 reg)
@@ -531,7 +590,11 @@ static u32 __init at91_pll_rate(struct clk *pll, u32 freq, u32 reg)
 	unsigned mul, div;
 
 	div = reg & 0xff;
-	mul = (reg >> 16) & 0x7ff;
+	if (cpu_is_sama5d3())
+		mul = AT91_PMC3_MUL_GET(reg);
+	else
+		mul = AT91_PMC_MUL_GET(reg);
+
 	if (div && mul) {
 		freq /= div;
 		freq *= mul + 1;
@@ -545,6 +608,8 @@ static u32 __init at91_usb_rate(struct clk *pll, u32 freq, u32 reg)
 {
 	if (pll == &pllb && (reg & AT91_PMC_USB96M))
 		return freq / 2;
+	else if (pll == &utmi_clk || cpu_is_at91sam9n12())
+		return freq / (1 + ((reg & AT91_PMC_OHCIUSBDIV) >> 8));
 	else
 		return freq;
 }
@@ -601,7 +666,7 @@ fail:
 	return 0;
 }
 
-static struct clk *const standard_pmc_clocks[] __initdata = {
+static struct clk *const standard_pmc_clocks[] __initconst = {
 	/* four primary clocks */
 	&clk32k,
 	&main_clk,
@@ -614,6 +679,8 @@ static struct clk *const standard_pmc_clocks[] __initdata = {
 /* PLLB generated USB full speed clock init */
 static void __init at91_pllb_usbfs_clock_init(unsigned long main_clock)
 {
+	unsigned int reg;
+
 	/*
 	 * USB clock init:  choose 48 MHz PLLB value,
 	 * disable 48MHz clock during usb peripheral suspend.
@@ -622,24 +689,35 @@ static void __init at91_pllb_usbfs_clock_init(unsigned long main_clock)
 	 */
 	uhpck.parent = &pllb;
 
-	at91_pllb_usb_init = at91_pll_calc(main_clock, 48000000 * 2) | AT91_PMC_USB96M;
+	reg = at91_pllb_usb_init = at91_pll_calc(main_clock, 48000000 * 2);
 	pllb.rate_hz = at91_pll_rate(&pllb, main_clock, at91_pllb_usb_init);
 	if (cpu_is_at91rm9200()) {
+		reg = at91_pllb_usb_init |= AT91_PMC_USB96M;
 		uhpck.pmc_mask = AT91RM9200_PMC_UHP;
 		udpck.pmc_mask = AT91RM9200_PMC_UDP;
-		at91_sys_write(AT91_PMC_SCER, AT91RM9200_PMC_MCKUDP);
+		at91_pmc_write(AT91_PMC_SCER, AT91RM9200_PMC_MCKUDP);
 	} else if (cpu_is_at91sam9260() || cpu_is_at91sam9261() ||
 		   cpu_is_at91sam9263() || cpu_is_at91sam9g20() ||
-		   cpu_is_at91sam9g10() || cpu_is_at572d940hf()) {
+		   cpu_is_at91sam9g10()) {
+		reg = at91_pllb_usb_init |= AT91_PMC_USB96M;
+		uhpck.pmc_mask = AT91SAM926x_PMC_UHP;
+		udpck.pmc_mask = AT91SAM926x_PMC_UDP;
+	} else if (cpu_is_at91sam9n12()) {
+		/* Divider for USB clock is in USB clock register for 9n12 */
+		reg = AT91_PMC_USBS_PLLB;
+
+		/* For PLLB output 96M, set usb divider 2 (USBDIV + 1) */
+		reg |= AT91_PMC_OHCIUSBDIV_2;
+		at91_pmc_write(AT91_PMC_USB, reg);
+
+		/* Still setup masks */
 		uhpck.pmc_mask = AT91SAM926x_PMC_UHP;
 		udpck.pmc_mask = AT91SAM926x_PMC_UDP;
-	} else if (cpu_is_at91cap9()) {
-		uhpck.pmc_mask = AT91CAP9_PMC_UHP;
 	}
-	at91_sys_write(AT91_CKGR_PLLBR, 0);
+	at91_pmc_write(AT91_CKGR_PLLBR, 0);
 
-	udpck.rate_hz = at91_usb_rate(&pllb, pllb.rate_hz, at91_pllb_usb_init);
-	uhpck.rate_hz = at91_usb_rate(&pllb, pllb.rate_hz, at91_pllb_usb_init);
+	udpck.rate_hz = at91_usb_rate(&pllb, pllb.rate_hz, reg);
+	uhpck.rate_hz = at91_usb_rate(&pllb, pllb.rate_hz, reg);
 }
 
 /* UPLL generated USB full speed clock init */
@@ -653,16 +731,15 @@ static void __init at91_upll_usbfs_clock_init(unsigned long main_clock)
 	/* Setup divider by 10 to reach 48 MHz */
 	usbr |= ((10 - 1) << 8) & AT91_PMC_OHCIUSBDIV;
 
-	at91_sys_write(AT91_PMC_USB, usbr);
+	at91_pmc_write(AT91_PMC_USB, usbr);
 
 	/* Now set uhpck values */
 	uhpck.parent = &utmi_clk;
 	uhpck.pmc_mask = AT91SAM926x_PMC_UHP;
-	uhpck.rate_hz = utmi_clk.parent->rate_hz;
-	uhpck.rate_hz /= 1 + ((at91_sys_read(AT91_PMC_USB) & AT91_PMC_OHCIUSBDIV) >> 8);
+	uhpck.rate_hz = at91_usb_rate(&utmi_clk, utmi_clk.rate_hz, usbr);
 }
 
-int __init at91_clock_init(unsigned long main_clock)
+static int __init at91_pmc_init(unsigned long main_clock)
 {
 	unsigned tmp, freq, mckr;
 	int i;
@@ -676,20 +753,29 @@ int __init at91_clock_init(unsigned long main_clock)
 	 */
 	if (!main_clock) {
 		do {
-			tmp = at91_sys_read(AT91_CKGR_MCFR);
+			tmp = at91_pmc_read(AT91_CKGR_MCFR);
 		} while (!(tmp & AT91_PMC_MAINRDY));
 		main_clock = (tmp & AT91_PMC_MAINF) * (AT91_SLOW_CLOCK / 16);
 	}
 	main_clk.rate_hz = main_clock;
 
 	/* report if PLLA is more than mildly overclocked */
-	plla.rate_hz = at91_pll_rate(&plla, main_clock, at91_sys_read(AT91_CKGR_PLLAR));
-	if (cpu_has_300M_plla()) {
-		if (plla.rate_hz > 300000000)
+	plla.rate_hz = at91_pll_rate(&plla, main_clock, at91_pmc_read(AT91_CKGR_PLLAR));
+	if (cpu_has_1056M_plla()) {
+		if (plla.rate_hz > 1056000000)
 			pll_overclock = true;
 	} else if (cpu_has_800M_plla()) {
 		if (plla.rate_hz > 800000000)
 			pll_overclock = true;
+	} else if (cpu_has_300M_plla()) {
+		if (plla.rate_hz > 300000000)
+			pll_overclock = true;
+	} else if (cpu_has_240M_plla()) {
+		if (plla.rate_hz > 240000000)
+			pll_overclock = true;
+	} else if (cpu_has_210M_plla()) {
+		if (plla.rate_hz > 210000000)
+			pll_overclock = true;
 	} else {
 		if (plla.rate_hz > 209000000)
 			pll_overclock = true;
@@ -697,8 +783,8 @@ int __init at91_clock_init(unsigned long main_clock)
 	if (pll_overclock)
 		pr_info("Clocks: PLLA overclocked, %ld MHz\n", plla.rate_hz / 1000000);
 
-	if (cpu_is_at91sam9g45()) {
-		mckr = at91_sys_read(AT91_PMC_MCKR);
+	if (cpu_has_plladiv2()) {
+		mckr = at91_pmc_read(AT91_PMC_MCKR);
 		plla.rate_hz /= (1 << ((mckr & AT91_PMC_PLLADIV2) >> 12));	/* plla divisor by 2 */
 	}
 
@@ -719,6 +805,10 @@ int __init at91_clock_init(unsigned long main_clock)
 		 * (obtain the USB High Speed 480 MHz when input is 12 MHz)
 		 */
 		utmi_clk.rate_hz = 40 * utmi_clk.parent->rate_hz;
+
+		/* UTMI bias and PLL are managed at the same time */
+		if (cpu_has_upll())
+			utmi_clk.pmc_mask |= AT91_PMC_BIASEN;
 	}
 
 	/*
@@ -734,10 +824,10 @@ int __init at91_clock_init(unsigned long main_clock)
 	 * MCK and CPU derive from one of those primary clocks.
 	 * For now, assume this parentage won't change.
 	 */
-	mckr = at91_sys_read(AT91_PMC_MCKR);
+	mckr = at91_pmc_read(AT91_PMC_MCKR);
 	mck.parent = at91_css_to_clk(mckr & AT91_PMC_CSS);
 	freq = mck.parent->rate_hz;
-	freq /= (1 << ((mckr & AT91_PMC_PRES) >> 2));				/* prescale */
+	freq /= pmc_prescaler_divider(mckr);					/* prescale */
 	if (cpu_is_at91rm9200()) {
 		mck.rate_hz = freq / (1 + ((mckr & AT91_PMC_MDIV) >> 8));	/* mdiv */
 	} else if (cpu_is_at91sam9g20()) {
@@ -745,28 +835,34 @@ int __init at91_clock_init(unsigned long main_clock)
 			freq / ((mckr & AT91_PMC_MDIV) >> 7) : freq;	/* mdiv ; (x >> 7) = ((x >> 8) * 2) */
 		if (mckr & AT91_PMC_PDIV)
 			freq /= 2;		/* processor clock division */
-	} else if (cpu_is_at91sam9g45()) {
+	} else if (cpu_has_mdiv3()) {
 		mck.rate_hz = (mckr & AT91_PMC_MDIV) == AT91SAM9_PMC_MDIV_3 ?
 			freq / 3 : freq / (1 << ((mckr & AT91_PMC_MDIV) >> 8));	/* mdiv */
 	} else {
 		mck.rate_hz = freq / (1 << ((mckr & AT91_PMC_MDIV) >> 8));		/* mdiv */
 	}
 
+	if (cpu_has_alt_prescaler()) {
+		/* Programmable clocks can use MCK */
+		mck.type |= CLK_TYPE_PRIMARY;
+		mck.id = 4;
+	}
+
 	/* Register the PMC's standard clocks */
 	for (i = 0; i < ARRAY_SIZE(standard_pmc_clocks); i++)
-		list_add_tail(&standard_pmc_clocks[i]->node, &clocks);
+		at91_clk_add(standard_pmc_clocks[i]);
 
 	if (cpu_has_pllb())
-		list_add_tail(&pllb.node, &clocks);
+		at91_clk_add(&pllb);
 
 	if (cpu_has_uhp())
-		list_add_tail(&uhpck.node, &clocks);
+		at91_clk_add(&uhpck);
 
 	if (cpu_has_udpfs())
-		list_add_tail(&udpck.node, &clocks);
+		at91_clk_add(&udpck);
 
 	if (cpu_has_utmi())
-		list_add_tail(&utmi_clk.node, &clocks);
+		at91_clk_add(&utmi_clk);
 
 	/* MCK and CPU clock are "always on" */
 	clk_enable(&mck);
@@ -779,12 +875,67 @@ int __init at91_clock_init(unsigned long main_clock)
 	return 0;
 }
 
+#if defined(CONFIG_OF)
+static struct of_device_id pmc_ids[] = {
+	{ .compatible = "atmel,at91rm9200-pmc" },
+	{ .compatible = "atmel,at91sam9260-pmc" },
+	{ .compatible = "atmel,at91sam9g45-pmc" },
+	{ .compatible = "atmel,at91sam9n12-pmc" },
+	{ .compatible = "atmel,at91sam9x5-pmc" },
+	{ .compatible = "atmel,sama5d3-pmc" },
+	{ /*sentinel*/ }
+};
+
+static struct of_device_id osc_ids[] = {
+	{ .compatible = "atmel,osc" },
+	{ /*sentinel*/ }
+};
+
+int __init at91_dt_clock_init(void)
+{
+	struct device_node *np;
+	u32 main_clock = 0;
+
+	np = of_find_matching_node(NULL, pmc_ids);
+	if (!np)
+		panic("unable to find compatible pmc node in dtb\n");
+
+	at91_pmc_base = of_iomap(np, 0);
+	if (!at91_pmc_base)
+		panic("unable to map pmc cpu registers\n");
+
+	of_node_put(np);
+
+	/* retrieve the freqency of fixed clocks from device tree */
+	np = of_find_matching_node(NULL, osc_ids);
+	if (np) {
+		u32 rate;
+		if (!of_property_read_u32(np, "clock-frequency", &rate))
+			main_clock = rate;
+	}
+
+	of_node_put(np);
+
+	return at91_pmc_init(main_clock);
+}
+#endif
+
+int __init at91_clock_init(unsigned long main_clock)
+{
+	at91_pmc_base = ioremap(AT91_PMC, 256);
+	if (!at91_pmc_base)
+		panic("Impossible to ioremap AT91_PMC 0x%x\n", AT91_PMC);
+
+	return at91_pmc_init(main_clock);
+}
+
 /*
  * Several unused clocks may be active.  Turn them off.
  */
 static int __init at91_clock_reset(void)
 {
 	unsigned long pcdr = 0;
+	unsigned long pcdr1 = 0;
 	unsigned long scdr = 0;
 	struct clk *clk;
 
@@ -792,8 +943,17 @@ static int __init at91_clock_reset(void)
 		if (clk->users > 0)
 			continue;
 
-		if (clk->mode == pmc_periph_mode)
-			pcdr |= clk->pmc_mask;
+		if (clk->mode == pmc_periph_mode) {
+			if (cpu_is_sama5d3()) {
+				u32 pmc_mask = 1 << (clk->pid % 32);
+
+				if (clk->pid > 31)
+					pcdr1 |= pmc_mask;
+				else
+					pcdr |= pmc_mask;
+			} else
+				pcdr |= clk->pmc_mask;
+		}
 
 		if (clk->mode == pmc_sys_mode)
 			scdr |= clk->pmc_mask;
@@ -801,9 +961,16 @@ static int __init at91_clock_reset(void)
 		pr_debug("Clocks: disable unused %s\n", clk->name);
 	}
 
-	at91_sys_write(AT91_PMC_PCDR, pcdr);
-	at91_sys_write(AT91_PMC_SCDR, scdr);
+	at91_pmc_write(AT91_PMC_SCDR, scdr);
+	if (cpu_is_sama5d3())
+		at91_pmc_write(AT91_PMC_PCDR1, pcdr1);
 
 	return 0;
 }
 late_initcall(at91_clock_reset);
+
+void at91sam9_idle(void)
+{
+	at91_pmc_write(AT91_PMC_SCDR, AT91_PMC_PCK);
+	cpu_do_idle();
+}