summaryrefslogtreecommitdiff
path: root/firmware/target/arm/bits-armv4.S
diff options
context:
space:
mode:
Diffstat (limited to 'firmware/target/arm/bits-armv4.S')
-rw-r--r--firmware/target/arm/bits-armv4.S78
1 files changed, 43 insertions, 35 deletions
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 @@
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
@@ -30,54 +30,59 @@ bitmod32:
30 mrs r12, cpsr 30 mrs r12, cpsr
31 orr r3, r12, #0xc0 31 orr r3, r12, #0xc0
32 msr cpsr_c, r3 32 msr cpsr_c, r3
33 ldr r3, [r0] 33 mov r3, r0
34 ldr r0, [r3]
34 and r1, r1, r2 @ Only allow mod of bits in 'mask' 35 and r1, r1, r2 @ Only allow mod of bits in 'mask'
35 bic r3, r3, r2 @ Clear mask bits 36 bic r2, r0, r2 @ Clear mask bits
36 orr r3, r3, r1 @ Set according to 'bits' 37 orr r1, r2, r1 @ Set according to 'bits'
37 str r3, [r0] 38 str r1, [r3]
38 msr cpsr_c, r12 39 msr cpsr_c, r12
39 bx lr 40 bx lr
40 .size bitmod32, .-bitmod32 41 .size bitmod32, .-bitmod32
41 42
43
42/*************************************************************************** 44/***************************************************************************
43 * void bitset32(volatile uint32_t *addr, uint32_t mask) 45 * uint32_t bitset32(volatile uint32_t *addr, uint32_t mask)
44 */ 46 */
45 .section .text.bitset32, "ax", %progbits 47 .section .text.bitset32, "ax", %progbits
46 .align 2 48 .align 2
47 .global bitset32 49 .global bitset32
48 .type bitset32, %function 50 .type bitset32, %function
49bitset32: 51bitset32:
52 mov r2, r0
50 mrs r12, cpsr 53 mrs r12, cpsr
51 orr r2, r12, #0xc0 54 orr r3, r12, #0xc0
52 msr cpsr_c, r2 55 msr cpsr_c, r3
53 ldr r2, [r0] 56 ldr r0, [r2]
54 orr r2, r2, r1 57 orr r1, r0, r1
55 str r2, [r0] 58 str r1, [r2]
56 msr cpsr_c, r12 59 msr cpsr_c, r12
57 bx lr 60 bx lr
58 .size bitset32, .-bitset32 61 .size bitset32, .-bitset32
59 62
60 63
61/*************************************************************************** 64/***************************************************************************
62 * void bitclr32(volatile uint32_t *addr, uint32_t mask) 65 * uint32_t bitclr32(volatile uint32_t *addr, uint32_t mask)
63 */ 66 */
64 .section .text.bitclr32, "ax", %progbits 67 .section .text.bitclr32, "ax", %progbits
65 .align 2 68 .align 2
66 .global bitclr32 69 .global bitclr32
67 .type bitclr32, %function 70 .type bitclr32, %function
68bitclr32: 71bitclr32:
72 mov r2, r0
69 mrs r12, cpsr 73 mrs r12, cpsr
70 orr r2, r12, #0xc0 74 orr r3, r12, #0xc0
71 msr cpsr_c, r2 75 msr cpsr_c, r3
72 ldr r2, [r0] 76 ldr r0, [r2]
73 bic r2, r2, r1 77 bic r1, r0, r1
74 str r2, [r0] 78 str r1, [r2]
75 msr cpsr_c, r12 79 msr cpsr_c, r12
76 bx lr 80 bx lr
77 .size bitclr32, .-bitclr32 81 .size bitclr32, .-bitclr32
78 82
83
79/*************************************************************************** 84/***************************************************************************
80 * void bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask) 85 * uint16_t bitmod16(volatile uint16_t *addr, uint16_t bits, uint16_t mask)
81 */ 86 */
82 .section .text.bitmod16, "ax", %progbits 87 .section .text.bitmod16, "ax", %progbits
83 .align 2 88 .align 2
@@ -87,49 +92,52 @@ bitmod16:
87 mrs r12, cpsr 92 mrs r12, cpsr
88 orr r3, r12, #0xc0 93 orr r3, r12, #0xc0
89 msr cpsr_c, r3 94 msr cpsr_c, r3
90 ldrh r3, [r0] 95 mov r3, r0
96 ldrh r0, [r3]
91 and r1, r1, r2 @ Only allow mod of bits in 'mask' 97 and r1, r1, r2 @ Only allow mod of bits in 'mask'
92 bic r3, r3, r2 @ Clear mask bits 98 bic r2, r0, r2 @ Clear mask bits
93 orr r3, r3, r1 @ Set according to 'bits' 99 orr r1, r2, r1 @ Set according to 'bits'
94 strh r3, [r0] 100 strh r1, [r3]
95 msr cpsr_c, r12 101 msr cpsr_c, r12
96 bx lr 102 bx lr
97 .size bitmod16, .-bitmod16 103 .size bitmod16, .-bitmod16
98 104
105
99/*************************************************************************** 106/***************************************************************************
100 * void bitset16(volatile uint16_t *addr, uint16_t mask) 107 * uint16_t bitset16(volatile uint16_t *addr, uint16_t mask)
101 */ 108 */
102 .section .text.bitset16, "ax", %progbits 109 .section .text.bitset16, "ax", %progbits
103 .align 2 110 .align 2
104 .global bitset16 111 .global bitset16
105 .type bitset16, %function 112 .type bitset16, %function
106bitset16: 113bitset16:
114 mov r2, r0
107 mrs r12, cpsr 115 mrs r12, cpsr
108 orr r2, r12, #0xc0 116 orr r3, r12, #0xc0
109 msr cpsr_c, r2 117 msr cpsr_c, r3
110 ldrh r2, [r0] 118 ldrh r0, [r2]
111 orr r2, r2, r1 119 orr r1, r0, r1
112 strh r2, [r0] 120 strh r1, [r2]
113 msr cpsr_c, r12 121 msr cpsr_c, r12
114 bx lr 122 bx lr
115 .size bitset16, .-bitset16 123 .size bitset16, .-bitset16
116 124
117 125
118/*************************************************************************** 126/***************************************************************************
119 * void bitclr16(volatile uint16_t *addr, uint16_t mask) 127 * uint16_t bitclr16(volatile uint16_t *addr, uint16_t mask)
120 */ 128 */
121 .section .text.bitclr16, "ax", %progbits 129 .section .text.bitclr16, "ax", %progbits
122 .align 2 130 .align 2
123 .global bitclr16 131 .global bitclr16
124 .type bitclr16, %function 132 .type bitclr16, %function
125bitclr16: 133bitclr16:
134 mov r2, r0
126 mrs r12, cpsr 135 mrs r12, cpsr
127 orr r2, r12, #0xc0 136 orr r3, r12, #0xc0
128 msr cpsr_c, r2 137 msr cpsr_c, r3
129 ldrh r2, [r0] 138 ldrh r0, [r2]
130 bic r2, r2, r1 139 bic r1, r0, r1
131 strh r2, [r0] 140 strh r1, [r2]
132 msr cpsr_c, r12 141 msr cpsr_c, r12
133 bx lr 142 bx lr
134 .size bitclr16, .-bitclr16 143 .size bitclr16, .-bitclr16
135
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-21 23:35:38 +0000