1
2
3
4
5
6
7
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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
| /* SPDX-License-Identifier: GPL-2.0 */
| #include <linux/errno.h>
| #include <linux/linkage.h>
| #include <asm/asm-offsets.h>
| #include <asm/assembler.h>
| #include <asm/smp.h>
|
| .text
| /*
| * Implementation of MPIDR_EL1 hash algorithm through shifting
| * and OR'ing.
| *
| * @dst: register containing hash result
| * @rs0: register containing affinity level 0 bit shift
| * @rs1: register containing affinity level 1 bit shift
| * @rs2: register containing affinity level 2 bit shift
| * @rs3: register containing affinity level 3 bit shift
| * @mpidr: register containing MPIDR_EL1 value
| * @mask: register containing MPIDR mask
| *
| * Pseudo C-code:
| *
| *u32 dst;
| *
| *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 rs3, u64 mpidr, u64 mask) {
| * u32 aff0, aff1, aff2, aff3;
| * u64 mpidr_masked = mpidr & mask;
| * aff0 = mpidr_masked & 0xff;
| * aff1 = mpidr_masked & 0xff00;
| * aff2 = mpidr_masked & 0xff0000;
| * aff3 = mpidr_masked & 0xff00000000;
| * dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2 | aff3 >> rs3);
| *}
| * Input registers: rs0, rs1, rs2, rs3, mpidr, mask
| * Output register: dst
| * Note: input and output registers must be disjoint register sets
| (eg: a macro instance with mpidr = x1 and dst = x1 is invalid)
| */
| .macro compute_mpidr_hash dst, rs0, rs1, rs2, rs3, mpidr, mask
| and \mpidr, \mpidr, \mask // mask out MPIDR bits
| and \dst, \mpidr, #0xff // mask=aff0
| lsr \dst ,\dst, \rs0 // dst=aff0>>rs0
| and \mask, \mpidr, #0xff00 // mask = aff1
| lsr \mask ,\mask, \rs1
| orr \dst, \dst, \mask // dst|=(aff1>>rs1)
| and \mask, \mpidr, #0xff0000 // mask = aff2
| lsr \mask ,\mask, \rs2
| orr \dst, \dst, \mask // dst|=(aff2>>rs2)
| and \mask, \mpidr, #0xff00000000 // mask = aff3
| lsr \mask ,\mask, \rs3
| orr \dst, \dst, \mask // dst|=(aff3>>rs3)
| .endm
| /*
| * Save CPU state in the provided sleep_stack_data area, and publish its
| * location for cpu_resume()'s use in sleep_save_stash.
| *
| * cpu_resume() will restore this saved state, and return. Because the
| * link-register is saved and restored, it will appear to return from this
| * function. So that the caller can tell the suspend/resume paths apart,
| * __cpu_suspend_enter() will always return a non-zero value, whereas the
| * path through cpu_resume() will return 0.
| *
| * x0 = struct sleep_stack_data area
| */
| SYM_FUNC_START(__cpu_suspend_enter)
| stp x29, lr, [x0, #SLEEP_STACK_DATA_CALLEE_REGS]
| stp x19, x20, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+16]
| stp x21, x22, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+32]
| stp x23, x24, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+48]
| stp x25, x26, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+64]
| stp x27, x28, [x0,#SLEEP_STACK_DATA_CALLEE_REGS+80]
|
| /* save the sp in cpu_suspend_ctx */
| mov x2, sp
| str x2, [x0, #SLEEP_STACK_DATA_SYSTEM_REGS + CPU_CTX_SP]
|
| /* find the mpidr_hash */
| ldr_l x1, sleep_save_stash
| mrs x7, mpidr_el1
| adr_l x9, mpidr_hash
| ldr x10, [x9, #MPIDR_HASH_MASK]
| /*
| * Following code relies on the struct mpidr_hash
| * members size.
| */
| ldp w3, w4, [x9, #MPIDR_HASH_SHIFTS]
| ldp w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)]
| compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10
| add x1, x1, x8, lsl #3
|
| str x0, [x1]
| add x0, x0, #SLEEP_STACK_DATA_SYSTEM_REGS
| stp x29, lr, [sp, #-16]!
| bl cpu_do_suspend
| ldp x29, lr, [sp], #16
| mov x0, #1
| ret
| SYM_FUNC_END(__cpu_suspend_enter)
|
| .pushsection ".idmap.text", "awx"
| SYM_CODE_START(cpu_resume)
| bl init_kernel_el
| bl switch_to_vhe
| bl __cpu_setup
| /* enable the MMU early - so we can access sleep_save_stash by va */
| adrp x1, swapper_pg_dir
| bl __enable_mmu
| ldr x8, =_cpu_resume
| br x8
| SYM_CODE_END(cpu_resume)
| .ltorg
| .popsection
|
| SYM_FUNC_START(_cpu_resume)
| mrs x1, mpidr_el1
| adr_l x8, mpidr_hash // x8 = struct mpidr_hash virt address
|
| /* retrieve mpidr_hash members to compute the hash */
| ldr x2, [x8, #MPIDR_HASH_MASK]
| ldp w3, w4, [x8, #MPIDR_HASH_SHIFTS]
| ldp w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)]
| compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2
|
| /* x7 contains hash index, let's use it to grab context pointer */
| ldr_l x0, sleep_save_stash
| ldr x0, [x0, x7, lsl #3]
| add x29, x0, #SLEEP_STACK_DATA_CALLEE_REGS
| add x0, x0, #SLEEP_STACK_DATA_SYSTEM_REGS
| /* load sp from context */
| ldr x2, [x0, #CPU_CTX_SP]
| mov sp, x2
| /*
| * cpu_do_resume expects x0 to contain context address pointer
| */
| bl cpu_do_resume
|
| #if defined(CONFIG_KASAN) && defined(CONFIG_KASAN_STACK)
| mov x0, sp
| bl kasan_unpoison_task_stack_below
| #endif
|
| ldp x19, x20, [x29, #16]
| ldp x21, x22, [x29, #32]
| ldp x23, x24, [x29, #48]
| ldp x25, x26, [x29, #64]
| ldp x27, x28, [x29, #80]
| ldp x29, lr, [x29]
| mov x0, #0
| ret
| SYM_FUNC_END(_cpu_resume)
|
|
