src/arch/arm/memcpy.S

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