diff --git a/arch/arm/mach-mvebu/coherency.c b/arch/arm/mach-mvebu/coherency.c
index 944372a18ed6..9d5ccd372712 100644
--- a/arch/arm/mach-mvebu/coherency.c
+++ b/arch/arm/mach-mvebu/coherency.c
@@ -60,8 +60,9 @@ static struct of_device_id of_coherency_table[] = {
 	{ /* end of list */ },
 };
 
-/* Function defined in coherency_ll.S */
-int ll_set_cpu_coherent(void);
+/* Functions defined in coherency_ll.S */
+int ll_enable_coherency(void);
+void ll_add_cpu_to_smp_group(void);
 
 int set_cpu_coherent(void)
 {
@@ -71,7 +72,8 @@ int set_cpu_coherent(void)
 		return 1;
 	}
 
-	return ll_set_cpu_coherent();
+	ll_add_cpu_to_smp_group();
+	return ll_enable_coherency();
 }
 
 /*
diff --git a/arch/arm/mach-mvebu/coherency_ll.S b/arch/arm/mach-mvebu/coherency_ll.S
index 98a0b73f909b..f2e2e8677c4b 100644
--- a/arch/arm/mach-mvebu/coherency_ll.S
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -24,52 +24,84 @@
 #include <asm/cp15.h>
 
 	.text
-
-ENTRY(ll_set_cpu_coherent)
+/* Returns with the coherency address in r1 (r0 is untouched)*/
+ENTRY(ll_get_coherency_base)
 	mrc	p15, 0, r1, c1, c0, 0
 	tst	r1, #CR_M @ Check MMU bit enabled
 	bne	1f
 
-	/* use physical address of the coherency register*/
-	adr	r0, 3f
-	ldr	r3, [r0]
-	ldr	r0, [r0, r3]
+	/* use physical address of the coherency register */
+	adr	r1, 3f
+	ldr	r3, [r1]
+	ldr	r1, [r1, r3]
 	b	2f
 1:
-	/* use virtual address of the coherency register*/
-	ldr	r0, =coherency_base
-	ldr	r0, [r0]
+	/* use virtual address of the coherency register */
+	ldr	r1, =coherency_base
+	ldr	r1, [r1]
 2:
-	/* Create bit by cpu index */
-	mrc	15, 0, r1, cr0, cr0, 5
-	and	r1, r1, #15
+	mov	pc, lr
+ENDPROC(ll_get_coherency_base)
+
+/* Returns with the CPU ID in r3 (r0 is untouched)*/
+ENTRY(ll_get_cpuid)
+	mrc	15, 0, r3, cr0, cr0, 5
+	and	r3, r3, #15
 	mov	r2, #(1 << 24)
-	lsl	r1, r2, r1
+	lsl	r3, r2, r3
 ARM_BE8(rev	r1, r1)
+	mov	pc, lr
+ENDPROC(ll_get_cpuid)
 
-	/* Add CPU to SMP group - Atomic */
-	add	r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
+/* ll_add_cpu_to_smp_group, ll_enable_coherency and
+ * ll_disable_coherency use strex/ldrex whereas MMU can be off. The
+ * Armada XP SoC has an exclusive monitor that can track transactions
+ * to Device and/or SO and as such also when MMU is disabled the
+ * exclusive transactions will be functional
+ */
+
+ENTRY(ll_add_cpu_to_smp_group)
+	/*
+	 * r0 being untouched in ll_get_coherency_base and
+	 * ll_get_cpuid, we can use it to save lr modifing it with the
+	 * following bl
+	 */
+	mov r0, lr
+	bl	ll_get_coherency_base
+	bl	ll_get_cpuid
+	mov lr, r0
+	add	r0, r1, #ARMADA_XP_CFB_CFG_REG_OFFSET
 1:
-	ldrex	r2, [r3]
-	orr	r2, r2, r1
-	strex 	r0, r2, [r3]
-	cmp	r0, #0
-	bne 1b
+	ldrex	r2, [r0]
+	orr	r2, r2, r3
+	strex	r1, r2, [r0]
+	cmp	r1, #0
+	bne	1b
+	mov	pc, lr
+ENDPROC(ll_add_cpu_to_smp_group)
 
-	/* Enable coherency on CPU - Atomic */
-	add	r3, r3, #ARMADA_XP_CFB_CFG_REG_OFFSET
+ENTRY(ll_enable_coherency)
+	/*
+	 * r0 being untouched in ll_get_coherency_base and
+	 * ll_get_cpuid, we can use it to save lr modifing it with the
+	 * following bl
+	 */
+	mov r0, lr
+	bl	ll_get_coherency_base
+	bl	ll_get_cpuid
+	mov lr, r0
+	add	r0, r1, #ARMADA_XP_CFB_CTL_REG_OFFSET
 1:
-	ldrex	r2, [r3]
-	orr	r2, r2, r1
-	strex	r0, r2, [r3]
-	cmp	r0, #0
-	bne 1b
-
+	ldrex	r2, [r0]
+	orr	r2, r2, r3
+	strex	r1, r2, [r0]
+	cmp	r1, #0
+	bne	1b
 	dsb
-
 	mov	r0, #0
 	mov	pc, lr
-ENDPROC(ll_set_cpu_coherent)
+ENDPROC(ll_enable_coherency)
+
 
 	.align 2
 3:
diff --git a/arch/arm/mach-mvebu/headsmp.S b/arch/arm/mach-mvebu/headsmp.S
index cf7abe6554f7..2c4032e368ba 100644
--- a/arch/arm/mach-mvebu/headsmp.S
+++ b/arch/arm/mach-mvebu/headsmp.S
@@ -31,8 +31,10 @@
 ENTRY(armada_xp_secondary_startup)
  ARM_BE8(setend	be )			@ go BE8 if entered LE
 
-	/* Add CPU to coherency fabric */
-	bl	ll_set_cpu_coherent
+	bl	ll_add_cpu_to_smp_group
+
+	bl	ll_enable_coherency
+
 	b	secondary_startup
 
 ENDPROC(armada_xp_secondary_startup)