sys/arm/arm/cpu_asm-v6.S

   1 /*-
   2  * Copyright 2014 Svatopluk Kraus <onwahe@gmail.com>
   3  * Copyright 2014 Michal Meloun <meloun@miracle.cz>
   4  * All rights reserved.
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  *
  15  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  16  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  18  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  19  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  20  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  21  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  22  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  23  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  24  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  25  * SUCH DAMAGE.
  26  *
  27  * $FreeBSD$
  28  */
  29 #include "assym.s"
  30
  31 #include <machine/acle-compat.h>
  32 #include <machine/asm.h>
  33 #include <machine/asmacros.h>
  34 #include <machine/armreg.h>
  35 #include <machine/sysreg.h>
  36
  37 #if __ARM_ARCH >= 6
  38 #define GET_PCB(tmp) \
  39         mrc CP15_TPIDRPRW(tmp); \
  40         add   tmp, tmp, #(TD_PCB)
  41 #else
  42 .Lcurpcb:
  43         .word _C_LABEL(__pcpu) + PC_CURPCB
  44 #define GET_PCB(tmp) \
  45         ldr   tmp, .Lcurpcb
  46 #endif
  47
  48 /*
  49  * Define cache functions used by startup code, which counts on the fact that
  50  * only r0-r3,r12 (ip) are modified and no stack space is used.  These functions
  51  * must be called with interrupts disabled.  Moreover, these work only with
  52  * caches integrated to CPU (accessible via CP15); systems with an external L2
  53  * cache controller such as a PL310 need separate calls to that device driver
  54  * to affect L2 caches.  This is not a factor during early kernel startup, as
  55  * any external L2 cache controller has not been enabled yet.
  56  */
  57
  58 /* Invalidate D cache to PoC. (aka all cache levels)*/
  59 ASENTRY_NP(dcache_inv_poc_all)
  60 #if __ARM_ARCH == 6
  61         mcr     CP15_DCIALL
  62         DSB
  63         bx      lr
  64 #else
  65         mrc     CP15_CLIDR(r0)
  66         ands    r0, r0, #0x07000000
  67         mov     r0, r0, lsr #23         /* Get LoC 'naturally' aligned for */
  68         beq     4f                      /* use in the CSSELR register below */
  69
  70 1:      sub     r0, #2
  71         mcr     CP15_CSSELR(r0)         /* set cache level */
  72         isb
  73         mrc     CP15_CCSIDR(r0)         /* read CCSIDR */
  74
  75         ubfx    r2, r0, #13, #15        /* get num sets - 1 from CCSIDR */
  76         ubfx    r3, r0, #3, #10         /* get num ways - 1 from CCSIDR */
  77         clz     r1, r3                  /* number of bits to MSB of way */
  78         lsl     r3, r3, r1              /* shift into position  */
  79         mov     ip, #1
  80         lsl     ip, ip, r1              /* ip now contains the way decr  */
  81
  82         ubfx    r0, r0, #0, #3          /* get linesize from CCSIDR  */
  83         add     r0, r0, #4              /* apply bias  */
  84         lsl     r2, r2, r0              /* shift sets by log2(linesize)  */
  85         add     r3, r3, r2              /* merge numsets - 1 with numways - 1 */
  86         sub     ip, ip, r2              /* subtract numsets - 1 from way decr */
  87         mov     r1, #1
  88         lsl     r1, r1, r0              /* r1 now contains the set decr */
  89         mov     r2, ip                  /* r2 now contains set way decr */
  90
  91         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
  92 2:      mcr     CP15_DCISW(r3)          /* invalidate line */
  93         movs    r0, r3                  /* get current way/set */
  94         beq     3f                      /* at 0 means we are done */
  95         movs    r0, r0, lsl #10         /* clear way bits leaving only set bits*/
  96         subne   r3, r3, r1              /* non-zero?, decrement set */
  97         subeq   r3, r3, r2              /* zero?, decrement way  and restore set count */
  98         b       2b
  99
 100 3:
 101         mrc     CP15_CSSELR(r0)         /* get cache level */
 102         teq     r0, #0
 103         bne     1b
 104
 105 4:      dsb                             /* wait for stores to finish */
 106         mov     r0, #0
 107         mcr     CP15_CSSELR(r0)
 108         isb
 109         bx      lr
 110 #endif /* __ARM_ARCH == 6 */
 111 END(dcache_inv_poc_all)
 112
 113 /* Invalidate D cache to PoU. (aka L1 cache only)*/
 114 ASENTRY_NP(dcache_inv_pou_all)
 115 #if __ARM_ARCH == 6
 116         mcr     CP15_DCIALL
 117         DSB
 118         bx      lr
 119 #else
 120         mrc     CP15_CLIDR(r0)
 121         ands    r0, r0, #0x38000000
 122         mov     r0, r0, lsr #26         /* Get LoUU (naturally aligned) */
 123         beq     4f
 124
 125 1:      sub     r0, #2
 126         mcr     CP15_CSSELR(r0)         /* set cache level */
 127         isb
 128         mrc     CP15_CCSIDR(r0)         /* read CCSIDR */
 129
 130         ubfx    r2, r0, #13, #15        /* get num sets - 1 from CCSIDR */
 131         ubfx    r3, r0, #3, #10         /* get num ways - 1 from CCSIDR */
 132         clz     r1, r3                  /* number of bits to MSB of way */
 133         lsl     r3, r3, r1              /* shift into position  */
 134         mov     ip, #1
 135         lsl     ip, ip, r1              /* ip now contains the way decr  */
 136
 137         ubfx    r0, r0, #0, #3          /* get linesize from CCSIDR  */
 138         add     r0, r0, #4              /* apply bias  */
 139         lsl     r2, r2, r0              /* shift sets by log2(linesize)  */
 140         add     r3, r3, r2              /* merge numsets - 1 with numways - 1 */
 141         sub     ip, ip, r2              /* subtract numsets - 1 from way decr */
 142         mov     r1, #1
 143         lsl     r1, r1, r0              /* r1 now contains the set decr */
 144         mov     r2, ip                  /* r2 now contains set way decr */
 145
 146         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
 147 2:      mcr     CP15_DCISW(r3)          /* invalidate line */
 148         movs    r0, r3                  /* get current way/set */
 149         beq     3f                      /* at 0 means we are done */
 150         movs    r0, r0, lsl #10         /* clear way bits leaving only set bits*/
 151         subne   r3, r3, r1              /* non-zero?, decrement set */
 152         subeq   r3, r3, r2              /* zero?, decrement way  and restore set count */
 153         b       2b
 154
 155 3:
 156         mrc     CP15_CSSELR(r0)         /* get cache level */
 157         teq     r0, #0
 158         bne     1b
 159
 160 4:      dsb                             /* wait for stores to finish */
 161         mov     r0, #0
 162         mcr     CP15_CSSELR(r0)
 163         bx      lr
 164 #endif
 165 END(dcache_inv_pou_all)
 166
 167 /* Write back and Invalidate D cache to PoC. */
 168 ASENTRY_NP(dcache_wbinv_poc_all)
 169 #if __ARM_ARCH == 6
 170         mcr     CP15_DCCIALL
 171         DSB
 172         bx      lr
 173 #else
 174         mrc     CP15_CLIDR(r0)
 175         ands    r0, r0, #0x07000000
 176         beq     4f
 177         mov     r0, #0                  /* Clean from inner to outer levels */
 178
 179 1:      mcr     CP15_CSSELR(r0)         /* set cache level */
 180         isb
 181         mrc     CP15_CCSIDR(r0)         /* read CCSIDR */
 182
 183         ubfx    r2, r0, #13, #15        /* get num sets - 1 from CCSIDR */
 184         ubfx    r3, r0, #3, #10         /* get num ways - 1 from CCSIDR */
 185         clz     r1, r3                  /* number of bits to MSB of way */
 186         lsl     r3, r3, r1              /* shift into position  */
 187         mov     ip, #1
 188         lsl     ip, ip, r1              /* ip now contains the way decr  */
 189
 190         ubfx    r0, r0, #0, #3          /* get linesize from CCSIDR  */
 191         add     r0, r0, #4              /* apply bias  */
 192         lsl     r2, r2, r0              /* shift sets by log2(linesize)  */
 193         add     r3, r3, r2              /* merge numsets - 1 with numways - 1 */
 194         sub     ip, ip, r2              /* subtract numsets - 1 from way decr */
 195         mov     r1, #1
 196         lsl     r1, r1, r0              /* r1 now contains the set decr */
 197         mov     r2, ip                  /* r2 now contains set way decr */
 198
 199         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
 200 2:      mcr     CP15_DCCISW(r3)         /* clean and invalidate line */
 201         movs    r0, r3                  /* get current way/set */
 202         beq     3f                      /* at 0 means we are done */
 203         movs    r0, r0, lsl #10         /* clear way bits leaving only set bits*/
 204         subne   r3, r3, r1              /* non-zero?, decrement set */
 205         subeq   r3, r3, r2              /* zero?, decrement way  and restore set count */
 206         b       2b
 207
 208 3:
 209         mrc     CP15_CSSELR(r0)         /* get cache level */
 210         add     r0, r0, #2              /* next level */
 211         mrc     CP15_CLIDR(r1)
 212         ands    r1, r1, #0x07000000
 213         mov     r1, r1, lsr #23         /* Get LoC (naturally aligned) */
 214         cmp     r1, r0
 215         bne     1b
 216
 217 4:      dsb                             /* wait for stores to finish */
 218         mov     r0, #0
 219         mcr     CP15_CSSELR(r0)
 220         bx      lr
 221 #endif /* __ARM_ARCH == 6 */
 222 END(dcache_wbinv_poc_all)
 223
 224 ASENTRY_NP(dcache_wb_pou_checked)
 225         ldr     ip, .Lcpuinfo
 226         ldr     ip, [ip, #DCACHE_LINE_SIZE]
 227
 228         GET_PCB(r2)
 229         ldr     r2, [r2]
 230
 231         adr     r3, _C_LABEL(cachebailout)
 232         str     r3, [r2, #PCB_ONFAULT]
 233 1:
 234         mcr     CP15_DCCMVAC(r0)
 235         add     r0, r0, ip
 236         subs    r1, r1, ip
 237         bhi     1b
 238         DSB
 239         mov     r0, #0
 240         str     r0, [r2, #PCB_ONFAULT]
 241         mov     r0, #1                  /* cannot be faulting address */
 242         RET
 243
 244 .Lcpuinfo:
 245         .word   cpuinfo
 246 END(dcache_wb_pou_checked)
 247
 248 ASENTRY_NP(icache_inv_pou_checked)
 249         ldr     ip, .Lcpuinfo
 250         ldr     ip, [ip, #ICACHE_LINE_SIZE]
 251
 252         GET_PCB(r2)
 253         ldr     r2, [r2]
 254
 255         adr     r3, _C_LABEL(cachebailout)
 256         str     r3, [r2, #PCB_ONFAULT]
 257
 258 1:
 259         mcr     CP15_ICIMVAU(r0)
 260         add     r0, r0, ip
 261         subs    r1, r1, ip
 262         bhi     1b
 263         DSB
 264         ISB
 265         mov     r0, #0
 266         str     r0, [r2, #PCB_ONFAULT]
 267         mov     r0, #1                  /* cannot be faulting address */
 268         RET
 269 END(icache_inv_pou_checked)
 270
 271 /* label must be global as trap-v6.c references it */
 272         .global _C_LABEL(cachebailout)
 273 _C_LABEL(cachebailout):
 274         DSB
 275         ISB
 276         mov     r1, #0
 277         str     r1, [r2, #PCB_ONFAULT]
 278         RET