bits: Add test_and_set_bit()

Borrowed from Linux Kernel

v5: New

Signed-off-by: Kirill Tkhai <ktkhai@virtuozzo.com>
Signed-off-by: Pavel Emelyanov <xemul@virtuozzo.com>

criu/arch/aarch64/Makefile

@@ -3,7 +3,9 @@ builtin-name		:= crtools.built-in.o
 ccflags-y		+= -iquote $(obj) -iquote $(SRC_DIR)
 ccflags-y		+= -iquote $(obj)/include -iquote $(SRC_DIR)/criu/include
 ccflags-y		+= -iquote $(SRC_DIR)/include
+asflags-y		+= -D__ASSEMBLY__
 
 obj-y			+= cpu.o
 obj-y			+= crtools.o
 obj-y			+= sigframe.o
+obj-y			+= bitops.o

criu/arch/aarch64/bitops.S

@@ -0,0 +1,18 @@
+#include "common/asm/linkage.h"
+
+	.text
+
+ENTRY(test_and_set_bit)
+	and     w3, w0, #63
+	eor     w0, w0, w3
+	mov     x2, #1
+	add     x1, x1, x0, lsr #3
+	lsl     x4, x2, x3
+1:	ldaxr   x2, [x1]
+	lsr     x0, x2, x3
+	orr  x2, x2, x4
+	stlxr   w5, x2, [x1]
+	cbnz    w5, 1b
+	and     x0, x0, #1
+3:	ret
+END(test_and_set_bit)

criu/arch/arm/Makefile

@@ -3,6 +3,9 @@ builtin-name		:= crtools.built-in.o
 ccflags-y		+= -iquote $(obj) -iquote $(SRC_DIR) -iquote $(obj)/include
 ccflags-y		+= -iquote $(SRC_DIR)/criu/include -iquote $(SRC_DIR)/include
 
+asflags-y		+= -D__ASSEMBLY__
+
 obj-y			+= cpu.o
 obj-y			+= crtools.o
 obj-y			+= sigframe.o
+obj-y			+= bitops.o

criu/arch/arm/bitops.S

@@ -0,0 +1,22 @@
+#include "common/asm/linkage.h"
+
+ENTRY(test_and_set_bit)
+	ands    ip, r1, #3
+	strneb  r1, [ip]                @ assert word-aligned
+	mov     r2, #1
+	and     r3, r0, #31             @ Get bit offset
+	mov     r0, r0, lsr #5
+	add     r1, r1, r0, lsl #2      @ Get word offset
+	mov     r3, r2, lsl r3          @ create mask
+	dmb     ish
+1:	ldrex   r2, [r1]
+	ands    r0, r2, r3              @ save old value of bit
+	orreq  r2, r2, r3              @ toggle bit
+	strex   ip, r2, [r1]
+	cmp     ip, #0
+	bne     1b
+	dmb     ish
+	cmp     r0, #0
+	movne   r0, #1
+2:	bx      lr
+END(test_and_set_bit)

include/common/arch/aarch64/asm/bitops.h

@@ -4,4 +4,6 @@
 #include "common/compiler.h"
 #include "common/asm-generic/bitops.h"
 
+extern int test_and_set_bit(int nr, volatile unsigned long *p);
+
 #endif /* __CR_ASM_BITOPS_H__ */

include/common/arch/arm/asm/bitops.h

@@ -4,4 +4,6 @@
 #include "common/compiler.h"
 #include "common/asm-generic/bitops.h"
 
+extern int test_and_set_bit(int nr, volatile unsigned long *p);
+
 #endif /* __CR_ASM_BITOPS_H__ */

include/common/arch/ppc64/asm/bitops.h

@@ -59,6 +59,17 @@
 #define PPC_BIT(bit)            (1UL << PPC_BITLSHIFT(bit))
 #define PPC_BITMASK(bs, be)     ((PPC_BIT(bs) - PPC_BIT(be)) | PPC_BIT(bs))
 
