[mirror_ubuntu-zesty-kernel.git] / arch / arm64 / kernel / sleep.S

#include <linux/errno.h>
#include <linux/linkage.h>
#include <asm/asm-offsets.h>
#include <asm/assembler.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;
 *	aff2 = 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 for a suspend.  This saves callee registers, and allocates
 * space on the kernel stack to save the CPU specific registers + some
 * other data for resume.
 *
 *  x0 = suspend finisher argument
 */
ENTRY(__cpu_suspend)
	stp	x29, lr, [sp, #-96]!
	stp	x19, x20, [sp,#16]
	stp	x21, x22, [sp,#32]
	stp	x23, x24, [sp,#48]
	stp	x25, x26, [sp,#64]
	stp	x27, x28, [sp,#80]
	mov	x2, sp
	sub	sp, sp, #CPU_SUSPEND_SZ	// allocate cpu_suspend_ctx
	mov	x1, sp
	/*
	 * x1 now points to struct cpu_suspend_ctx allocated on the stack
	 */
	str	x2, [x1, #CPU_CTX_SP]
	ldr	x2, =sleep_save_sp
	ldr	x2, [x2, #SLEEP_SAVE_SP_VIRT]
#ifdef CONFIG_SMP
	mrs	x7, mpidr_el1
	ldr	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	x2, x2, x8, lsl #3
#endif
	bl	__cpu_suspend_finisher
        /*
	 * Never gets here, unless suspend fails.
	 * Successful cpu_suspend should return from cpu_resume, returning
	 * through this code path is considered an error
	 * If the return value is set to 0 force x0 = -EOPNOTSUPP
	 * to make sure a proper error condition is propagated
	 */
	cmp	x0, #0
	mov	x3, #-EOPNOTSUPP
	csel	x0, x3, x0, eq
	add	sp, sp, #CPU_SUSPEND_SZ	// rewind stack pointer
	ldp	x19, x20, [sp, #16]
	ldp	x21, x22, [sp, #32]
	ldp	x23, x24, [sp, #48]
	ldp	x25, x26, [sp, #64]
	ldp	x27, x28, [sp, #80]
	ldp	x29, lr, [sp], #96
	ret
ENDPROC(__cpu_suspend)
	.ltorg

/*
 * x0 must contain the sctlr value retrieved from restored context
 */
ENTRY(cpu_resume_mmu)
	ldr	x3, =cpu_resume_after_mmu
	msr	sctlr_el1, x0		// restore sctlr_el1
	isb
	br	x3			// global jump to virtual address
ENDPROC(cpu_resume_mmu)
cpu_resume_after_mmu:
	mov	x0, #0			// return zero on success
	ldp	x19, x20, [sp, #16]
	ldp	x21, x22, [sp, #32]
	ldp	x23, x24, [sp, #48]
	ldp	x25, x26, [sp, #64]
	ldp	x27, x28, [sp, #80]
	ldp	x29, lr, [sp], #96
	ret
ENDPROC(cpu_resume_after_mmu)

	.data
ENTRY(cpu_resume)
	bl	el2_setup		// if in EL2 drop to EL1 cleanly
#ifdef CONFIG_SMP
	mrs	x1, mpidr_el1
	adr	x4, mpidr_hash_ptr
	ldr	x5, [x4]
	add	x8, x4, x5		// x8 = struct mpidr_hash phys 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 */
#else
	mov	x7, xzr
#endif
	adr	x0, sleep_save_sp
	ldr	x0, [x0, #SLEEP_SAVE_SP_PHYS]
	ldr	x0, [x0, x7, lsl #3]
	/* load sp from context */
	ldr	x2, [x0, #CPU_CTX_SP]
	adr	x1, sleep_idmap_phys
	/* load physical address of identity map page table in x1 */
	ldr	x1, [x1]
	mov	sp, x2
	/*
	 * cpu_do_resume expects x0 to contain context physical address
	 * pointer and x1 to contain physical address of 1:1 page tables
	 */
	bl	cpu_do_resume		// PC relative jump, MMU off
	b	cpu_resume_mmu		// Resume MMU, never returns
ENDPROC(cpu_resume)

	.align 3
mpidr_hash_ptr:
	/*
	 * offset of mpidr_hash symbol from current location
	 * used to obtain run-time mpidr_hash address with MMU off
         */
	.quad	mpidr_hash - .
/*
 * physical address of identity mapped page tables
 */
	.type	sleep_idmap_phys, #object
ENTRY(sleep_idmap_phys)
	.quad	0
/*
 * struct sleep_save_sp {
 *	phys_addr_t *save_ptr_stash;
 *	phys_addr_t save_ptr_stash_phys;
 * };
 */
	.type	sleep_save_sp, #object
ENTRY(sleep_save_sp)
	.space	SLEEP_SAVE_SP_SZ	// struct sleep_save_sp
Commit	Line	Data
95322526 LP	1	#include <linux/errno.h>
	2	#include <linux/linkage.h>
	3	#include <asm/asm-offsets.h>
	4	#include <asm/assembler.h>
	5
	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
108
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)
128
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)
162
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