1/* SPDX-License-Identifier: GPL-2.0 */
2#include <linux/linkage.h>
3#include <linux/threads.h>
4#include <asm/asm-offsets.h>
5#include <asm/assembler.h>
6#include <asm/glue-cache.h>
7#include <asm/glue-proc.h>
8	.text
9
10/*
11 * Implementation of MPIDR hash algorithm through shifting
12 * and OR'ing.
13 *
14 * @dst: register containing hash result
15 * @rs0: register containing affinity level 0 bit shift
16 * @rs1: register containing affinity level 1 bit shift
17 * @rs2: register containing affinity level 2 bit shift
18 * @mpidr: register containing MPIDR value
19 * @mask: register containing MPIDR mask
20 *
21 * Pseudo C-code:
22 *
23 *u32 dst;
24 *
25 *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 mpidr, u32 mask) {
26 *	u32 aff0, aff1, aff2;
27 *	u32 mpidr_masked = mpidr & mask;
28 *	aff0 = mpidr_masked & 0xff;
29 *	aff1 = mpidr_masked & 0xff00;
30 *	aff2 = mpidr_masked & 0xff0000;
31 *	dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2);
32 *}
33 * Input registers: rs0, rs1, rs2, mpidr, mask
34 * Output register: dst
35 * Note: input and output registers must be disjoint register sets
36         (eg: a macro instance with mpidr = r1 and dst = r1 is invalid)
37 */
38	.macro compute_mpidr_hash dst, rs0, rs1, rs2, mpidr, mask
39	and	\mpidr, \mpidr, \mask			@ mask out MPIDR bits
40	and	\dst, \mpidr, #0xff			@ mask=aff0
41 ARM(	mov	\dst, \dst, lsr \rs0		)	@ dst=aff0>>rs0
42 THUMB(	lsr	\dst, \dst, \rs0		)
43	and	\mask, \mpidr, #0xff00			@ mask = aff1
44 ARM(	orr	\dst, \dst, \mask, lsr \rs1	)	@ dst|=(aff1>>rs1)
45 THUMB(	lsr	\mask, \mask, \rs1		)
46 THUMB(	orr	\dst, \dst, \mask		)
47	and	\mask, \mpidr, #0xff0000		@ mask = aff2
48 ARM(	orr	\dst, \dst, \mask, lsr \rs2	)	@ dst|=(aff2>>rs2)
49 THUMB(	lsr	\mask, \mask, \rs2		)
50 THUMB(	orr	\dst, \dst, \mask		)
51	.endm
52
53/*
54 * Save CPU state for a suspend.  This saves the CPU general purpose
55 * registers, and allocates space on the kernel stack to save the CPU
56 * specific registers and some other data for resume.
57 *  r0 = suspend function arg0
58 *  r1 = suspend function
59 *  r2 = MPIDR value the resuming CPU will use
60 */
61ENTRY(__cpu_suspend)
62	stmfd	sp!, {r4 - r11, lr}
63#ifdef MULTI_CPU
64	ldr	r10, =processor
65	ldr	r4, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
66#else
67	ldr	r4, =cpu_suspend_size
68#endif
69	mov	r5, sp			@ current virtual SP
70#ifdef CONFIG_VMAP_STACK
71	@ Run the suspend code from the overflow stack so we don't have to rely
72	@ on vmalloc-to-phys conversions anywhere in the arch suspend code.
73	@ The original SP value captured in R5 will be restored on the way out.
74	ldr_this_cpu sp, overflow_stack_ptr, r6, r7
75#endif
76	add	r4, r4, #12		@ Space for pgd, virt sp, phys resume fn
77	sub	sp, sp, r4		@ allocate CPU state on stack
78	ldr	r3, =sleep_save_sp
79	stmfd	sp!, {r0, r1}		@ save suspend func arg and pointer
80	ldr	r3, [r3, #SLEEP_SAVE_SP_VIRT]
81	ALT_SMP(W(nop))			@ don't use adr_l inside ALT_SMP()
82	ALT_UP_B(1f)
83	adr_l	r0, mpidr_hash
84	/* This ldmia relies on the memory layout of the mpidr_hash struct */
85	ldmia	r0, {r1, r6-r8}	@ r1 = mpidr mask (r6,r7,r8) = l[0,1,2] shifts
86	compute_mpidr_hash	r0, r6, r7, r8, r2, r1
87	add	r3, r3, r0, lsl #2
881:	mov	r2, r5			@ virtual SP
89	mov	r1, r4			@ size of save block
90	add	r0, sp, #8		@ pointer to save block
91	bl	__cpu_suspend_save
92	badr	lr, cpu_suspend_abort
93	ldmfd	sp!, {r0, pc}		@ call suspend fn
94ENDPROC(__cpu_suspend)
95	.ltorg
96
97cpu_suspend_abort:
98	ldmia	sp!, {r1 - r3}		@ pop phys pgd, virt SP, phys resume fn
99	teq	r0, #0
100	moveq	r0, #1			@ force non-zero value
101	mov	sp, r2
102	ldmfd	sp!, {r4 - r11, pc}
103ENDPROC(cpu_suspend_abort)
104
105/*
106 * r0 = control register value
107 */
108	.align	5
109	.pushsection	.idmap.text,"ax"
110ENTRY(cpu_resume_mmu)
111	ldr	r3, =cpu_resume_after_mmu
112	instr_sync
113	mcr	p15, 0, r0, c1, c0, 0	@ turn on MMU, I-cache, etc
114	mrc	p15, 0, r0, c0, c0, 0	@ read id reg
115	instr_sync
116	mov	r0, r0
117	mov	r0, r0
118	ret	r3			@ jump to virtual address
119ENDPROC(cpu_resume_mmu)
120	.popsection
121cpu_resume_after_mmu:
122#if defined(CONFIG_VMAP_STACK) && !defined(CONFIG_ARM_LPAE)
123	@ Before using the vmap'ed stack, we have to switch to swapper_pg_dir
124	@ as the ID map does not cover the vmalloc region.
125	mrc	p15, 0, ip, c2, c0, 1	@ read TTBR1
126	mcr	p15, 0, ip, c2, c0, 0	@ set TTBR0
127	instr_sync
128#endif
129	bl	cpu_init		@ restore the und/abt/irq banked regs
130	mov	r0, #0			@ return zero on success
131	ldmfd	sp!, {r4 - r11, pc}
132ENDPROC(cpu_resume_after_mmu)
133
134	.text
135	.align
136
137#ifdef CONFIG_MCPM
138	.arm
139THUMB(	.thumb			)
140ENTRY(cpu_resume_no_hyp)
141ARM_BE8(setend be)			@ ensure we are in BE mode
142	b	no_hyp
143#endif
144
145#ifdef CONFIG_MMU
146	.arm
147ENTRY(cpu_resume_arm)
148 THUMB(	badr	r9, 1f		)	@ Kernel is entered in ARM.
149 THUMB(	bx	r9		)	@ If this is a Thumb-2 kernel,
150 THUMB(	.thumb			)	@ switch to Thumb now.
151 THUMB(1:			)
152#endif
153
154ENTRY(cpu_resume)
155ARM_BE8(setend be)			@ ensure we are in BE mode
156#ifdef CONFIG_ARM_VIRT_EXT
157	bl	__hyp_stub_install_secondary
158#endif
159	safe_svcmode_maskall r1
160no_hyp:
161	mov	r1, #0
162	ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
163	ALT_UP_B(1f)
164	adr_l	r2, mpidr_hash		@ r2 = struct mpidr_hash phys address
165
166	/*
167	 * This ldmia relies on the memory layout of the mpidr_hash
168	 * struct mpidr_hash.
169	 */
170	ldmia	r2, { r3-r6 }	@ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
171	compute_mpidr_hash	r1, r4, r5, r6, r0, r3
1721:
173	ldr_l	r0, sleep_save_sp + SLEEP_SAVE_SP_PHYS
174	ldr	r0, [r0, r1, lsl #2]
175
176	@ load phys pgd, stack, resume fn
177  ARM(	ldmia	r0!, {r1, sp, pc}	)
178THUMB(	ldmia	r0!, {r1, r2, r3}	)
179THUMB(	mov	sp, r2			)
180THUMB(	bx	r3			)
181ENDPROC(cpu_resume)
182
183#ifdef CONFIG_MMU
184ENDPROC(cpu_resume_arm)
185#endif
186#ifdef CONFIG_MCPM
187ENDPROC(cpu_resume_no_hyp)
188#endif
189
190	.data
191	.align	2
192	.type	sleep_save_sp, #object
193ENTRY(sleep_save_sp)
194	.space	SLEEP_SAVE_SP_SZ		@ struct sleep_save_sp
195