1 files changed, 117 insertions, 26 deletions

diff --git a/arch/arm/mach-mvebu/coherency_ll.S b/arch/arm/mach-mvebu/coherency_ll.S
index ee7598fe75db..510c29e079ca 100644
--- a/arch/arm/mach-mvebu/coherency_ll.S
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -21,38 +21,129 @@
 #define ARMADA_XP_CFB_CFG_REG_OFFSET 0x4
 
 #include <asm/assembler.h>
+#include <asm/cp15.h>
 
         .text
+/* Returns the coherency base 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
+
+        /*
+         * MMU is disabled, use the physical address of the coherency
+         * base address.
+         */
+        adr     r1, 3f
+        ldr     r3, [r1]
+        ldr     r1, [r1, r3]
+        b       2f
+1:
+        /*
+         * MMU is enabled, use the virtual address of the coherency
+         * base address.
+         */
+        ldr     r1, =coherency_base
+        ldr     r1, [r1]
+2:
+        mov     pc, lr
+ENDPROC(ll_get_coherency_base)
+
 /*
- * r0: Coherency fabric base register address
- * r1: HW CPU id
+ * Returns the coherency CPU mask in r3 (r0 is untouched). This
+ * coherency CPU mask can be used with the coherency fabric
+ * configuration and control registers. Note that the mask is already
+ * endian-swapped as appropriate so that the calling functions do not
+ * have to care about endianness issues while accessing the coherency
+ * fabric registers
  */
-ENTRY(ll_set_cpu_coherent)
-        /* Create bit by cpu index */
-        mov     r3, #(1 << 24)
-        lsl     r1, r3, r1
-ARM_BE8(rev     r1, r1)
-
-        /* Add CPU to SMP group - Atomic */
-        add     r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
-1:
-        ldrex   r2, [r3]
-        orr     r2, r2, r1
-        strex   r0, r2, [r3]
-        cmp     r0, #0
-        bne 1b
-
-        /* Enable coherency on CPU - Atomic */
-        add     r3, r3, #ARMADA_XP_CFB_CFG_REG_OFFSET
+ENTRY(ll_get_coherency_cpumask)
+        mrc     15, 0, r3, cr0, cr0, 5
+        and     r3, r3, #15
+        mov     r2, #(1 << 24)
+        lsl     r3, r2, r3
+ARM_BE8(rev     r3, r3)
+        mov     pc, lr
+ENDPROC(ll_get_coherency_cpumask)
+
+/*
+ * ll_add_cpu_to_smp_group(), ll_enable_coherency() and
+ * ll_disable_coherency() use the strex/ldrex instructions while the
+ * MMU can be disabled. The Armada XP SoC has an exclusive monitor
+ * that tracks transactions to Device and/or SO memory and thanks to
+ * that, exclusive transactions are functional even when the MMU is
+ * disabled.
+ */
+
+ENTRY(ll_add_cpu_to_smp_group)
+        /*
+         * As r0 is not modified by ll_get_coherency_base() and
+         * ll_get_coherency_cpumask(), we use it to temporarly save lr
+         * and avoid it being modified by the branch and link
+         * calls. This function is used very early in the secondary
+         * CPU boot, and no stack is available at this point.
+         */
+        mov     r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_coherency_cpumask
+        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)
 
+ENTRY(ll_enable_coherency)
+        /*
+         * As r0 is not modified by ll_get_coherency_base() and
+         * ll_get_coherency_cpumask(), we use it to temporarly save lr
+         * and avoid it being modified by the branch and link
+         * calls. This function is used very early in the secondary
+         * CPU boot, and no stack is available at this point.
+         */
+        mov r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_coherency_cpumask
+        mov lr, r0
+        add     r0, r1, #ARMADA_XP_CFB_CTL_REG_OFFSET
+1:
+        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)
+
+ENTRY(ll_disable_coherency)
+        /*
+         * As r0 is not modified by ll_get_coherency_base() and
+         * ll_get_coherency_cpumask(), we use it to temporarly save lr
+         * and avoid it being modified by the branch and link
+         * calls. This function is used very early in the secondary
+         * CPU boot, and no stack is available at this point.
+         */
+        mov     r0, lr
+        bl      ll_get_coherency_base
+        bl      ll_get_coherency_cpumask
+        mov     lr, r0
+        add     r0, r1, #ARMADA_XP_CFB_CTL_REG_OFFSET
+1:
+        ldrex   r2, [r0]
+        bic     r2, r2, r3
+        strex   r1, r2, [r0]
+        cmp     r1, #0
+        bne     1b
+        dsb
+        mov     pc, lr
+ENDPROC(ll_disable_coherency)
+
+        .align 2
+3:
+        .long   coherency_phys_base - .