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 | */ |
61 | ENTRY(__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 |
88 | 1: 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 |
94 | ENDPROC(__cpu_suspend) |
95 | .ltorg |
96 | |
97 | cpu_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} |
103 | ENDPROC(cpu_suspend_abort) |
104 | |
105 | /* |
106 | * r0 = control register value |
107 | */ |
108 | .align 5 |
109 | .pushsection .idmap.text,"ax" |
110 | ENTRY(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 |
119 | ENDPROC(cpu_resume_mmu) |
120 | .popsection |
121 | cpu_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} |
132 | ENDPROC(cpu_resume_after_mmu) |
133 | |
134 | .text |
135 | .align |
136 | |
137 | #ifdef CONFIG_MCPM |
138 | .arm |
139 | THUMB( .thumb ) |
140 | ENTRY(cpu_resume_no_hyp) |
141 | ARM_BE8(setend be) @ ensure we are in BE mode |
142 | b no_hyp |
143 | #endif |
144 | |
145 | #ifdef CONFIG_MMU |
146 | .arm |
147 | ENTRY(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 | |
154 | ENTRY(cpu_resume) |
155 | ARM_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 |
160 | no_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 |
172 | 1: |
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} ) |
178 | THUMB( ldmia r0!, {r1, r2, r3} ) |
179 | THUMB( mov sp, r2 ) |
180 | THUMB( bx r3 ) |
181 | ENDPROC(cpu_resume) |
182 | |
183 | #ifdef CONFIG_MMU |
184 | ENDPROC(cpu_resume_arm) |
185 | #endif |
186 | #ifdef CONFIG_MCPM |
187 | ENDPROC(cpu_resume_no_hyp) |
188 | #endif |
189 | |
190 | .data |
191 | .align 2 |
192 | .type sleep_save_sp, #object |
193 | ENTRY(sleep_save_sp) |
194 | .space SLEEP_SAVE_SP_SZ @ struct sleep_save_sp |
195 | |