1
2 #include <linux/errno.h>
3 #include <linux/linkage.h>
4 #include <asm/asm-offsets.h>
5 #include <asm/assembler.h>
6
7 .text
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38 .macro compute_mpidr_hash dst, rs0, rs1, rs2, rs3, mpidr, mask
39 and \mpidr, \mpidr, \mask
40 and \dst, \mpidr, #0xff
41 lsr \dst ,\dst, \rs0
42 and \mask, \mpidr, #0xff00
43 lsr \mask ,\mask, \rs1
44 orr \dst, \dst, \mask
45 and \mask, \mpidr, #0xff0000
46 lsr \mask ,\mask, \rs2
47 orr \dst, \dst, \mask
48 and \mask, \mpidr, #0xff00000000
49 lsr \mask ,\mask, \rs3
50 orr \dst, \dst, \mask
51 .endm
52
53
54
55
56
57
58
59
60
61
62
63
64 ENTRY(__cpu_suspend_enter)
65 stp x29, lr, [x0, #SLEEP_STACK_DATA_CALLEE_REGS]
66 stp x19, x20, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+16]
67 stp x21, x22, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+32]
68 stp x23, x24, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+48]
69 stp x25, x26, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+64]
70 stp x27, x28, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+80]
71
72
73 mov x2, sp
74 str x2, [x0, #SLEEP_STACK_DATA_SYSTEM_REGS + CPU_CTX_SP]
75
76
77 ldr_l x1, sleep_save_stash
78 mrs x7, mpidr_el1
79 adr_l x9, mpidr_hash
80 ldr x10, [x9, #MPIDR_HASH_MASK]
81
82
83
84
85 ldp w3, w4, [x9, #MPIDR_HASH_SHIFTS]
86 ldp w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)]
87 compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10
88 add x1, x1, x8, lsl #3
89
90 str x0, [x1]
91 add x0, x0, #SLEEP_STACK_DATA_SYSTEM_REGS
92 stp x29, lr, [sp, #-16]!
93 bl cpu_do_suspend
94 ldp x29, lr, [sp], #16
95 mov x0, #1
96 ret
97 ENDPROC(__cpu_suspend_enter)
98
99 .pushsection ".idmap.text", "awx"
100 ENTRY(cpu_resume)
101 bl el2_setup
102 bl __cpu_setup
103
104 adrp x1, swapper_pg_dir
105 bl __enable_mmu
106 ldr x8, =_cpu_resume
107 br x8
108 ENDPROC(cpu_resume)
109 .ltorg
110 .popsection
111
112 ENTRY(_cpu_resume)
113 mrs x1, mpidr_el1
114 adr_l x8, mpidr_hash
115
116
117 ldr x2, [x8, #MPIDR_HASH_MASK]
118 ldp w3, w4, [x8, #MPIDR_HASH_SHIFTS]
119 ldp w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
120 compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
121
122
123 ldr_l x0, sleep_save_stash
124 ldr x0, [x0, x7, lsl #3]
125 add x29, x0, #SLEEP_STACK_DATA_CALLEE_REGS
126 add x0, x0, #SLEEP_STACK_DATA_SYSTEM_REGS
127
128 ldr x2, [x0, #CPU_CTX_SP]
129 mov sp, x2
130
131
132
133 bl cpu_do_resume
134
135 #ifdef CONFIG_KASAN
136 mov x0, sp
137 bl kasan_unpoison_task_stack_below
138 #endif
139
140 ldp x19, x20, [x29, #16]
141 ldp x21, x22, [x29, #32]
142 ldp x23, x24, [x29, #48]
143 ldp x25, x26, [x29, #64]
144 ldp x27, x28, [x29, #80]
145 ldp x29, lr, [x29]
146 mov x0, #0
147 ret
148 ENDPROC(_cpu_resume)