From f728559bf951b515be39d61f3ffd0eaae64b2b0d Mon Sep 17 00:00:00 2001 From: Michael Sevakis Date: Tue, 24 Jan 2017 10:15:53 -0500 Subject: [PATCH] Make atomic bit modification functions return previous value. Change-Id: I19a94cf946735e1d9e51c3207cd82198fd4dfc1a --- firmware/export/system.h | 15 +++--- firmware/target/arm/bits-armv4.S | 78 ++++++++++++++++++-------------- firmware/target/arm/bits-armv6.S | 30 ++++++------ 3 files changed, 68 insertions(+), 55 deletions(-) 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