1 files changed, 58 insertions, 0 deletions

diff --git a/firmware/target/arm/bits-armv4.S b/firmware/target/arm/bits-armv4.S
index 05d61b8b7e..542e362f77 100644
--- a/firmware/target/arm/bits-armv4.S
+++ b/firmware/target/arm/bits-armv4.S
@@ -75,3 +75,61 @@ bitclr32:
     msr    cpsr_c, r12
     bx     lr
     .size  bitclr32, .-bitclr32
+
+/***************************************************************************
+ * void bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitmod16
+    .type       bitmod16, %function
+bitmod16:
+    mrs    r12, cpsr
+    orr    r3, r12, #0xc0
+    msr    cpsr_c, r3
+    ldrh   r3, [r0]
+    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]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitmod16, .-bitmod16
+
+/***************************************************************************
+ * void bitset16(volatile uint16_t *addr, uint16_t mask)
+ */
+    .section    .text, "ax", %progbits
+    .align      2
+    .global     bitset16
+    .type       bitset16, %function
+bitset16:
+    mrs    r12, cpsr
+    orr    r2, r12, #0xc0
+    msr    cpsr_c, r2
+    ldrh   r2, [r0]
+    orr    r2, r2, r1
+    strh   r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitset16, .-bitset16
+
+
+/***************************************************************************
+ * void bitclr16(volatile uint16_t *addr, uint16_t mask)
+ */
+     .section    .text, "ax", %progbits
+     .align      2
+     .global     bitclr16
+     .type       bitclr16, %function
+bitclr16:
+    mrs    r12, cpsr
+    orr    r2, r12, #0xc0
+    msr    cpsr_c, r2
+    ldrh   r2, [r0]
+    bic    r2, r2, r1
+    strh   r2, [r0]
+    msr    cpsr_c, r12
+    bx     lr
+    .size  bitclr16, .-bitclr16
+