arch/arm/mach-prima2/headsmp.S

   1 /*
   2  * Entry of the second core for CSR Marco dual-core SMP SoCs
   3  *
   4  * Copyright (c) 2012 Cambridge Silicon Radio Limited, a CSR plc group company.
   5  *
   6  * Licensed under GPLv2 or later.
   7  */
   8
   9 #include <linux/linkage.h>
  10 #include <linux/init.h>
  11
  12         __CPUINIT
  13
  14 /*
  15  * Cold boot and hardware reset show different behaviour,
  16  * system will be always panic if we warm-reset the board
  17  * Here we invalidate L1 of CPU1 to make sure there isn't
  18  * uninitialized data written into memory later
  19  */
  20 ENTRY(v7_invalidate_l1)
  21         mov     r0, #0
  22         mcr     p15, 0, r0, c7, c5, 0   @ invalidate I cache
  23         mcr     p15, 2, r0, c0, c0, 0
  24         mrc     p15, 1, r0, c0, c0, 0
  25
  26         ldr     r1, =0x7fff
  27         and     r2, r1, r0, lsr #13
  28
  29         ldr     r1, =0x3ff
  30
  31         and     r3, r1, r0, lsr #3      @ NumWays - 1
  32         add     r2, r2, #1              @ NumSets
  33
  34         and     r0, r0, #0x7
  35         add     r0, r0, #4      @ SetShift
  36
  37         clz     r1, r3          @ WayShift
  38         add     r4, r3, #1      @ NumWays
  39 1:      sub     r2, r2, #1      @ NumSets--
  40         mov     r3, r4          @ Temp = NumWays
  41 2:      subs    r3, r3, #1      @ Temp--
  42         mov     r5, r3, lsl r1
  43         mov     r6, r2, lsl r0
  44         orr     r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
  45         mcr     p15, 0, r5, c7, c6, 2
  46         bgt     2b
  47         cmp     r2, #0
  48         bgt     1b
  49         dsb
  50         isb
  51         mov     pc, lr
  52 ENDPROC(v7_invalidate_l1)
  53
  54 /*
  55  * SIRFSOC specific entry point for secondary CPUs.  This provides
  56  * a "holding pen" into which all secondary cores are held until we're
  57  * ready for them to initialise.
  58  */
  59 ENTRY(sirfsoc_secondary_startup)
  60         bl v7_invalidate_l1
  61         mrc     p15, 0, r0, c0, c0, 5
  62         and     r0, r0, #15
  63         adr     r4, 1f
  64         ldmia   r4, {r5, r6}
  65         sub     r4, r4, r5
  66         add     r6, r6, r4
  67 pen:    ldr     r7, [r6]
  68         cmp     r7, r0
  69         bne     pen
  70
  71         /*
  72          * we've been released from the holding pen: secondary_stack
  73          * should now contain the SVC stack for this core
  74          */
  75         b       secondary_startup
  76 ENDPROC(sirfsoc_secondary_startup)
  77
  78         .align
  79 1:      .long   .
  80         .long   pen_release