diff options
Diffstat (limited to 'firmware/target/arm/bits-armv6.S')
| -rw-r--r-- | firmware/target/arm/bits-armv6.S | 30 |
1 files changed, 17 insertions, 13 deletions
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 @@ | |||
| 20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
| 21 | 21 | ||
| 22 | /*************************************************************************** | 22 | /*************************************************************************** |
| 23 | * void bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask) | 23 | * uint32_t bitmod32(volatile uint32_t *addr, uint32_t bits, uint32_t mask) |
| 24 | */ | 24 | */ |
| 25 | .section .text.bitmod32, "ax", %progbits | 25 | .section .text.bitmod32, "ax", %progbits |
| 26 | .align 2 | 26 | .align 2 |
| 27 | .global bitmod32 | 27 | .global bitmod32 |
| 28 | .type bitmod32, %function | 28 | .type bitmod32, %function |
| 29 | bitmod32: | 29 | bitmod32: |
| 30 | mov r3, r0 | ||
| 30 | mrs r12, cpsr | 31 | mrs r12, cpsr |
| 31 | cpsid if | 32 | cpsid if |
| 32 | ldr r3, [r0] | 33 | ldr r0, [r3] |
| 33 | and r1, r1, r2 @ Only allow mod of bits in 'mask' | 34 | and r1, r1, r2 @ Only allow mod of bits in 'mask' |
| 34 | bic r3, r3, r2 @ Clear mask bits | 35 | bic r2, r0, r2 @ Clear mask bits |
| 35 | orr r3, r3, r1 @ Set according to 'bits' | 36 | orr r1, r2, r1 @ Set according to 'bits' |
| 36 | str r3, [r0] | 37 | str r1, [r3] |
| 37 | msr cpsr_c, r12 | 38 | msr cpsr_c, r12 |
| 38 | bx lr | 39 | bx lr |
| 39 | .size bitmod32, .-bitmod32 | 40 | .size bitmod32, .-bitmod32 |
| 40 | 41 | ||
| 42 | |||
| 41 | /*************************************************************************** | 43 | /*************************************************************************** |
| 42 | * void bitset32(volatile uint32_t *addr, uint32_t mask) | 44 | * uint32_t bitset32(volatile uint32_t *addr, uint32_t mask) |
| 43 | */ | 45 | */ |
| 44 | .section .text.bitset32, "ax", %progbits | 46 | .section .text.bitset32, "ax", %progbits |
| 45 | .align 2 | 47 | .align 2 |
| 46 | .global bitset32 | 48 | .global bitset32 |
| 47 | .type bitset32, %function | 49 | .type bitset32, %function |
| 48 | bitset32: | 50 | bitset32: |
| 51 | mov r2, r0 | ||
| 49 | mrs r12, cpsr | 52 | mrs r12, cpsr |
| 50 | cpsid if | 53 | cpsid if |
| 51 | ldr r2, [r0] | 54 | ldr r0, [r2] |
| 52 | orr r2, r2, r1 | 55 | orr r1, r0, r1 |
| 53 | str r2, [r0] | 56 | str r1, [r2] |
| 54 | msr cpsr_c, r12 | 57 | msr cpsr_c, r12 |
| 55 | bx lr | 58 | bx lr |
| 56 | .size bitset32, .-bitset32 | 59 | .size bitset32, .-bitset32 |
| 57 | 60 | ||
| 58 | 61 | ||
| 59 | /*************************************************************************** | 62 | /*************************************************************************** |
| 60 | * void bitclr32(volatile uint32_t *addr, uint32_t mask) | 63 | * uint32_t bitclr32(volatile uint32_t *addr, uint32_t mask) |
| 61 | */ | 64 | */ |
| 62 | .section .text.bitclr32, "ax", %progbits | 65 | .section .text.bitclr32, "ax", %progbits |
| 63 | .align 2 | 66 | .align 2 |
| 64 | .global bitclr32 | 67 | .global bitclr32 |
| 65 | .type bitclr32, %function | 68 | .type bitclr32, %function |
| 66 | bitclr32: | 69 | bitclr32: |
| 70 | mov r2, r0 | ||
| 67 | mrs r12, cpsr | 71 | mrs r12, cpsr |
| 68 | cpsid if | 72 | cpsid if |
| 69 | ldr r2, [r0] | 73 | ldr r0, [r2] |
| 70 | bic r2, r2, r1 | 74 | bic r1, r0, r1 |
| 71 | str r2, [r0] | 75 | str r1, [r2] |
| 72 | msr cpsr_c, r12 | 76 | msr cpsr_c, r12 |
| 73 | bx lr | 77 | bx lr |
| 74 | .size bitclr32, .-bitclr32 | 78 | .size bitclr32, .-bitclr32 |