1 #include <linux/errno.h>
2 #include <linux/linkage.h>
3 #include <asm/asm-offsets.h>
4 #include <asm/assembler.h>
8 * Implementation of MPIDR_EL1 hash algorithm through shifting
11 * @dst: register containing hash result
12 * @rs0: register containing affinity level 0 bit shift
13 * @rs1: register containing affinity level 1 bit shift
14 * @rs2: register containing affinity level 2 bit shift
15 * @rs3: register containing affinity level 3 bit shift
16 * @mpidr: register containing MPIDR_EL1 value
17 * @mask: register containing MPIDR mask
23 *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 rs3, u64 mpidr, u64 mask) {
24 * u32 aff0, aff1, aff2, aff3;
25 * u64 mpidr_masked = mpidr & mask;
26 * aff0 = mpidr_masked & 0xff;
27 * aff1 = mpidr_masked & 0xff00;
28 * aff2 = mpidr_masked & 0xff0000;
29 * aff2 = mpidr_masked & 0xff00000000;
30 * dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2 | aff3 >> rs3);
32 * Input registers: rs0, rs1, rs2, rs3, mpidr, mask
33 * Output register: dst
34 * Note: input and output registers must be disjoint register sets
35 (eg: a macro instance with mpidr = x1 and dst = x1 is invalid)
37 .macro compute_mpidr_hash dst, rs0, rs1, rs2, rs3, mpidr, mask
38 and \mpidr, \mpidr, \mask // mask out MPIDR bits
39 and \dst, \mpidr, #0xff // mask=aff0
40 lsr \dst ,\dst, \rs0 // dst=aff0>>rs0
41 and \mask, \mpidr, #0xff00 // mask = aff1
42 lsr \mask ,\mask, \rs1
43 orr \dst, \dst, \mask // dst|=(aff1>>rs1)
44 and \mask, \mpidr, #0xff0000 // mask = aff2
45 lsr \mask ,\mask, \rs2
46 orr \dst, \dst, \mask // dst|=(aff2>>rs2)
47 and \mask, \mpidr, #0xff00000000 // mask = aff3
48 lsr \mask ,\mask, \rs3
49 orr \dst, \dst, \mask // dst|=(aff3>>rs3)
53 .macro pclog_va t1, t2, t3, t4
57 add \t2, \t2, \t3, lsl #2
58 ldr \t3, =sleep_aee_rec_cpu_dormant_va
62 str \t4, [ \t3, \t2, lsl #3 ]
66 .macro pclog_pa t1, t2, t3, t4
67 /* since SRAM CONSOLE BUFFER is not kernel mapping space,..
72 add \t2, \t2, \t3, lsl #2
73 adr \t3, sleep_aee_rec_cpu_dormant
77 str \t4, [ \t3, \t2, lsl #3 ]
84 * Save CPU state for a suspend. This saves callee registers, and allocates
85 * space on the kernel stack to save the CPU specific registers + some
86 * other data for resume.
88 * x0 = suspend finisher argument
91 stp x29, lr, [sp, #-96]!
92 stp x19, x20, [sp,#16]
93 stp x21, x22, [sp,#32]
94 stp x23, x24, [sp,#48]
95 stp x25, x26, [sp,#64]
96 stp x27, x28, [sp,#80]
98 sub sp, sp, #CPU_SUSPEND_SZ // allocate cpu_suspend_ctx
101 pclog_va x7, x8, x9, x10
103 * x1 now points to struct cpu_suspend_ctx allocated on the stack
105 str x2, [x1, #CPU_CTX_SP]
106 ldr x2, =sleep_save_sp
107 ldr x2, [x2, #SLEEP_SAVE_SP_VIRT]
111 ldr x10, [x9, #MPIDR_HASH_MASK]
113 * Following code relies on the struct mpidr_hash
116 ldp w3, w4, [x9, #MPIDR_HASH_SHIFTS]
117 ldp w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)]
118 compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10
119 add x2, x2, x8, lsl #3
121 bl __cpu_suspend_finisher
123 * Never gets here, unless suspend fails.
124 * Successful cpu_suspend should return from cpu_resume, returning
125 * through this code path is considered an error
126 * If the return value is set to 0 force x0 = -EOPNOTSUPP
127 * to make sure a proper error condition is propagated
132 add sp, sp, #CPU_SUSPEND_SZ // rewind stack pointer
133 ldp x19, x20, [sp, #16]
134 ldp x21, x22, [sp, #32]
135 ldp x23, x24, [sp, #48]
136 ldp x25, x26, [sp, #64]
137 ldp x27, x28, [sp, #80]
138 ldp x29, lr, [sp], #96
140 ENDPROC(__cpu_suspend)
144 * x0 must contain the sctlr value retrieved from restored context
146 ENTRY(cpu_resume_mmu)
147 ldr x3, =cpu_resume_after_mmu
148 msr sctlr_el1, x0 // restore sctlr_el1
150 br x3 // global jump to virtual address
151 ENDPROC(cpu_resume_mmu)
152 cpu_resume_after_mmu:
153 pclog_va x7, x8, x9, x10
154 mov x0, #0 // return zero on success
155 ldp x19, x20, [sp, #16]
156 ldp x21, x22, [sp, #32]
157 ldp x23, x24, [sp, #48]
158 ldp x25, x26, [sp, #64]
159 ldp x27, x28, [sp, #80]
160 ldp x29, lr, [sp], #96
162 ENDPROC(cpu_resume_after_mmu)
167 bl __calc_phys_offset //el2_setup needs x28 for phys-page_offset
168 bl el2_setup // if in EL2 drop to EL1 cleanly
170 pclog_pa x7, x8, x9, x10
173 adr x4, mpidr_hash_ptr
175 add x8, x4, x5 // x8 = struct mpidr_hash phys address
176 /* retrieve mpidr_hash members to compute the hash */
177 ldr x2, [x8, #MPIDR_HASH_MASK]
178 ldp w3, w4, [x8, #MPIDR_HASH_SHIFTS]
179 ldp w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
180 compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
181 /* x7 contains hash index, let's use it to grab context pointer */
185 adr x0, sleep_save_sp
186 ldr x0, [x0, #SLEEP_SAVE_SP_PHYS]
187 ldr x0, [x0, x7, lsl #3]
188 /* load sp from context */
189 ldr x2, [x0, #CPU_CTX_SP]
190 adr x1, sleep_idmap_phys
191 /* load physical address of identity map page table in x1 */
193 /* bfi x1, xzr, #48, #16 // set the ASID as 0 */
196 * cpu_do_resume expects x0 to contain context physical address
197 * pointer and x1 to contain physical address of 1:1 page tables
199 bl cpu_do_resume // PC relative jump, MMU off
200 b cpu_resume_mmu // Resume MMU, never returns
206 * offset of mpidr_hash symbol from current location
207 * used to obtain run-time mpidr_hash address with MMU off
211 * physical address of identity mapped page tables
213 .type sleep_idmap_phys, #object
214 ENTRY(sleep_idmap_phys)
217 * struct sleep_save_sp {
218 * phys_addr_t *save_ptr_stash;
219 * phys_addr_t save_ptr_stash_phys;
222 .type sleep_save_sp, #object
224 .space SLEEP_SAVE_SP_SZ // struct sleep_save_sp
228 .type sleep_aee_rec_cpu_dormant, #object
229 ENTRY(sleep_aee_rec_cpu_dormant)
232 .type sleep_aee_rec_cpu_dormant_va, #object
233 ENTRY(sleep_aee_rec_cpu_dormant_va)