1/* 2 * Copyright 2015 Freescale Semiconductor, Inc. 3 * Author: Wang Dongsheng <dongsheng.wang@freescale.com> 4 * 5 * SPDX-License-Identifier: GPL-2.0+ 6 */ 7 8#include <config.h> 9#include <linux/linkage.h> 10 11#include <asm/armv7.h> 12#include <asm/arch-armv7/generictimer.h> 13#include <asm/psci.h> 14 15#define SCFG_CORE0_SFT_RST 0x130 16#define SCFG_CORESRENCR 0x204 17 18#define DCFG_CCSR_BRR 0x0E4 19#define DCFG_CCSR_SCRATCHRW1 0x200 20 21 .pushsection ._secure.text, "ax" 22 23 .arch_extension sec 24 25#define ONE_MS (GENERIC_TIMER_CLK / 1000) 26#define RESET_WAIT (30 * ONE_MS) 27 28 @ r1 = target CPU 29 @ r2 = target PC 30.globl psci_cpu_on 31psci_cpu_on: 32 push {lr} 33 34 @ Clear and Get the correct CPU number 35 @ r1 = 0xf01 36 and r1, r1, #0xff 37 38 mov r0, r1 39 bl psci_get_cpu_stack_top 40 str r2, [r0] 41 dsb 42 43 @ Get DCFG base address 44 movw r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff) 45 movt r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16) 46 47 @ Detect target CPU state 48 ldr r2, [r4, #DCFG_CCSR_BRR] 49 rev r2, r2 50 lsr r2, r2, r1 51 ands r2, r2, #1 52 beq holdoff_release 53 54 @ Reset target CPU 55 @ Get SCFG base address 56 movw r0, #(CONFIG_SYS_FSL_SCFG_ADDR & 0xffff) 57 movt r0, #(CONFIG_SYS_FSL_SCFG_ADDR >> 16) 58 59 @ Enable CORE Soft Reset 60 movw r5, #0 61 movt r5, #(1 << 15) 62 rev r5, r5 63 str r5, [r0, #SCFG_CORESRENCR] 64 65 @ Get CPUx offset register 66 mov r6, #0x4 67 mul r6, r6, r1 68 add r2, r0, r6 69 70 @ Do reset on target CPU 71 movw r5, #0 72 movt r5, #(1 << 15) 73 rev r5, r5 74 str r5, [r2, #SCFG_CORE0_SFT_RST] 75 76 @ Wait target CPU up 77 timer_wait r2, RESET_WAIT 78 79 @ Disable CORE soft reset 80 mov r5, #0 81 str r5, [r0, #SCFG_CORESRENCR] 82 83holdoff_release: 84 @ Release on target CPU 85 ldr r2, [r4, #DCFG_CCSR_BRR] 86 mov r6, #1 87 lsl r6, r6, r1 @ 32 bytes per CPU 88 89 rev r6, r6 90 orr r2, r2, r6 91 str r2, [r4, #DCFG_CCSR_BRR] 92 93 @ Set secondary boot entry 94 ldr r6, =psci_cpu_entry 95 rev r6, r6 96 str r6, [r4, #DCFG_CCSR_SCRATCHRW1] 97 98 isb 99 dsb 100 101 @ Return 102 mov r0, #ARM_PSCI_RET_SUCCESS 103 104 pop {lr} 105 bx lr 106 107.globl psci_cpu_off 108psci_cpu_off: 109 bl psci_cpu_off_common 110 1111: wfi 112 b 1b 113 114.globl psci_arch_init 115psci_arch_init: 116 mov r6, lr 117 118 bl psci_get_cpu_id 119 bl psci_get_cpu_stack_top 120 mov sp, r0 121 122 bx r6 123 124 .globl psci_text_end 125psci_text_end: 126 .popsection 127