| blob | b1925729c692c72a0d7a1e7a33a7b2ca0eba591e |
1 #include <linux/errno.h>
2 #include <linux/linkage.h>
3 #include <asm/asm-offsets.h>
4 #include <asm/assembler.h>
6 .text
7 /*
8 * Implementation of MPIDR_EL1 hash algorithm through shifting
9 * and OR'ing.
10 *
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
18 *
19 * Pseudo C-code:
20 *
21 *u32 dst;
22 *
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);
31 *}
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)
36 */
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)
50 .endm
51 /*
52 * Save CPU state for a suspend. This saves callee registers, and allocates
53 * space on the kernel stack to save the CPU specific registers + some
54 * other data for resume.
55 *
56 * x0 = suspend finisher argument
57 */
58 ENTRY(__cpu_suspend)
59 stp x29, lr, [sp, #-96]!
60 stp x19, x20, [sp,#16]
61 stp x21, x22, [sp,#32]
62 stp x23, x24, [sp,#48]
63 stp x25, x26, [sp,#64]
64 stp x27, x28, [sp,#80]
65 mov x2, sp
66 sub sp, sp, #CPU_SUSPEND_SZ // allocate cpu_suspend_ctx
67 mov x1, sp
68 /*
69 * x1 now points to struct cpu_suspend_ctx allocated on the stack
70 */
71 str x2, [x1, #CPU_CTX_SP]
72 ldr x2, =sleep_save_sp
73 ldr x2, [x2, #SLEEP_SAVE_SP_VIRT]
74 #ifdef CONFIG_SMP
75 mrs x7, mpidr_el1
76 ldr x9, =mpidr_hash
77 ldr x10, [x9, #MPIDR_HASH_MASK]
78 /*
79 * Following code relies on the struct mpidr_hash
80 * members size.
81 */
82 ldp w3, w4, [x9, #MPIDR_HASH_SHIFTS]
83 ldp w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)]
84 compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10
85 add x2, x2, x8, lsl #3
86 #endif
87 bl __cpu_suspend_finisher
88 /*
89 * Never gets here, unless suspend fails.
90 * Successful cpu_suspend should return from cpu_resume, returning
91 * through this code path is considered an error
92 * If the return value is set to 0 force x0 = -EOPNOTSUPP
93 * to make sure a proper error condition is propagated
94 */
95 cmp x0, #0
96 mov x3, #-EOPNOTSUPP
97 csel x0, x3, x0, eq
98 add sp, sp, #CPU_SUSPEND_SZ // rewind stack pointer
99 ldp x19, x20, [sp, #16]
100 ldp x21, x22, [sp, #32]
101 ldp x23, x24, [sp, #48]
102 ldp x25, x26, [sp, #64]
103 ldp x27, x28, [sp, #80]
104 ldp x29, lr, [sp], #96
105 ret
106 ENDPROC(__cpu_suspend)
107 .ltorg
109 /*
110 * x0 must contain the sctlr value retrieved from restored context
111 */
112 ENTRY(cpu_resume_mmu)
113 ldr x3, =cpu_resume_after_mmu
114 msr sctlr_el1, x0 // restore sctlr_el1
115 isb
116 br x3 // global jump to virtual address
117 ENDPROC(cpu_resume_mmu)
118 cpu_resume_after_mmu:
119 mov x0, #0 // return zero on success
120 ldp x19, x20, [sp, #16]
121 ldp x21, x22, [sp, #32]
122 ldp x23, x24, [sp, #48]
123 ldp x25, x26, [sp, #64]
124 ldp x27, x28, [sp, #80]
125 ldp x29, lr, [sp], #96
126 ret
127 ENDPROC(cpu_resume_after_mmu)
129 .data
130 ENTRY(cpu_resume)
131 bl el2_setup // if in EL2 drop to EL1 cleanly
132 #ifdef CONFIG_SMP
133 mrs x1, mpidr_el1
134 adr x4, mpidr_hash_ptr
135 ldr x5, [x4]
136 add x8, x4, x5 // x8 = struct mpidr_hash phys address
137 /* retrieve mpidr_hash members to compute the hash */
138 ldr x2, [x8, #MPIDR_HASH_MASK]
139 ldp w3, w4, [x8, #MPIDR_HASH_SHIFTS]
140 ldp w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
141 compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
142 /* x7 contains hash index, let's use it to grab context pointer */
143 #else
144 mov x7, xzr
145 #endif
146 adr x0, sleep_save_sp
147 ldr x0, [x0, #SLEEP_SAVE_SP_PHYS]
148 ldr x0, [x0, x7, lsl #3]
149 /* load sp from context */
150 ldr x2, [x0, #CPU_CTX_SP]
151 adr x1, sleep_idmap_phys
152 /* load physical address of identity map page table in x1 */
153 ldr x1, [x1]
154 mov sp, x2
155 /*
156 * cpu_do_resume expects x0 to contain context physical address
157 * pointer and x1 to contain physical address of 1:1 page tables
158 */
159 bl cpu_do_resume // PC relative jump, MMU off
160 b cpu_resume_mmu // Resume MMU, never returns
161 ENDPROC(cpu_resume)
163 .align 3
164 mpidr_hash_ptr:
165 /*
166 * offset of mpidr_hash symbol from current location
167 * used to obtain run-time mpidr_hash address with MMU off
168 */
169 .quad mpidr_hash - .
170 /*
171 * physical address of identity mapped page tables
172 */
173 .type sleep_idmap_phys, #object
174 ENTRY(sleep_idmap_phys)
175 .quad 0
176 /*
177 * struct sleep_save_sp {
178 * phys_addr_t *save_ptr_stash;
179 * phys_addr_t save_ptr_stash_phys;
180 * };
181 */
182 .type sleep_save_sp, #object
183 ENTRY(sleep_save_sp)
184 .space SLEEP_SAVE_SP_SZ // struct sleep_save_sp