diff options
Diffstat (limited to 'firmware/target')
-rw-r--r-- | firmware/target/arm/bits-armv4.S | 58 |
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: | |||
75 | msr cpsr_c, r12 | 75 | msr cpsr_c, r12 |
76 | bx lr | 76 | bx lr |
77 | .size bitclr32, .-bitclr32 | 77 | .size bitclr32, .-bitclr32 |
78 | |||
79 | /*************************************************************************** | ||
80 | * void bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask) | ||
81 | */ | ||
82 | .section .text, "ax", %progbits | ||
83 | .align 2 | ||
84 | .global bitmod16 | ||
85 | .type bitmod16, %function | ||
86 | bitmod16: | ||
87 | mrs r12, cpsr | ||
88 | orr r3, r12, #0xc0 | ||
89 | msr cpsr_c, r3 | ||
90 | ldrh r3, [r0] | ||
91 | and r1, r1, r2 @ Only allow mod of bits in 'mask' | ||
92 | bic r3, r3, r2 @ Clear mask bits | ||
93 | orr r3, r3, r1 @ Set according to 'bits' | ||
94 | strh r3, [r0] | ||
95 | msr cpsr_c, r12 | ||
96 | bx lr | ||
97 | .size bitmod16, .-bitmod16 | ||
98 | |||
99 | /*************************************************************************** | ||
100 | * void bitset16(volatile uint16_t *addr, uint16_t mask) | ||
101 | */ | ||
102 | .section .text, "ax", %progbits | ||
103 | .align 2 | ||
104 | .global bitset16 | ||
105 | .type bitset16, %function | ||
106 | bitset16: | ||
107 | mrs r12, cpsr | ||
108 | orr r2, r12, #0xc0 | ||
109 | msr cpsr_c, r2 | ||
110 | ldrh r2, [r0] | ||
111 | orr r2, r2, r1 | ||
112 | strh r2, [r0] | ||
113 | msr cpsr_c, r12 | ||
114 | bx lr | ||
115 | .size bitset16, .-bitset16 | ||
116 | |||
117 | |||
118 | /*************************************************************************** | ||
119 | * void bitclr16(volatile uint16_t *addr, uint16_t mask) | ||
120 | */ | ||
121 | .section .text, "ax", %progbits | ||
122 | .align 2 | ||
123 | .global bitclr16 | ||
124 | .type bitclr16, %function | ||
125 | bitclr16: | ||
126 | mrs r12, cpsr | ||
127 | orr r2, r12, #0xc0 | ||
128 | msr cpsr_c, r2 | ||
129 | ldrh r2, [r0] | ||
130 | bic r2, r2, r1 | ||
131 | strh r2, [r0] | ||
132 | msr cpsr_c, r12 | ||
133 | bx lr | ||
134 | .size bitclr16, .-bitclr16 | ||
135 | |||