summaryrefslogtreecommitdiff
path: root/arch/arm/cpu/armv7/sunxi/psci.S
blob: 0084c811f32e6f71c6b65b5c2a10ca0747302c24 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
/*
 * Copyright (C) 2013 - ARM Ltd
 * Author: Marc Zyngier <marc.zyngier@arm.com>
 *
 * Based on code by Carl van Schaik <carl@ok-labs.com>.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <config.h>
#include <asm/psci.h>
#include <asm/arch/cpu.h>

/*
 * Memory layout:
 *
 * SECURE_RAM to text_end :
 *	._secure_text section
 * text_end to ALIGN_PAGE(text_end):
 *	nothing
 * ALIGN_PAGE(text_end) to ALIGN_PAGE(text_end) + 0x1000)
 *	1kB of stack per CPU (4 CPUs max).
 */

	.pushsection ._secure.text, "ax"

	.arch_extension sec

#define	ONE_MS			(CONFIG_SYS_CLK_FREQ / 1000)
#define	TEN_MS			(10 * ONE_MS)

.macro	timer_wait	reg, ticks
	@ Program CNTP_TVAL
	movw	\reg, #(\ticks & 0xffff)
	movt	\reg, #(\ticks >> 16)
	mcr	p15, 0, \reg, c14, c2, 0
	isb
	@ Enable physical timer, mask interrupt
	mov	\reg, #3
	mcr	p15, 0, \reg, c14, c2, 1
	@ Poll physical timer until ISTATUS is on
1:	isb
	mrc	p15, 0, \reg, c14, c2, 1
	ands	\reg, \reg, #4
	bne	1b
	@ Disable timer
	mov	\reg, #0
	mcr	p15, 0, \reg, c14, c2, 1
	isb
.endm

.globl	psci_arch_init
psci_arch_init:
	mrc	p15, 0, r5, c1, c1, 0	@ Read SCR
	bic	r5, r5, #1		@ Secure mode
	mcr	p15, 0, r5, c1, c1, 0	@ Write SCR
	isb

	mrc	p15, 0, r4, c0, c0, 5	@ MPIDR
	and	r4, r4, #3		@ cpu number in cluster
	mov	r5, #400		@ 1kB of stack per CPU
	mul	r4, r4, r5

	adr	r5, text_end		@ end of text
	add	r5, r5, #0x2000		@ Skip two pages
	lsr	r5, r5, #12		@ Align to start of page
	lsl	r5, r5, #12
	sub	sp, r5, r4		@ here's our stack!

	bx	lr

	@ r1 = target CPU
	@ r2 = target PC
.globl	psci_cpu_on
psci_cpu_on:
	adr	r0, _target_pc
	str	r2, [r0]
	dsb

	movw	r0, #(SUNXI_CPUCFG_BASE & 0xffff)
	movt	r0, #(SUNXI_CPUCFG_BASE >> 16)

	@ CPU mask
	and	r1, r1, #3	@ only care about first cluster
	mov	r4, #1
	lsl	r4, r4, r1

	adr	r6, _sunxi_cpu_entry
	str	r6, [r0, #0x1a4] @ PRIVATE_REG (boot vector)

	@ Assert reset on target CPU
	mov	r6, #0
	lsl	r5, r1, #6	@ 64 bytes per CPU
	add	r5, r5, #0x40	@ Offset from base
	add	r5, r5, r0	@ CPU control block
	str	r6, [r5]	@ Reset CPU

	@ l1 invalidate
	ldr	r6, [r0, #0x184]
	bic	r6, r6, r4
	str	r6, [r0, #0x184]

	@ Lock CPU
	ldr	r6, [r0, #0x1e4]
	bic	r6, r6, r4
	str	r6, [r0, #0x1e4]

	@ Release power clamp
	movw	r6, #0x1ff
	movt	r6, #0
1:	lsrs	r6, r6, #1
	str	r6, [r0, #0x1b0]
	bne	1b

	timer_wait r1, TEN_MS

	@ Clear power gating
	ldr	r6, [r0, #0x1b4]
	bic	r6, r6, #1
	str	r6, [r0, #0x1b4]

	@ Deassert reset on target CPU
	mov	r6, #3
	str	r6, [r5]

	@ Unlock CPU
	ldr	r6, [r0, #0x1e4]
	orr	r6, r6, r4
	str	r6, [r0, #0x1e4]

	mov	r0, #ARM_PSCI_RET_SUCCESS	@ Return PSCI_RET_SUCCESS
	mov	pc, lr

_target_pc:
	.word	0

_sunxi_cpu_entry:
	@ Set SMP bit
	mrc	p15, 0, r0, c1, c0, 1
	orr	r0, r0, #0x40
	mcr	p15, 0, r0, c1, c0, 1
	isb

	bl	_nonsec_init
	bl	psci_arch_init

	adr	r0, _target_pc
	ldr	r0, [r0]
	b	_do_nonsec_entry

text_end:
	.popsection