+#define PPC_INST_LDARX		0x7c0000a8
+#define ___PPC_RA(a)		(((a) & 0x1f) << 16)
+#define ___PPC_RB(b)		(((b) & 0x1f) << 11)
+#define ___PPC_RS(s)		(((s) & 0x1f) << 21)
+#define __PPC_EH(eh)		(((eh) & 0x1) << 0)
+#define ___PPC_RT(t)		___PPC_RS(t)
+
+#define PPC_LDARX(t, a, b, eh)  stringify_in_c(.long PPC_INST_LDARX | \
+					___PPC_RT(t) | ___PPC_RA(a) | \
+					___PPC_RB(b) | __PPC_EH(eh))
+#define PPC_LLARX(t, a, b, eh)  PPC_LDARX(t, a, b, eh)
 
 /* Macro for generating the ***_bits() functions */
 #define DEFINE_BITOP(fn, op)            	\
@@ -101,6 +112,36 @@ static inline int test_bit(int nr, const volatile unsigned long *addr)
         return 1UL & (addr[BIT_WORD(nr)] >> (nr & (BITS_PER_LONG-1)));
 }
 
+/* Like DEFINE_BITOP(), with changes to the arguments to 'op' and the output
+ * operands. */
+#define DEFINE_TESTOP(fn, op, prefix, postfix, eh)	\
+static __inline__ unsigned long fn(			\
+		unsigned long mask,			\
+		volatile unsigned long *_p)		\
+{							\
+	unsigned long old, t;				\
+	unsigned long *p = (unsigned long *)_p;		\
+	__asm__ __volatile__ (				\
+	prefix						\
+"1:"	PPC_LLARX(%0,0,%3,eh) "\n"			\
+	stringify_in_c(op) "%1,%0,%2\n"			\
+	"stdcx. %1,0,%3\n"				\
+	"bne- 1b\n"					\
+	postfix						\
+	: "=&r" (old), "=&r" (t)			\
+	: "r" (mask), "r" (p)				\
+	: "cc", "memory");				\
+	return (old & mask);				\
+}
+
+DEFINE_TESTOP(test_and_set_bits, or, "\nLWSYNC\n", "\nsync\n", 0)
+
+static __inline__ int test_and_set_bit(unsigned long nr,
+				       volatile unsigned long *addr)
+{
+	return test_and_set_bits(BIT_MASK(nr), addr + BIT_WORD(nr)) != 0;
+}
+
 /*
  * Return the zero-based bit position (LE, not IBM bit numbering) of
  * the most significant 1-bit in a double word.

include/common/arch/x86/asm/bitops.h

@@ -1,6 +1,7 @@
 #ifndef __CR_BITOPS_H__
 #define __CR_BITOPS_H__
 
+#include "common/arch/x86/asm/cmpxchg.h"
 #include "common/asm/bitsperlong.h"
 
 #define DIV_ROUND_UP(n,d)	(((n) + (d) - 1) / (d))
@@ -46,6 +47,24 @@ static inline void clear_bit(int nr, volatile unsigned long *addr)
 	asm volatile("btrl %1,%0" : ADDR : "Ir" (nr));
 }
 
+/**
+ * test_and_set_bit - Set a bit and return its old value
+ * @nr: Bit to set
+ * @addr: Address to count from
+ *
+ * This operation is atomic and cannot be reordered.
+ * It also implies a memory barrier.
+ */
+static inline int test_and_set_bit(int nr, volatile unsigned long *addr)
+{
+	int oldbit;
+
+	asm volatile(LOCK_PREFIX "bts %2,%1\n\t"
+		     "sbb %0,%0" : "=r" (oldbit), ADDR : "Ir" (nr) : "memory");
+
+	return oldbit;
+}
+
 /**
  * __ffs - find first set bit in word
  * @word: The word to search