Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2013 - ARM Ltd |
| 3 | * Author: Marc Zyngier <marc.zyngier@arm.com> |
| 4 | * |
| 5 | * Based on code by Carl van Schaik <carl@ok-labs.com>. |
| 6 | * |
| 7 | * This program is free software; you can redistribute it and/or modify |
| 8 | * it under the terms of the GNU General Public License version 2 as |
| 9 | * published by the Free Software Foundation. |
| 10 | * |
| 11 | * This program is distributed in the hope that it will be useful, |
| 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 14 | * GNU General Public License for more details. |
| 15 | * |
| 16 | * You should have received a copy of the GNU General Public License |
| 17 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 18 | */ |
| 19 | |
| 20 | #include <config.h> |
Jan Kiszka | fdb0ff7 | 2014-12-31 13:46:40 +0100 | [diff] [blame] | 21 | #include <asm/gic.h> |
Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 22 | #include <asm/psci.h> |
| 23 | #include <asm/arch/cpu.h> |
| 24 | |
| 25 | /* |
| 26 | * Memory layout: |
| 27 | * |
| 28 | * SECURE_RAM to text_end : |
| 29 | * ._secure_text section |
| 30 | * text_end to ALIGN_PAGE(text_end): |
| 31 | * nothing |
| 32 | * ALIGN_PAGE(text_end) to ALIGN_PAGE(text_end) + 0x1000) |
| 33 | * 1kB of stack per CPU (4 CPUs max). |
| 34 | */ |
| 35 | |
| 36 | .pushsection ._secure.text, "ax" |
| 37 | |
| 38 | .arch_extension sec |
| 39 | |
Iain Paton | 630df14 | 2015-03-28 10:26:38 +0000 | [diff] [blame] | 40 | #define ONE_MS (CONFIG_TIMER_CLK_FREQ / 1000) |
Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 41 | #define TEN_MS (10 * ONE_MS) |
Jan Kiszka | fdb0ff7 | 2014-12-31 13:46:40 +0100 | [diff] [blame] | 42 | #define GICD_BASE 0x1c81000 |
| 43 | #define GICC_BASE 0x1c82000 |
Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 44 | |
| 45 | .macro timer_wait reg, ticks |
| 46 | @ Program CNTP_TVAL |
| 47 | movw \reg, #(\ticks & 0xffff) |
| 48 | movt \reg, #(\ticks >> 16) |
| 49 | mcr p15, 0, \reg, c14, c2, 0 |
| 50 | isb |
| 51 | @ Enable physical timer, mask interrupt |
| 52 | mov \reg, #3 |
| 53 | mcr p15, 0, \reg, c14, c2, 1 |
| 54 | @ Poll physical timer until ISTATUS is on |
| 55 | 1: isb |
| 56 | mrc p15, 0, \reg, c14, c2, 1 |
| 57 | ands \reg, \reg, #4 |
| 58 | bne 1b |
| 59 | @ Disable timer |
| 60 | mov \reg, #0 |
| 61 | mcr p15, 0, \reg, c14, c2, 1 |
| 62 | isb |
| 63 | .endm |
| 64 | |
Jan Kiszka | fdb0ff7 | 2014-12-31 13:46:40 +0100 | [diff] [blame] | 65 | .globl psci_fiq_enter |
| 66 | psci_fiq_enter: |
| 67 | push {r0-r12} |
| 68 | |
| 69 | @ Switch to secure |
| 70 | mrc p15, 0, r7, c1, c1, 0 |
| 71 | bic r8, r7, #1 |
| 72 | mcr p15, 0, r8, c1, c1, 0 |
| 73 | isb |
| 74 | |
| 75 | @ Validate reason based on IAR and acknowledge |
| 76 | movw r8, #(GICC_BASE & 0xffff) |
| 77 | movt r8, #(GICC_BASE >> 16) |
| 78 | ldr r9, [r8, #GICC_IAR] |
| 79 | movw r10, #0x3ff |
| 80 | movt r10, #0 |
| 81 | cmp r9, r10 @ skip spurious interrupt 1023 |
| 82 | beq out |
| 83 | movw r10, #0x3fe @ ...and 1022 |
| 84 | cmp r9, r10 |
| 85 | beq out |
| 86 | str r9, [r8, #GICC_EOIR] @ acknowledge the interrupt |
| 87 | dsb |
| 88 | |
| 89 | @ Compute CPU number |
| 90 | lsr r9, r9, #10 |
| 91 | and r9, r9, #0xf |
| 92 | |
| 93 | movw r8, #(SUN7I_CPUCFG_BASE & 0xffff) |
| 94 | movt r8, #(SUN7I_CPUCFG_BASE >> 16) |
| 95 | |
| 96 | @ Wait for the core to enter WFI |
| 97 | lsl r11, r9, #6 @ x64 |
| 98 | add r11, r11, r8 |
| 99 | |
| 100 | 1: ldr r10, [r11, #0x48] |
| 101 | tst r10, #(1 << 2) |
| 102 | bne 2f |
| 103 | timer_wait r10, ONE_MS |
| 104 | b 1b |
| 105 | |
| 106 | @ Reset CPU |
| 107 | 2: mov r10, #0 |
| 108 | str r10, [r11, #0x40] |
| 109 | |
| 110 | @ Lock CPU |
| 111 | mov r10, #1 |
| 112 | lsl r9, r10, r9 @ r9 is now CPU mask |
| 113 | ldr r10, [r8, #0x1e4] |
| 114 | bic r10, r10, r9 |
| 115 | str r10, [r8, #0x1e4] |
| 116 | |
| 117 | @ Set power gating |
| 118 | ldr r10, [r8, #0x1b4] |
| 119 | orr r10, r10, #1 |
| 120 | str r10, [r8, #0x1b4] |
| 121 | timer_wait r10, ONE_MS |
| 122 | |
| 123 | @ Activate power clamp |
| 124 | mov r10, #1 |
| 125 | 1: str r10, [r8, #0x1b0] |
| 126 | lsl r10, r10, #1 |
| 127 | orr r10, r10, #1 |
| 128 | tst r10, #0x100 |
| 129 | beq 1b |
| 130 | |
| 131 | @ Restore security level |
| 132 | out: mcr p15, 0, r7, c1, c1, 0 |
| 133 | |
| 134 | pop {r0-r12} |
| 135 | subs pc, lr, #4 |
| 136 | |
Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 137 | @ r1 = target CPU |
| 138 | @ r2 = target PC |
| 139 | .globl psci_cpu_on |
| 140 | psci_cpu_on: |
| 141 | adr r0, _target_pc |
| 142 | str r2, [r0] |
| 143 | dsb |
| 144 | |
Hans de Goede | 316e29b | 2014-10-27 23:59:27 +0100 | [diff] [blame] | 145 | movw r0, #(SUN7I_CPUCFG_BASE & 0xffff) |
| 146 | movt r0, #(SUN7I_CPUCFG_BASE >> 16) |
Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 147 | |
| 148 | @ CPU mask |
| 149 | and r1, r1, #3 @ only care about first cluster |
| 150 | mov r4, #1 |
| 151 | lsl r4, r4, r1 |
| 152 | |
| 153 | adr r6, _sunxi_cpu_entry |
| 154 | str r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector) |
| 155 | |
| 156 | @ Assert reset on target CPU |
| 157 | mov r6, #0 |
| 158 | lsl r5, r1, #6 @ 64 bytes per CPU |
| 159 | add r5, r5, #0x40 @ Offset from base |
| 160 | add r5, r5, r0 @ CPU control block |
| 161 | str r6, [r5] @ Reset CPU |
| 162 | |
| 163 | @ l1 invalidate |
| 164 | ldr r6, [r0, #0x184] |
| 165 | bic r6, r6, r4 |
| 166 | str r6, [r0, #0x184] |
| 167 | |
| 168 | @ Lock CPU |
| 169 | ldr r6, [r0, #0x1e4] |
| 170 | bic r6, r6, r4 |
| 171 | str r6, [r0, #0x1e4] |
| 172 | |
| 173 | @ Release power clamp |
| 174 | movw r6, #0x1ff |
| 175 | movt r6, #0 |
| 176 | 1: lsrs r6, r6, #1 |
| 177 | str r6, [r0, #0x1b0] |
| 178 | bne 1b |
| 179 | |
| 180 | timer_wait r1, TEN_MS |
| 181 | |
| 182 | @ Clear power gating |
| 183 | ldr r6, [r0, #0x1b4] |
| 184 | bic r6, r6, #1 |
| 185 | str r6, [r0, #0x1b4] |
| 186 | |
| 187 | @ Deassert reset on target CPU |
| 188 | mov r6, #3 |
| 189 | str r6, [r5] |
| 190 | |
| 191 | @ Unlock CPU |
| 192 | ldr r6, [r0, #0x1e4] |
| 193 | orr r6, r6, r4 |
| 194 | str r6, [r0, #0x1e4] |
| 195 | |
| 196 | mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS |
| 197 | mov pc, lr |
| 198 | |
| 199 | _target_pc: |
| 200 | .word 0 |
| 201 | |
Jan Kiszka | fdb0ff7 | 2014-12-31 13:46:40 +0100 | [diff] [blame] | 202 | /* Imported from Linux kernel */ |
| 203 | v7_flush_dcache_all: |
| 204 | dmb @ ensure ordering with previous memory accesses |
| 205 | mrc p15, 1, r0, c0, c0, 1 @ read clidr |
| 206 | ands r3, r0, #0x7000000 @ extract loc from clidr |
| 207 | mov r3, r3, lsr #23 @ left align loc bit field |
| 208 | beq finished @ if loc is 0, then no need to clean |
| 209 | mov r10, #0 @ start clean at cache level 0 |
| 210 | flush_levels: |
| 211 | add r2, r10, r10, lsr #1 @ work out 3x current cache level |
| 212 | mov r1, r0, lsr r2 @ extract cache type bits from clidr |
| 213 | and r1, r1, #7 @ mask of the bits for current cache only |
| 214 | cmp r1, #2 @ see what cache we have at this level |
| 215 | blt skip @ skip if no cache, or just i-cache |
| 216 | mrs r9, cpsr @ make cssr&csidr read atomic |
| 217 | mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr |
| 218 | isb @ isb to sych the new cssr&csidr |
| 219 | mrc p15, 1, r1, c0, c0, 0 @ read the new csidr |
| 220 | msr cpsr_c, r9 |
| 221 | and r2, r1, #7 @ extract the length of the cache lines |
| 222 | add r2, r2, #4 @ add 4 (line length offset) |
| 223 | ldr r4, =0x3ff |
| 224 | ands r4, r4, r1, lsr #3 @ find maximum number on the way size |
| 225 | clz r5, r4 @ find bit position of way size increment |
| 226 | ldr r7, =0x7fff |
| 227 | ands r7, r7, r1, lsr #13 @ extract max number of the index size |
| 228 | loop1: |
| 229 | mov r9, r7 @ create working copy of max index |
| 230 | loop2: |
| 231 | orr r11, r10, r4, lsl r5 @ factor way and cache number into r11 |
| 232 | orr r11, r11, r9, lsl r2 @ factor index number into r11 |
| 233 | mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way |
| 234 | subs r9, r9, #1 @ decrement the index |
| 235 | bge loop2 |
| 236 | subs r4, r4, #1 @ decrement the way |
| 237 | bge loop1 |
| 238 | skip: |
| 239 | add r10, r10, #2 @ increment cache number |
| 240 | cmp r3, r10 |
| 241 | bgt flush_levels |
| 242 | finished: |
| 243 | mov r10, #0 @ swith back to cache level 0 |
| 244 | mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr |
| 245 | dsb st |
| 246 | isb |
| 247 | bx lr |
| 248 | |
Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 249 | _sunxi_cpu_entry: |
| 250 | @ Set SMP bit |
| 251 | mrc p15, 0, r0, c1, c0, 1 |
| 252 | orr r0, r0, #0x40 |
| 253 | mcr p15, 0, r0, c1, c0, 1 |
| 254 | isb |
| 255 | |
| 256 | bl _nonsec_init |
| 257 | bl psci_arch_init |
| 258 | |
| 259 | adr r0, _target_pc |
| 260 | ldr r0, [r0] |
| 261 | b _do_nonsec_entry |
| 262 | |
Jan Kiszka | fdb0ff7 | 2014-12-31 13:46:40 +0100 | [diff] [blame] | 263 | .globl psci_cpu_off |
| 264 | psci_cpu_off: |
| 265 | mrc p15, 0, r0, c1, c0, 0 @ SCTLR |
| 266 | bic r0, r0, #(1 << 2) @ Clear C bit |
| 267 | mcr p15, 0, r0, c1, c0, 0 @ SCTLR |
| 268 | isb |
| 269 | dsb |
| 270 | |
| 271 | bl v7_flush_dcache_all |
| 272 | |
| 273 | clrex @ Why??? |
| 274 | |
| 275 | mrc p15, 0, r0, c1, c0, 1 @ ACTLR |
| 276 | bic r0, r0, #(1 << 6) @ Clear SMP bit |
| 277 | mcr p15, 0, r0, c1, c0, 1 @ ACTLR |
| 278 | isb |
| 279 | dsb |
| 280 | |
| 281 | @ Ask CPU0 to pull the rug... |
| 282 | movw r0, #(GICD_BASE & 0xffff) |
| 283 | movt r0, #(GICD_BASE >> 16) |
| 284 | movw r1, #15 @ SGI15 |
| 285 | movt r1, #1 @ Target is CPU0 |
| 286 | str r1, [r0, #GICD_SGIR] |
| 287 | dsb |
| 288 | |
| 289 | 1: wfi |
| 290 | b 1b |
| 291 | |
Jan Kiszka | f2f9a80 | 2014-12-31 13:46:42 +0100 | [diff] [blame] | 292 | .globl psci_arch_init |
| 293 | psci_arch_init: |
| 294 | movw r4, #(GICD_BASE & 0xffff) |
| 295 | movt r4, #(GICD_BASE >> 16) |
| 296 | |
| 297 | ldr r5, [r4, #GICD_IGROUPRn] |
| 298 | bic r5, r5, #(1 << 15) @ SGI15 as Group-0 |
| 299 | str r5, [r4, #GICD_IGROUPRn] |
| 300 | |
| 301 | mov r5, #0 @ Set SGI15 priority to 0 |
| 302 | strb r5, [r4, #(GICD_IPRIORITYRn + 15)] |
| 303 | |
| 304 | add r4, r4, #0x1000 @ GICC address |
| 305 | |
| 306 | mov r5, #0xff |
| 307 | str r5, [r4, #GICC_PMR] @ Be cool with non-secure |
| 308 | |
| 309 | ldr r5, [r4, #GICC_CTLR] |
| 310 | orr r5, r5, #(1 << 3) @ Switch FIQEn on |
| 311 | str r5, [r4, #GICC_CTLR] |
| 312 | |
| 313 | mrc p15, 0, r5, c1, c1, 0 @ Read SCR |
| 314 | orr r5, r5, #4 @ Enable FIQ in monitor mode |
| 315 | bic r5, r5, #1 @ Secure mode |
| 316 | mcr p15, 0, r5, c1, c1, 0 @ Write SCR |
| 317 | isb |
| 318 | |
| 319 | mrc p15, 0, r4, c0, c0, 5 @ MPIDR |
| 320 | and r4, r4, #3 @ cpu number in cluster |
| 321 | mov r5, #0x400 @ 1kB of stack per CPU |
| 322 | mul r4, r4, r5 |
| 323 | |
| 324 | adr r5, text_end @ end of text |
| 325 | add r5, r5, #0x2000 @ Skip two pages |
| 326 | lsr r5, r5, #12 @ Align to start of page |
| 327 | lsl r5, r5, #12 |
| 328 | sub sp, r5, r4 @ here's our stack! |
| 329 | |
| 330 | bx lr |
| 331 | |
Marc Zyngier | 0237c63 | 2014-07-18 21:06:38 +0100 | [diff] [blame] | 332 | text_end: |
| 333 | .popsection |