1 files changed, 20 insertions, 0 deletions

diff --git a/arch/arm/mach-mvebu/coherency_ll.S b/arch/arm/mach-mvebu/coherency_ll.S
index f2e2e8677c4b..6828f9f157b0 100644
--- a/arch/arm/mach-mvebu/coherency_ll.S
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -102,6 +102,26 @@ ENTRY(ll_enable_coherency)
         mov     pc, lr
 ENDPROC(ll_enable_coherency)
 
+ENTRY(ll_disable_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, [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: