1/*
2 * Copyright (c) 2006-2021, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date           Author        Notes
8 * 2010-11-13     weety     first version
9 */
10//#include <rtthread.h>
11                //.text
12
13/*
14 * Purpose  : Find a 'zero' bit
15 * Prototype: int find_first_zero_bit(void *addr, unsigned int maxbit);
16 */
17.globl _find_first_zero_bit_le
18_find_first_zero_bit_le:
19        teq r1, #0
20        beq 3f
21        mov r2, #0
221:
23        ldrb    r3, [r0, r2, lsr #3]
24        lsr r3, r2, #3
25        ldrb    r3, [r0, r3]
26        eors    r3, r3, #0xff       @ invert bits
27        bne .L_found        @ any now set - found zero bit
28        add r2, r2, #8      @ next bit pointer
292:      cmp r2, r1          @ any more?
30        blo 1b
313:      mov r0, r1          @ no free bits
32        mov pc, lr
33//ENDPROC(_find_first_zero_bit_le)
34
35/*
36 * Purpose  : Find next 'zero' bit
37 * Prototype: int find_next_zero_bit(void *addr, unsigned int maxbit, int offset)
38 */
39.globl _find_next_zero_bit_le
40_find_next_zero_bit_le:
41        teq r1, #0
42        beq 3b
43        ands    ip, r2, #7
44        beq 1b          @ If new byte, goto old routine
45        ldrb    r3, [r0, r2, lsr #3]
46        lsr r3, r2, #3
47        ldrb    r3, [r0, r3]
48        eor r3, r3, #0xff       @ now looking for a 1 bit
49        movs    r3, r3, lsr ip      @ shift off unused bits
50        bne .L_found
51        orr r2, r2, #7      @ if zero, then no bits here
52        add r2, r2, #1      @ align bit pointer
53        b   2b          @ loop for next bit
54//ENDPROC(_find_next_zero_bit_le)
55
56/*
57 * Purpose  : Find a 'one' bit
58 * Prototype: int find_first_bit(const unsigned long *addr, unsigned int maxbit);
59 */
60.globl _find_first_bit_le
61_find_first_bit_le:
62        teq r1, #0
63        beq 3f
64        mov r2, #0
651:
66        ldrb    r3, [r0, r2, lsr #3]
67        lsr r3, r2, #3
68        ldrb    r3, [r0, r3]
69        movs    r3, r3
70        bne .L_found        @ any now set - found zero bit
71        add r2, r2, #8      @ next bit pointer
722:      cmp r2, r1          @ any more?
73        blo 1b
743:      mov r0, r1          @ no free bits
75        mov pc, lr
76//ENDPROC(_find_first_bit_le)
77
78/*
79 * Purpose  : Find next 'one' bit
80 * Prototype: int find_next_zero_bit(void *addr, unsigned int maxbit, int offset)
81 */
82.globl _find_next_bit_le
83_find_next_bit_le:
84        teq r1, #0
85        beq 3b
86        ands    ip, r2, #7
87        beq 1b          @ If new byte, goto old routine
88        ldrb    r3, [r0, r2, lsr #3]
89        lsr r3, r2, #3
90        ldrb    r3, [r0, r3]
91        movs    r3, r3, lsr ip      @ shift off unused bits
92        bne .L_found
93        orr r2, r2, #7      @ if zero, then no bits here
94        add r2, r2, #1      @ align bit pointer
95        b   2b          @ loop for next bit
96//ENDPROC(_find_next_bit_le)
97
98#ifdef __ARMEB__
99
100ENTRY(_find_first_zero_bit_be)
101        teq r1, #0
102        beq 3f
103        mov r2, #0
1041:      eor r3, r2, #0x18       @ big endian byte ordering
105 ARM(       ldrb    r3, [r0, r3, lsr #3]    )
106 THUMB(     lsr r3, #3          )
107 THUMB(     ldrb    r3, [r0, r3]        )
108        eors    r3, r3, #0xff       @ invert bits
109        bne .L_found        @ any now set - found zero bit
110        add r2, r2, #8      @ next bit pointer
1112:      cmp r2, r1          @ any more?
112        blo 1b
1133:      mov r0, r1          @ no free bits
114        mov pc, lr
115ENDPROC(_find_first_zero_bit_be)
116
117ENTRY(_find_next_zero_bit_be)
118        teq r1, #0
119        beq 3b
120        ands    ip, r2, #7
121        beq 1b          @ If new byte, goto old routine
122        eor r3, r2, #0x18       @ big endian byte ordering
123 ARM(       ldrb    r3, [r0, r3, lsr #3]    )
124 THUMB(     lsr r3, #3          )
125 THUMB(     ldrb    r3, [r0, r3]        )
126        eor r3, r3, #0xff       @ now looking for a 1 bit
127        movs    r3, r3, lsr ip      @ shift off unused bits
128        bne .L_found
129        orr r2, r2, #7      @ if zero, then no bits here
130        add r2, r2, #1      @ align bit pointer
131        b   2b          @ loop for next bit
132ENDPROC(_find_next_zero_bit_be)
133
134ENTRY(_find_first_bit_be)
135        teq r1, #0
136        beq 3f
137        mov r2, #0
1381:      eor r3, r2, #0x18       @ big endian byte ordering
139 ARM(       ldrb    r3, [r0, r3, lsr #3]    )
140 THUMB(     lsr r3, #3          )
141 THUMB(     ldrb    r3, [r0, r3]        )
142        movs    r3, r3
143        bne .L_found        @ any now set - found zero bit
144        add r2, r2, #8      @ next bit pointer
1452:      cmp r2, r1          @ any more?
146        blo 1b
1473:      mov r0, r1          @ no free bits
148        mov pc, lr
149ENDPROC(_find_first_bit_be)
150
151ENTRY(_find_next_bit_be)
152        teq r1, #0
153        beq 3b
154        ands    ip, r2, #7
155        beq 1b          @ If new byte, goto old routine
156        eor r3, r2, #0x18       @ big endian byte ordering
157 ARM(       ldrb    r3, [r0, r3, lsr #3]    )
158 THUMB(     lsr r3, #3          )
159 THUMB(     ldrb    r3, [r0, r3]        )
160        movs    r3, r3, lsr ip      @ shift off unused bits
161        bne .L_found
162        orr r2, r2, #7      @ if zero, then no bits here
163        add r2, r2, #1      @ align bit pointer
164        b   2b          @ loop for next bit
165ENDPROC(_find_next_bit_be)
166
167#endif
168
169/*
170 * One or more bits in the LSB of r3 are assumed to be set.
171 */
172.L_found:
173#if 1 //__LINUX_ARM_ARCH__ >= 5
174        rsb r0, r3, #0
175        and r3, r3, r0
176        clz r3, r3
177        rsb r3, r3, #31
178        add r0, r2, r3
179#else
180        tst r3, #0x0f
181        addeq   r2, r2, #4
182        movne   r3, r3, lsl #4
183        tst r3, #0x30
184        addeq   r2, r2, #2
185        movne   r3, r3, lsl #2
186        tst r3, #0x40
187        addeq   r2, r2, #1
188        mov r0, r2
189#endif
190        cmp r1, r0          @ Clamp to maxbit
191        movlo   r0, r1
192        mov pc, lr
193
194