195322526SLorenzo Pieralisi#include <linux/errno.h> 295322526SLorenzo Pieralisi#include <linux/linkage.h> 395322526SLorenzo Pieralisi#include <asm/asm-offsets.h> 495322526SLorenzo Pieralisi#include <asm/assembler.h> 595322526SLorenzo Pieralisi 695322526SLorenzo Pieralisi .text 795322526SLorenzo Pieralisi/* 895322526SLorenzo Pieralisi * Implementation of MPIDR_EL1 hash algorithm through shifting 995322526SLorenzo Pieralisi * and OR'ing. 1095322526SLorenzo Pieralisi * 1195322526SLorenzo Pieralisi * @dst: register containing hash result 1295322526SLorenzo Pieralisi * @rs0: register containing affinity level 0 bit shift 1395322526SLorenzo Pieralisi * @rs1: register containing affinity level 1 bit shift 1495322526SLorenzo Pieralisi * @rs2: register containing affinity level 2 bit shift 1595322526SLorenzo Pieralisi * @rs3: register containing affinity level 3 bit shift 1695322526SLorenzo Pieralisi * @mpidr: register containing MPIDR_EL1 value 1795322526SLorenzo Pieralisi * @mask: register containing MPIDR mask 1895322526SLorenzo Pieralisi * 1995322526SLorenzo Pieralisi * Pseudo C-code: 2095322526SLorenzo Pieralisi * 2195322526SLorenzo Pieralisi *u32 dst; 2295322526SLorenzo Pieralisi * 2395322526SLorenzo Pieralisi *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 rs3, u64 mpidr, u64 mask) { 2495322526SLorenzo Pieralisi * u32 aff0, aff1, aff2, aff3; 2595322526SLorenzo Pieralisi * u64 mpidr_masked = mpidr & mask; 2695322526SLorenzo Pieralisi * aff0 = mpidr_masked & 0xff; 2795322526SLorenzo Pieralisi * aff1 = mpidr_masked & 0xff00; 2895322526SLorenzo Pieralisi * aff2 = mpidr_masked & 0xff0000; 2995322526SLorenzo Pieralisi * aff2 = mpidr_masked & 0xff00000000; 3095322526SLorenzo Pieralisi * dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2 | aff3 >> rs3); 3195322526SLorenzo Pieralisi *} 3295322526SLorenzo Pieralisi * Input registers: rs0, rs1, rs2, rs3, mpidr, mask 3395322526SLorenzo Pieralisi * Output register: dst 3495322526SLorenzo Pieralisi * Note: input and output registers must be disjoint register sets 3595322526SLorenzo Pieralisi (eg: a macro instance with mpidr = x1 and dst = x1 is invalid) 3695322526SLorenzo Pieralisi */ 3795322526SLorenzo Pieralisi .macro compute_mpidr_hash dst, rs0, rs1, rs2, rs3, mpidr, mask 3895322526SLorenzo Pieralisi and \mpidr, \mpidr, \mask // mask out MPIDR bits 3995322526SLorenzo Pieralisi and \dst, \mpidr, #0xff // mask=aff0 4095322526SLorenzo Pieralisi lsr \dst ,\dst, \rs0 // dst=aff0>>rs0 4195322526SLorenzo Pieralisi and \mask, \mpidr, #0xff00 // mask = aff1 4295322526SLorenzo Pieralisi lsr \mask ,\mask, \rs1 4395322526SLorenzo Pieralisi orr \dst, \dst, \mask // dst|=(aff1>>rs1) 4495322526SLorenzo Pieralisi and \mask, \mpidr, #0xff0000 // mask = aff2 4595322526SLorenzo Pieralisi lsr \mask ,\mask, \rs2 4695322526SLorenzo Pieralisi orr \dst, \dst, \mask // dst|=(aff2>>rs2) 4795322526SLorenzo Pieralisi and \mask, \mpidr, #0xff00000000 // mask = aff3 4895322526SLorenzo Pieralisi lsr \mask ,\mask, \rs3 4995322526SLorenzo Pieralisi orr \dst, \dst, \mask // dst|=(aff3>>rs3) 5095322526SLorenzo Pieralisi .endm 5195322526SLorenzo Pieralisi/* 5295322526SLorenzo Pieralisi * Save CPU state for a suspend. This saves callee registers, and allocates 5395322526SLorenzo Pieralisi * space on the kernel stack to save the CPU specific registers + some 5495322526SLorenzo Pieralisi * other data for resume. 5595322526SLorenzo Pieralisi * 5695322526SLorenzo Pieralisi * x0 = suspend finisher argument 5795322526SLorenzo Pieralisi */ 5895322526SLorenzo PieralisiENTRY(__cpu_suspend) 5995322526SLorenzo Pieralisi stp x29, lr, [sp, #-96]! 6095322526SLorenzo Pieralisi stp x19, x20, [sp,#16] 6195322526SLorenzo Pieralisi stp x21, x22, [sp,#32] 6295322526SLorenzo Pieralisi stp x23, x24, [sp,#48] 6395322526SLorenzo Pieralisi stp x25, x26, [sp,#64] 6495322526SLorenzo Pieralisi stp x27, x28, [sp,#80] 6595322526SLorenzo Pieralisi mov x2, sp 6695322526SLorenzo Pieralisi sub sp, sp, #CPU_SUSPEND_SZ // allocate cpu_suspend_ctx 6795322526SLorenzo Pieralisi mov x1, sp 6895322526SLorenzo Pieralisi /* 6995322526SLorenzo Pieralisi * x1 now points to struct cpu_suspend_ctx allocated on the stack 7095322526SLorenzo Pieralisi */ 7195322526SLorenzo Pieralisi str x2, [x1, #CPU_CTX_SP] 7295322526SLorenzo Pieralisi ldr x2, =sleep_save_sp 7395322526SLorenzo Pieralisi ldr x2, [x2, #SLEEP_SAVE_SP_VIRT] 7495322526SLorenzo Pieralisi#ifdef CONFIG_SMP 7595322526SLorenzo Pieralisi mrs x7, mpidr_el1 7695322526SLorenzo Pieralisi ldr x9, =mpidr_hash 7795322526SLorenzo Pieralisi ldr x10, [x9, #MPIDR_HASH_MASK] 7895322526SLorenzo Pieralisi /* 7995322526SLorenzo Pieralisi * Following code relies on the struct mpidr_hash 8095322526SLorenzo Pieralisi * members size. 8195322526SLorenzo Pieralisi */ 8295322526SLorenzo Pieralisi ldp w3, w4, [x9, #MPIDR_HASH_SHIFTS] 8395322526SLorenzo Pieralisi ldp w5, w6, [x9, #(MPIDR_HASH_SHIFTS + 8)] 8495322526SLorenzo Pieralisi compute_mpidr_hash x8, x3, x4, x5, x6, x7, x10 8595322526SLorenzo Pieralisi add x2, x2, x8, lsl #3 8695322526SLorenzo Pieralisi#endif 8795322526SLorenzo Pieralisi bl __cpu_suspend_finisher 8895322526SLorenzo Pieralisi /* 8995322526SLorenzo Pieralisi * Never gets here, unless suspend fails. 9095322526SLorenzo Pieralisi * Successful cpu_suspend should return from cpu_resume, returning 9195322526SLorenzo Pieralisi * through this code path is considered an error 9295322526SLorenzo Pieralisi * If the return value is set to 0 force x0 = -EOPNOTSUPP 9395322526SLorenzo Pieralisi * to make sure a proper error condition is propagated 9495322526SLorenzo Pieralisi */ 9595322526SLorenzo Pieralisi cmp x0, #0 9695322526SLorenzo Pieralisi mov x3, #-EOPNOTSUPP 9795322526SLorenzo Pieralisi csel x0, x3, x0, eq 9895322526SLorenzo Pieralisi add sp, sp, #CPU_SUSPEND_SZ // rewind stack pointer 9995322526SLorenzo Pieralisi ldp x19, x20, [sp, #16] 10095322526SLorenzo Pieralisi ldp x21, x22, [sp, #32] 10195322526SLorenzo Pieralisi ldp x23, x24, [sp, #48] 10295322526SLorenzo Pieralisi ldp x25, x26, [sp, #64] 10395322526SLorenzo Pieralisi ldp x27, x28, [sp, #80] 10495322526SLorenzo Pieralisi ldp x29, lr, [sp], #96 10595322526SLorenzo Pieralisi ret 10695322526SLorenzo PieralisiENDPROC(__cpu_suspend) 10795322526SLorenzo Pieralisi .ltorg 10895322526SLorenzo Pieralisi 10995322526SLorenzo Pieralisi/* 11095322526SLorenzo Pieralisi * x0 must contain the sctlr value retrieved from restored context 11195322526SLorenzo Pieralisi */ 11295322526SLorenzo PieralisiENTRY(cpu_resume_mmu) 11395322526SLorenzo Pieralisi ldr x3, =cpu_resume_after_mmu 11495322526SLorenzo Pieralisi msr sctlr_el1, x0 // restore sctlr_el1 11595322526SLorenzo Pieralisi isb 11695322526SLorenzo Pieralisi br x3 // global jump to virtual address 11795322526SLorenzo PieralisiENDPROC(cpu_resume_mmu) 11895322526SLorenzo Pieralisicpu_resume_after_mmu: 11995322526SLorenzo Pieralisi mov x0, #0 // return zero on success 12095322526SLorenzo Pieralisi ldp x19, x20, [sp, #16] 12195322526SLorenzo Pieralisi ldp x21, x22, [sp, #32] 12295322526SLorenzo Pieralisi ldp x23, x24, [sp, #48] 12395322526SLorenzo Pieralisi ldp x25, x26, [sp, #64] 12495322526SLorenzo Pieralisi ldp x27, x28, [sp, #80] 12595322526SLorenzo Pieralisi ldp x29, lr, [sp], #96 12695322526SLorenzo Pieralisi ret 12795322526SLorenzo PieralisiENDPROC(cpu_resume_after_mmu) 12895322526SLorenzo Pieralisi 12995322526SLorenzo Pieralisi .data 13095322526SLorenzo PieralisiENTRY(cpu_resume) 13195322526SLorenzo Pieralisi bl el2_setup // if in EL2 drop to EL1 cleanly 13295322526SLorenzo Pieralisi#ifdef CONFIG_SMP 13395322526SLorenzo Pieralisi mrs x1, mpidr_el1 13495322526SLorenzo Pieralisi adr x4, mpidr_hash_ptr 13595322526SLorenzo Pieralisi ldr x5, [x4] 13695322526SLorenzo Pieralisi add x8, x4, x5 // x8 = struct mpidr_hash phys address 13795322526SLorenzo Pieralisi /* retrieve mpidr_hash members to compute the hash */ 13895322526SLorenzo Pieralisi ldr x2, [x8, #MPIDR_HASH_MASK] 13995322526SLorenzo Pieralisi ldp w3, w4, [x8, #MPIDR_HASH_SHIFTS] 14095322526SLorenzo Pieralisi ldp w5, w6, [x8, #(MPIDR_HASH_SHIFTS + 8)] 14195322526SLorenzo Pieralisi compute_mpidr_hash x7, x3, x4, x5, x6, x1, x2 14295322526SLorenzo Pieralisi /* x7 contains hash index, let's use it to grab context pointer */ 14395322526SLorenzo Pieralisi#else 14495322526SLorenzo Pieralisi mov x7, xzr 14595322526SLorenzo Pieralisi#endif 14695322526SLorenzo Pieralisi adr x0, sleep_save_sp 14795322526SLorenzo Pieralisi ldr x0, [x0, #SLEEP_SAVE_SP_PHYS] 14895322526SLorenzo Pieralisi ldr x0, [x0, x7, lsl #3] 14995322526SLorenzo Pieralisi /* load sp from context */ 15095322526SLorenzo Pieralisi ldr x2, [x0, #CPU_CTX_SP] 15195322526SLorenzo Pieralisi adr x1, sleep_idmap_phys 15295322526SLorenzo Pieralisi /* load physical address of identity map page table in x1 */ 15395322526SLorenzo Pieralisi ldr x1, [x1] 15495322526SLorenzo Pieralisi mov sp, x2 15595322526SLorenzo Pieralisi /* 15695322526SLorenzo Pieralisi * cpu_do_resume expects x0 to contain context physical address 15795322526SLorenzo Pieralisi * pointer and x1 to contain physical address of 1:1 page tables 15895322526SLorenzo Pieralisi */ 15995322526SLorenzo Pieralisi bl cpu_do_resume // PC relative jump, MMU off 16095322526SLorenzo Pieralisi b cpu_resume_mmu // Resume MMU, never returns 16195322526SLorenzo PieralisiENDPROC(cpu_resume) 16295322526SLorenzo Pieralisi 16395322526SLorenzo Pieralisi .align 3 16495322526SLorenzo Pieralisimpidr_hash_ptr: 16595322526SLorenzo Pieralisi /* 16695322526SLorenzo Pieralisi * offset of mpidr_hash symbol from current location 16795322526SLorenzo Pieralisi * used to obtain run-time mpidr_hash address with MMU off 16895322526SLorenzo Pieralisi */ 16995322526SLorenzo Pieralisi .quad mpidr_hash - . 17095322526SLorenzo Pieralisi/* 17195322526SLorenzo Pieralisi * physical address of identity mapped page tables 17295322526SLorenzo Pieralisi */ 17395322526SLorenzo Pieralisi .type sleep_idmap_phys, #object 17495322526SLorenzo PieralisiENTRY(sleep_idmap_phys) 17595322526SLorenzo Pieralisi .quad 0 17695322526SLorenzo Pieralisi/* 17795322526SLorenzo Pieralisi * struct sleep_save_sp { 17895322526SLorenzo Pieralisi * phys_addr_t *save_ptr_stash; 17995322526SLorenzo Pieralisi * phys_addr_t save_ptr_stash_phys; 18095322526SLorenzo Pieralisi * }; 18195322526SLorenzo Pieralisi */ 18295322526SLorenzo Pieralisi .type sleep_save_sp, #object 18395322526SLorenzo PieralisiENTRY(sleep_save_sp) 18495322526SLorenzo Pieralisi .space SLEEP_SAVE_SP_SZ // struct sleep_save_sp 185