xref: /openbmc/linux/arch/arm/kernel/sleep.S (revision e23feb16)
1#include <linux/linkage.h>
2#include <linux/threads.h>
3#include <asm/asm-offsets.h>
4#include <asm/assembler.h>
5#include <asm/glue-cache.h>
6#include <asm/glue-proc.h>
7	.text
8
9/*
10 * Implementation of MPIDR hash algorithm through shifting
11 * and OR'ing.
12 *
13 * @dst: register containing hash result
14 * @rs0: register containing affinity level 0 bit shift
15 * @rs1: register containing affinity level 1 bit shift
16 * @rs2: register containing affinity level 2 bit shift
17 * @mpidr: register containing MPIDR value
18 * @mask: register containing MPIDR mask
19 *
20 * Pseudo C-code:
21 *
22 *u32 dst;
23 *
24 *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 mpidr, u32 mask) {
25 *	u32 aff0, aff1, aff2;
26 *	u32 mpidr_masked = mpidr & mask;
27 *	aff0 = mpidr_masked & 0xff;
28 *	aff1 = mpidr_masked & 0xff00;
29 *	aff2 = mpidr_masked & 0xff0000;
30 *	dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2);
31 *}
32 * Input registers: rs0, rs1, rs2, mpidr, mask
33 * Output register: dst
34 * Note: input and output registers must be disjoint register sets
35         (eg: a macro instance with mpidr = r1 and dst = r1 is invalid)
36 */
37	.macro compute_mpidr_hash dst, rs0, rs1, rs2, mpidr, mask
38	and	\mpidr, \mpidr, \mask			@ mask out MPIDR bits
39	and	\dst, \mpidr, #0xff			@ mask=aff0
40 ARM(	mov	\dst, \dst, lsr \rs0		)	@ dst=aff0>>rs0
41 THUMB(	lsr	\dst, \dst, \rs0		)
42	and	\mask, \mpidr, #0xff00			@ mask = aff1
43 ARM(	orr	\dst, \dst, \mask, lsr \rs1	)	@ dst|=(aff1>>rs1)
44 THUMB(	lsr	\mask, \mask, \rs1		)
45 THUMB(	orr	\dst, \dst, \mask		)
46	and	\mask, \mpidr, #0xff0000		@ mask = aff2
47 ARM(	orr	\dst, \dst, \mask, lsr \rs2	)	@ dst|=(aff2>>rs2)
48 THUMB(	lsr	\mask, \mask, \rs2		)
49 THUMB(	orr	\dst, \dst, \mask		)
50	.endm
51
52/*
53 * Save CPU state for a suspend.  This saves the CPU general purpose
54 * registers, and allocates space on the kernel stack to save the CPU
55 * specific registers and some other data for resume.
56 *  r0 = suspend function arg0
57 *  r1 = suspend function
58 */
59ENTRY(__cpu_suspend)
60	stmfd	sp!, {r4 - r11, lr}
61#ifdef MULTI_CPU
62	ldr	r10, =processor
63	ldr	r4, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
64#else
65	ldr	r4, =cpu_suspend_size
66#endif
67	mov	r5, sp			@ current virtual SP
68	add	r4, r4, #12		@ Space for pgd, virt sp, phys resume fn
69	sub	sp, sp, r4		@ allocate CPU state on stack
70	stmfd	sp!, {r0, r1}		@ save suspend func arg and pointer
71	add	r0, sp, #8		@ save pointer to save block
72	mov	r1, r4			@ size of save block
73	mov	r2, r5			@ virtual SP
74	ldr	r3, =sleep_save_sp
75	ldr	r3, [r3, #SLEEP_SAVE_SP_VIRT]
76	ALT_SMP(mrc p15, 0, r9, c0, c0, 5)
77        ALT_UP_B(1f)
78	ldr	r8, =mpidr_hash
79	/*
80	 * This ldmia relies on the memory layout of the mpidr_hash
81	 * struct mpidr_hash.
82	 */
83	ldmia	r8, {r4-r7}	@ r4 = mpidr mask (r5,r6,r7) = l[0,1,2] shifts
84	compute_mpidr_hash	lr, r5, r6, r7, r9, r4
85	add	r3, r3, lr, lsl #2
861:
87	bl	__cpu_suspend_save
88	adr	lr, BSYM(cpu_suspend_abort)
89	ldmfd	sp!, {r0, pc}		@ call suspend fn
90ENDPROC(__cpu_suspend)
91	.ltorg
92
93cpu_suspend_abort:
94	ldmia	sp!, {r1 - r3}		@ pop phys pgd, virt SP, phys resume fn
95	teq	r0, #0
96	moveq	r0, #1			@ force non-zero value
97	mov	sp, r2
98	ldmfd	sp!, {r4 - r11, pc}
99ENDPROC(cpu_suspend_abort)
100
101/*
102 * r0 = control register value
103 */
104	.align	5
105	.pushsection	.idmap.text,"ax"
106ENTRY(cpu_resume_mmu)
107	ldr	r3, =cpu_resume_after_mmu
108	instr_sync
109	mcr	p15, 0, r0, c1, c0, 0	@ turn on MMU, I-cache, etc
110	mrc	p15, 0, r0, c0, c0, 0	@ read id reg
111	instr_sync
112	mov	r0, r0
113	mov	r0, r0
114	mov	pc, r3			@ jump to virtual address
115ENDPROC(cpu_resume_mmu)
116	.popsection
117cpu_resume_after_mmu:
118	bl	cpu_init		@ restore the und/abt/irq banked regs
119	mov	r0, #0			@ return zero on success
120	ldmfd	sp!, {r4 - r11, pc}
121ENDPROC(cpu_resume_after_mmu)
122
123/*
124 * Note: Yes, part of the following code is located into the .data section.
125 *       This is to allow sleep_save_sp to be accessed with a relative load
126 *       while we can't rely on any MMU translation.  We could have put
127 *       sleep_save_sp in the .text section as well, but some setups might
128 *       insist on it to be truly read-only.
129 */
130	.data
131	.align
132ENTRY(cpu_resume)
133	mov	r1, #0
134	ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
135	ALT_UP_B(1f)
136	adr	r2, mpidr_hash_ptr
137	ldr	r3, [r2]
138	add	r2, r2, r3		@ r2 = struct mpidr_hash phys address
139	/*
140	 * This ldmia relies on the memory layout of the mpidr_hash
141	 * struct mpidr_hash.
142	 */
143	ldmia	r2, { r3-r6 }	@ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
144	compute_mpidr_hash	r1, r4, r5, r6, r0, r3
1451:
146	adr	r0, _sleep_save_sp
147	ldr	r0, [r0, #SLEEP_SAVE_SP_PHYS]
148	ldr	r0, [r0, r1, lsl #2]
149
150	setmode	PSR_I_BIT | PSR_F_BIT | SVC_MODE, r1  @ set SVC, irqs off
151	@ load phys pgd, stack, resume fn
152  ARM(	ldmia	r0!, {r1, sp, pc}	)
153THUMB(	ldmia	r0!, {r1, r2, r3}	)
154THUMB(	mov	sp, r2			)
155THUMB(	bx	r3			)
156ENDPROC(cpu_resume)
157
158	.align 2
159mpidr_hash_ptr:
160	.long	mpidr_hash - .			@ mpidr_hash struct offset
161
162	.type	sleep_save_sp, #object
163ENTRY(sleep_save_sp)
164_sleep_save_sp:
165	.space	SLEEP_SAVE_SP_SZ		@ struct sleep_save_sp
166