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