196 lines
3.8 KiB
ArmAsm
196 lines
3.8 KiB
ArmAsm
/*
|
|
* (C) Copyright 2017 Rockchip Electronics Co., Ltd.
|
|
*
|
|
* SPDX-License-Identifier: GPL-2.0+
|
|
*/
|
|
|
|
#include <asm/macro.h>
|
|
#include <asm-offsets.h>
|
|
#include <asm/psci.h>
|
|
#include <config.h>
|
|
#include <linux/linkage.h>
|
|
|
|
.globl cpu_suspend
|
|
.globl cpu_do_suspend
|
|
.globl cpu_suspend_save
|
|
.globl cpu_resume
|
|
.globl cpu_do_resume
|
|
|
|
/*
|
|
* int cpu_suspend(unsigned long arg, int (*fn)(unsigned long))
|
|
* @arg will be passed to fn as argument
|
|
* return value: 0 - cpu resumed from suspended state.
|
|
* -1 - cpu not suspended.
|
|
*/
|
|
ENTRY(cpu_suspend)
|
|
/*
|
|
* Save x8~x30(lr is x30, sp is x29), total (23 + 1 reserved)*8=192
|
|
*/
|
|
stp x29, lr, [sp, #-192]!
|
|
/* Reserve 8-byte after x8, just for offset with 16-byte aligned */
|
|
str x8, [sp, #16]
|
|
stp x9, x10, [sp, #32]
|
|
stp x11, x12, [sp, #48]
|
|
stp x13, x14, [sp, #64]
|
|
stp x15, x16, [sp, #80]
|
|
stp x17, x18, [sp, #96]
|
|
stp x19, x20, [sp,#112]
|
|
stp x21, x22, [sp,#128]
|
|
stp x23, x24, [sp,#144]
|
|
stp x25, x26, [sp,#160]
|
|
stp x27, x28, [sp,#176]
|
|
|
|
mov x19, sp
|
|
mov x20, x0
|
|
mov x21, x1
|
|
|
|
/* Save arch specific suspend fn and arg to stack */
|
|
sub sp, sp, #PM_CTX_SIZE
|
|
stp x0, x1, [sp, #-16]!
|
|
|
|
/* x18 is gd, save it to _suspend_gd !! */
|
|
adr x0, _suspend_gd
|
|
str x18, [x0]
|
|
|
|
/* x0: pm_ctx; x1: sp where restore x8~x30 from */
|
|
add x0, sp, #16
|
|
mov x1, x19
|
|
bl cpu_suspend_save
|
|
|
|
adr lr, aborted
|
|
/* Jump to arch specific suspend */
|
|
mov x0, x20
|
|
br x21
|
|
|
|
/* Should never reach here, otherwise failed */
|
|
aborted:
|
|
/* cpu not suspended */
|
|
add sp, sp, #(16 + PM_CTX_SIZE)
|
|
/* Return -1 to the caller */
|
|
mov x0, #(-1)
|
|
|
|
suspend_return:
|
|
ldr x8, [sp, #16]
|
|
ldp x9, x10, [sp, #32]
|
|
ldp x11, x12, [sp, #48]
|
|
ldp x13, x14, [sp, #64]
|
|
ldp x15, x16, [sp, #80]
|
|
ldp x17, x18, [sp, #96]
|
|
ldp x19, x20, [sp,#112]
|
|
ldp x21, x22, [sp,#128]
|
|
ldp x23, x24, [sp,#144]
|
|
ldp x25, x26, [sp,#160]
|
|
ldp x27, x28, [sp,#176]
|
|
ldp x29, lr, [sp], #192
|
|
ret
|
|
ENDPROC(cpu_suspend)
|
|
|
|
ENTRY(cpu_do_suspend)
|
|
/*
|
|
* Save temporary x2~x12, total: 11*8=88, maybe you need not so many
|
|
* registers now, but I save them for future extendion.
|
|
*/
|
|
stp x2, x3, [sp, #-88]!
|
|
stp x4, x5, [sp, #16]
|
|
stp x6, x7, [sp, #32]
|
|
stp x8, x9, [sp, #48]
|
|
stp x10, x11, [sp,#64]
|
|
str x12, [sp, #80]
|
|
|
|
/*
|
|
* Save core registers.
|
|
*
|
|
* Note: If you want to add/sub the register here,
|
|
* remember update suspend_regs[] of struct pm_ctx.
|
|
*/
|
|
mrs x2, vbar_el2
|
|
mrs x3, cptr_el2
|
|
mrs x4, ttbr0_el2
|
|
mrs x5, tcr_el2
|
|
mrs x6, mair_el2
|
|
mrs x7, cntvoff_el2
|
|
mrs x8, sctlr_el2
|
|
mrs x9, hcr_el2
|
|
mrs x10, daif
|
|
|
|
stp x2, x3, [x0, #0]
|
|
stp x4, x5, [x0, #16]
|
|
stp x6, x7, [x0, #32]
|
|
stp x8, x9, [x0, #48]
|
|
str x10, [x0, #64]
|
|
|
|
/* Restore temporary x2~x12 */
|
|
ldp x4, x5, [sp, #16]
|
|
ldp x6, x7, [sp, #32]
|
|
ldp x8, x9, [sp, #48]
|
|
ldp x10, x11, [sp,#64]
|
|
ldr x12, [sp, #80]
|
|
ldp x2, x3, [sp], #88
|
|
ret
|
|
ENDPROC(cpu_do_suspend)
|
|
|
|
ENTRY(cpu_resume)
|
|
/* Disable interrupt */
|
|
msr daifset, #0x03
|
|
|
|
/* Load gd !! */
|
|
adr x1, _suspend_gd
|
|
ldr x2, [x1]
|
|
|
|
/* Get pm_ctx */
|
|
add x2, x2, #PM_CTX_PHYS
|
|
ldr x0, [x2]
|
|
|
|
/* Need x0=x0-16, because cpu_do_resume needs it */
|
|
ldp x1, lr, [x0], #16
|
|
mov sp, x1
|
|
ret
|
|
ENDPROC(cpu_resume)
|
|
|
|
/*
|
|
* void sm_do_cpu_do_resume(paddr suspend_regs) __noreturn;
|
|
* Restore the registers stored when cpu_do_suspend
|
|
* x0 points to the physical base address of the suspend_regs
|
|
* field of struct pm_ctx.
|
|
*/
|
|
ENTRY(cpu_do_resume)
|
|
/*
|
|
* Invalidate local tlb entries before turning on MMU !!!
|
|
*/
|
|
tlbi alle2
|
|
dsb sy
|
|
isb
|
|
|
|
ldp x2, x3, [x0]
|
|
ldp x4, x5, [x0, #16]
|
|
ldp x6, x7, [x0, #32]
|
|
ldp x8, x9, [x0, #48]
|
|
ldp x10, x11, [x0, #64]
|
|
ldr x12, [x0, #80]
|
|
|
|
/* Restore core register */
|
|
msr vbar_el2, x2
|
|
msr cptr_el2, x3
|
|
msr ttbr0_el2, x4
|
|
msr tcr_el2, x5
|
|
msr mair_el2, x6
|
|
msr cntvoff_el2, x7
|
|
msr hcr_el2, x9
|
|
|
|
/* Enable MMU here */
|
|
msr sctlr_el2, x8
|
|
dsb sy
|
|
isb
|
|
|
|
/* resume interrupt */
|
|
msr daif, x10
|
|
|
|
mov x0, #0
|
|
b suspend_return
|
|
ENDPROC(cpu_do_resume)
|
|
|
|
.align 3
|
|
_suspend_gd:
|
|
.long 0x0
|
|
|