First version
[3rdparty/ote_partner/tlk.git] / lib / libc / string / arch / arm / memcpy.S
1 /*
2  * Copyright (c) 2008 Travis Geiselbrecht
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files
6  * (the "Software"), to deal in the Software without restriction,
7  * including without limitation the rights to use, copy, modify, merge,
8  * publish, distribute, sublicense, and/or sell copies of the Software,
9  * and to permit persons to whom the Software is furnished to do so,
10  * subject to the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23 #include <asm.h>
24 #include <arch/arm/cores.h>
25
26 .text
27 .align 2
28
29 /* void bcopy(const void *src, void *dest, size_t n); */
30 FUNCTION(bcopy)
31         // swap args for bcopy
32         mov     r12, r0
33         mov     r0, r1
34         mov r1, r12
35
36 /* void *memcpy(void *dest, const void *src, size_t n); */
37 FUNCTION(memmove)
38 FUNCTION(memcpy)
39         // check for zero length copy or the same pointer
40         cmp             r2, #0
41         cmpne   r1, r0
42         bxeq    lr
43
44         // save a few registers for use and the return code (input dst)
45         stmfd   sp!, {r0, r4, r5, lr}
46
47         // check for forwards overlap (src > dst, distance < len)
48         subs    r3, r0, r1
49         cmpgt   r2, r3
50         bgt             .L_forwardoverlap
51
52         // check for a short copy len.
53         // 20 bytes is enough so that if a 16 byte alignment needs to happen there is at least a 
54         //   wordwise copy worth of work to be done.
55         cmp             r2, #(16+4)
56         blt             .L_bytewise
57
58         // see if they are similarly aligned on 4 byte boundaries
59         eor             r3, r0, r1
60         tst             r3, #3
61         bne             .L_bytewise             // dissimilarly aligned, nothing we can do (for now)
62
63         // check for 16 byte alignment on dst.
64         // this will also catch src being not 4 byte aligned, since it is similarly 4 byte 
65         //   aligned with dst at this point.
66         tst             r0, #15
67         bne             .L_not16bytealigned
68
69         // check to see if we have at least 32 bytes of data to copy.
70         // if not, just revert to wordwise copy
71         cmp             r2, #32
72         blt             .L_wordwise
73
74 .L_bigcopy:
75         // copy 32 bytes at a time. src & dst need to be at least 4 byte aligned, 
76         // and we need at least 32 bytes remaining to copy
77
78         // save r6-r7 for use in the big copy
79         stmfd   sp!, {r6-r7}
80
81         sub             r2, r2, #32             // subtract an extra 32 to the len so we can avoid an extra compare
82
83 .L_bigcopy_loop:
84         ldmia   r1!, {r4, r5, r6, r7}
85         stmia   r0!, {r4, r5, r6, r7}
86         ldmia   r1!, {r4, r5, r6, r7}
87         subs    r2, r2, #32
88         stmia   r0!, {r4, r5, r6, r7}
89         bge             .L_bigcopy_loop
90
91         // restore r6-r7
92         ldmfd   sp!, {r6-r7}
93
94         // see if we are done
95         adds    r2, r2, #32
96         beq             .L_done
97
98         // less then 4 bytes left?
99         cmp             r2, #4
100         blt             .L_bytewise
101
102 .L_wordwise:
103         // copy 4 bytes at a time.
104         // src & dst are guaranteed to be word aligned, and at least 4 bytes are left to copy.
105         subs    r2, r2, #4
106
107 .L_wordwise_loop:
108         ldr             r3, [r1], #4
109         subs    r2, r2, #4
110         str             r3, [r0], #4
111         bge             .L_wordwise_loop
112
113         // correct the remaining len and test for completion
114         adds    r2, r2, #4      
115         beq             .L_done
116
117 .L_bytewise:
118         // simple bytewise copy
119         ldrb    r3, [r1], #1
120         subs    r2, r2, #1
121         strb    r3, [r0], #1
122         bgt             .L_bytewise
123
124 .L_done:
125         // load dst for return and restore r4,r5
126 #if ARM_ARCH_LEVEL >= 5
127         ldmfd   sp!, {r0, r4, r5, pc}
128 #else
129         ldmfd   sp!, {r0, r4, r5, lr}
130         bx              lr
131 #endif
132
133 .L_not16bytealigned:
134         // dst is not 16 byte aligned, so we will copy up to 15 bytes to get it aligned.
135         // src is guaranteed to be similarly word aligned with dst.
136
137         // set the condition flags based on the alignment.
138         lsl             r12, r0, #28
139         rsb             r12, r12, #0
140         msr             CPSR_f, r12                             // move into NZCV fields in CPSR
141
142         // move as many bytes as necessary to get the dst aligned
143         ldrvsb  r3, [r1], #1                    // V set
144         ldrcsh  r4, [r1], #2                    // C set
145         ldreq   r5, [r1], #4                    // Z set
146
147         strvsb  r3, [r0], #1
148         strcsh  r4, [r0], #2
149         streq   r5, [r0], #4
150
151         ldmmiia r1!, {r3-r4}                    // N set
152         stmmiia r0!, {r3-r4}
153
154         // fix the remaining len
155         sub             r2, r2, r12, lsr #28
156
157         // test to see what we should do now
158         cmp             r2, #32
159         bge             .L_bigcopy
160         b               .L_wordwise
161         
162         // src and dest overlap 'forwards' or dst > src
163 .L_forwardoverlap:
164
165         // do a bytewise reverse copy for now
166         add             r1, r1, r2
167         add             r0, r0, r2
168         sub             r1, r1, #1
169         sub             r0, r0, #1
170
171 .L_bytewisereverse:
172         // simple bytewise reverse copy
173         ldrb    r3, [r1], #-1
174         subs    r2, r2, #1
175         strb    r3, [r0], #-1
176         bgt             .L_bytewisereverse
177
178         b               .L_done
179