diff --git a/firmware/export/system.h b/firmware/export/system.h
index 911c9edf4b..c7f5a8112c 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -163,14 +163,15 @@ static inline __attribute__((always_inline))
 uint32_t isolate_first_bit(uint32_t val)
     { return val & -val; }
 
-/* Functions to set and clear register or variable bits atomically */
-void bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask);
-void bitset16(volatile uint16_t *addr, uint16_t mask);
-void bitclr16(volatile uint16_t *addr, uint16_t mask);
+/* Functions to set and clear register or variable bits atomically;
+ * return value is the previous value of *addr */
+uint16_t bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask);
+uint16_t bitset16(volatile uint16_t *addr, uint16_t mask);
+uint16_t bitclr16(volatile uint16_t *addr, uint16_t mask);
 
-void bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask);
-void bitset32(volatile uint32_t *addr, uint32_t mask);
-void bitclr32(volatile uint32_t *addr, uint32_t mask);
+uint32_t bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask);
+uint32_t bitset32(volatile uint32_t *addr, uint32_t mask);
+uint32_t bitclr32(volatile uint32_t *addr, uint32_t mask);
 
 /* gcc 3.4 changed the format of the constraints */
 #if (__GNUC__ >= 3) && (__GNUC_MINOR__ > 3) || (__GNUC__ >= 4)
diff --git a/firmware/target/arm/bits-armv4.S b/firmware/target/arm/bits-armv4.S
index fdfc9f3227..33a1508c2c 100644
--- a/firmware/target/arm/bits-armv4.S
+++ b/firmware/target/arm/bits-armv4.S
@@ -20,7 +20,7 @@
 ****************************************************************************/
 
 /***************************************************************************
- * void bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask)
+ * uint32_t bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask)
  */
     .section    .text.bitmod32, "ax", %progbits
     .align      2
@@ -30,54 +30,59 @@ bitmod32:
     mrs    r12, cpsr
     orr    r3, r12, #0xc0
     msr    cpsr_c, r3
-    ldr    r3, [r0]
+    mov    r3, r0
+    ldr    r0, [r3]
     and    r1, r1, r2               @ Only allow mod of bits in 'mask'
-    bic    r3, r3, r2               @ Clear mask bits
-    orr    r3, r3, r1               @ Set according to 'bits'
-    str    r3, [r0]
+    bic    r2, r0, r2               @ Clear mask bits
+    orr    r1, r2, r1               @ Set according to 'bits'
+    str    r1, [r3]
     msr    cpsr_c, r12
     bx     lr
     .size  bitmod32, .-bitmod32
 
+
 /***************************************************************************
- * void bitset32(volatile uint32_t *addr, uint32_t mask)
+ * uint32_t bitset32(volatile uint32_t *addr, uint32_t mask)
  */
     .section    .text.bitset32, "ax", %progbits
     .align      2
     .global     bitset32
     .type       bitset32, %function
 bitset32:
+    mov    r2, r0
     mrs    r12, cpsr
-    orr    r2, r12, #0xc0
-    msr    cpsr_c, r2
-    ldr    r2, [r0]
-    orr    r2, r2, r1
-    str    r2, [r0]
+    orr    r3, r12, #0xc0
+    msr    cpsr_c, r3
+    ldr    r0, [r2]
+    orr    r1, r0, r1
+    str    r1, [r2]
     msr    cpsr_c, r12
     bx     lr
     .size  bitset32, .-bitset32
 
 
 /***************************************************************************
- * void bitclr32(volatile uint32_t *addr, uint32_t mask)
+ * uint32_t bitclr32(volatile uint32_t *addr, uint32_t mask)
  */
      .section    .text.bitclr32, "ax", %progbits
      .align      2
      .global     bitclr32
      .type       bitclr32, %function
 bitclr32:
+    mov    r2, r0
     mrs    r12, cpsr
-    orr    r2, r12, #0xc0
-    msr    cpsr_c, r2
-    ldr    r2, [r0]
-    bic    r2, r2, r1
-    str    r2, [r0]
+    orr    r3, r12, #0xc0
+    msr    cpsr_c, r3
+    ldr    r0, [r2]
+    bic    r1, r0, r1
+    str    r1, [r2]
     msr    cpsr_c, r12
     bx     lr
     .size  bitclr32, .-bitclr32
 
+
 /***************************************************************************
- * void bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask)
+ * uint16_t bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask)
  */
     .section    .text.bitmod16, "ax", %progbits
     .align      2
@@ -87,49 +92,52 @@ bitmod16:
     mrs    r12, cpsr
     orr    r3, r12, #0xc0
     msr    cpsr_c, r3
-    ldrh   r3, [r0]
+    mov    r3, r0
+    ldrh   r0, [r3]
     and    r1, r1, r2               @ Only allow mod of bits in 'mask'
