sys/arm/arm/cpufunc_asm_armv7.S

   1 /*-
   2  * Copyright (c) 2010 Per Odlund <per.odlund@armagedon.se>
   3  * Copyright (C) 2011 MARVELL INTERNATIONAL LTD.
   4  * All rights reserved.
   5  *
   6  * Developed by Semihalf.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  * 3. Neither the name of MARVELL nor the names of contributors
  17  *    may be used to endorse or promote products derived from this software
  18  *    without specific prior written permission.
  19  *
  20  * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  21  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  23  * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
  24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  26  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  27  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  29  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  30  * SUCH DAMAGE.
  31  */
  32
  33 #include <machine/asm.h>
  34 __FBSDID("$FreeBSD$");
  35
  36 #include <machine/sysreg.h>
  37
  38         .cpu cortex-a8
  39
  40 .Lcoherency_level:
  41         .word   _C_LABEL(arm_cache_loc)
  42 .Lcache_type:
  43         .word   _C_LABEL(arm_cache_type)
  44 .Larmv7_dcache_line_size:
  45         .word   _C_LABEL(arm_dcache_min_line_size)
  46 .Larmv7_icache_line_size:
  47         .word   _C_LABEL(arm_icache_min_line_size)
  48 .Larmv7_idcache_line_size:
  49         .word   _C_LABEL(arm_idcache_min_line_size)
  50 .Lway_mask:
  51         .word   0x3ff
  52 .Lmax_index:
  53         .word   0x7fff
  54 .Lpage_mask:
  55         .word   0xfff
  56
  57 #define PT_NOS          (1 << 5)
  58 #define PT_S            (1 << 1)
  59 #define PT_INNER_NC     0
  60 #define PT_INNER_WT     (1 << 0)
  61 #define PT_INNER_WB     ((1 << 0) | (1 << 6))
  62 #define PT_INNER_WBWA   (1 << 6)
  63 #define PT_OUTER_NC     0
  64 #define PT_OUTER_WT     (2 << 3)
  65 #define PT_OUTER_WB     (3 << 3)
  66 #define PT_OUTER_WBWA   (1 << 3)
  67
  68 #ifdef SMP
  69 #define PT_ATTR (PT_S|PT_INNER_WBWA|PT_OUTER_WBWA|PT_NOS)
  70 #else
  71 #define PT_ATTR (PT_INNER_WBWA|PT_OUTER_WBWA)
  72 #endif
  73
  74 ENTRY(armv7_setttb)
  75         stmdb   sp!, {r0, lr}
  76         bl      _C_LABEL(armv7_idcache_wbinv_all) /* clean the D cache */
  77         ldmia   sp!, {r0, lr}
  78         dsb
  79
  80         orr     r0, r0, #PT_ATTR
  81         mcr     CP15_TTBR0(r0)
  82         isb
  83 #ifdef SMP
  84         mcr     CP15_TLBIALLIS
  85 #else
  86         mcr     CP15_TLBIALL
  87 #endif
  88         dsb
  89         isb
  90         RET
  91 END(armv7_setttb)
  92
  93 ENTRY(armv7_tlb_flushID)
  94         dsb
  95 #ifdef SMP
  96         mcr     CP15_TLBIALLIS
  97         mcr     CP15_BPIALLIS
  98 #else
  99         mcr     CP15_TLBIALL
 100         mcr     CP15_BPIALL
 101 #endif
 102         dsb
 103         isb
 104         mov     pc, lr
 105 END(armv7_tlb_flushID)
 106
 107 ENTRY(armv7_tlb_flushID_SE)
 108         ldr     r1, .Lpage_mask
 109         bic     r0, r0, r1
 110 #ifdef SMP
 111         mcr     CP15_TLBIMVAAIS(r0)
 112         mcr     CP15_BPIALLIS
 113 #else
 114         mcr     CP15_TLBIMVA(r0)
 115         mcr     CP15_BPIALL
 116 #endif
 117         dsb
 118         isb
 119         mov     pc, lr
 120 END(armv7_tlb_flushID_SE)
 121
 122 /* Based on algorithm from ARM Architecture Reference Manual */
 123 ENTRY(armv7_dcache_wbinv_all)
 124         stmdb   sp!, {r4, r5, r6, r7, r8, r9}
 125
 126         /* Get cache level */
 127         ldr     r0, .Lcoherency_level
 128         ldr     r3, [r0]
 129         cmp     r3, #0
 130         beq     Finished
 131         /* For each cache level */
 132         mov     r8, #0
 133 Loop1:
 134         /* Get cache type for given level */
 135         mov     r2, r8, lsl #2
 136         add     r2, r2, r2
 137         ldr     r0, .Lcache_type
 138         ldr     r1, [r0, r2]
 139
 140         /* Get line size */
 141         and     r2, r1, #7
 142         add     r2, r2, #4
 143
 144         /* Get number of ways */
 145         ldr     r4, .Lway_mask
 146         ands    r4, r4, r1, lsr #3
 147         clz     r5, r4
 148
 149         /* Get max index */
 150         ldr     r7, .Lmax_index
 151         ands    r7, r7, r1, lsr #13
 152 Loop2:
 153         mov     r9, r4
 154 Loop3:
 155         mov     r6, r8, lsl #1
 156         orr     r6, r6, r9, lsl r5
 157         orr     r6, r6, r7, lsl r2
 158
 159         /* Clean and invalidate data cache by way/index */
 160         mcr     CP15_DCCISW(r6)
 161         subs    r9, r9, #1
 162         bge     Loop3
 163         subs    r7, r7, #1
 164         bge     Loop2
 165 Skip:
 166         add     r8, r8, #1
 167         cmp     r3, r8
 168         bne Loop1
 169 Finished:
 170         dsb
 171         ldmia   sp!, {r4, r5, r6, r7, r8, r9}
 172         RET
 173 END(armv7_dcache_wbinv_all)
 174
 175 ENTRY(armv7_idcache_wbinv_all)
 176         stmdb   sp!, {lr}
 177         bl armv7_dcache_wbinv_all
 178 #ifdef SMP
 179         mcr     CP15_ICIALLUIS
 180 #else
 181         mcr     CP15_ICIALLU
 182 #endif
 183         dsb
 184         isb
 185         ldmia   sp!, {lr}
 186         RET
 187 END(armv7_idcache_wbinv_all)
 188
 189 ENTRY(armv7_dcache_wb_range)
 190         ldr     ip, .Larmv7_dcache_line_size
 191         ldr     ip, [ip]
 192         sub     r3, ip, #1
 193         and     r2, r0, r3
 194         add     r1, r1, r2
 195         bic     r0, r0, r3
 196 .Larmv7_wb_next:
 197         mcr     CP15_DCCMVAC(r0)
 198         add     r0, r0, ip
 199         subs    r1, r1, ip
 200         bhi     .Larmv7_wb_next
 201         dsb                             /* data synchronization barrier */
 202         RET
 203 END(armv7_dcache_wb_range)
 204
 205 ENTRY(armv7_dcache_wbinv_range)
 206         ldr     ip, .Larmv7_dcache_line_size
 207         ldr     ip, [ip]
 208         sub     r3, ip, #1
 209         and     r2, r0, r3
 210         add     r1, r1, r2
 211         bic     r0, r0, r3
 212 .Larmv7_wbinv_next:
 213         mcr     CP15_DCCIMVAC(r0)
 214         add     r0, r0, ip
 215         subs    r1, r1, ip
 216         bhi     .Larmv7_wbinv_next
 217         dsb                             /* data synchronization barrier */
 218         RET
 219 END(armv7_dcache_wbinv_range)
 220
 221 /*
 222  * Note, we must not invalidate everything.  If the range is too big we
 223  * must use wb-inv of the entire cache.
 224  */
 225 ENTRY(armv7_dcache_inv_range)
 226         ldr     ip, .Larmv7_dcache_line_size
 227         ldr     ip, [ip]
 228         sub     r3, ip, #1
 229         and     r2, r0, r3
 230         add     r1, r1, r2
 231         bic     r0, r0, r3
 232 .Larmv7_inv_next:
 233         mcr     CP15_DCIMVAC(r0)
 234         add     r0, r0, ip
 235         subs    r1, r1, ip
 236         bhi     .Larmv7_inv_next
 237         dsb                             /* data synchronization barrier */
 238         RET
 239 END(armv7_dcache_inv_range)
 240
 241 ENTRY(armv7_idcache_wbinv_range)
 242         ldr     ip, .Larmv7_idcache_line_size
 243         ldr     ip, [ip]
 244         sub     r3, ip, #1
 245         and     r2, r0, r3
 246         add     r1, r1, r2
 247         bic     r0, r0, r3
 248 .Larmv7_id_wbinv_next:
 249         mcr     CP15_ICIMVAU(r0)
 250         mcr     CP15_DCCIMVAC(r0)
 251         add     r0, r0, ip
 252         subs    r1, r1, ip
 253         bhi     .Larmv7_id_wbinv_next
 254         isb                             /* instruction synchronization barrier */
 255         dsb                             /* data synchronization barrier */
 256         RET
 257 END(armv7_idcache_wbinv_range)
 258
 259 ENTRY_NP(armv7_icache_sync_all)
 260 #ifdef SMP
 261         mcr     CP15_ICIALLUIS
 262 #else
 263         mcr     CP15_ICIALLU
 264 #endif
 265         isb                             /* instruction synchronization barrier */
 266         dsb                             /* data synchronization barrier */
 267         RET
 268 END(armv7_icache_sync_all)
 269
 270 ENTRY_NP(armv7_icache_sync_range)
 271         ldr     ip, .Larmv7_icache_line_size
 272         ldr     ip, [ip]
 273 .Larmv7_sync_next:
 274         mcr     CP15_ICIMVAU(r0)
 275         mcr     CP15_DCCMVAC(r0)
 276         add     r0, r0, ip
 277         subs    r1, r1, ip
 278         bhi     .Larmv7_sync_next
 279         isb                             /* instruction synchronization barrier */
 280         dsb                             /* data synchronization barrier */
 281         RET
 282 END(armv7_icache_sync_range)
 283
 284 ENTRY(armv7_cpu_sleep)
 285         dsb                             /* data synchronization barrier */
 286         wfi                             /* wait for interrupt */
 287         RET
 288 END(armv7_cpu_sleep)
 289
 290 ENTRY(armv7_context_switch)
 291         dsb
 292         orr     r0, r0, #PT_ATTR
 293
 294         mcr     CP15_TTBR0(r0)
 295         isb
 296 #ifdef SMP
 297         mcr     CP15_TLBIALLIS
 298 #else
 299         mcr     CP15_TLBIALL
 300 #endif
 301         dsb
 302         isb
 303         RET
 304 END(armv7_context_switch)
 305
 306 ENTRY(armv7_drain_writebuf)
 307         dsb
 308         RET
 309 END(armv7_drain_writebuf)
 310
 311 ENTRY(armv7_sev)
 312         dsb
 313         sev
 314         nop
 315         RET
 316 END(armv7_sev)
 317
 318 ENTRY(armv7_auxctrl)
 319         mrc     CP15_ACTLR(r2)
 320         bic r3, r2, r0  /* Clear bits */
 321         eor r3, r3, r1  /* XOR bits */
 322
 323         teq r2, r3
 324         mcrne   CP15_ACTLR(r3)
 325         mov r0, r2
 326         RET
 327 END(armv7_auxctrl)
 328
 329 /*
 330  * Invalidate all I+D+branch cache.  Used by startup code, which counts
 331  * on the fact that only r0-r3,ip are modified and no stack space is used.
 332  */
 333 ENTRY(armv7_idcache_inv_all)
 334         mov     r0, #0
 335         mcr     CP15_CSSELR(r0)         @ set cache level to L1
 336         mrc     CP15_CCSIDR(r0)
 337
 338         ubfx    r2, r0, #13, #15        @ get num sets - 1 from CCSIDR
 339         ubfx    r3, r0, #3, #10         @ get numways - 1 from CCSIDR
 340         clz     r1, r3                  @ number of bits to MSB of way
 341         lsl     r3, r3, r1              @ shift into position
 342         mov     ip, #1                  @
 343         lsl     ip, ip, r1              @ ip now contains the way decr
 344
 345         ubfx    r0, r0, #0, #3          @ get linesize from CCSIDR
 346         add     r0, r0, #4              @ apply bias
 347         lsl     r2, r2, r0              @ shift sets by log2(linesize)
 348         add     r3, r3, r2              @ merge numsets - 1 with numways - 1
 349         sub     ip, ip, r2              @ subtract numsets - 1 from way decr
 350         mov     r1, #1
 351         lsl     r1, r1, r0              @ r1 now contains the set decr
 352         mov     r2, ip                  @ r2 now contains set way decr
 353
 354         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
 355 1:      mcr     CP15_DCISW(r3)          @ invalidate line
 356         movs    r0, r3                  @ get current way/set
 357         beq     2f                      @ at 0 means we are done.
 358         movs    r0, r0, lsl #10         @ clear way bits leaving only set bits
 359         subne   r3, r3, r1              @ non-zero?, decrement set #
 360         subeq   r3, r3, r2              @ zero?, decrement way # and restore set count
 361         b       1b
 362
 363 2:      dsb                             @ wait for stores to finish
 364         mov     r0, #0                  @ and ...
 365         mcr     CP15_ICIALLU            @ invalidate instruction+branch cache
 366         isb                             @ instruction sync barrier
 367         bx      lr                      @ return
 368 END(armv7_idcache_inv_all)
 369
 370 ENTRY_NP(armv7_sleep)
 371         dsb
 372         wfi
 373         bx      lr
 374 END(armv7_sleep)
 375