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         .cpu cortex-a8
  37
  38 .Lcoherency_level:
  39         .word   _C_LABEL(arm_cache_loc)
  40 .Lcache_type:
  41         .word   _C_LABEL(arm_cache_type)
  42 .Lway_mask:
  43         .word   0x3ff
  44 .Lmax_index:
  45         .word   0x7fff
  46 .Lpage_mask:
  47         .word   0xfff
  48
  49 #define PT_NOS          (1 << 5)
  50 #define PT_S            (1 << 1)
  51 #define PT_INNER_NC     0
  52 #define PT_INNER_WT     (1 << 0)
  53 #define PT_INNER_WB     ((1 << 0) | (1 << 6))
  54 #define PT_INNER_WBWA   (1 << 6)
  55 #define PT_OUTER_NC     0
  56 #define PT_OUTER_WT     (2 << 3)
  57 #define PT_OUTER_WB     (3 << 3)
  58 #define PT_OUTER_WBWA   (1 << 3)
  59
  60 #ifdef SMP
  61 #define PT_ATTR (PT_S|PT_INNER_WBWA|PT_OUTER_WBWA|PT_NOS)
  62 #else
  63 #define PT_ATTR (PT_INNER_WBWA|PT_OUTER_WBWA)
  64 #endif
  65
  66 ENTRY(armv7_setttb)
  67         stmdb   sp!, {r0, lr}
  68         bl      _C_LABEL(armv7_idcache_wbinv_all) /* clean the D cache */
  69         ldmia   sp!, {r0, lr}
  70         dsb
  71
  72         orr     r0, r0, #PT_ATTR
  73         mcr     p15, 0, r0, c2, c0, 0   /* Translation Table Base Register 0 (TTBR0) */
  74         isb
  75 #ifdef SMP
  76         mcr     p15, 0, r0, c8, c3, 0   /* invalidate I+D TLBs Inner Shareable*/
  77 #else
  78         mcr     p15, 0, r0, c8, c7, 0   /* invalidate I+D TLBs */
  79 #endif
  80         dsb
  81         isb
  82         RET
  83 END(armv7_setttb)
  84
  85 ENTRY(armv7_tlb_flushID)
  86         dsb
  87 #ifdef SMP
  88         mcr     p15, 0, r0, c8, c3, 0   /* flush Unified TLB all entries Inner Shareable */
  89         mcr     p15, 0, r0, c7, c1, 6   /* flush BTB Inner Shareable */
  90 #else
  91         mcr     p15, 0, r0, c8, c7, 0   /* flush Unified TLB all entries */
  92         mcr     p15, 0, r0, c7, c5, 6   /* flush BTB */
  93 #endif
  94         dsb
  95         isb
  96         mov     pc, lr
  97 END(armv7_tlb_flushID)
  98
  99 ENTRY(armv7_tlb_flushID_SE)
 100         ldr     r1, .Lpage_mask
 101         bic     r0, r0, r1
 102 #ifdef SMP
 103         mcr     p15, 0, r0, c8, c3, 3   /* flush Unified TLB single entry Inner Shareable */
 104         mcr     p15, 0, r0, c7, c1, 6   /* flush BTB Inner Shareable */
 105 #else
 106         mcr     p15, 0, r0, c8, c7, 1   /* flush Unified TLB single entry */
 107         mcr     p15, 0, r0, c7, c5, 6   /* flush BTB */
 108 #endif
 109         dsb
 110         isb
 111         mov     pc, lr
 112 END(armv7_tlb_flushID_SE)
 113
 114 /* Based on algorithm from ARM Architecture Reference Manual */
 115 ENTRY(armv7_dcache_wbinv_all)
 116         stmdb   sp!, {r4, r5, r6, r7, r8, r9}
 117
 118         /* Get cache level */
 119         ldr     r0, .Lcoherency_level
 120         ldr     r3, [r0]
 121         cmp     r3, #0
 122         beq     Finished
 123         /* For each cache level */
 124         mov     r8, #0
 125 Loop1:
 126         /* Get cache type for given level */
 127         mov     r2, r8, lsl #2
 128         add     r2, r2, r2
 129         ldr     r0, .Lcache_type
 130         ldr     r1, [r0, r2]
 131
 132         /* Get line size */
 133         and     r2, r1, #7
 134         add     r2, r2, #4
 135
 136         /* Get number of ways */
 137         ldr     r4, .Lway_mask
 138         ands    r4, r4, r1, lsr #3
 139         clz     r5, r4
 140
 141         /* Get max index */
 142         ldr     r7, .Lmax_index
 143         ands    r7, r7, r1, lsr #13
 144 Loop2:
 145         mov     r9, r4
 146 Loop3:
 147         mov     r6, r8, lsl #1
 148         orr     r6, r6, r9, lsl r5
 149         orr     r6, r6, r7, lsl r2
 150
 151         /* Clean and invalidate data cache by way/index */
 152         mcr     p15, 0, r6, c7, c14, 2
 153         subs    r9, r9, #1
 154         bge     Loop3
 155         subs    r7, r7, #1
 156         bge     Loop2
 157 Skip:
 158         add     r8, r8, #1
 159         cmp     r3, r8
 160         bne Loop1
 161 Finished:
 162         dsb
 163         ldmia   sp!, {r4, r5, r6, r7, r8, r9}
 164         RET
 165 END(armv7_dcache_wbinv_all)
 166
 167 ENTRY(armv7_idcache_wbinv_all)
 168         stmdb   sp!, {lr}
 169         bl armv7_dcache_wbinv_all
 170 #ifdef SMP
 171         mcr     p15, 0, r0, c7, c1, 0   /* Invalidate all I caches to PoU (ICIALLUIS) */
 172 #else
 173         mcr     p15, 0, r0, c7, c5, 0   /* Invalidate all I caches to PoU (ICIALLU) */
 174 #endif
 175         dsb
 176         isb
 177         ldmia   sp!, {lr}
 178         RET
 179 END(armv7_idcache_wbinv_all)
 180
 181 /* XXX Temporary set it to 32 for MV cores, however this value should be
 182  * get from Cache Type register
 183  */
 184 .Larmv7_line_size:
 185         .word   32
 186
 187 ENTRY(armv7_dcache_wb_range)
 188         ldr     ip, .Larmv7_line_size
 189         sub     r3, ip, #1
 190         and     r2, r0, r3
 191         add     r1, r1, r2
 192         bic     r0, r0, r3
 193 .Larmv7_wb_next:
 194         mcr     p15, 0, r0, c7, c10, 1  /* Clean D cache SE with VA */
 195         add     r0, r0, ip
 196         subs    r1, r1, ip
 197         bhi     .Larmv7_wb_next
 198         dsb                             /* data synchronization barrier */
 199         RET
 200 END(armv7_dcache_wb_range)
 201
 202 ENTRY(armv7_dcache_wbinv_range)
 203         ldr     ip, .Larmv7_line_size
 204         sub     r3, ip, #1
 205         and     r2, r0, r3
 206         add     r1, r1, r2
 207         bic     r0, r0, r3
 208 .Larmv7_wbinv_next:
 209         mcr     p15, 0, r0, c7, c14, 1  /* Purge D cache SE with VA */
 210         add     r0, r0, ip
 211         subs    r1, r1, ip
 212         bhi     .Larmv7_wbinv_next
 213         dsb                             /* data synchronization barrier */
 214         RET
 215 END(armv7_dcache_wbinv_range)
 216
 217 /*
 218  * Note, we must not invalidate everything.  If the range is too big we
 219  * must use wb-inv of the entire cache.
 220  */
 221 ENTRY(armv7_dcache_inv_range)
 222         ldr     ip, .Larmv7_line_size
 223         sub     r3, ip, #1
 224         and     r2, r0, r3
 225         add     r1, r1, r2
 226         bic     r0, r0, r3
 227 .Larmv7_inv_next:
 228         mcr     p15, 0, r0, c7, c6, 1   /* Invalidate D cache SE with VA */
 229         add     r0, r0, ip
 230         subs    r1, r1, ip
 231         bhi     .Larmv7_inv_next
 232         dsb                             /* data synchronization barrier */
 233         RET
 234 END(armv7_dcache_inv_range)
 235
 236 ENTRY(armv7_idcache_wbinv_range)
 237         ldr     ip, .Larmv7_line_size
 238         sub     r3, ip, #1
 239         and     r2, r0, r3
 240         add     r1, r1, r2
 241         bic     r0, r0, r3
 242 .Larmv7_id_wbinv_next:
 243         mcr     p15, 0, r0, c7, c5, 1   /* Invalidate I cache SE with VA */
 244         mcr     p15, 0, r0, c7, c14, 1  /* Purge D cache SE with VA */
 245         add     r0, r0, ip
 246         subs    r1, r1, ip
 247         bhi     .Larmv7_id_wbinv_next
 248         isb                             /* instruction synchronization barrier */
 249         dsb                             /* data synchronization barrier */
 250         RET
 251 END(armv7_idcache_wbinv_range)
 252
 253 ENTRY_NP(armv7_icache_sync_all)
 254 #ifdef SMP
 255         mcr     p15, 0, r0, c7, c1, 0   /* Invalidate all I cache to PoU Inner Shareable */
 256 #else
 257         mcr     p15, 0, r0, c7, c5, 0   /* Invalidate all I cache to PoU (ICIALLU) */
 258 #endif
 259         isb                             /* instruction synchronization barrier */
 260         dsb                             /* data synchronization barrier */
 261         RET
 262 END(armv7_icache_sync_all)
 263
 264 ENTRY_NP(armv7_icache_sync_range)
 265         ldr     ip, .Larmv7_line_size
 266 .Larmv7_sync_next:
 267         mcr     p15, 0, r0, c7, c5, 1   /* Invalidate I cache SE with VA */
 268         mcr     p15, 0, r0, c7, c10, 1  /* Clean D cache SE with VA */
 269         add     r0, r0, ip
 270         subs    r1, r1, ip
 271         bhi     .Larmv7_sync_next
 272         isb                             /* instruction synchronization barrier */
 273         dsb                             /* data synchronization barrier */
 274         RET
 275 END(armv7_icache_sync_range)
 276
 277 ENTRY(armv7_cpu_sleep)
 278         dsb                             /* data synchronization barrier */
 279         wfi                             /* wait for interrupt */
 280         RET
 281 END(armv7_cpu_sleep)
 282
 283 ENTRY(armv7_context_switch)
 284         dsb
 285         orr     r0, r0, #PT_ATTR
 286
 287         mcr     p15, 0, r0, c2, c0, 0   /* set the new TTB */
 288         isb
 289 #ifdef SMP
 290         mcr     p15, 0, r0, c8, c3, 0   /* and flush the I+D tlbs Inner Sharable */
 291 #else
 292         mcr     p15, 0, r0, c8, c7, 0   /* and flush the I+D tlbs */
 293 #endif
 294         dsb
 295         isb
 296         RET
 297 END(armv7_context_switch)
 298
 299 ENTRY(armv7_drain_writebuf)
 300         dsb
 301         RET
 302 END(armv7_drain_writebuf)
 303
 304 ENTRY(armv7_sev)
 305         dsb
 306         sev
 307         nop
 308         RET
 309 END(armv7_sev)
 310
 311 ENTRY(armv7_auxctrl)
 312         mrc p15, 0, r2, c1, c0, 1
 313         bic r3, r2, r0  /* Clear bits */
 314         eor r3, r3, r1  /* XOR bits */
 315
 316         teq r2, r3
 317         mcrne p15, 0, r3, c1, c0, 1
 318         mov r0, r2
 319         RET
 320 END(armv7_auxctrl)
 321
 322 /*
 323  * Invalidate all I+D+branch cache.  Used by startup code, which counts
 324  * on the fact that only r0-r3,ip are modified and no stack space is used.
 325  */
 326 ENTRY(armv7_idcache_inv_all)
 327         mov     r0, #0
 328         mcr     p15, 2, r0, c0, c0, 0   @ set cache level to L1
 329         mrc     p15, 1, r0, c0, c0, 0   @ read CCSIDR
 330
 331         ubfx    r2, r0, #13, #15        @ get num sets - 1 from CCSIDR
 332         ubfx    r3, r0, #3, #10         @ get numways - 1 from CCSIDR
 333         clz     r1, r3                  @ number of bits to MSB of way
 334         lsl     r3, r3, r1              @ shift into position
 335         mov     ip, #1                  @
 336         lsl     ip, ip, r1              @ ip now contains the way decr
 337
 338         ubfx    r0, r0, #0, #3          @ get linesize from CCSIDR
 339         add     r0, r0, #4              @ apply bias
 340         lsl     r2, r2, r0              @ shift sets by log2(linesize)
 341         add     r3, r3, r2              @ merge numsets - 1 with numways - 1
 342         sub     ip, ip, r2              @ subtract numsets - 1 from way decr
 343         mov     r1, #1
 344         lsl     r1, r1, r0              @ r1 now contains the set decr
 345         mov     r2, ip                  @ r2 now contains set way decr
 346
 347         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
 348 1:      mcr     p15, 0, r3, c7, c6, 2   @ invalidate line
 349         movs    r0, r3                  @ get current way/set
 350         beq     2f                      @ at 0 means we are done.
 351         movs    r0, r0, lsl #10         @ clear way bits leaving only set bits
 352         subne   r3, r3, r1              @ non-zero?, decrement set #
 353         subeq   r3, r3, r2              @ zero?, decrement way # and restore set count
 354         b       1b
 355
 356 2:      dsb                             @ wait for stores to finish
 357         mov     r0, #0                  @ and ...
 358         mcr     p15, 0, r0, c7, c5, 0   @ invalidate instruction+branch cache
 359         isb                             @ instruction sync barrier
 360         bx      lr                      @ return
 361 END(armv7_idcache_inv_all)
 362
 363 ENTRY_NP(armv7_sleep)
 364         dsb
 365         wfi
 366         bx      lr
 367 END(armv7_sleep)
 368