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 *  r2 = MPIDR value the resuming CPU will use
59 */
60ENTRY(__cpu_suspend)
61	stmfd	sp!, {r4 - r11, lr}
62#ifdef MULTI_CPU
63	ldr	r10, =processor
64	ldr	r4, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
65#else
66	ldr	r4, =cpu_suspend_size
67#endif
68	mov	r5, sp			@ current virtual SP
69	add	r4, r4, #12		@ Space for pgd, virt sp, phys resume fn
70	sub	sp, sp, r4		@ allocate CPU state on stack
71	ldr	r3, =sleep_save_sp
72	stmfd	sp!, {r0, r1}		@ save suspend func arg and pointer
73	ldr	r3, [r3, #SLEEP_SAVE_SP_VIRT]
74	ALT_SMP(ldr r0, =mpidr_hash)
75	ALT_UP_B(1f)
76	/* This ldmia relies on the memory layout of the mpidr_hash struct */
77	ldmia	r0, {r1, r6-r8}	@ r1 = mpidr mask (r6,r7,r8) = l[0,1,2] shifts
78	compute_mpidr_hash	r0, r6, r7, r8, r2, r1
79	add	r3, r3, r0, lsl #2
801:	mov	r2, r5			@ virtual SP
81	mov	r1, r4			@ size of save block
82	add	r0, sp, #8		@ pointer to save block
83	bl	__cpu_suspend_save
84	adr	lr, BSYM(cpu_suspend_abort)
85	ldmfd	sp!, {r0, pc}		@ call suspend fn
86ENDPROC(__cpu_suspend)
87	.ltorg
88
89cpu_suspend_abort:
90	ldmia	sp!, {r1 - r3}		@ pop phys pgd, virt SP, phys resume fn
91	teq	r0, #0
92	moveq	r0, #1			@ force non-zero value
93	mov	sp, r2
94	ldmfd	sp!, {r4 - r11, pc}
95ENDPROC(cpu_suspend_abort)
96
97/*
98 * r0 = control register value
99 */
100	.align	5
101	.pushsection	.idmap.text,"ax"
102ENTRY(cpu_resume_mmu)
103	ldr	r3, =cpu_resume_after_mmu
104	instr_sync
105	mcr	p15, 0, r0, c1, c0, 0	@ turn on MMU, I-cache, etc
106	mrc	p15, 0, r0, c0, c0, 0	@ read id reg
107	instr_sync
108	mov	r0, r0
109	mov	r0, r0
110	ret	r3			@ jump to virtual address
111ENDPROC(cpu_resume_mmu)
112	.popsection
113cpu_resume_after_mmu:
114	bl	cpu_init		@ restore the und/abt/irq banked regs
115	mov	r0, #0			@ return zero on success
116	ldmfd	sp!, {r4 - r11, pc}
117ENDPROC(cpu_resume_after_mmu)
118
119	.text
120	.align
121ENTRY(cpu_resume)
122ARM_BE8(setend be)			@ ensure we are in BE mode
123#ifdef CONFIG_ARM_VIRT_EXT
124	bl	__hyp_stub_install_secondary
125#endif
126	safe_svcmode_maskall r1
127	mov	r1, #0
128	ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
129	ALT_UP_B(1f)
130	adr	r2, mpidr_hash_ptr
131	ldr	r3, [r2]
132	add	r2, r2, r3		@ r2 = struct mpidr_hash phys address
133	/*
134	 * This ldmia relies on the memory layout of the mpidr_hash
135	 * struct mpidr_hash.
136	 */
137	ldmia	r2, { r3-r6 }	@ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
138	compute_mpidr_hash	r1, r4, r5, r6, r0, r3
1391:
140	adr	r0, _sleep_save_sp
141	ldr	r2, [r0]
142	add	r0, r0, r2
143	ldr	r0, [r0, #SLEEP_SAVE_SP_PHYS]
144	ldr	r0, [r0, r1, lsl #2]
145
146	@ load phys pgd, stack, resume fn
147  ARM(	ldmia	r0!, {r1, sp, pc}	)
148THUMB(	ldmia	r0!, {r1, r2, r3}	)
149THUMB(	mov	sp, r2			)
150THUMB(	bx	r3			)
151ENDPROC(cpu_resume)
152
153	.align 2
154_sleep_save_sp:
155	.long	sleep_save_sp - .
156mpidr_hash_ptr:
157	.long	mpidr_hash - .			@ mpidr_hash struct offset
158
159	.data
160	.type	sleep_save_sp, #object
161ENTRY(sleep_save_sp)
162	.space	SLEEP_SAVE_SP_SZ		@ struct sleep_save_sp
163