arch/arm/mach-actions/headsmp.S

   1 /*
   2  * Copyright 2012 Actions Semi Inc.
   3  * Author: Actions Semi, Inc.
   4  *
   5  * This program is free software; you can redistribute  it and/or modify it
   6  * under  the terms of  the GNU General  Public License as published by the
   7  * Free Software Foundation;  either version 2 of the  License, or (at your
   8  * option) any later version.
   9  */
  10
  11 #include <linux/linkage.h>
  12 #include <linux/init.h>
  13
  14 ENTRY(owl_v7_invalidate_l1)
  15         mov     r0, #0
  16         mcr     p15, 0, r0, c7, c5, 0   @ invalidate I cache
  17         mcr     p15, 2, r0, c0, c0, 0
  18         mrc     p15, 1, r0, c0, c0, 0
  19
  20         ldr     r1, =0x7fff
  21         and     r2, r1, r0, lsr #13
  22
  23         ldr     r1, =0x3ff
  24
  25         and     r3, r1, r0, lsr #3      @ NumWays - 1
  26         add     r2, r2, #1              @ NumSets
  27
  28         and     r0, r0, #0x7
  29         add     r0, r0, #4      @ SetShift
  30
  31         clz     r1, r3          @ WayShift
  32         add     r4, r3, #1      @ NumWays
  33 1:      sub     r2, r2, #1      @ NumSets--
  34         mov     r3, r4          @ Temp = NumWays
  35 2:      subs    r3, r3, #1      @ Temp--
  36         mov     r5, r3, lsl r1
  37         mov     r6, r2, lsl r0
  38         orr     r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
  39         mcr     p15, 0, r5, c7, c6, 2
  40         bgt     2b
  41         cmp     r2, #0
  42         bgt     1b
  43         dsb
  44         isb
  45         mov     pc, lr
  46 ENDPROC(owl_v7_invalidate_l1)
  47
  48 ENTRY(owl_secondary_startup)
  49         mrc     p15, 0, r0, c0, c0, 5
  50         and     r0, r0, #0xf
  51         adr     r4, 1f
  52         ldmia   r4, {r5, r6}
  53         sub     r4, r4, r5
  54         add     r6, r6, r4
  55 pen:
  56         ldr     r7, [r6]
  57         cmp     r7, r0
  58         bne     pen
  59
  60         /*
  61          * we've been released from the holding pen: secondary_stack
  62          * should now contain the SVC stack for this core
  63          */
  64         bl      owl_v7_invalidate_l1
  65         b       secondary_startup
  66
  67 1:      .long   .
  68         .long   pen_release