-    bic    r3, r3, r2               @ Clear mask bits
-    orr    r3, r3, r1               @ Set according to 'bits'
-    strh   r3, [r0]
+    bic    r2, r0, r2               @ Clear mask bits
+    orr    r1, r2, r1               @ Set according to 'bits'
+    strh   r1, [r3]
     msr    cpsr_c, r12
     bx     lr
     .size  bitmod16, .-bitmod16
 
+
 /***************************************************************************
- * void bitset16(volatile uint16_t *addr, uint16_t mask)
+ * uint16_t bitset16(volatile uint16_t *addr, uint16_t mask)
  */
     .section    .text.bitset16, "ax", %progbits
     .align      2
     .global     bitset16
     .type       bitset16, %function
 bitset16:
+    mov    r2, r0
     mrs    r12, cpsr
-    orr    r2, r12, #0xc0
-    msr    cpsr_c, r2
-    ldrh   r2, [r0]
-    orr    r2, r2, r1
-    strh   r2, [r0]
+    orr    r3, r12, #0xc0
+    msr    cpsr_c, r3
+    ldrh   r0, [r2]
+    orr    r1, r0, r1
+    strh   r1, [r2]
     msr    cpsr_c, r12
     bx     lr
     .size  bitset16, .-bitset16
 
 
 /***************************************************************************
- * void bitclr16(volatile uint16_t *addr, uint16_t mask)
+ * uint16_t bitclr16(volatile uint16_t *addr, uint16_t mask)
  */
      .section    .text.bitclr16, "ax", %progbits
      .align      2
      .global     bitclr16
      .type       bitclr16, %function
 bitclr16:
+    mov    r2, r0
     mrs    r12, cpsr
-    orr    r2, r12, #0xc0
-    msr    cpsr_c, r2
-    ldrh   r2, [r0]
-    bic    r2, r2, r1
-    strh   r2, [r0]
+    orr    r3, r12, #0xc0
+    msr    cpsr_c, r3
+    ldrh   r0, [r2]
+    bic    r1, r0, r1
+    strh   r1, [r2]
     msr    cpsr_c, r12
     bx     lr
     .size  bitclr16, .-bitclr16
-
diff --git a/firmware/target/arm/bits-armv6.S b/firmware/target/arm/bits-armv6.S
index fbac7f9cf9..d0e48477ed 100644
--- a/firmware/target/arm/bits-armv6.S
+++ b/firmware/target/arm/bits-armv6.S
@@ -20,55 +20,59 @@
 ****************************************************************************/
 
 /***************************************************************************
- * void bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask)
+ * uint32_t bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask)
  */
     .section    .text.bitmod32, "ax", %progbits
     .align      2
     .global     bitmod32
     .type       bitmod32, %function
 bitmod32:
+    mov    r3, r0
     mrs    r12, cpsr
     cpsid  if
-    ldr    r3, [r0]
+    ldr    r0, [r3]
     and    r1, r1, r2               @ Only allow mod of bits in 'mask'
-    bic    r3, r3, r2               @ Clear mask bits
-    orr    r3, r3, r1               @ Set according to 'bits'
-    str    r3, [r0]
+    bic    r2, r0, r2               @ Clear mask bits
+    orr    r1, r2, r1               @ Set according to 'bits'
+    str    r1, [r3]
     msr    cpsr_c, r12
     bx     lr
     .size  bitmod32, .-bitmod32
 
+
 /***************************************************************************
- * void bitset32(volatile uint32_t *addr, uint32_t mask)
+ * uint32_t bitset32(volatile uint32_t *addr, uint32_t mask)
  */
     .section    .text.bitset32, "ax", %progbits
     .align      2
     .global     bitset32
     .type       bitset32, %function
 bitset32:
+    mov    r2, r0
     mrs    r12, cpsr
     cpsid  if
-    ldr    r2, [r0]
-    orr    r2, r2, r1
-    str    r2, [r0]
+    ldr    r0, [r2]
+    orr    r1, r0, r1
+    str    r1, [r2]
     msr    cpsr_c, r12
     bx     lr
     .size  bitset32, .-bitset32
 
 
 /***************************************************************************
- * void bitclr32(volatile uint32_t *addr, uint32_t mask)
+ * uint32_t bitclr32(volatile uint32_t *addr, uint32_t mask)
  */
      .section    .text.bitclr32, "ax", %progbits
      .align      2
      .global     bitclr32
      .type       bitclr32, %function
 bitclr32:
+    mov    r2, r0
     mrs    r12, cpsr
     cpsid  if
-    ldr    r2, [r0]
-    bic    r2, r2, r1
-    str    r2, [r0]
+    ldr    r0, [r2]
+    bic    r1, r0, r1
+    str    r1, [r2]
     msr    cpsr_c, r12
     bx     lr
     .size  bitclr32, .-bitclr32