kernel/fiasco/src/kern/arm/tramp-mp.S

   1 #include "globalconfig.h"
   2 #include "tcboffset.h"
   3
   4         .section .mp_tramp, "awx"
   5         .p2align 12
   6
   7 #ifdef CONFIG_ARM_V7
   8 /* See cache_func_gen.cpp */
   9         .global v7_invalidate_l1
  10 invalidate_l1_v7:
  11         mov     r3, #0
  12         mcr     p15, 2, r3, c0, c0, 0
  13         mrc     p15, 1, r2, c0, c0, 0
  14         mov     r3, r2, lsr #3
  15         mov     r0, r2, lsr #13
  16         mov     r3, r3, asl #22
  17         mov     r3, r3, lsr #22
  18         and     r2, r2, #7
  19         mov     r0, r0, asl #17
  20         add     ip, r2, #4
  21         mov     r0, r0, lsr #17
  22         mov     r2, r3
  23         clz     r3, r3
  24 .L3:
  25         mov     r4, r2, asl r3
  26         mov     r1, r0
  27 .L2:
  28         orr     r5, r4, r1, asl ip
  29         mcr     p15, 0, r5, c7, c6, 2
  30         subs    r1, r1, #1
  31         bcs     .L2
  32         subs    r2, r2, #1
  33         bcs     .L3
  34         isb
  35         dsb
  36         mov     pc, lr
  37 #endif
  38
  39         .global _tramp_mp_entry
  40 _tramp_mp_entry:
  41         // IRQs off, SVC
  42         mrs r0, cpsr
  43         orr r0, #0xd3
  44         msr cpsr_c, r0
  45
  46         // enable SMP
  47         mov r0, #0x1f000000
  48         ldr r1, [r0]
  49         orr r1, #1
  50         str r1, [r0]
  51
  52 #ifdef CONFIG_ARM_V7
  53         bl invalidate_l1_v7
  54 #endif
  55
  56         mcr p15, 0, r0, c7, c10, 4 // dsb
  57 #ifdef CONFIG_ARM_V6
  58         mcr p15, 0, r0, c7, c7, 0  // inv both
  59 #endif
  60
  61         mrc p15, 0, r0, c1, c0, 1
  62 #ifdef CONFIG_ARM_V7
  63         orr r0, r0, #0x41
  64 #else
  65         orr r0, r0, #0x20
  66 #endif
  67         mcr p15, 0, r0, c1, c0, 1
  68
  69         // TLB flush
  70         mcr p15, 0, r0, c8, c7, 0
  71
  72         adr r0, _tramp_mp_start_dcr
  73         ldr r0, [r0]
  74         mcr p15, 0, r0, c3, c0
  75
  76         adr r0, _tramp_mp_startup_pdbr
  77         ldr r0, [r0]
  78         mcr p15, 0, r0, c2, c0
  79
  80         adr r0, _tramp_mp_startup_cp15_c1
  81         ldr r0, [r0]
  82         mcr p15, 0, r0, c1, c0
  83
  84         // barrier
  85         mrc p15, 0, r0, c2, c0, 0
  86         mov r0, r0
  87         ldr r0, 1f
  88         mov pc, r0
  89 1:
  90         .long _tramp_mp_virt
  91
  92         // we run paged now
  93 _tramp_mp_virt:
  94         ldr     r0, _cpu_counter_address
  95 1:      ldrex   r3, [r0]
  96         add     r3, r3, #1
  97         strex   r2, r3, [r0]
  98         teq     r2, #0
  99         bne     1b
 100
 101         // spinlock on cpu-init
 102         adr     r0, _tramp_mp_spinlock
 103 1:      ldr     r1, [r0]
 104         teq     r1, #0
 105         wfene
 106         bne     1b
 107
 108 2:      ldrex   r1, [r0]
 109         teq     r1, #0
 110         bne     1b
 111         mov     r1, #2
 112         strex   r2, r1, [r0]
 113         teq     r2, #0
 114         bne     1b
 115
 116         // TLB flush
 117         mcr p15, 0, r0, c8, c7, 0
 118
 119         adr     sp, _tramp_mp_init_stack_top
 120         mov     r0, r3
 121         nop
 122         ldr     pc, [pc, #-4]
 123
 124         .long BOOT_AP_CPU
 125
 126 _cpu_counter_address:
 127         .long (my_kernel_info_page + OFS__KIP__PROCESSOR_INFO)
 128
 129 .global _tramp_mp_startup_cp15_c1
 130 _tramp_mp_startup_cp15_c1:
 131         .long 0x00000000
 132
 133 .global _tramp_mp_startup_pdbr
 134 _tramp_mp_startup_pdbr:
 135         .long 0x00000000
 136
 137 .global _tramp_mp_start_dcr
 138 _tramp_mp_start_dcr:
 139         .long 0x00000000
 140
 141 .global _tramp_mp_spinlock
 142 _tramp_mp_spinlock:
 143         .long 0
 144
 145 _tramp_mp_init_stack:
 146         .space 1024
 147 _tramp_mp_init_stack_top:
 148