2 files changed, 11 insertions, 15 deletions

diff --git a/arch/arm/mach-kirkwood/board-ts219.c b/arch/arm/mach-kirkwood/board-ts219.c
index acb0187c7ee1..4695d5f35fc9 100644
--- a/arch/arm/mach-kirkwood/board-ts219.c
+++ b/arch/arm/mach-kirkwood/board-ts219.c
@@ -41,13 +41,3 @@ void __init qnap_dt_ts219_init(void)
 
         pm_power_off = qnap_tsx1x_power_off;
 }
-
-/* FIXME: Will not work with DT. Maybe use MPP40_GPIO? */
-static int __init ts219_pci_init(void)
-{
-        if (machine_is_ts219())
-                kirkwood_pcie_init(KW_PCIE0);
-
-        return 0;
-}
-subsys_initcall(ts219_pci_init);
diff --git a/arch/arm/mach-mvebu/coherency_ll.S b/arch/arm/mach-mvebu/coherency_ll.S
index 53e8391192cd..5476669ba905 100644
--- a/arch/arm/mach-mvebu/coherency_ll.S
+++ b/arch/arm/mach-mvebu/coherency_ll.S
@@ -32,15 +32,21 @@ ENTRY(ll_set_cpu_coherent)
 
         /* Add CPU to SMP group - Atomic */
         add     r3, r0, #ARMADA_XP_CFB_CTL_REG_OFFSET
-        ldr     r2, [r3]
+1:
+        ldrex   r2, [r3]
         orr     r2, r2, r1
-        str     r2, [r3]
+        strex   r0, r2, [r3]
+        cmp     r0, #0
+        bne 1b
 
         /* Enable coherency on CPU - Atomic */
-        add     r3, r0, #ARMADA_XP_CFB_CFG_REG_OFFSET
-        ldr     r2, [r3]
+        add     r3, r3, #ARMADA_XP_CFB_CFG_REG_OFFSET
+1:
+        ldrex   r2, [r3]
         orr     r2, r2, r1
-        str     r2, [r3]
+        strex   r0, r2, [r3]
+        cmp     r0, #0
+        bne 1b
 
         dsb