payloads/libpayload/arch/arm/memcpy.S

   1 /*
   2  *  linux/arch/arm/lib/memcpy.S
   3  *
   4  *  Author:     Nicolas Pitre
   5  *  Created:    Sep 28, 2005
   6  *  Copyright:  MontaVista Software, Inc.
   7  *
   8  *  This program is free software; you can redistribute it and/or modify
   9  *  it under the terms of the GNU General Public License version 2 as
  10  *  published by the Free Software Foundation.
  11  */
  12
  13 #include <arch/asm.h>
  14 #include "asmlib.h"
  15
  16 #define LDR1W_SHIFT     0
  17 #define STR1W_SHIFT     0
  18
  19         .macro ldr1w ptr reg abort
  20         W(ldr) \reg, [\ptr], #4
  21         .endm
  22
  23         .macro ldr4w ptr reg1 reg2 reg3 reg4 abort
  24         ldmia \ptr!, {\reg1, \reg2, \reg3, \reg4}
  25         .endm
  26
  27         .macro ldr8w ptr reg1 reg2 reg3 reg4 reg5 reg6 reg7 reg8 abort
  28         ldmia \ptr!, {\reg1, \reg2, \reg3, \reg4, \reg5, \reg6, \reg7, \reg8}
  29         .endm
  30
  31         .macro ldr1b ptr reg cond=al abort
  32         ldr\cond\()b \reg, [\ptr], #1
  33         .endm
  34
  35         .macro str1w ptr reg abort
  36         W(str) \reg, [\ptr], #4
  37         .endm
  38
  39         .macro str8w ptr reg1 reg2 reg3 reg4 reg5 reg6 reg7 reg8 abort
  40         stmia \ptr!, {\reg1, \reg2, \reg3, \reg4, \reg5, \reg6, \reg7, \reg8}
  41         .endm
  42
  43         .macro str1b ptr reg cond=al abort
  44         str\cond\()b \reg, [\ptr], #1
  45         .endm
  46
  47         .macro enter reg1 reg2
  48         stmdb sp!, {r0, \reg1, \reg2}
  49         .endm
  50
  51         .macro exit reg1 reg2
  52         ldmfd sp!, {r0, \reg1, \reg2}
  53         .endm
  54
  55 /* Prototype: void *memcpy(void *dest, const void *src, size_t n); */
  56
  57 ENTRY(memcpy)
  58
  59                 enter   r4, lr
  60
  61                 subs    r2, r2, #4
  62                 blt     8f
  63                 ands    ip, r0, #3
  64         PLD(    pld     [r1, #0]                )
  65                 bne     9f
  66                 ands    ip, r1, #3
  67                 bne     10f
  68
  69 1:              subs    r2, r2, #(28)
  70                 stmfd   sp!, {r5 - r8}
  71                 blt     5f
  72
  73         CALGN(  ands    ip, r0, #31             )
  74         CALGN(  rsb     r3, ip, #32             )
  75         CALGN(  sbcnes  r4, r3, r2              )  @ C is always set here
  76         CALGN(  bcs     2f                      )
  77         CALGN(  adr     r4, 6f                  )
  78         CALGN(  subs    r2, r2, r3              )  @ C gets set
  79         CALGN(  add     pc, r4, ip              )
  80
  81         PLD(    pld     [r1, #0]                )
  82 2:      PLD(    subs    r2, r2, #96             )
  83         PLD(    pld     [r1, #28]               )
  84         PLD(    blt     4f                      )
  85         PLD(    pld     [r1, #60]               )
  86         PLD(    pld     [r1, #92]               )
  87
  88 3:      PLD(    pld     [r1, #124]              )
  89 4:              ldr8w   r1, r3, r4, r5, r6, r7, r8, ip, lr, abort=20f
  90                 subs    r2, r2, #32
  91                 str8w   r0, r3, r4, r5, r6, r7, r8, ip, lr, abort=20f
  92                 bge     3b
  93         PLD(    cmn     r2, #96                 )
  94         PLD(    bge     4b                      )
  95
  96 5:              ands    ip, r2, #28
  97                 rsb     ip, ip, #32
  98 #if LDR1W_SHIFT > 0
  99                 lsl     ip, ip, #LDR1W_SHIFT
 100 #endif
 101                 addne   pc, pc, ip              @ C is always clear here
 102                 b       7f
 103 6:
 104                 .rept   (1 << LDR1W_SHIFT)
 105                 W(nop)
 106                 .endr
 107                 ldr1w   r1, r3, abort=20f
 108                 ldr1w   r1, r4, abort=20f
 109                 ldr1w   r1, r5, abort=20f
 110                 ldr1w   r1, r6, abort=20f
 111                 ldr1w   r1, r7, abort=20f
 112                 ldr1w   r1, r8, abort=20f
 113                 ldr1w   r1, lr, abort=20f
 114
 115 #if LDR1W_SHIFT < STR1W_SHIFT
 116                 lsl     ip, ip, #STR1W_SHIFT - LDR1W_SHIFT
 117 #elif LDR1W_SHIFT > STR1W_SHIFT
 118                 lsr     ip, ip, #LDR1W_SHIFT - STR1W_SHIFT
 119 #endif
 120                 add     pc, pc, ip
 121                 nop
 122                 .rept   (1 << STR1W_SHIFT)
 123                 W(nop)
 124                 .endr
 125                 str1w   r0, r3, abort=20f
 126                 str1w   r0, r4, abort=20f
 127                 str1w   r0, r5, abort=20f
 128                 str1w   r0, r6, abort=20f
 129                 str1w   r0, r7, abort=20f
 130                 str1w   r0, r8, abort=20f
 131                 str1w   r0, lr, abort=20f
 132
 133         CALGN(  bcs     2b                      )
 134
 135 7:              ldmfd   sp!, {r5 - r8}
 136
 137 8:              movs    r2, r2, lsl #31
 138                 ldr1b   r1, r3, ne, abort=21f
 139                 ldr1b   r1, r4, cs, abort=21f
 140                 ldr1b   r1, ip, cs, abort=21f
 141                 str1b   r0, r3, ne, abort=21f
 142                 str1b   r0, r4, cs, abort=21f
 143                 str1b   r0, ip, cs, abort=21f
 144
 145                 exit    r4, pc
 146
 147 9:              rsb     ip, ip, #4
 148                 cmp     ip, #2
 149                 ldr1b   r1, r3, gt, abort=21f
 150                 ldr1b   r1, r4, ge, abort=21f
 151                 ldr1b   r1, lr, abort=21f
 152                 str1b   r0, r3, gt, abort=21f
 153                 str1b   r0, r4, ge, abort=21f
 154                 subs    r2, r2, ip
 155                 str1b   r0, lr, abort=21f
 156                 blt     8b
 157                 ands    ip, r1, #3
 158                 beq     1b
 159
 160 10:             bic     r1, r1, #3
 161                 cmp     ip, #2
 162                 ldr1w   r1, lr, abort=21f
 163                 beq     17f
 164                 bgt     18f
 165
 166
 167                 .macro  forward_copy_shift pull push
 168
 169                 subs    r2, r2, #28
 170                 blt     14f
 171
 172         CALGN(  ands    ip, r0, #31             )
 173         CALGN(  rsb     ip, ip, #32             )
 174         CALGN(  sbcnes  r4, ip, r2              )  @ C is always set here
 175         CALGN(  subcc   r2, r2, ip              )
 176         CALGN(  bcc     15f                     )
 177
 178 11:             stmfd   sp!, {r5 - r9}
 179
 180         PLD(    pld     [r1, #0]                )
 181         PLD(    subs    r2, r2, #96             )
 182         PLD(    pld     [r1, #28]               )
 183         PLD(    blt     13f                     )
 184         PLD(    pld     [r1, #60]               )
 185         PLD(    pld     [r1, #92]               )
 186
 187 12:     PLD(    pld     [r1, #124]              )
 188 13:             ldr4w   r1, r4, r5, r6, r7, abort=19f
 189                 mov     r3, lr, pull #\pull
 190                 subs    r2, r2, #32
 191                 ldr4w   r1, r8, r9, ip, lr, abort=19f
 192                 orr     r3, r3, r4, push #\push
 193                 mov     r4, r4, pull #\pull
 194                 orr     r4, r4, r5, push #\push
 195                 mov     r5, r5, pull #\pull
 196                 orr     r5, r5, r6, push #\push
 197                 mov     r6, r6, pull #\pull
 198                 orr     r6, r6, r7, push #\push
 199                 mov     r7, r7, pull #\pull
 200                 orr     r7, r7, r8, push #\push
 201                 mov     r8, r8, pull #\pull
 202                 orr     r8, r8, r9, push #\push
 203                 mov     r9, r9, pull #\pull
 204                 orr     r9, r9, ip, push #\push
 205                 mov     ip, ip, pull #\pull
 206                 orr     ip, ip, lr, push #\push
 207                 str8w   r0, r3, r4, r5, r6, r7, r8, r9, ip, , abort=19f
 208                 bge     12b
 209         PLD(    cmn     r2, #96                 )
 210         PLD(    bge     13b                     )
 211
 212                 ldmfd   sp!, {r5 - r9}
 213
 214 14:             ands    ip, r2, #28
 215                 beq     16f
 216
 217 15:             mov     r3, lr, pull #\pull
 218                 ldr1w   r1, lr, abort=21f
 219                 subs    ip, ip, #4
 220                 orr     r3, r3, lr, push #\push
 221                 str1w   r0, r3, abort=21f
 222                 bgt     15b
 223         CALGN(  cmp     r2, #0                  )
 224         CALGN(  bge     11b                     )
 225
 226 16:             sub     r1, r1, #(\push / 8)
 227                 b       8b
 228
 229                 .endm
 230
 231
 232                 forward_copy_shift      pull=8  push=24
 233
 234 17:             forward_copy_shift      pull=16 push=16
 235
 236 18:             forward_copy_shift      pull=24 push=8
 237 ENDPROC(memcpy)