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