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.inc"
  30
  31 #include <machine/asm.h>
  32 #include <machine/asmacros.h>
  33 #include <machine/armreg.h>
  34 #include <machine/sysreg.h>
  35
  36 #define GET_PCB(tmp) \
  37         mrc     CP15_TPIDRPRW(tmp); \
  38         add     tmp, tmp, #(TD_PCB)
  39
  40 /*
  41  * Define cache functions used by startup code, which counts on the fact that
  42  * only r0-r3,r12 (ip) are modified and no stack space is used.  These functions
  43  * must be called with interrupts disabled.  Moreover, these work only with
  44  * caches integrated to CPU (accessible via CP15); systems with an external L2
  45  * cache controller such as a PL310 need separate calls to that device driver
  46  * to affect L2 caches.  This is not a factor during early kernel startup, as
  47  * any external L2 cache controller has not been enabled yet.
  48  */
  49
  50 /* Invalidate D cache to PoC. (aka all cache levels)*/
  51 ASENTRY_NP(dcache_inv_poc_all)
  52 #if __ARM_ARCH == 6
  53         mcr     CP15_DCIALL
  54         DSB
  55         bx      lr
  56 #else
  57         mrc     CP15_CLIDR(r0)
  58         ands    r0, r0, #0x07000000
  59         mov     r0, r0, lsr #23         /* Get LoC 'naturally' aligned for */
  60         beq     4f                      /* use in the CSSELR register below */
  61
  62 1:      sub     r0, #2
  63         mcr     CP15_CSSELR(r0)         /* set cache level */
  64         isb
  65         mrc     CP15_CCSIDR(r0)         /* read CCSIDR */
  66
  67         ubfx    r2, r0, #13, #15        /* get num sets - 1 from CCSIDR */
  68         ubfx    r3, r0, #3, #10         /* get num ways - 1 from CCSIDR */
  69         clz     r1, r3                  /* number of bits to MSB of way */
  70         lsl     r3, r3, r1              /* shift into position  */
  71         mov     ip, #1
  72         lsl     ip, ip, r1              /* ip now contains the way decr  */
  73
  74         ubfx    r0, r0, #0, #3          /* get linesize from CCSIDR  */
  75         add     r0, r0, #4              /* apply bias  */
  76         lsl     r2, r2, r0              /* shift sets by log2(linesize)  */
  77         add     r3, r3, r2              /* merge numsets - 1 with numways - 1 */
  78         sub     ip, ip, r2              /* subtract numsets - 1 from way decr */
  79         mov     r1, #1
  80         lsl     r1, r1, r0              /* r1 now contains the set decr */
  81         mov     r2, ip                  /* r2 now contains set way decr */
  82
  83         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
  84 2:      mcr     CP15_DCISW(r3)          /* invalidate line */
  85         movs    r0, r3                  /* get current way/set */
  86         beq     3f                      /* at 0 means we are done */
  87         movs    r0, r0, lsl #10         /* clear way bits leaving only set bits*/
  88         subne   r3, r3, r1              /* non-zero?, decrement set */
  89         subeq   r3, r3, r2              /* zero?, decrement way  and restore set count */
  90         b       2b
  91
  92 3:
  93         mrc     CP15_CSSELR(r0)         /* get cache level */
  94         teq     r0, #0
  95         bne     1b
  96
  97 4:      dsb                             /* wait for stores to finish */
  98         mov     r0, #0
  99         mcr     CP15_CSSELR(r0)
 100         isb
 101         bx      lr
 102 #endif /* __ARM_ARCH == 6 */
 103 END(dcache_inv_poc_all)
 104
 105 /* Invalidate D cache to PoU. (aka L1 cache only)*/
 106 ASENTRY_NP(dcache_inv_pou_all)
 107 #if __ARM_ARCH == 6
 108         mcr     CP15_DCIALL
 109         DSB
 110         bx      lr
 111 #else
 112         mrc     CP15_CLIDR(r0)
 113         ands    r0, r0, #0x38000000
 114         mov     r0, r0, lsr #26         /* Get LoUU (naturally aligned) */
 115         beq     4f
 116
 117 1:      sub     r0, #2
 118         mcr     CP15_CSSELR(r0)         /* set cache level */
 119         isb
 120         mrc     CP15_CCSIDR(r0)         /* read CCSIDR */
 121
 122         ubfx    r2, r0, #13, #15        /* get num sets - 1 from CCSIDR */
 123         ubfx    r3, r0, #3, #10         /* get num ways - 1 from CCSIDR */
 124         clz     r1, r3                  /* number of bits to MSB of way */
 125         lsl     r3, r3, r1              /* shift into position  */
 126         mov     ip, #1
 127         lsl     ip, ip, r1              /* ip now contains the way decr  */
 128
 129         ubfx    r0, r0, #0, #3          /* get linesize from CCSIDR  */
 130         add     r0, r0, #4              /* apply bias  */
 131         lsl     r2, r2, r0              /* shift sets by log2(linesize)  */
 132         add     r3, r3, r2              /* merge numsets - 1 with numways - 1 */
 133         sub     ip, ip, r2              /* subtract numsets - 1 from way decr */
 134         mov     r1, #1
 135         lsl     r1, r1, r0              /* r1 now contains the set decr */
 136         mov     r2, ip                  /* r2 now contains set way decr */
 137
 138         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
 139 2:      mcr     CP15_DCISW(r3)          /* invalidate line */
 140         movs    r0, r3                  /* get current way/set */
 141         beq     3f                      /* at 0 means we are done */
 142         movs    r0, r0, lsl #10         /* clear way bits leaving only set bits*/
 143         subne   r3, r3, r1              /* non-zero?, decrement set */
 144         subeq   r3, r3, r2              /* zero?, decrement way  and restore set count */
 145         b       2b
 146
 147 3:
 148         mrc     CP15_CSSELR(r0)         /* get cache level */
 149         teq     r0, #0
 150         bne     1b
 151
 152 4:      dsb                             /* wait for stores to finish */
 153         mov     r0, #0
 154         mcr     CP15_CSSELR(r0)
 155         bx      lr
 156 #endif
 157 END(dcache_inv_pou_all)
 158
 159 /* Write back and Invalidate D cache to PoC. */
 160 ASENTRY_NP(dcache_wbinv_poc_all)
 161 #if __ARM_ARCH == 6
 162         mcr     CP15_DCCIALL
 163         DSB
 164         bx      lr
 165 #else
 166         mrc     CP15_CLIDR(r0)
 167         ands    r0, r0, #0x07000000
 168         beq     4f
 169         mov     r0, #0                  /* Clean from inner to outer levels */
 170
 171 1:      mcr     CP15_CSSELR(r0)         /* set cache level */
 172         isb
 173         mrc     CP15_CCSIDR(r0)         /* read CCSIDR */
 174
 175         ubfx    r2, r0, #13, #15        /* get num sets - 1 from CCSIDR */
 176         ubfx    r3, r0, #3, #10         /* get num ways - 1 from CCSIDR */
 177         clz     r1, r3                  /* number of bits to MSB of way */
 178         lsl     r3, r3, r1              /* shift into position  */
 179         mov     ip, #1
 180         lsl     ip, ip, r1              /* ip now contains the way decr  */
 181
 182         ubfx    r0, r0, #0, #3          /* get linesize from CCSIDR  */
 183         add     r0, r0, #4              /* apply bias  */
 184         lsl     r2, r2, r0              /* shift sets by log2(linesize)  */
 185         add     r3, r3, r2              /* merge numsets - 1 with numways - 1 */
 186         sub     ip, ip, r2              /* subtract numsets - 1 from way decr */
 187         mov     r1, #1
 188         lsl     r1, r1, r0              /* r1 now contains the set decr */
 189         mov     r2, ip                  /* r2 now contains set way decr */
 190
 191         /* r3 = ways/sets, r2 = way decr, r1 = set decr, r0 and ip are free */
 192 2:      mcr     CP15_DCCISW(r3)         /* clean and invalidate line */
 193         movs    r0, r3                  /* get current way/set */
 194         beq     3f                      /* at 0 means we are done */
 195         movs    r0, r0, lsl #10         /* clear way bits leaving only set bits*/
 196         subne   r3, r3, r1              /* non-zero?, decrement set */
 197         subeq   r3, r3, r2              /* zero?, decrement way  and restore set count */
 198         b       2b
 199
 200 3:
 201         mrc     CP15_CSSELR(r0)         /* get cache level */
 202         add     r0, r0, #2              /* next level */
 203         mrc     CP15_CLIDR(r1)
 204         ands    r1, r1, #0x07000000
 205         mov     r1, r1, lsr #23         /* Get LoC (naturally aligned) */
 206         cmp     r1, r0
 207         bne     1b
 208
 209 4:      dsb                             /* wait for stores to finish */
 210         mov     r0, #0
 211         mcr     CP15_CSSELR(r0)
 212         bx      lr
 213 #endif /* __ARM_ARCH == 6 */
 214 END(dcache_wbinv_poc_all)
 215
 216 ASENTRY_NP(dcache_wb_pou_checked)
 217         ldr     ip, .Lcpuinfo
 218         ldr     ip, [ip, #DCACHE_LINE_SIZE]
 219
 220         GET_PCB(r2)
 221         ldr     r2, [r2]
 222
 223         adr     r3, _C_LABEL(cachebailout)
 224         str     r3, [r2, #PCB_ONFAULT]
 225 1:
 226         mcr     CP15_DCCMVAC(r0)
 227         add     r0, r0, ip
 228         subs    r1, r1, ip
 229         bhi     1b
 230         DSB
 231         mov     r0, #0
 232         str     r0, [r2, #PCB_ONFAULT]
 233         mov     r0, #1                  /* cannot be faulting address */
 234         RET
 235
 236 .Lcpuinfo:
 237         .word   cpuinfo
 238 END(dcache_wb_pou_checked)
 239
 240 ASENTRY_NP(icache_inv_pou_checked)
 241         ldr     ip, .Lcpuinfo
 242         ldr     ip, [ip, #ICACHE_LINE_SIZE]
 243
 244         GET_PCB(r2)
 245         ldr     r2, [r2]
 246
 247         adr     r3, _C_LABEL(cachebailout)
 248         str     r3, [r2, #PCB_ONFAULT]
 249
 250 1:
 251         mcr     CP15_ICIMVAU(r0)
 252         add     r0, r0, ip
 253         subs    r1, r1, ip
 254         bhi     1b
 255         DSB
 256         ISB
 257         mov     r0, #0
 258         str     r0, [r2, #PCB_ONFAULT]
 259         mov     r0, #1                  /* cannot be faulting address */
 260         RET
 261 END(icache_inv_pou_checked)
 262
 263 /* label must be global as trap-v6.c references it */
 264         .global _C_LABEL(cachebailout)
 265 _C_LABEL(cachebailout):
 266         DSB
 267         ISB
 268         mov     r1, #0
 269         str     r1, [r2, #PCB_ONFAULT]
 270         RET