@@ -8,6 +8,7 @@
#include <linux/compiler.h>
#include <linux/types.h>
+#include <asm/byteorder.h>
#if defined(CONFIG_GUSA_RB)
#include <asm/cmpxchg-grb.h>
@@ -19,6 +20,26 @@
extern void __xchg_called_with_bad_pointer(void);
+static inline u32 __xchg_cmpxchg(void *ptr, u32 x, int size)
+{
+ int off = (unsigned long)ptr % sizeof(u32);
+ u32 *p = ptr - off;
+ int bitoff = __BYTE_ORDER == __BIG_ENDIAN ?
+ ((sizeof(u32) - 1 - off) * BITS_PER_BYTE) :
+ (off * BITS_PER_BYTE);
+ u32 bitmask = ((0x1 << size * BITS_PER_BYTE) - 1) << bitoff;
+ u32 oldv, newv;
+ u32 ret;
+
+ do {
+ oldv = READ_ONCE(*p);
+ ret = (oldv & bitmask) >> bitoff;
+ newv = (oldv & ~bitmask) | (x << bitoff);
+ } while(cmpxchg(p, oldv, newv) != oldv);
+
+ return ret;
+}
+
#define __xchg(ptr, x, size) \
({ \
unsigned long __xchg__res; \
@@ -27,8 +48,10 @@ extern void __xchg_called_with_bad_pointer(void);
case 4: \
__xchg__res = xchg_u32(__xchg_ptr, x); \
break; \
+ case 2: \
case 1: \
- __xchg__res = xchg_u8(__xchg_ptr, x); \
+ __xchg__res = __xchg_cmpxchg(__xchg_ptr,\
+ x, size); \
break; \
default: \
__xchg_called_with_bad_pointer(); \