sys/arm/arm/cpufunc_asm_sheeva.S

   1 /*-
   2  * Copyright (C) 2008 MARVELL INTERNATIONAL LTD.
   3  * All rights reserved.
   4  *
   5  * Developed by Semihalf.
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  * 1. Redistributions of source code must retain the above copyright
  11  *    notice, this list of conditions and the following disclaimer.
  12  * 2. Redistributions in binary form must reproduce the above copyright
  13  *    notice, this list of conditions and the following disclaimer in the
  14  *    documentation and/or other materials provided with the distribution.
  15  * 3. Neither the name of MARVELL nor the names of contributors
  16  *    may be used to endorse or promote products derived from this software
  17  *    without specific prior written permission.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  20  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  22  * ARE DISCLAIMED.  IN NO EVENT SHALL AUTHOR OR CONTRIBUTORS BE LIABLE
  23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  25  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  26  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  28  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  29  * SUCH DAMAGE.
  30  */
  31
  32 #include <machine/asm.h>
  33 __FBSDID("$FreeBSD$");
  34
  35 #include <machine/armreg.h>
  36 #include <machine/param.h>
  37
  38 #ifndef ELF_TRAMPOLINE
  39 .Lsheeva_cache_line_size:
  40         .word   _C_LABEL(arm_pdcache_line_size)
  41 .Lsheeva_asm_page_mask:
  42         .word   _C_LABEL(PAGE_MASK)
  43
  44 ENTRY(sheeva_setttb)
  45         /* Disable irqs */
  46         mrs     r2, cpsr
  47         orr     r3, r2, #PSR_I | PSR_F
  48         msr     cpsr_c, r3
  49
  50         mov     r1, #0
  51         mcr     p15, 0, r1, c7, c5, 0   /* Invalidate ICache */
  52 1:      mrc     p15, 0, APSR_nzcv, c7, c14, 3   /* Test, clean and invalidate DCache */
  53         bne     1b                      /* More to do? */
  54
  55         mcr     p15, 1, r1, c15, c9, 0  /* Clean L2 */
  56         mcr     p15, 1, r1, c15, c11, 0 /* Invalidate L2 */
  57
  58         /* Reenable irqs */
  59         msr     cpsr_c, r2
  60
  61         mcr     p15, 0, r1, c7, c10, 4  /* drain the write buffer */
  62
  63         mcr     p15, 0, r0, c2, c0, 0   /* load new TTB */
  64
  65         mcr     p15, 0, r0, c8, c7, 0   /* invalidate I+D TLBs */
  66         RET
  67 END(sheeva_setttb)
  68
  69 ENTRY(sheeva_dcache_wbinv_range)
  70         str     lr, [sp, #-4]!
  71         mrs     lr, cpsr
  72         /* Start with cache line aligned address */
  73         ldr     ip, .Lsheeva_cache_line_size
  74         ldr     ip, [ip]
  75         sub     ip, ip, #1
  76         and     r2, r0, ip
  77         add     r1, r1, r2
  78         add     r1, r1, ip
  79         bics    r1, r1, ip
  80         bics    r0, r0, ip
  81
  82         ldr     ip, .Lsheeva_asm_page_mask
  83         and     r2, r0, ip
  84         rsb     r2, r2, #PAGE_SIZE
  85         cmp     r1, r2
  86         movcc   ip, r1
  87         movcs   ip, r2
  88 1:
  89         add     r3, r0, ip
  90         sub     r2, r3, #1
  91         /* Disable irqs */
  92         orr     r3, lr, #PSR_I | PSR_F
  93         msr     cpsr_c, r3
  94         mcr     p15, 5, r0, c15, c15, 0 /* Clean and inv zone start address */
  95         mcr     p15, 5, r2, c15, c15, 1 /* Clean and inv zone end address */
  96         /* Enable irqs */
  97         msr     cpsr_c, lr
  98
  99         add     r0, r0, ip
 100         sub     r1, r1, ip
 101         cmp     r1, #PAGE_SIZE
 102         movcc   ip, r1
 103         movcs   ip, #PAGE_SIZE
 104         cmp     r1, #0
 105         bne     1b
 106         mov     r0, #0
 107         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 108         ldr     lr, [sp], #4
 109         RET
 110 END(sheeva_dcache_wbinv_range)
 111
 112 ENTRY(sheeva_idcache_wbinv_range)
 113         str     lr, [sp, #-4]!
 114         mrs     lr, cpsr
 115         /* Start with cache line aligned address */
 116         ldr     ip, .Lsheeva_cache_line_size
 117         ldr     ip, [ip]
 118         sub     ip, ip, #1
 119         and     r2, r0, ip
 120         add     r1, r1, r2
 121         add     r1, r1, ip
 122         bics    r1, r1, ip
 123         bics    r0, r0, ip
 124
 125         ldr     ip, .Lsheeva_asm_page_mask
 126         and     r2, r0, ip
 127         rsb     r2, r2, #PAGE_SIZE
 128         cmp     r1, r2
 129         movcc   ip, r1
 130         movcs   ip, r2
 131 1:
 132         add     r3, r0, ip
 133         sub     r2, r3, #1
 134         /* Disable irqs */
 135         orr     r3, lr, #PSR_I | PSR_F
 136         msr     cpsr_c, r3
 137         mcr     p15, 5, r0, c15, c15, 0 /* Clean and inv zone start address */
 138         mcr     p15, 5, r2, c15, c15, 1 /* Clean and inv zone end address */
 139         /* Enable irqs */
 140         msr     cpsr_c, lr
 141
 142         /* Invalidate and clean icache line by line */
 143         ldr     r3, .Lsheeva_cache_line_size
 144         ldr     r3, [r3]
 145 2:
 146         mcr     p15, 0, r0, c7, c5, 1
 147         add     r0, r0, r3
 148         cmp     r2, r0
 149         bhi     2b
 150
 151         add     r0, r2, #1
 152         sub     r1, r1, ip
 153         cmp     r1, #PAGE_SIZE
 154         movcc   ip, r1
 155         movcs   ip, #PAGE_SIZE
 156         cmp     r1, #0
 157         bne     1b
 158         mov     r0, #0
 159         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 160         ldr     lr, [sp], #4
 161         RET
 162 END(sheeva_idcache_wbinv_range)
 163
 164 ENTRY(sheeva_dcache_inv_range)
 165         str     lr, [sp, #-4]!
 166         mrs     lr, cpsr
 167         /* Start with cache line aligned address */
 168         ldr     ip, .Lsheeva_cache_line_size
 169         ldr     ip, [ip]
 170         sub     ip, ip, #1
 171         and     r2, r0, ip
 172         add     r1, r1, r2
 173         add     r1, r1, ip
 174         bics    r1, r1, ip
 175         bics    r0, r0, ip
 176
 177         ldr     ip, .Lsheeva_asm_page_mask
 178         and     r2, r0, ip
 179         rsb     r2, r2, #PAGE_SIZE
 180         cmp     r1, r2
 181         movcc   ip, r1
 182         movcs   ip, r2
 183 1:
 184         add     r3, r0, ip
 185         sub     r2, r3, #1
 186         /* Disable irqs */
 187         orr     r3, lr, #PSR_I | PSR_F
 188         msr     cpsr_c, r3
 189         mcr     p15, 5, r0, c15, c14, 0 /* Inv zone start address */
 190         mcr     p15, 5, r2, c15, c14, 1 /* Inv zone end address */
 191         /* Enable irqs */
 192         msr     cpsr_c, lr
 193
 194         add     r0, r0, ip
 195         sub     r1, r1, ip
 196         cmp     r1, #PAGE_SIZE
 197         movcc   ip, r1
 198         movcs   ip, #PAGE_SIZE
 199         cmp     r1, #0
 200         bne     1b
 201         mov     r0, #0
 202         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 203         ldr     lr, [sp], #4
 204         RET
 205 END(sheeva_dcache_inv_range)
 206
 207 ENTRY(sheeva_dcache_wb_range)
 208         str     lr, [sp, #-4]!
 209         mrs     lr, cpsr
 210         /* Start with cache line aligned address */
 211         ldr     ip, .Lsheeva_cache_line_size
 212         ldr     ip, [ip]
 213         sub     ip, ip, #1
 214         and     r2, r0, ip
 215         add     r1, r1, r2
 216         add     r1, r1, ip
 217         bics    r1, r1, ip
 218         bics    r0, r0, ip
 219
 220         ldr     ip, .Lsheeva_asm_page_mask
 221         and     r2, r0, ip
 222         rsb     r2, r2, #PAGE_SIZE
 223         cmp     r1, r2
 224         movcc   ip, r1
 225         movcs   ip, r2
 226 1:
 227         add     r3, r0, ip
 228         sub     r2, r3, #1
 229         /* Disable irqs */
 230         orr     r3, lr, #PSR_I | PSR_F
 231         msr     cpsr_c, r3
 232         mcr     p15, 5, r0, c15, c13, 0 /* Clean zone start address */
 233         mcr     p15, 5, r2, c15, c13, 1 /* Clean zone end address */
 234         /* Enable irqs */
 235         msr     cpsr_c, lr
 236
 237         add     r0, r0, ip
 238         sub     r1, r1, ip
 239         cmp     r1, #PAGE_SIZE
 240         movcc   ip, r1
 241         movcs   ip, #PAGE_SIZE
 242         cmp     r1, #0
 243         bne     1b
 244         mov     r0, #0
 245         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 246         ldr     lr, [sp], #4
 247         RET
 248 END(sheeva_dcache_wb_range)
 249
 250 ENTRY(sheeva_l2cache_wbinv_range)
 251         str     lr, [sp, #-4]!
 252         mrs     lr, cpsr
 253         /* Start with cache line aligned address */
 254         ldr     ip, .Lsheeva_cache_line_size
 255         ldr     ip, [ip]
 256         sub     ip, ip, #1
 257         and     r2, r0, ip
 258         add     r1, r1, r2
 259         add     r1, r1, ip
 260         bics    r1, r1, ip
 261         bics    r0, r0, ip
 262
 263         ldr     ip, .Lsheeva_asm_page_mask
 264         and     r2, r0, ip
 265         rsb     r2, r2, #PAGE_SIZE
 266         cmp     r1, r2
 267         movcc   ip, r1
 268         movcs   ip, r2
 269 1:
 270         add     r3, r0, ip
 271         sub     r2, r3, #1
 272         /* Disable irqs */
 273         orr     r3, lr, #PSR_I | PSR_F
 274         msr     cpsr_c, r3
 275         mcr     p15, 1, r0, c15, c9, 4  /* Clean L2 zone start address */
 276         mcr     p15, 1, r2, c15, c9, 5  /* Clean L2 zone end address */
 277         mcr     p15, 1, r0, c15, c11, 4 /* Inv L2 zone start address */
 278         mcr     p15, 1, r2, c15, c11, 5 /* Inv L2 zone end address */
 279         /* Enable irqs */
 280         msr     cpsr_c, lr
 281
 282         add     r0, r0, ip
 283         sub     r1, r1, ip
 284         cmp     r1, #PAGE_SIZE
 285         movcc   ip, r1
 286         movcs   ip, #PAGE_SIZE
 287         cmp     r1, #0
 288         bne     1b
 289         mov     r0, #0
 290         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 291         ldr     lr, [sp], #4
 292         RET
 293 END(sheeva_l2cache_wbinv_range)
 294
 295 ENTRY(sheeva_l2cache_inv_range)
 296         str     lr, [sp, #-4]!
 297         mrs     lr, cpsr
 298         /* Start with cache line aligned address */
 299         ldr     ip, .Lsheeva_cache_line_size
 300         ldr     ip, [ip]
 301         sub     ip, ip, #1
 302         and     r2, r0, ip
 303         add     r1, r1, r2
 304         add     r1, r1, ip
 305         bics    r1, r1, ip
 306         bics    r0, r0, ip
 307
 308         ldr     ip, .Lsheeva_asm_page_mask
 309         and     r2, r0, ip
 310         rsb     r2, r2, #PAGE_SIZE
 311         cmp     r1, r2
 312         movcc   ip, r1
 313         movcs   ip, r2
 314 1:
 315         add     r3, r0, ip
 316         sub     r2, r3, #1
 317         /* Disable irqs */
 318         orr     r3, lr, #PSR_I | PSR_F
 319         msr     cpsr_c, r3
 320         mcr     p15, 1, r0, c15, c11, 4 /* Inv L2 zone start address */
 321         mcr     p15, 1, r2, c15, c11, 5 /* Inv L2 zone end address */
 322         /* Enable irqs */
 323         msr     cpsr_c, lr
 324
 325         add     r0, r0, ip
 326         sub     r1, r1, ip
 327         cmp     r1, #PAGE_SIZE
 328         movcc   ip, r1
 329         movcs   ip, #PAGE_SIZE
 330         cmp     r1, #0
 331         bne     1b
 332         mov     r0, #0
 333         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 334         ldr     lr, [sp], #4
 335         RET
 336 END(sheeva_l2cache_inv_range)
 337
 338 ENTRY(sheeva_l2cache_wb_range)
 339         str     lr, [sp, #-4]!
 340         mrs     lr, cpsr
 341         /* Start with cache line aligned address */
 342         ldr     ip, .Lsheeva_cache_line_size
 343         ldr     ip, [ip]
 344         sub     ip, ip, #1
 345         and     r2, r0, ip
 346         add     r1, r1, r2
 347         add     r1, r1, ip
 348         bics    r1, r1, ip
 349         bics    r0, r0, ip
 350
 351         ldr     ip, .Lsheeva_asm_page_mask
 352         and     r2, r0, ip
 353         rsb     r2, r2, #PAGE_SIZE
 354         cmp     r1, r2
 355         movcc   ip, r1
 356         movcs   ip, r2
 357 1:
 358         add     r3, r0, ip
 359         sub     r2, r3, #1
 360         /* Disable irqs */
 361         orr     r3, lr, #PSR_I | PSR_F
 362         msr     cpsr_c, r3
 363         mcr     p15, 1, r0, c15, c9, 4  /* Clean L2 zone start address */
 364         mcr     p15, 1, r2, c15, c9, 5  /* Clean L2 zone end address */
 365         /* Enable irqs */
 366         msr     cpsr_c, lr
 367
 368         add     r0, r0, ip
 369         sub     r1, r1, ip
 370         cmp     r1, #PAGE_SIZE
 371         movcc   ip, r1
 372         movcs   ip, #PAGE_SIZE
 373         cmp     r1, #0
 374         bne     1b
 375         mov     r0, #0
 376         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 377         ldr     lr, [sp], #4
 378         RET
 379 END(sheeva_l2cache_wb_range)
 380 #endif /* !ELF_TRAMPOLINE */
 381
 382 ENTRY(sheeva_l2cache_wbinv_all)
 383         /* Disable irqs */
 384         mrs     r1, cpsr
 385         orr     r2, r1, #PSR_I | PSR_F
 386         msr     cpsr_c, r2
 387
 388         mov     r0, #0
 389         mcr     p15, 1, r0, c15, c9, 0  /* Clean L2 */
 390         mcr     p15, 1, r0, c15, c11, 0 /* Invalidate L2 */
 391
 392         msr     cpsr_c, r1              /* Reenable irqs */
 393
 394         mcr     p15, 0, r0, c7, c10, 4  /* drain the write buffer */
 395         RET
 396 END(sheeva_l2cache_wbinv_all)
 397
 398 #ifndef ELF_TRAMPOLINE
 399 /* This function modifies register value as follows:
 400  *
 401  * arg1  arg            EFFECT (bit value saved into register)
 402  *    0     0           not changed
 403  *    0     1           negated
 404  *    1     0           cleared
 405  *    1     1           set
 406  */
 407 ENTRY(sheeva_control_ext)
 408         mrc     p15, 1, r3, c15, c1, 0  /* Read the control register */
 409         bic     r2, r3, r0              /* Clear bits */
 410         eor     r2, r2, r1              /* XOR bits */
 411
 412         teq     r2, r3                  /* Only write if there is a change */
 413         mcrne   p15, 1, r2, c15, c1, 0  /* Write new control register */
 414         mov     r0, r3                  /* Return old value */
 415         RET
 416 END(sheeva_control_ext)
 417
 418 ENTRY(sheeva_cpu_sleep)
 419         mov     r0, #0
 420         mcr     p15, 0, r0, c7, c10, 4  /* Drain write buffer */
 421         mcr     p15, 0, r0, c7, c0, 4   /* Wait for interrupt */
 422         mov     pc, lr
 423 END(sheeva_cpu_sleep)
 424 #endif /* !ELF_TRAMPOLINE */