arch:arm: add atomic_cmpxchg

Signed-off-by: Ruslan Kuprieiev <kupruser@gmail.com>
Signed-off-by: Pavel Emelyanov <xemul@parallels.com>

arch/arm/include/asm/atomic.h

 #ifndef __CR_ATOMIC_H__
 #define __CR_ATOMIC_H__
 
+#include "asm/processor.h"
+
 typedef struct {
 	int counter;
 } atomic_t;
@@ -89,4 +91,28 @@ static inline int atomic_dec(atomic_t *v) { return atomic_sub_return(1, v) + 1;
 
 #define atomic_inc_return(v)	(atomic_add_return(1, v))
 
+static inline int atomic_cmpxchg(atomic_t *ptr, int old, int new)
+{
+	int oldval;
+	unsigned long res;
+
+	smp_mb();
+	prefetchw(&ptr->counter);
+
+	do {
+		__asm__ __volatile__("@ atomic_cmpxchg\n"
+		"ldrex	%1, [%3]\n"
+		"mov	%0, #0\n"
+		"teq	%1, %4\n"
+		"strexeq %0, %5, [%3]\n"
+		    : "=&r" (res), "=&r" (oldval), "+Qo" (ptr->counter)
+		    : "r" (&ptr->counter), "Ir" (old), "r" (new)
+		    : "cc");
+	} while (res);
+
+	smp_mb();
+
+	return oldval;
+}
+
 #endif /* __CR_ATOMIC_H__ */

arch/arm/include/asm/processor.h

+#ifndef __CR_PROCESSOR_H__
+#define __CR_PROCESSOR_H__
+
+/* Copied from linux kernel arch/arm/include/asm/unified.h */
+
+#define WASM(instr)     #instr
+
+/* Copied from linux kernel arch/arm/include/asm/processor.h */
+
+#define __ALT_SMP_ASM(smp, up)						\
+	"9998:	" smp "\n"						\
+	"	.pushsection \".alt.smp.init\", \"a\"\n"		\
+	"	.long	9998b\n"					\
+	"	" up "\n"						\
+	"	.popsection\n"
+
+static inline void prefetchw(const void *ptr)
+{
+	__asm__ __volatile__(
+		".arch_extension	mp\n"
+		__ALT_SMP_ASM(
+			WASM(pldw)		"\t%a0",
+			WASM(pld)		"\t%a0"
+		)
+		:: "p" (ptr));
+}
+
+#endif /* __CR_PROCESSOR_H__ */