fel: Implement fel_clrsetbits_le32() helper

This function provides bitwise clear/set operations on 32-bit words
via FEL. It may help with implementing future functionality, where
ARM register manipulations often involve such bit level access.
Signed-off-by: Bernhard Nortmann <bernhard.nortmann@web.de>

fel_lib.c

@@ -484,6 +484,31 @@ void fel_memmove(feldev_handle *dev,
 		fel_memcpy_up(dev, dst_addr, src_addr, size);
 }
 
+/*
+ * Bitwise manipulation of a 32-bit word at given address, via bit masks that
+ * specify which bits to clear and which to set.
+ */
+void fel_clrsetbits_le32(feldev_handle *dev,
+			 uint32_t addr, uint32_t clrbits, uint32_t setbits)
+{
+	uint32_t arm_code[] = {
+		htole32(0xe59f0018), /*    0:  ldr   r0, [addr]              */
+		htole32(0xe5901000), /*    4:  ldr   r1, [r0]                */
+		htole32(0xe59f2014), /*    8:  ldr   r2, [clrbits]           */
+		htole32(0xe1c11002), /*    c:  bic   r1, r1, r2              */
+		htole32(0xe59f2010), /*   10:  ldr   r2, [setbits]           */
+		htole32(0xe1811002), /*   14:  orr   r1, r1, r2              */
+		htole32(0xe5801000), /*   18:  str   r1, [r0]                */
+		htole32(0xe12fff1e), /*   1c:  bx    lr                      */
+
+		htole32(addr),    /* address */
+		htole32(clrbits), /* bits to clear */
+		htole32(setbits), /* bits to set */
+	};
+	aw_fel_write(dev, arm_code, dev->soc_info->scratch_addr, sizeof(arm_code));
+	aw_fel_execute(dev, dev->soc_info->scratch_addr);
+}
+
 /*
  * Memory access to the SID (root) keys proved to be unreliable for certain
  * SoCs. This function uses an alternative, register-based approach to retrieve

fel_lib.h

@@ -70,6 +70,13 @@ void fel_writel_n(feldev_handle *dev, uint32_t addr, uint32_t *src, size_t count
 void fel_memmove(feldev_handle *dev,
 		 uint32_t dst_addr, uint32_t src_addr, size_t size);
 
+void fel_clrsetbits_le32(feldev_handle *dev,
+			 uint32_t addr, uint32_t clrbits, uint32_t setbits);
+#define fel_clrbits_le32(dev, addr, value) \
+	fel_clrsetbits_le32(dev, addr, value, 0)
+#define fel_setbits_le32(dev, addr, value) \
+	fel_clrsetbits_le32(dev, addr, 0, value)
+
 /* retrieve SID root key */
 bool fel_get_sid_root_key(feldev_handle *dev, uint32_t *result,
 			  bool force_workaround);

thunks/Makefile

@@ -3,7 +3,8 @@
 #
 
 SPL_THUNK := fel-to-spl-thunk.h
-THUNKS := memcpy.h
+THUNKS := clrsetbits.h
+THUNKS += memcpy.h
 THUNKS += readl_writel.h
 THUNKS += rmr-thunk.h
 THUNKS += sid_read_root.h

thunks/clrsetbits.S

+/*
+ * Thunk code to assist with bitwise operations (set/clear) via FEL
+ */
+
+fel_clrsetbits_le32:
+	ldr	r0, 1f		/* address */
+	ldr	r1, [r0]	/* load value */
+	ldr	r2, 2f		/* clrbits mask */
+	bic	r1, r2		/* clear bits, post-increment r1 */
+	ldr	r2, 3f		/* setbits mask */
+	orr	r1, r2		/* set bits (logical "or") */
+	str	r1, [r0]	/* store result */
+	bx	lr
+
+1:	.word	0	/* addr */
+2:	.word	0	/* clrbits (= bits to clear) */
+3:	.word	0	/* setbits (= bits to set) */

thunks/clrsetbits.h

+		/* <fel_clrsetbits_le32>: */
+		htole32(0xe59f0018), /*    0:  ldr   r0, [pc, #24]           */
+		htole32(0xe5901000), /*    4:  ldr   r1, [r0]                */
+		htole32(0xe59f2014), /*    8:  ldr   r2, [pc, #20]           */
+		htole32(0xe1c11002), /*    c:  bic   r1, r1, r2              */
+		htole32(0xe59f2010), /*   10:  ldr   r2, [pc, #16]           */
+		htole32(0xe1811002), /*   14:  orr   r1, r1, r2              */
+		htole32(0xe5801000), /*   18:  str   r1, [r0]                */
+		htole32(0xe12fff1e), /*   1c:  bx    lr                      */