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	add	r4, r4, #12		@ Space for pgd, virt sp, phys resume fn
71	sub	sp, sp, r4		@ allocate CPU state on stack
72	ldr	r3, =sleep_save_sp
73	stmfd	sp!, {r0, r1}		@ save suspend func arg and pointer
74	ldr	r3, [r3, #SLEEP_SAVE_SP_VIRT]
75	ALT_SMP(W(nop))			@ don't use adr_l inside ALT_SMP()
76	ALT_UP_B(1f)
77	adr_l	r0, mpidr_hash
78	/* This ldmia relies on the memory layout of the mpidr_hash struct */
79	ldmia	r0, {r1, r6-r8}	@ r1 = mpidr mask (r6,r7,r8) = l[0,1,2] shifts
80	compute_mpidr_hash	r0, r6, r7, r8, r2, r1
81	add	r3, r3, r0, lsl #2
821:	mov	r2, r5			@ virtual SP
83	mov	r1, r4			@ size of save block
84	add	r0, sp, #8		@ pointer to save block
85	bl	__cpu_suspend_save
86	badr	lr, cpu_suspend_abort
87	ldmfd	sp!, {r0, pc}		@ call suspend fn
88ENDPROC(__cpu_suspend)
89	.ltorg
90
91cpu_suspend_abort:
92	ldmia	sp!, {r1 - r3}		@ pop phys pgd, virt SP, phys resume fn
93	teq	r0, #0
94	moveq	r0, #1			@ force non-zero value
95	mov	sp, r2
96	ldmfd	sp!, {r4 - r11, pc}
97ENDPROC(cpu_suspend_abort)
98
99/*
100 * r0 = control register value
101 */
102	.align	5
103	.pushsection	.idmap.text,"ax"
104ENTRY(cpu_resume_mmu)
105	ldr	r3, =cpu_resume_after_mmu
106	instr_sync
107	mcr	p15, 0, r0, c1, c0, 0	@ turn on MMU, I-cache, etc
108	mrc	p15, 0, r0, c0, c0, 0	@ read id reg
109	instr_sync
110	mov	r0, r0
111	mov	r0, r0
112	ret	r3			@ jump to virtual address
113ENDPROC(cpu_resume_mmu)
114	.popsection
115cpu_resume_after_mmu:
116	bl	cpu_init		@ restore the und/abt/irq banked regs
117	mov	r0, #0			@ return zero on success
118	ldmfd	sp!, {r4 - r11, pc}
119ENDPROC(cpu_resume_after_mmu)
120
121	.text
122	.align
123
124#ifdef CONFIG_MCPM
125	.arm
126THUMB(	.thumb			)
127ENTRY(cpu_resume_no_hyp)
128ARM_BE8(setend be)			@ ensure we are in BE mode
129	b	no_hyp
130#endif
131
132#ifdef CONFIG_MMU
133	.arm
134ENTRY(cpu_resume_arm)
135 THUMB(	badr	r9, 1f		)	@ Kernel is entered in ARM.
136 THUMB(	bx	r9		)	@ If this is a Thumb-2 kernel,
137 THUMB(	.thumb			)	@ switch to Thumb now.
138 THUMB(1:			)
139#endif
140
141ENTRY(cpu_resume)
142ARM_BE8(setend be)			@ ensure we are in BE mode
143#ifdef CONFIG_ARM_VIRT_EXT
144	bl	__hyp_stub_install_secondary
145#endif
146	safe_svcmode_maskall r1
147no_hyp:
148	mov	r1, #0
149	ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
150	ALT_UP_B(1f)
151	adr_l	r2, mpidr_hash		@ r2 = struct mpidr_hash phys address
152
153	/*
154	 * This ldmia relies on the memory layout of the mpidr_hash
155	 * struct mpidr_hash.
156	 */
157	ldmia	r2, { r3-r6 }	@ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
158	compute_mpidr_hash	r1, r4, r5, r6, r0, r3
1591:
160	ldr_l	r0, sleep_save_sp + SLEEP_SAVE_SP_PHYS
161	ldr	r0, [r0, r1, lsl #2]
162
163	@ load phys pgd, stack, resume fn
164  ARM(	ldmia	r0!, {r1, sp, pc}	)
165THUMB(	ldmia	r0!, {r1, r2, r3}	)
166THUMB(	mov	sp, r2			)
167THUMB(	bx	r3			)
168ENDPROC(cpu_resume)
169
170#ifdef CONFIG_MMU
171ENDPROC(cpu_resume_arm)
172#endif
173#ifdef CONFIG_MCPM
174ENDPROC(cpu_resume_no_hyp)
175#endif
176
177	.data
178	.align	2
179	.type	sleep_save_sp, #object
180ENTRY(sleep_save_sp)
181	.space	SLEEP_SAVE_SP_SZ		@ struct sleep_save_sp
182