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