diff --git a/arch/arm/mach-at91/clock.c b/arch/arm/mach-at91/clock.c
index 1f2cfdc..9d2e3a3 100644
--- a/arch/arm/mach-at91/clock.c
+++ b/arch/arm/mach-at91/clock.c
@@ -108,6 +108,18 @@
 
 #define cpu_has_dual_matrix()	(cpu_is_sama5d4())
 
+static void *pmc;
+
+static inline void at91_pmc_write(unsigned int offset, u32 val)
+{
+	writel(val, pmc + offset);
+}
+
+static inline u32 at91_pmc_read(unsigned int offset)
+{
+	return readl(pmc + offset);
+}
+
 static LIST_HEAD(clocks);
 
 static u32 at91_pllb_usb_init;
@@ -648,6 +660,15 @@
 	int i;
 	unsigned long main_clock;
 
+	if (cpu_is_sama5d4())
+		pmc = IOMEM(0xf0018000);
+	else
+		pmc = IOMEM(0xfffffc00); /*
+					  * All other supported SoCs use this
+					  * base address (new ones should use of
+					  * clock support)
+		                          */
+
 	main_clock = at91_main_clock;
 
 	/*
diff --git a/arch/arm/mach-at91/include/mach/at91_pmc.h b/arch/arm/mach-at91/include/mach/at91_pmc.h
index d74c140..bbbd497 100644
--- a/arch/arm/mach-at91/include/mach/at91_pmc.h
+++ b/arch/arm/mach-at91/include/mach/at91_pmc.h
@@ -16,12 +16,6 @@
 #ifndef AT91_PMC_H
 #define AT91_PMC_H
 
-#define at91_pmc_read(field) \
-	__raw_readl(AT91_PMC + field)
-
-#define at91_pmc_write(field, value) \
-	__raw_writel(value, AT91_PMC + field)
-
 #define	AT91_PMC_SCER		0x00			/* System Clock Enable Register */
 #define	AT91_PMC_SCDR		0x04			/* System Clock Disable Register */
 
diff --git a/arch/arm/mach-at91/include/mach/at91sam926x_board_init.h b/arch/arm/mach-at91/include/mach/at91sam926x_board_init.h
index ee4dfa7..36fb84b 100644
--- a/arch/arm/mach-at91/include/mach/at91sam926x_board_init.h
+++ b/arch/arm/mach-at91/include/mach/at91sam926x_board_init.h
@@ -20,6 +20,8 @@
 #include <mach/gpio.h>
 #include <mach/at91sam926x.h>
 
+#define AT91SAM926X_BASE_PMC	0xfffffc00
+
 struct at91sam926x_board_cfg {
 	/* SoC specific */
 	void __iomem *pio;
@@ -59,7 +61,7 @@
 	u32 r;
 
 	do {
-		r = at91_pmc_read(AT91_PMC_SR);
+		r = readl(AT91SAM926X_BASE_PMC + AT91_PMC_SR);
 	} while (!(r & AT91_PMC_MCKRDY));
 }