code:
/* SPDX-License-Identifier: GPL-2.0-only */
/*
 *  linux/arch/arm/kernel/entry-common.S
 *
 *  Copyright (C) 2000 Russell King
 */

#include <asm/assembler.h>
#include <asm/unistd.h>
#include <asm/ftrace.h>
#include <asm/unwind.h>
#include <asm/memory.h>
#ifdef CONFIG_AEABI
#include <asm/unistd-oabi.h>
#endif

	.equ	NR_syscalls, __NR_syscalls

	.macro  arch_ret_to_user, tmp
#ifdef CONFIG_ARCH_IOP32X
	mrc	p15, 0, \tmp, c15, c1, 0
	tst	\tmp, #(1 << 6)
	bicne	\tmp, \tmp, #(1 << 6)
	mcrne	p15, 0, \tmp, c15, c1, 0	@ Disable cp6 access
#endif
	.endm

#include "entry-header.S"

saved_psr	.req	r8
#if defined(CONFIG_TRACE_IRQFLAGS) || defined(CONFIG_CONTEXT_TRACKING_USER)
saved_pc	.req	r9
#define TRACE(x...) x
#else
saved_pc	.req	lr
#define TRACE(x...)
#endif

	.section .entry.text,"ax",%progbits
	.align	5
#if !(IS_ENABLED(CONFIG_TRACE_IRQFLAGS) || IS_ENABLED(CONFIG_CONTEXT_TRACKING_USER) || \
	IS_ENABLED(CONFIG_DEBUG_RSEQ))
/*
 * This is the fast syscall return path.  We do as little as possible here,
 * such as avoiding writing r0 to the stack.  We only use this path if we
 * have tracing, context tracking and rseq debug disabled - the overheads
 * from those features make this path too inefficient.
 */
ret_fast_syscall:
__ret_fast_syscall:
 UNWIND(.fnstart	)
 UNWIND(.cantunwind	)
	disable_irq_notrace			@ disable interrupts
	ldr	r1, [tsk, #TI_FLAGS]		@ re-check for syscall tracing
	movs	r1, r1, lsl #16
	bne	fast_work_pending

	/* perform architecture specific actions before user return */
	arch_ret_to_user r1

	restore_user_regs fast = 1, offset = S_OFF
 UNWIND(.fnend		)
ENDPROC(ret_fast_syscall)

	/* Ok, we need to do extra processing, enter the slow path. */
fast_work_pending:
	str	r0, [sp, #S_R0+S_OFF]!		@ returned r0
	/* fall through to work_pending */
#else
/*
 * The "replacement" ret_fast_syscall for when tracing, context tracking,
 * or rseq debug is enabled.  As we will need to call out to some C functions,
 * we save r0 first to avoid needing to save registers around each C function
 * call.
 */
ret_fast_syscall:
__ret_fast_syscall:
 UNWIND(.fnstart	)
 UNWIND(.cantunwind	)
	str	r0, [sp, #S_R0 + S_OFF]!	@ save returned r0
#if IS_ENABLED(CONFIG_DEBUG_RSEQ)
	/* do_rseq_syscall needs interrupts enabled. */
	mov	r0, sp				@ 'regs'
	bl	do_rseq_syscall
#endif
	disable_irq_notrace			@ disable interrupts
	ldr	r1, [tsk, #TI_FLAGS]		@ re-check for syscall tracing
	movs	r1, r1, lsl #16
	beq	no_work_pending
 UNWIND(.fnend		)
ENDPROC(ret_fast_syscall)

	/* Slower path - fall through to work_pending */
#endif

	tst	r1, #_TIF_SYSCALL_WORK
	bne	__sys_trace_return_nosave
slow_work_pending:
	mov	r0, sp				@ 'regs'
	mov	r2, why				@ 'syscall'
	bl	do_work_pending
	cmp	r0, #0
	beq	no_work_pending
	movlt	scno, #(__NR_restart_syscall - __NR_SYSCALL_BASE)
	ldmia	sp, {r0 - r6}			@ have to reload r0 - r6
	b	local_restart			@ ... and off we go
ENDPROC(ret_fast_syscall)

/*
 * "slow" syscall return path.  "why" tells us if this was a real syscall.
 * IRQs may be enabled here, so always disable them.  Note that we use the
 * "notrace" version to avoid calling into the tracing code unnecessarily.
 * do_work_pending() will update this state if necessary.
 */
ENTRY(ret_to_user)
ret_slow_syscall:
#if IS_ENABLED(CONFIG_DEBUG_RSEQ)
	/* do_rseq_syscall needs interrupts enabled. */
	enable_irq_notrace			@ enable interrupts
	mov	r0, sp				@ 'regs'
	bl	do_rseq_syscall
#endif
	disable_irq_notrace			@ disable interrupts
ENTRY(ret_to_user_from_irq)
	ldr	r1, [tsk, #TI_FLAGS]
	movs	r1, r1, lsl #16
	bne	slow_work_pending
no_work_pending:
	asm_trace_hardirqs_on save = 0

	/* perform architecture specific actions before user return */
	arch_ret_to_user r1
	ct_user_enter save = 0

	restore_user_regs fast = 0, offset = 0
ENDPROC(ret_to_user_from_irq)
ENDPROC(ret_to_user)

/*
 * This is how we return from a fork.
 */
ENTRY(ret_from_fork)
	bl	schedule_tail
	cmp	r5, #0
	movne	r0, r4
	badrne	lr, 1f
	retne	r5
1:	get_thread_info tsk
	b	ret_slow_syscall
ENDPROC(ret_from_fork)

/*=============================================================================
 * SWI handler
 *-----------------------------------------------------------------------------
 */

	.align	5
#ifdef CONFIG_HARDEN_BRANCH_HISTORY
ENTRY(vector_bhb_loop8_swi)
	sub	sp, sp, #PT_REGS_SIZE
	stmia	sp, {r0 - r12}
	mov	r8, #8
1:	b	2f
2:	subs	r8, r8, #1
	bne	1b
	dsb	nsh
	isb
	b	3f
ENDPROC(vector_bhb_loop8_swi)

	.align	5
ENTRY(vector_bhb_bpiall_swi)
	sub	sp, sp, #PT_REGS_SIZE
	stmia	sp, {r0 - r12}
	mcr	p15, 0, r8, c7, c5, 6	@ BPIALL
	isb
	b	3f
ENDPROC(vector_bhb_bpiall_swi)
#endif
	.align	5
ENTRY(vector_swi)
#ifdef CONFIG_CPU_V7M
	v7m_exception_entry
#else
	sub	sp, sp, #PT_REGS_SIZE
	stmia	sp, {r0 - r12}			@ Calling r0 - r12
3:
 ARM(	add	r8, sp, #S_PC		)
 ARM(	stmdb	r8, {sp, lr}^		)	@ Calling sp, lr
 THUMB(	mov	r8, sp			)
 THUMB(	store_user_sp_lr r8, r10, S_SP	)	@ calling sp, lr
	mrs	saved_psr, spsr			@ called from non-FIQ mode, so ok.
 TRACE(	mov	saved_pc, lr		)
	str	saved_pc, [sp, #S_PC]		@ Save calling PC
	str	saved_psr, [sp, #S_PSR]		@ Save CPSR
	str	r0, [sp, #S_OLD_R0]		@ Save OLD_R0
#endif
	reload_current r10, ip
	zero_fp
	alignment_trap r10, ip, cr_alignment
	asm_trace_hardirqs_on save=0
	enable_irq_notrace
	ct_user_exit save=0

	/*
	 * Get the system call number.
	 */

#if defined(CONFIG_OABI_COMPAT)

	/*
	 * If we have CONFIG_OABI_COMPAT then we need to look at the swi
	 * value to determine if it is an EABI or an old ABI call.
	 */
#ifdef CONFIG_ARM_THUMB
	tst	saved_psr, #PSR_T_BIT
	movne	r10, #0				@ no thumb OABI emulation
 USER(	ldreq	r10, [saved_pc, #-4]	)	@ get SWI instruction
#else
 USER(	ldr	r10, [saved_pc, #-4]	)	@ get SWI instruction
#endif
 ARM_BE8(rev	r10, r10)			@ little endian instruction

#elif defined(CONFIG_AEABI)

	/*
	 * Pure EABI user space always put syscall number into scno (r7).
	 */
#elif defined(CONFIG_ARM_THUMB)
	/* Legacy ABI only, possibly thumb mode. */
	tst	saved_psr, #PSR_T_BIT		@ this is SPSR from save_user_regs
	addne	scno, r7, #__NR_SYSCALL_BASE	@ put OS number in
 USER(	ldreq	scno, [saved_pc, #-4]	)

#else
	/* Legacy ABI only. */
 USER(	ldr	scno, [saved_pc, #-4]	)	@ get SWI instruction
#endif

	/* saved_psr and saved_pc are now dead */

	uaccess_disable tbl
	get_thread_info tsk

	adr	tbl, sys_call_table		@ load syscall table pointer

#if defined(CONFIG_OABI_COMPAT)
	/*
	 * If the swi argument is zero, this is an EABI call and we do nothing.
	 *
	 * If this is an old ABI call, get the syscall number into scno and
	 * get the old ABI syscall table address.
	 */
	bics	r10, r10, #0xff000000
	strne	r10, [tsk, #TI_ABI_SYSCALL]
	streq	scno, [tsk, #TI_ABI_SYSCALL]
	eorne	scno, r10, #__NR_OABI_SYSCALL_BASE
	ldrne	tbl, =sys_oabi_call_table
#elif !defined(CONFIG_AEABI)
	bic	scno, scno, #0xff000000		@ mask off SWI op-code
	str	scno, [tsk, #TI_ABI_SYSCALL]
	eor	scno, scno, #__NR_SYSCALL_BASE	@ check OS number
#else
	str	scno, [tsk, #TI_ABI_SYSCALL]
#endif
	/*
	 * Reload the registers that may have been corrupted on entry to
	 * the syscall assembly (by tracing or context tracking.)
	 */
 TRACE(	ldmia	sp, {r0 - r3}		)

local_restart:
	ldr	r10, [tsk, #TI_FLAGS]		@ check for syscall tracing
	stmdb	sp!, {r4, r5}			@ push fifth and sixth args

	tst	r10, #_TIF_SYSCALL_WORK		@ are we tracing syscalls?
	bne	__sys_trace

	invoke_syscall tbl, scno, r10, __ret_fast_syscall

	add	r1, sp, #S_OFF
2:	cmp	scno, #(__ARM_NR_BASE - __NR_SYSCALL_BASE)
	eor	r0, scno, #__NR_SYSCALL_BASE	@ put OS number back
	bcs	arm_syscall
	mov	why, #0				@ no longer a real syscall
	b	sys_ni_syscall			@ not private func

#if defined(CONFIG_OABI_COMPAT) || !defined(CONFIG_AEABI)
	/*
	 * We failed to handle a fault trying to access the page
	 * containing the swi instruction, but we're not really in a
	 * position to return -EFAULT. Instead, return back to the
	 * instruction and re-enter the user fault handling path trying
	 * to page it in. This will likely result in sending SEGV to the
	 * current task.
	 */
9001:
	sub	lr, saved_pc, #4
	str	lr, [sp, #S_PC]
	get_thread_info tsk
	b	ret_fast_syscall
#endif
ENDPROC(vector_swi)
	.ltorg

	/*
	 * This is the really slow path.  We're going to be doing
	 * context switches, and waiting for our parent to respond.
	 */
__sys_trace:
	add	r0, sp, #S_OFF
	bl	syscall_trace_enter
	mov	scno, r0
	invoke_syscall tbl, scno, r10, __sys_trace_return, reload=1
	cmp	scno, #-1			@ skip the syscall?
	bne	2b
	add	sp, sp, #S_OFF			@ restore stack

__sys_trace_return_nosave:
	enable_irq_notrace
	mov	r0, sp
	bl	syscall_trace_exit
	b	ret_slow_syscall

__sys_trace_return:
	str	r0, [sp, #S_R0 + S_OFF]!	@ save returned r0
	mov	r0, sp
	bl	syscall_trace_exit
	b	ret_slow_syscall

	.macro	syscall_table_start, sym
	.equ	__sys_nr, 0
	.type	\sym, #object
ENTRY(\sym)
	.endm

	.macro	syscall, nr, func
	.ifgt	__sys_nr - \nr
	.error	"Duplicated/unorded system call entry"
	.endif
	.rept	\nr - __sys_nr
	.long	sys_ni_syscall
	.endr
	.long	\func
	.equ	__sys_nr, \nr + 1
	.endm

	.macro	syscall_table_end, sym
	.ifgt	__sys_nr - __NR_syscalls
	.error	"System call table too big"
	.endif
	.rept	__NR_syscalls - __sys_nr
	.long	sys_ni_syscall
	.endr
	.size	\sym, . - \sym
	.endm

#define __SYSCALL_WITH_COMPAT(nr, native, compat)	__SYSCALL(nr, native)
#define __SYSCALL(nr, func) syscall nr, func

/*
 * This is the syscall table declaration for native ABI syscalls.
 * With EABI a couple syscalls are obsolete and defined as sys_ni_syscall.
 */
	syscall_table_start sys_call_table
#ifdef CONFIG_AEABI
#include <calls-eabi.S>
#else
#include <calls-oabi.S>
#endif
	syscall_table_end sys_call_table

/*============================================================================
 * Special system call wrappers
 */
@ r0 = syscall number
@ r8 = syscall table
sys_syscall:
		bic	scno, r0, #__NR_OABI_SYSCALL_BASE
		cmp	scno, #__NR_syscall - __NR_SYSCALL_BASE
		cmpne	scno, #NR_syscalls	@ check range
#ifdef CONFIG_CPU_SPECTRE
		movhs	scno, #0
		csdb
#endif
		stmialo	sp, {r5, r6}		@ shuffle args
		movlo	r0, r1
		movlo	r1, r2
		movlo	r2, r3
		movlo	r3, r4
		ldrlo	pc, [tbl, scno, lsl #2]
		b	sys_ni_syscall
ENDPROC(sys_syscall)

sys_sigreturn_wrapper:
		add	r0, sp, #S_OFF
		mov	why, #0		@ prevent syscall restart handling
		b	sys_sigreturn
ENDPROC(sys_sigreturn_wrapper)

sys_rt_sigreturn_wrapper:
		add	r0, sp, #S_OFF
		mov	why, #0		@ prevent syscall restart handling
		b	sys_rt_sigreturn
ENDPROC(sys_rt_sigreturn_wrapper)

sys_statfs64_wrapper:
		teq	r1, #88
		moveq	r1, #84
		b	sys_statfs64
ENDPROC(sys_statfs64_wrapper)

sys_fstatfs64_wrapper:
		teq	r1, #88
		moveq	r1, #84
		b	sys_fstatfs64
ENDPROC(sys_fstatfs64_wrapper)

/*
 * Note: off_4k (r5) is always units of 4K.  If we can't do the requested
 * offset, we return EINVAL.
 */
sys_mmap2:
		str	r5, [sp, #4]
		b	sys_mmap_pgoff
ENDPROC(sys_mmap2)

#ifdef CONFIG_OABI_COMPAT

/*
 * These are syscalls with argument register differences
 */

sys_oabi_pread64:
		stmia	sp, {r3, r4}
		b	sys_pread64
ENDPROC(sys_oabi_pread64)

sys_oabi_pwrite64:
		stmia	sp, {r3, r4}
		b	sys_pwrite64
ENDPROC(sys_oabi_pwrite64)

sys_oabi_truncate64:
		mov	r3, r2
		mov	r2, r1
		b	sys_truncate64
ENDPROC(sys_oabi_truncate64)

sys_oabi_ftruncate64:
		mov	r3, r2
		mov	r2, r1
		b	sys_ftruncate64
ENDPROC(sys_oabi_ftruncate64)

sys_oabi_readahead:
		str	r3, [sp]
		mov	r3, r2
		mov	r2, r1
		b	sys_readahead
ENDPROC(sys_oabi_readahead)

/*
 * Let's declare a second syscall table for old ABI binaries
 * using the compatibility syscall entries.
 */
	syscall_table_start sys_oabi_call_table
#undef __SYSCALL_WITH_COMPAT
#define __SYSCALL_WITH_COMPAT(nr, native, compat)	__SYSCALL(nr, compat)
#include <calls-oabi.S>
	syscall_table_end sys_oabi_call_table

#endif


code:
/* SPDX-License-Identifier: GPL-2.0-only */

#include <asm/assembler.h>
#include <asm/ftrace.h>
#include <asm/unwind.h>

#include "entry-header.S"

/*
 * When compiling with -pg, gcc inserts a call to the mcount routine at the
 * start of every function.  In mcount, apart from the function's address (in
 * lr), we need to get hold of the function's caller's address.
 *
 * Newer GCCs (4.4+) solve this problem by using a version of mcount with call
 * sites like:
 *
 *	push	{lr}
 *	bl	__gnu_mcount_nc
 *
 * With these compilers, frame pointers are not necessary.
 *
 * mcount can be thought of as a function called in the middle of a subroutine
 * call.  As such, it needs to be transparent for both the caller and the
 * callee: the original lr needs to be restored when leaving mcount, and no
 * registers should be clobbered.
 *
 * When using dynamic ftrace, we patch out the mcount call by a "add sp, #4"
 * instead of the __gnu_mcount_nc call (see arch/arm/kernel/ftrace.c).
 */

.macro mcount_adjust_addr rd, rn
	bic	\rd, \rn, #1		@ clear the Thumb bit if present
	sub	\rd, \rd, #MCOUNT_INSN_SIZE
.endm

.macro __mcount suffix
	mcount_enter
	ldr_va	r2, ftrace_trace_function
	badr	r0, .Lftrace_stub
	cmp	r0, r2
	bne	1f

#ifdef CONFIG_FUNCTION_GRAPH_TRACER
	ldr_va	r2, ftrace_graph_return
	cmp	r0, r2
	bne	ftrace_graph_caller\suffix

	ldr_va	r2, ftrace_graph_entry
	mov_l	r0, ftrace_graph_entry_stub
	cmp	r0, r2
	bne	ftrace_graph_caller\suffix
#endif

	mcount_exit

1: 	mcount_get_lr	r1			@ lr of instrumented func
	mcount_adjust_addr	r0, lr		@ instrumented function
	badr	lr, 2f
	mov	pc, r2
2:	mcount_exit
.endm

#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS

.macro __ftrace_regs_caller

	str	lr, [sp, #-8]!	@ store LR as PC and make space for CPSR/OLD_R0,
				@ OLD_R0 will overwrite previous LR

	ldr	lr, [sp, #8]    @ get previous LR

	str	r0, [sp, #8]	@ write r0 as OLD_R0 over previous LR

	str	lr, [sp, #-4]!	@ store previous LR as LR

	add 	lr, sp, #16	@ move in LR the value of SP as it was
				@ before the push {lr} of the mcount mechanism

	push	{r0-r11, ip, lr}

	@ stack content at this point:
	@ 0  4          48   52       56            60   64    68       72
	@ R0 | R1 | ... | IP | SP + 4 | previous LR | LR | PSR | OLD_R0 |

	mov	r3, sp				@ struct pt_regs*

	ldr_va	r2, function_trace_op		@ pointer to the current
						@ function tracing op

	ldr	r1, [sp, #S_LR]			@ lr of instrumented func

	ldr	lr, [sp, #S_PC]			@ get LR

	mcount_adjust_addr	r0, lr		@ instrumented function

	.globl ftrace_regs_call
ftrace_regs_call:
	bl	ftrace_stub

#ifdef CONFIG_FUNCTION_GRAPH_TRACER
	.globl ftrace_graph_regs_call
ftrace_graph_regs_call:
ARM(	mov	r0, r0	)
THUMB(	nop.w		)
#endif

	@ pop saved regs
	pop	{r0-r11, ip, lr}		@ restore r0 through r12
	ldr	lr, [sp], #4			@ restore LR
	ldr	pc, [sp], #12
.endm

#ifdef CONFIG_FUNCTION_GRAPH_TRACER
.macro __ftrace_graph_regs_caller

#ifdef CONFIG_UNWINDER_FRAME_POINTER
	sub	r0, fp, #4		@ lr of instrumented routine (parent)
#else
	add	r0, sp, #S_LR
#endif

	@ called from __ftrace_regs_caller
	ldr	r1, [sp, #S_PC]		@ instrumented routine (func)
	mcount_adjust_addr	r1, r1

	mov	r2, fpreg		@ frame pointer
	add	r3, sp, #PT_REGS_SIZE
	bl	prepare_ftrace_return

	@ pop registers saved in ftrace_regs_caller
	pop	{r0-r11, ip, lr}		@ restore r0 through r12
	ldr	lr, [sp], #4			@ restore LR
	ldr	pc, [sp], #12

.endm
#endif
#endif

.macro __ftrace_caller suffix
	mcount_enter

	mcount_get_lr	r1			@ lr of instrumented func
	mcount_adjust_addr	r0, lr		@ instrumented function

#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
	ldr_va	r2, function_trace_op		@ pointer to the current
						@ function tracing op
	mov r3, #0				@ regs is NULL
#endif

	.globl ftrace_call\suffix
ftrace_call\suffix:
	bl	ftrace_stub

#ifdef CONFIG_FUNCTION_GRAPH_TRACER
	.globl ftrace_graph_call\suffix
ftrace_graph_call\suffix:
ARM(	mov	r0, r0	)
THUMB(	nop.w		)
#endif

	mcount_exit
.endm

.macro __ftrace_graph_caller
#ifdef CONFIG_UNWINDER_FRAME_POINTER
	sub	r0, fp, #4		@ &lr of instrumented routine (&parent)
#else
	add	r0, sp, #20
#endif
#ifdef CONFIG_DYNAMIC_FTRACE
	@ called from __ftrace_caller, saved in mcount_enter
	ldr	r1, [sp, #16]		@ instrumented routine (func)
	mcount_adjust_addr	r1, r1
#else
	@ called from __mcount, untouched in lr
	mcount_adjust_addr	r1, lr	@ instrumented routine (func)
#endif
	mov	r2, fpreg		@ frame pointer
	add	r3, sp, #24
	bl	prepare_ftrace_return
	mcount_exit
.endm

/*
 * __gnu_mcount_nc
 */

.macro mcount_enter
/*
 * This pad compensates for the push {lr} at the call site.  Note that we are
 * unable to unwind through a function which does not otherwise save its lr.
 */
 UNWIND(.pad	#4)
	stmdb	sp!, {r0-r3, lr}
 UNWIND(.save	{r0-r3, lr})
.endm

.macro mcount_get_lr reg
	ldr	\reg, [sp, #20]
.endm

.macro mcount_exit
	ldmia	sp!, {r0-r3}
	ldr	lr, [sp, #4]
	ldr	pc, [sp], #8
.endm

ENTRY(__gnu_mcount_nc)
UNWIND(.fnstart)
#ifdef CONFIG_DYNAMIC_FTRACE
	push	{lr}
	ldr	lr, [sp, #4]
	ldr	pc, [sp], #8
#else
	__mcount
#endif
UNWIND(.fnend)
ENDPROC(__gnu_mcount_nc)

#ifdef CONFIG_DYNAMIC_FTRACE
ENTRY(ftrace_caller)
UNWIND(.fnstart)
	__ftrace_caller
UNWIND(.fnend)
ENDPROC(ftrace_caller)

#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
ENTRY(ftrace_regs_caller)
UNWIND(.fnstart)
	__ftrace_regs_caller
UNWIND(.fnend)
ENDPROC(ftrace_regs_caller)
#endif

#endif

#ifdef CONFIG_FUNCTION_GRAPH_TRACER
ENTRY(ftrace_graph_caller)
UNWIND(.fnstart)
	__ftrace_graph_caller
UNWIND(.fnend)
ENDPROC(ftrace_graph_caller)

#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
ENTRY(ftrace_graph_regs_caller)
UNWIND(.fnstart)
	__ftrace_graph_regs_caller
UNWIND(.fnend)
ENDPROC(ftrace_graph_regs_caller)
#endif
#endif

.purgem mcount_enter
.purgem mcount_get_lr
.purgem mcount_exit

#ifdef CONFIG_FUNCTION_GRAPH_TRACER
ENTRY(return_to_handler)
	stmdb	sp!, {r0-r3}
	add	r0, sp, #16		@ sp at exit of instrumented routine
	bl	ftrace_return_to_handler
	mov	lr, r0			@ r0 has real ret addr
	ldmia	sp!, {r0-r3}
	ret	lr
ENDPROC(return_to_handler)
#endif

ENTRY(ftrace_stub)
.Lftrace_stub:
	ret	lr
ENDPROC(ftrace_stub)

#ifdef CONFIG_DYNAMIC_FTRACE

	__INIT

	.macro	init_tramp, dst:req
ENTRY(\dst\()_from_init)
	ldr	pc, =\dst
ENDPROC(\dst\()_from_init)
	.endm

	init_tramp	ftrace_caller
#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
	init_tramp	ftrace_regs_caller
#endif
#endif

repo_name: angery/linux


code:
/* SPDX-License-Identifier: GPL-2.0 */
#include <linux/init.h>
#include <linux/linkage.h>

#include <asm/assembler.h>
#include <asm/asm-offsets.h>
#include <asm/errno.h>
#include <asm/thread_info.h>
#include <asm/uaccess-asm.h>
#include <asm/v7m.h>

@ Bad Abort numbers
@ -----------------
@
#define BAD_PREFETCH	0
#define BAD_DATA	1
#define BAD_ADDREXCPTN	2
#define BAD_IRQ		3
#define BAD_UNDEFINSTR	4

@
@ Most of the stack format comes from struct pt_regs, but with
@ the addition of 8 bytes for storing syscall args 5 and 6.
@ This _must_ remain a multiple of 8 for EABI.
@
#define S_OFF		8

/*
 * The SWI code relies on the fact that R0 is at the bottom of the stack
 * (due to slow/fast restore user regs).
 */
#if S_R0 != 0
#error "Please fix"
#endif

	.macro	zero_fp
#ifdef CONFIG_FRAME_POINTER
	mov	fp, #0
#endif
	.endm

#ifdef CONFIG_ALIGNMENT_TRAP
#define ATRAP(x...) x
#else
#define ATRAP(x...)
#endif

	.macro	alignment_trap, rtmp1, rtmp2, label
#ifdef CONFIG_ALIGNMENT_TRAP
	mrc	p15, 0, \rtmp2, c1, c0, 0
	ldr_va	\rtmp1, \label
	teq	\rtmp1, \rtmp2
	mcrne	p15, 0, \rtmp1, c1, c0, 0
#endif
	.endm

#ifdef CONFIG_CPU_V7M
/*
 * ARMv7-M exception entry/exit macros.
 *
 * xPSR, ReturnAddress(), LR (R14), R12, R3, R2, R1, and R0 are
 * automatically saved on the current stack (32 words) before
 * switching to the exception stack (SP_main).
 *
 * If exception is taken while in user mode, SP_main is
 * empty. Otherwise, SP_main is aligned to 64 bit automatically
 * (CCR.STKALIGN set).
 *
 * Linux assumes that the interrupts are disabled when entering an
 * exception handler and it may BUG if this is not the case. Interrupts
 * are disabled during entry and reenabled in the exit macro.
 *
 * v7m_exception_slow_exit is used when returning from SVC or PendSV.
 * When returning to kernel mode, we don't return from exception.
 */
	.macro	v7m_exception_entry
	@ determine the location of the registers saved by the core during
	@ exception entry. Depending on the mode the cpu was in when the
	@ exception happend that is either on the main or the process stack.
	@ Bit 2 of EXC_RETURN stored in the lr register specifies which stack
	@ was used.
	tst	lr, #EXC_RET_STACK_MASK
	mrsne	r12, psp
	moveq	r12, sp

	@ we cannot rely on r0-r3 and r12 matching the value saved in the
	@ exception frame because of tail-chaining. So these have to be
	@ reloaded.
	ldmia	r12!, {r0-r3}

	@ Linux expects to have irqs off. Do it here before taking stack space
	cpsid	i

	sub	sp, #PT_REGS_SIZE-S_IP
	stmdb	sp!, {r0-r11}

	@ load saved r12, lr, return address and xPSR.
	@ r0-r7 are used for signals and never touched from now on. Clobbering
	@ r8-r12 is OK.
	mov	r9, r12
	ldmia	r9!, {r8, r10-r12}

	@ calculate the original stack pointer value.
	@ r9 currently points to the memory location just above the auto saved
	@ xPSR.
	@ The cpu might automatically 8-byte align the stack. Bit 9
	@ of the saved xPSR specifies if stack aligning took place. In this case
	@ another 32-bit value is included in the stack.

	tst	r12, V7M_xPSR_FRAMEPTRALIGN
	addne	r9, r9, #4

	@ store saved r12 using str to have a register to hold the base for stm
	str	r8, [sp, #S_IP]
	add	r8, sp, #S_SP
	@ store r13-r15, xPSR
	stmia	r8!, {r9-r12}
	@ store old_r0
	str	r0, [r8]
	.endm

        /*
	 * PENDSV and SVCALL are configured to have the same exception
	 * priorities. As a kernel thread runs at SVCALL execution priority it
	 * can never be preempted and so we will never have to return to a
	 * kernel thread here.
         */
	.macro	v7m_exception_slow_exit ret_r0
	cpsid	i
	ldr	lr, =exc_ret
	ldr	lr, [lr]

	@ read original r12, sp, lr, pc and xPSR
	add	r12, sp, #S_IP
	ldmia	r12, {r1-r5}

	@ an exception frame is always 8-byte aligned. To tell the hardware if
	@ the sp to be restored is aligned or not set bit 9 of the saved xPSR
	@ accordingly.
	tst	r2, #4
	subne	r2, r2, #4
	orrne	r5, V7M_xPSR_FRAMEPTRALIGN
	biceq	r5, V7M_xPSR_FRAMEPTRALIGN

	@ ensure bit 0 is cleared in the PC, otherwise behaviour is
	@ unpredictable
	bic	r4, #1

	@ write basic exception frame
	stmdb	r2!, {r1, r3-r5}
	ldmia	sp, {r1, r3-r5}
	.if	\ret_r0
	stmdb	r2!, {r0, r3-r5}
	.else
	stmdb	r2!, {r1, r3-r5}
	.endif

	@ restore process sp
	msr	psp, r2

	@ restore original r4-r11
	ldmia	sp!, {r0-r11}

	@ restore main sp
	add	sp, sp, #PT_REGS_SIZE-S_IP

	cpsie	i
	bx	lr
	.endm
#endif	/* CONFIG_CPU_V7M */

	@
	@ Store/load the USER SP and LR registers by switching to the SYS
	@ mode. Useful in Thumb-2 mode where "stm/ldm rd, {sp, lr}^" is not
	@ available. Should only be called from SVC mode
	@
	.macro	store_user_sp_lr, rd, rtemp, offset = 0
	mrs	\rtemp, cpsr
	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
	msr	cpsr_c, \rtemp			@ switch to the SYS mode

	str	sp, [\rd, #\offset]		@ save sp_usr
	str	lr, [\rd, #\offset + 4]		@ save lr_usr

	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
	msr	cpsr_c, \rtemp			@ switch back to the SVC mode
	.endm

	.macro	load_user_sp_lr, rd, rtemp, offset = 0
	mrs	\rtemp, cpsr
	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
	msr	cpsr_c, \rtemp			@ switch to the SYS mode

	ldr	sp, [\rd, #\offset]		@ load sp_usr
	ldr	lr, [\rd, #\offset + 4]		@ load lr_usr

	eor	\rtemp, \rtemp, #(SVC_MODE ^ SYSTEM_MODE)
	msr	cpsr_c, \rtemp			@ switch back to the SVC mode
	.endm

	.macro	svc_exit, rpsr, irq = 0
	.if	\irq != 0
	@ IRQs already off
#ifdef CONFIG_TRACE_IRQFLAGS
	@ The parent context IRQs must have been enabled to get here in
	@ the first place, so there's no point checking the PSR I bit.
	bl	trace_hardirqs_on
#endif
	.else
	@ IRQs off again before pulling preserved data off the stack
	disable_irq_notrace
#ifdef CONFIG_TRACE_IRQFLAGS
	tst	\rpsr, #PSR_I_BIT
	bleq	trace_hardirqs_on
	tst	\rpsr, #PSR_I_BIT
	blne	trace_hardirqs_off
#endif
	.endif
	uaccess_exit tsk, r0, r1

#ifndef CONFIG_THUMB2_KERNEL
	@ ARM mode SVC restore
	msr	spsr_cxsf, \rpsr
#if defined(CONFIG_CPU_V6) || defined(CONFIG_CPU_32v6K)
	@ We must avoid clrex due to Cortex-A15 erratum #830321
	sub	r0, sp, #4			@ uninhabited address
	strex	r1, r2, [r0]			@ clear the exclusive monitor
#endif
	ldmia	sp, {r0 - pc}^			@ load r0 - pc, cpsr
#else
	@ Thumb mode SVC restore
	ldr	lr, [sp, #S_SP]			@ top of the stack
	ldrd	r0, r1, [sp, #S_LR]		@ calling lr and pc

	@ We must avoid clrex due to Cortex-A15 erratum #830321
	strex	r2, r1, [sp, #S_LR]		@ clear the exclusive monitor

	stmdb	lr!, {r0, r1, \rpsr}		@ calling lr and rfe context
	ldmia	sp, {r0 - r12}
	mov	sp, lr
	ldr	lr, [sp], #4
	rfeia	sp!
#endif
	.endm

	@
	@ svc_exit_via_fiq - like svc_exit but switches to FIQ mode before exit
	@
	@ This macro acts in a similar manner to svc_exit but switches to FIQ
	@ mode to restore the final part of the register state.
	@
	@ We cannot use the normal svc_exit procedure because that would
	@ clobber spsr_svc (FIQ could be delivered during the first few
	@ instructions of vector_swi meaning its contents have not been
	@ saved anywhere).
	@
	@ Note that, unlike svc_exit, this macro also does not allow a caller
	@ supplied rpsr. This is because the FIQ exceptions are not re-entrant
	@ and the handlers cannot call into the scheduler (meaning the value
	@ on the stack remains correct).
	@
	.macro  svc_exit_via_fiq
	uaccess_exit tsk, r0, r1
#ifndef CONFIG_THUMB2_KERNEL
	@ ARM mode restore
	mov	r0, sp
	ldmib	r0, {r1 - r14}	@ abort is deadly from here onward (it will
				@ clobber state restored below)
	msr	cpsr_c, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT
	add	r8, r0, #S_PC
	ldr	r9, [r0, #S_PSR]
	msr	spsr_cxsf, r9
	ldr	r0, [r0, #S_R0]
	ldmia	r8, {pc}^
#else
	@ Thumb mode restore
	add	r0, sp, #S_R2
	ldr	lr, [sp, #S_LR]
	ldr	sp, [sp, #S_SP] @ abort is deadly from here onward (it will
			        @ clobber state restored below)
	ldmia	r0, {r2 - r12}
	mov	r1, #FIQ_MODE | PSR_I_BIT | PSR_F_BIT
	msr	cpsr_c, r1
	sub	r0, #S_R2
	add	r8, r0, #S_PC
	ldmia	r0, {r0 - r1}
	rfeia	r8
#endif
	.endm

	.macro	restore_user_regs, fast = 0, offset = 0
#if defined(CONFIG_CPU_32v6K) && \
    (!defined(CONFIG_CPU_V6) || defined(CONFIG_SMP))
#ifdef CONFIG_CPU_V6
ALT_SMP(nop)
ALT_UP_B(.L1_\@)
#endif
	@ The TLS register update is deferred until return to user space so we
	@ can use it for other things while running in the kernel
	mrc	p15, 0, r1, c13, c0, 3		@ get current_thread_info pointer
	ldr	r1, [r1, #TI_TP_VALUE]
	mcr	p15, 0, r1, c13, c0, 3		@ set TLS register
.L1_\@:
#endif

	uaccess_enable r1, isb=0
#ifndef CONFIG_THUMB2_KERNEL
	@ ARM mode restore
	mov	r2, sp
	ldr	r1, [r2, #\offset + S_PSR]	@ get calling cpsr
	ldr	lr, [r2, #\offset + S_PC]!	@ get pc
	tst	r1, #PSR_I_BIT | 0x0f
	bne	1f
	msr	spsr_cxsf, r1			@ save in spsr_svc
#if defined(CONFIG_CPU_V6) || defined(CONFIG_CPU_32v6K)
	@ We must avoid clrex due to Cortex-A15 erratum #830321
	strex	r1, r2, [r2]			@ clear the exclusive monitor
#endif
	.if	\fast
	ldmdb	r2, {r1 - lr}^			@ get calling r1 - lr
	.else
	ldmdb	r2, {r0 - lr}^			@ get calling r0 - lr
	.endif
	mov	r0, r0				@ ARMv5T and earlier require a nop
						@ after ldm {}^
	add	sp, sp, #\offset + PT_REGS_SIZE
	movs	pc, lr				@ return & move spsr_svc into cpsr
1:	bug	"Returning to usermode but unexpected PSR bits set?", \@
#elif defined(CONFIG_CPU_V7M)
	@ V7M restore.
	@ Note that we don't need to do clrex here as clearing the local
	@ monitor is part of the exception entry and exit sequence.
	.if	\offset
	add	sp, #\offset
	.endif
	v7m_exception_slow_exit ret_r0 = \fast
#else
	@ Thumb mode restore
	mov	r2, sp
	load_user_sp_lr r2, r3, \offset + S_SP	@ calling sp, lr
	ldr	r1, [sp, #\offset + S_PSR]	@ get calling cpsr
	ldr	lr, [sp, #\offset + S_PC]	@ get pc
	add	sp, sp, #\offset + S_SP
	tst	r1, #PSR_I_BIT | 0x0f
	bne	1f
	msr	spsr_cxsf, r1			@ save in spsr_svc

	@ We must avoid clrex due to Cortex-A15 erratum #830321
	strex	r1, r2, [sp]			@ clear the exclusive monitor

	.if	\fast
	ldmdb	sp, {r1 - r12}			@ get calling r1 - r12
	.else
	ldmdb	sp, {r0 - r12}			@ get calling r0 - r12
	.endif
	add	sp, sp, #PT_REGS_SIZE - S_SP
	movs	pc, lr				@ return & move spsr_svc into cpsr
1:	bug	"Returning to usermode but unexpected PSR bits set?", \@
#endif	/* !CONFIG_THUMB2_KERNEL */
	.endm

/*
 * Context tracking subsystem.  Used to instrument transitions
 * between user and kernel mode.
 */
	.macro ct_user_exit, save = 1
#ifdef CONFIG_CONTEXT_TRACKING_USER
	.if	\save
	stmdb   sp!, {r0-r3, ip, lr}
	bl	user_exit_callable
	ldmia	sp!, {r0-r3, ip, lr}
	.else
	bl	user_exit_callable
	.endif
#endif
	.endm

	.macro ct_user_enter, save = 1
#ifdef CONFIG_CONTEXT_TRACKING_USER
	.if	\save
	stmdb   sp!, {r0-r3, ip, lr}
	bl	user_enter_callable
	ldmia	sp!, {r0-r3, ip, lr}
	.else
	bl	user_enter_callable
	.endif
#endif
	.endm

	.macro	invoke_syscall, table, nr, tmp, ret, reload=0
#ifdef CONFIG_CPU_SPECTRE
	mov	\tmp, \nr
	cmp	\tmp, #NR_syscalls		@ check upper syscall limit
	movcs	\tmp, #0
	csdb
	badr	lr, \ret			@ return address
	.if	\reload
	add	r1, sp, #S_R0 + S_OFF		@ pointer to regs
	ldmiacc	r1, {r0 - r6}			@ reload r0-r6
	stmiacc	sp, {r4, r5}			@ update stack arguments
	.endif
	ldrcc	pc, [\table, \tmp, lsl #2]	@ call sys_* routine
#else
	cmp	\nr, #NR_syscalls		@ check upper syscall limit
	badr	lr, \ret			@ return address
	.if	\reload
	add	r1, sp, #S_R0 + S_OFF		@ pointer to regs
	ldmiacc	r1, {r0 - r6}			@ reload r0-r6
	stmiacc	sp, {r4, r5}			@ update stack arguments
	.endif
	ldrcc	pc, [\table, \nr, lsl #2]	@ call sys_* routine
#endif
	.endm

/*
 * These are the registers used in the syscall handler, and allow us to
 * have in theory up to 7 arguments to a function - r0 to r6.
 *
 * r7 is reserved for the system call number for thumb mode.
 *
 * Note that tbl == why is intentional.
 *
 * We must set at least "tsk" and "why" when calling ret_with_reschedule.
 */
scno	.req	r7		@ syscall number
tbl	.req	r8		@ syscall table pointer
why	.req	r8		@ Linux syscall (!= 0)
tsk	.req	r9		@ current thread_info

	.macro	do_overflow_check, frame_size:req
#ifdef CONFIG_VMAP_STACK
	@
	@ Test whether the SP has overflowed. Task and IRQ stacks are aligned
	@ so that SP & BIT(THREAD_SIZE_ORDER + PAGE_SHIFT) should always be
	@ zero.
	@
ARM(	tst	sp, #1 << (THREAD_SIZE_ORDER + PAGE_SHIFT)	)
THUMB(	tst	r1, #1 << (THREAD_SIZE_ORDER + PAGE_SHIFT)	)
THUMB(	it	ne						)
	bne	.Lstack_overflow_check\@

	.pushsection	.text
.Lstack_overflow_check\@:
	@
	@ The stack pointer is not pointing to a valid vmap'ed stack, but it
	@ may be pointing into the linear map instead, which may happen if we
	@ are already running from the overflow stack. We cannot detect overflow
	@ in such cases so just carry on.
	@
	str	ip, [r0, #12]			@ Stash IP on the mode stack
	ldr_va	ip, high_memory			@ Start of VMALLOC space
ARM(	cmp	sp, ip			)	@ SP in vmalloc space?
THUMB(	cmp	r1, ip			)
THUMB(	itt	lo			)
	ldrlo	ip, [r0, #12]			@ Restore IP
	blo	.Lout\@				@ Carry on

THUMB(	sub	r1, sp, r1		)	@ Restore original R1
THUMB(	sub	sp, r1			)	@ Restore original SP
	add	sp, sp, #\frame_size		@ Undo svc_entry's SP change
	b	__bad_stack			@ Handle VMAP stack overflow
	.popsection
.Lout\@:
#endif
	.endm


code:
/* SPDX-License-Identifier: GPL-2.0-only */
/*
 * linux/arch/arm/kernel/entry-v7m.S
 *
 * Copyright (C) 2008 ARM Ltd.
 *
 * Low-level vector interface routines for the ARMv7-M architecture
 */
#include <asm/memory.h>
#include <asm/glue.h>
#include <asm/thread_notify.h>
#include <asm/v7m.h>

#include "entry-header.S"

#ifdef CONFIG_TRACE_IRQFLAGS
#error "CONFIG_TRACE_IRQFLAGS not supported on the current ARMv7M implementation"
#endif

__invalid_entry:
	v7m_exception_entry
#ifdef CONFIG_PRINTK
	adr	r0, strerr
	mrs	r1, ipsr
	mov	r2, lr
	bl	_printk
#endif
	mov	r0, sp
	bl	show_regs
1:	b	1b
ENDPROC(__invalid_entry)

strerr:	.asciz	"\nUnhandled exception: IPSR = %08lx LR = %08lx\n"

	.align	2
__irq_entry:
	v7m_exception_entry

	@
	@ Invoke the IRQ handler
	@
	mov	r0, sp
	ldr_this_cpu sp, irq_stack_ptr, r1, r2

	@
	@ If we took the interrupt while running in the kernel, we may already
	@ be using the IRQ stack, so revert to the original value in that case.
	@
	subs	r2, sp, r0		@ SP above bottom of IRQ stack?
	rsbscs	r2, r2, #THREAD_SIZE	@ ... and below the top?
	movcs	sp, r0

	push	{r0, lr}		@ preserve LR and original SP

	@ routine called with r0 = struct pt_regs *
	bl	generic_handle_arch_irq

	pop	{r0, lr}
	mov	sp, r0

	@
	@ Check for any pending work if returning to user
	@
	ldr	r1, =BASEADDR_V7M_SCB
	ldr	r0, [r1, V7M_SCB_ICSR]
	tst	r0, V7M_SCB_ICSR_RETTOBASE
	beq	2f

	get_thread_info tsk
	ldr	r2, [tsk, #TI_FLAGS]
	movs	r2, r2, lsl #16
	beq	2f			@ no work pending
	mov	r0, #V7M_SCB_ICSR_PENDSVSET
	str	r0, [r1, V7M_SCB_ICSR]	@ raise PendSV

2:
	@ registers r0-r3 and r12 are automatically restored on exception
	@ return. r4-r7 were not clobbered in v7m_exception_entry so for
	@ correctness they don't need to be restored. So only r8-r11 must be
	@ restored here. The easiest way to do so is to restore r0-r7, too.
	ldmia	sp!, {r0-r11}
	add	sp, #PT_REGS_SIZE-S_IP
	cpsie	i
	bx	lr
ENDPROC(__irq_entry)

__pendsv_entry:
	v7m_exception_entry

	ldr	r1, =BASEADDR_V7M_SCB
	mov	r0, #V7M_SCB_ICSR_PENDSVCLR
	str	r0, [r1, V7M_SCB_ICSR]	@ clear PendSV

	@ execute the pending work, including reschedule
	get_thread_info tsk
	mov	why, #0
	b	ret_to_user_from_irq
ENDPROC(__pendsv_entry)

/*
 * Register switch for ARMv7-M processors.
 * r0 = previous task_struct, r1 = previous thread_info, r2 = next thread_info
 * previous and next are guaranteed not to be the same.
 */
ENTRY(__switch_to)
	.fnstart
	.cantunwind
	add	ip, r1, #TI_CPU_SAVE
	stmia	ip!, {r4 - r11}		@ Store most regs on stack
	str	sp, [ip], #4
	str	lr, [ip], #4
	mov	r5, r0
	mov	r6, r2			@ Preserve 'next'
	add	r4, r2, #TI_CPU_SAVE
	ldr	r0, =thread_notify_head
	mov	r1, #THREAD_NOTIFY_SWITCH
	bl	atomic_notifier_call_chain
	mov	r0, r5
	mov	r1, r6
	ldmia	r4, {r4 - r12, lr}	@ Load all regs saved previously
	set_current r1, r2
	mov	sp, ip
	bx	lr
	.fnend
ENDPROC(__switch_to)

	.data
#if CONFIG_CPU_V7M_NUM_IRQ <= 112
	.align	9
#else
	.align	10
#endif

/*
 * Vector table (Natural alignment need to be ensured)
 */
ENTRY(vector_table)
	.long	0			@ 0 - Reset stack pointer
	.long	__invalid_entry		@ 1 - Reset
	.long	__invalid_entry		@ 2 - NMI
	.long	__invalid_entry		@ 3 - HardFault
	.long	__invalid_entry		@ 4 - MemManage
	.long	__invalid_entry		@ 5 - BusFault
	.long	__invalid_entry		@ 6 - UsageFault
	.long	__invalid_entry		@ 7 - Reserved
	.long	__invalid_entry		@ 8 - Reserved
	.long	__invalid_entry		@ 9 - Reserved
	.long	__invalid_entry		@ 10 - Reserved
	.long	vector_swi		@ 11 - SVCall
	.long	__invalid_entry		@ 12 - Debug Monitor
	.long	__invalid_entry		@ 13 - Reserved
	.long	__pendsv_entry		@ 14 - PendSV
	.long	__invalid_entry		@ 15 - SysTick
	.rept	CONFIG_CPU_V7M_NUM_IRQ
	.long	__irq_entry		@ External Interrupts
	.endr
	.align	2
	.globl	exc_ret
exc_ret:
	.space	4


code:
// SPDX-License-Identifier: GPL-2.0
/*
 *  linux/arch/arm/kernel/fiq.c
 *
 *  Copyright (C) 1998 Russell King
 *  Copyright (C) 1998, 1999 Phil Blundell
 *
 *  FIQ support written by Philip Blundell <philb@gnu.org>, 1998.
 *
 *  FIQ support re-written by Russell King to be more generic
 *
 * We now properly support a method by which the FIQ handlers can
 * be stacked onto the vector.  We still do not support sharing
 * the FIQ vector itself.
 *
 * Operation is as follows:
 *  1. Owner A claims FIQ:
 *     - default_fiq relinquishes control.
 *  2. Owner A:
 *     - inserts code.
 *     - sets any registers,
 *     - enables FIQ.
 *  3. Owner B claims FIQ:
 *     - if owner A has a relinquish function.
 *       - disable FIQs.
 *       - saves any registers.
 *       - returns zero.
 *  4. Owner B:
 *     - inserts code.
 *     - sets any registers,
 *     - enables FIQ.
 *  5. Owner B releases FIQ:
 *     - Owner A is asked to reacquire FIQ:
 *	 - inserts code.
 *	 - restores saved registers.
 *	 - enables FIQ.
 *  6. Goto 3
 */
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/interrupt.h>
#include <linux/seq_file.h>

#include <asm/cacheflush.h>
#include <asm/cp15.h>
#include <asm/fiq.h>
#include <asm/irq.h>
#include <asm/traps.h>

#define FIQ_OFFSET ({					\
		extern void *vector_fiq_offset;		\
		(unsigned)&vector_fiq_offset;		\
	})

static unsigned long dfl_fiq_insn;
static struct pt_regs dfl_fiq_regs;

/* Default reacquire function
 * - we always relinquish FIQ control
 * - we always reacquire FIQ control
 */
static int fiq_def_op(void *ref, int relinquish)
{
	if (!relinquish) {
		/* Restore default handler and registers */
		local_fiq_disable();
		set_fiq_regs(&dfl_fiq_regs);
		set_fiq_handler(&dfl_fiq_insn, sizeof(dfl_fiq_insn));
		local_fiq_enable();

		/* FIXME: notify irq controller to standard enable FIQs */
	}

	return 0;
}

static struct fiq_handler default_owner = {
	.name	= "default",
	.fiq_op = fiq_def_op,
};

static struct fiq_handler *current_fiq = &default_owner;

int show_fiq_list(struct seq_file *p, int prec)
{
	if (current_fiq != &default_owner)
		seq_printf(p, "%*s:              %s\n", prec, "FIQ",
			current_fiq->name);

	return 0;
}

void set_fiq_handler(void *start, unsigned int length)
{
	void *base = vectors_page;
	unsigned offset = FIQ_OFFSET;

	memcpy(base + offset, start, length);
	if (!cache_is_vipt_nonaliasing())
		flush_icache_range((unsigned long)base + offset,
				   (unsigned long)base + offset + length);
	flush_icache_range(0xffff0000 + offset, 0xffff0000 + offset + length);
}

int claim_fiq(struct fiq_handler *f)
{
	int ret = 0;

	if (current_fiq) {
		ret = -EBUSY;

		if (current_fiq->fiq_op != NULL)
			ret = current_fiq->fiq_op(current_fiq->dev_id, 1);
	}

	if (!ret) {
		f->next = current_fiq;
		current_fiq = f;
	}

	return ret;
}

void release_fiq(struct fiq_handler *f)
{
	if (current_fiq != f) {
		pr_err("%s FIQ trying to release %s FIQ\n",
		       f->name, current_fiq->name);
		dump_stack();
		return;
	}

	do
		current_fiq = current_fiq->next;
	while (current_fiq->fiq_op(current_fiq->dev_id, 0));
}

static int fiq_start;

void enable_fiq(int fiq)
{
	enable_irq(fiq + fiq_start);
}

void disable_fiq(int fiq)
{
	disable_irq(fiq + fiq_start);
}

EXPORT_SYMBOL(set_fiq_handler);
EXPORT_SYMBOL(__set_fiq_regs);	/* defined in fiqasm.S */
EXPORT_SYMBOL(__get_fiq_regs);	/* defined in fiqasm.S */
EXPORT_SYMBOL(claim_fiq);
EXPORT_SYMBOL(release_fiq);
EXPORT_SYMBOL(enable_fiq);
EXPORT_SYMBOL(disable_fiq);

void __init init_FIQ(int start)
{
	unsigned offset = FIQ_OFFSET;
	dfl_fiq_insn = *(unsigned long *)(0xffff0000 + offset);
	get_fiq_regs(&dfl_fiq_regs);
	fiq_start = start;
}


code:
/*
 *  linux/arch/arm/kernel/fiqasm.S
 *
 *  Derived from code originally in linux/arch/arm/kernel/fiq.c:
 *
 *  Copyright (C) 1998 Russell King
 *  Copyright (C) 1998, 1999 Phil Blundell
 *  Copyright (C) 2011, Linaro Limited
 *
 *  FIQ support written by Philip Blundell <philb@gnu.org>, 1998.
 *
 *  FIQ support re-written by Russell King to be more generic
 *
 *  v7/Thumb-2 compatibility modifications by Linaro Limited, 2011.
 */

#include <linux/linkage.h>
#include <asm/assembler.h>

/*
 * Taking an interrupt in FIQ mode is death, so both these functions
 * disable irqs for the duration.
 */

ENTRY(__set_fiq_regs)
	mov	r2, #PSR_I_BIT | PSR_F_BIT | FIQ_MODE
	mrs	r1, cpsr
	msr	cpsr_c, r2	@ select FIQ mode
	mov	r0, r0		@ avoid hazard prior to ARMv4
	ldmia	r0!, {r8 - r12}
	ldr	sp, [r0], #4
	ldr	lr, [r0]
	msr	cpsr_c, r1	@ return to SVC mode
	mov	r0, r0		@ avoid hazard prior to ARMv4
	ret	lr
ENDPROC(__set_fiq_regs)

ENTRY(__get_fiq_regs)
	mov	r2, #PSR_I_BIT | PSR_F_BIT | FIQ_MODE
	mrs	r1, cpsr
	msr	cpsr_c, r2	@ select FIQ mode
	mov	r0, r0		@ avoid hazard prior to ARMv4
	stmia	r0!, {r8 - r12}
	str	sp, [r0], #4
	str	lr, [r0]
	msr	cpsr_c, r1	@ return to SVC mode
	mov	r0, r0		@ avoid hazard prior to ARMv4
	ret	lr
ENDPROC(__get_fiq_regs)


code:
/*
 * Dynamic function tracing support.
 *
 * Copyright (C) 2008 Abhishek Sagar <sagar.abhishek@gmail.com>
 * Copyright (C) 2010 Rabin Vincent <rabin@rab.in>
 *
 * For licencing details, see COPYING.
 *
 * Defines low-level handling of mcount calls when the kernel
 * is compiled with the -pg flag. When using dynamic ftrace, the
 * mcount call-sites get patched with NOP till they are enabled.
 * All code mutation routines here are called under stop_machine().
 */

#include <linux/ftrace.h>
#include <linux/uaccess.h>
#include <linux/module.h>
#include <linux/stop_machine.h>

#include <asm/cacheflush.h>
#include <asm/opcodes.h>
#include <asm/ftrace.h>
#include <asm/insn.h>
#include <asm/set_memory.h>
#include <asm/stacktrace.h>
#include <asm/patch.h>

/*
 * The compiler emitted profiling hook consists of
 *
 *   PUSH    {LR}
 *   BL	     __gnu_mcount_nc
 *
 * To turn this combined sequence into a NOP, we need to restore the value of
 * SP before the PUSH. Let's use an ADD rather than a POP into LR, as LR is not
 * modified anyway, and reloading LR from memory is highly likely to be less
 * efficient.
 */
#ifdef CONFIG_THUMB2_KERNEL
#define	NOP		0xf10d0d04	/* add.w sp, sp, #4 */
#else
#define	NOP		0xe28dd004	/* add   sp, sp, #4 */
#endif

#ifdef CONFIG_DYNAMIC_FTRACE

static int __ftrace_modify_code(void *data)
{
	int *command = data;

	ftrace_modify_all_code(*command);

	return 0;
}

void arch_ftrace_update_code(int command)
{
	stop_machine(__ftrace_modify_code, &command, NULL);
}

static unsigned long ftrace_nop_replace(struct dyn_ftrace *rec)
{
	return NOP;
}

void ftrace_caller_from_init(void);
void ftrace_regs_caller_from_init(void);

static unsigned long __ref adjust_address(struct dyn_ftrace *rec,
					  unsigned long addr)
{
	if (!IS_ENABLED(CONFIG_DYNAMIC_FTRACE) ||
	    system_state >= SYSTEM_FREEING_INITMEM ||
	    likely(!is_kernel_inittext(rec->ip)))
		return addr;
	if (!IS_ENABLED(CONFIG_DYNAMIC_FTRACE_WITH_REGS) ||
	    addr == (unsigned long)&ftrace_caller)
		return (unsigned long)&ftrace_caller_from_init;
	return (unsigned long)&ftrace_regs_caller_from_init;
}

void ftrace_arch_code_modify_prepare(void)
{
}

void ftrace_arch_code_modify_post_process(void)
{
	/* Make sure any TLB misses during machine stop are cleared. */
	flush_tlb_all();
}

static unsigned long ftrace_call_replace(unsigned long pc, unsigned long addr,
					 bool warn)
{
	return arm_gen_branch_link(pc, addr, warn);
}

static int ftrace_modify_code(unsigned long pc, unsigned long old,
			      unsigned long new, bool validate)
{
	unsigned long replaced;

	if (IS_ENABLED(CONFIG_THUMB2_KERNEL))
		old = __opcode_to_mem_thumb32(old);
	else
		old = __opcode_to_mem_arm(old);

	if (validate) {
		if (copy_from_kernel_nofault(&replaced, (void *)pc,
				MCOUNT_INSN_SIZE))
			return -EFAULT;

		if (replaced != old)
			return -EINVAL;
	}

	__patch_text((void *)pc, new);

	return 0;
}

int ftrace_update_ftrace_func(ftrace_func_t func)
{
	unsigned long pc;
	unsigned long new;
	int ret;

	pc = (unsigned long)&ftrace_call;
	new = ftrace_call_replace(pc, (unsigned long)func, true);

	ret = ftrace_modify_code(pc, 0, new, false);

#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
	if (!ret) {
		pc = (unsigned long)&ftrace_regs_call;
		new = ftrace_call_replace(pc, (unsigned long)func, true);

		ret = ftrace_modify_code(pc, 0, new, false);
	}
#endif

	return ret;
}

int ftrace_make_call(struct dyn_ftrace *rec, unsigned long addr)
{
	unsigned long new, old;
	unsigned long ip = rec->ip;
	unsigned long aaddr = adjust_address(rec, addr);
	struct module *mod = NULL;

#ifdef CONFIG_ARM_MODULE_PLTS
	mod = rec->arch.mod;
#endif

	old = ftrace_nop_replace(rec);

	new = ftrace_call_replace(ip, aaddr, !mod);
#ifdef CONFIG_ARM_MODULE_PLTS
	if (!new && mod) {
		aaddr = get_module_plt(mod, ip, aaddr);
		new = ftrace_call_replace(ip, aaddr, true);
	}
#endif

	return ftrace_modify_code(rec->ip, old, new, true);
}

#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS

int ftrace_modify_call(struct dyn_ftrace *rec, unsigned long old_addr,
				unsigned long addr)
{
	unsigned long new, old;
	unsigned long ip = rec->ip;

	old = ftrace_call_replace(ip, adjust_address(rec, old_addr), true);

	new = ftrace_call_replace(ip, adjust_address(rec, addr), true);

	return ftrace_modify_code(rec->ip, old, new, true);
}

#endif

int ftrace_make_nop(struct module *mod,
		    struct dyn_ftrace *rec, unsigned long addr)
{
	unsigned long aaddr = adjust_address(rec, addr);
	unsigned long ip = rec->ip;
	unsigned long old;
	unsigned long new;
	int ret;

#ifdef CONFIG_ARM_MODULE_PLTS
	/* mod is only supplied during module loading */
	if (!mod)
		mod = rec->arch.mod;
	else
		rec->arch.mod = mod;
#endif

	old = ftrace_call_replace(ip, aaddr,
				  !IS_ENABLED(CONFIG_ARM_MODULE_PLTS) || !mod);
#ifdef CONFIG_ARM_MODULE_PLTS
	if (!old && mod) {
		aaddr = get_module_plt(mod, ip, aaddr);
		old = ftrace_call_replace(ip, aaddr, true);
	}
#endif

	new = ftrace_nop_replace(rec);
	/*
	 * Locations in .init.text may call __gnu_mcount_mc via a linker
	 * emitted veneer if they are too far away from its implementation, and
	 * so validation may fail spuriously in such cases. Let's work around
	 * this by omitting those from validation.
	 */
	ret = ftrace_modify_code(ip, old, new, !is_kernel_inittext(ip));

	return ret;
}
#endif /* CONFIG_DYNAMIC_FTRACE */

#ifdef CONFIG_FUNCTION_GRAPH_TRACER
asmlinkage
void prepare_ftrace_return(unsigned long *parent, unsigned long self_addr,
			   unsigned long frame_pointer,
			   unsigned long stack_pointer)
{
	unsigned long return_hooker = (unsigned long) &return_to_handler;
	unsigned long old;

	if (unlikely(atomic_read(&current->tracing_graph_pause)))
		return;

	if (IS_ENABLED(CONFIG_UNWINDER_FRAME_POINTER)) {
		/* FP points one word below parent's top of stack */
		frame_pointer += 4;
	} else {
		struct stackframe frame = {
			.fp = frame_pointer,
			.sp = stack_pointer,
			.lr = self_addr,
			.pc = self_addr,
		};
		if (unwind_frame(&frame) < 0)
			return;
		if (frame.lr != self_addr)
			parent = frame.lr_addr;
		frame_pointer = frame.sp;
	}

	old = *parent;
	*parent = return_hooker;

	if (function_graph_enter(old, self_addr, frame_pointer, NULL))
		*parent = old;
}

#ifdef CONFIG_DYNAMIC_FTRACE
extern unsigned long ftrace_graph_call;
extern unsigned long ftrace_graph_call_old;
extern void ftrace_graph_caller_old(void);
extern unsigned long ftrace_graph_regs_call;
extern void ftrace_graph_regs_caller(void);

static int __ftrace_modify_caller(unsigned long *callsite,
				  void (*func) (void), bool enable)
{
	unsigned long caller_fn = (unsigned long) func;
	unsigned long pc = (unsigned long) callsite;
	unsigned long branch = arm_gen_branch(pc, caller_fn);
	unsigned long nop = arm_gen_nop();
	unsigned long old = enable ? nop : branch;
	unsigned long new = enable ? branch : nop;

	return ftrace_modify_code(pc, old, new, true);
}

static int ftrace_modify_graph_caller(bool enable)
{
	int ret;

	ret = __ftrace_modify_caller(&ftrace_graph_call,
				     ftrace_graph_caller,
				     enable);

#ifdef CONFIG_DYNAMIC_FTRACE_WITH_REGS
	if (!ret)
		ret = __ftrace_modify_caller(&ftrace_graph_regs_call,
				     ftrace_graph_regs_caller,
				     enable);
#endif

	return ret;
}

int ftrace_enable_ftrace_graph_caller(void)
{
	return ftrace_modify_graph_caller(true);
}

int ftrace_disable_ftrace_graph_caller(void)
{
	return ftrace_modify_graph_caller(false);
}
#endif /* CONFIG_DYNAMIC_FTRACE */
#endif /* CONFIG_FUNCTION_GRAPH_TRACER */


code:
/* SPDX-License-Identifier: GPL-2.0-only */
/*
 *  linux/arch/arm/kernel/head-common.S
 *
 *  Copyright (C) 1994-2002 Russell King
 *  Copyright (c) 2003 ARM Limited
 *  All Rights Reserved
 */
#include <asm/assembler.h>

#define ATAG_CORE 0x54410001
#define ATAG_CORE_SIZE ((2*4 + 3*4) >> 2)
#define ATAG_CORE_SIZE_EMPTY ((2*4) >> 2)

#ifdef CONFIG_CPU_BIG_ENDIAN
#define OF_DT_MAGIC 0xd00dfeed
#else
#define OF_DT_MAGIC 0xedfe0dd0 /* 0xd00dfeed in big-endian */
#endif

/*
 * Exception handling.  Something went wrong and we can't proceed.  We
 * ought to tell the user, but since we don't have any guarantee that
 * we're even running on the right architecture, we do virtually nothing.
 *
 * If CONFIG_DEBUG_LL is set we try to print out something about the error
 * and hope for the best (useful if bootloader fails to pass a proper
 * machine ID for example).
 */
	__HEAD

/* Determine validity of the r2 atags pointer.  The heuristic requires
 * that the pointer be aligned, in the first 16k of physical RAM and
 * that the ATAG_CORE marker is first and present.  If CONFIG_OF_FLATTREE
 * is selected, then it will also accept a dtb pointer.  Future revisions
 * of this function may be more lenient with the physical address and
 * may also be able to move the ATAGS block if necessary.
 *
 * Returns:
 *  r2 either valid atags pointer, valid dtb pointer, or zero
 *  r5, r6 corrupted
 */
__vet_atags:
	tst	r2, #0x3			@ aligned?
	bne	1f

	ldr	r5, [r2, #0]
#ifdef CONFIG_OF_FLATTREE
	ldr	r6, =OF_DT_MAGIC		@ is it a DTB?
	cmp	r5, r6
	beq	2f
#endif
	cmp	r5, #ATAG_CORE_SIZE		@ is first tag ATAG_CORE?
	cmpne	r5, #ATAG_CORE_SIZE_EMPTY
	bne	1f
	ldr	r5, [r2, #4]
	ldr	r6, =ATAG_CORE
	cmp	r5, r6
	bne	1f

2:	ret	lr				@ atag/dtb pointer is ok

1:	mov	r2, #0
	ret	lr
ENDPROC(__vet_atags)

/*
 * The following fragment of code is executed with the MMU on in MMU mode,
 * and uses absolute addresses; this is not position independent.
 *
 *  r0  = cp#15 control register (exc_ret for M-class)
 *  r1  = machine ID
 *  r2  = atags/dtb pointer
 *  r9  = processor ID
 */
	__INIT
__mmap_switched:

	mov	r7, r1
	mov	r8, r2
	mov	r10, r0

	adr	r4, __mmap_switched_data
	mov	fp, #0

#if defined(CONFIG_XIP_DEFLATED_DATA)
   ARM(	ldr	sp, [r4], #4 )
 THUMB(	ldr	sp, [r4] )
 THUMB(	add	r4, #4 )
	bl	__inflate_kernel_data		@ decompress .data to RAM
	teq	r0, #0
	bne	__error
#elif defined(CONFIG_XIP_KERNEL)
   ARM(	ldmia	r4!, {r0, r1, r2, sp} )
 THUMB(	ldmia	r4!, {r0, r1, r2, r3} )
 THUMB(	mov	sp, r3 )
	sub	r2, r2, r1
	bl	__memcpy			@ copy .data to RAM
#endif

   ARM(	ldmia	r4!, {r0, r1, sp} )
 THUMB(	ldmia	r4!, {r0, r1, r3} )
 THUMB(	mov	sp, r3 )
	sub	r2, r1, r0
	mov	r1, #0
	bl	__memset			@ clear .bss

	adr_l	r0, init_task			@ get swapper task_struct
	set_current r0, r1

	ldmia	r4, {r0, r1, r2, r3}
	str	r9, [r0]			@ Save processor ID
	str	r7, [r1]			@ Save machine type
	str	r8, [r2]			@ Save atags pointer
	cmp	r3, #0
	strne	r10, [r3]			@ Save control register values
#ifdef CONFIG_KASAN
	bl	kasan_early_init
#endif
	mov	lr, #0
	b	start_kernel
ENDPROC(__mmap_switched)

	.align	2
	.type	__mmap_switched_data, %object
__mmap_switched_data:
#ifdef CONFIG_XIP_KERNEL
#ifndef CONFIG_XIP_DEFLATED_DATA
	.long	_sdata				@ r0
	.long	__data_loc			@ r1
	.long	_edata_loc			@ r2
#endif
	.long	__bss_stop			@ sp (temporary stack in .bss)
#endif

	.long	__bss_start			@ r0
	.long	__bss_stop			@ r1
	.long	init_thread_union + THREAD_START_SP @ sp

	.long	processor_id			@ r0
	.long	__machine_arch_type		@ r1
	.long	__atags_pointer			@ r2
#ifdef CONFIG_CPU_CP15
	.long	cr_alignment			@ r3
#else
M_CLASS(.long	exc_ret)			@ r3
AR_CLASS(.long	0)				@ r3
#endif
	.size	__mmap_switched_data, . - __mmap_switched_data

	__FINIT
	.text

/*
 * This provides a C-API version of __lookup_processor_type
 */
ENTRY(lookup_processor_type)
	stmfd	sp!, {r4 - r6, r9, lr}
	mov	r9, r0
	bl	__lookup_processor_type
	mov	r0, r5
	ldmfd	sp!, {r4 - r6, r9, pc}
ENDPROC(lookup_processor_type)

/*
 * Read processor ID register (CP#15, CR0), and look up in the linker-built
 * supported processor list.  Note that we can't use the absolute addresses
 * for the __proc_info lists since we aren't running with the MMU on
 * (and therefore, we are not in the correct address space).  We have to
 * calculate the offset.
 *
 *	r9 = cpuid
 * Returns:
 *	r3, r4, r6 corrupted
 *	r5 = proc_info pointer in physical address space
 *	r9 = cpuid (preserved)
 */
__lookup_processor_type:
	/*
	 * Look in <asm/procinfo.h> for information about the __proc_info
	 * structure.
	 */
	adr_l	r5, __proc_info_begin
	adr_l	r6, __proc_info_end
1:	ldmia	r5, {r3, r4}			@ value, mask
	and	r4, r4, r9			@ mask wanted bits
	teq	r3, r4
	beq	2f
	add	r5, r5, #PROC_INFO_SZ		@ sizeof(proc_info_list)
	cmp	r5, r6
	blo	1b
	mov	r5, #0				@ unknown processor
2:	ret	lr
ENDPROC(__lookup_processor_type)

__error_lpae:
#ifdef CONFIG_DEBUG_LL
	adr	r0, str_lpae
	bl 	printascii
	b	__error
str_lpae: .asciz "\nError: Kernel with LPAE support, but CPU does not support LPAE.\n"
#else
	b	__error
#endif
	.align
ENDPROC(__error_lpae)

__error_p:
#ifdef CONFIG_DEBUG_LL
	adr	r0, str_p1
	bl	printascii
	mov	r0, r9
	bl	printhex8
	adr	r0, str_p2
	bl	printascii
	b	__error
str_p1:	.asciz	"\nError: unrecognized/unsupported processor variant (0x"
str_p2:	.asciz	").\n"
	.align
#endif
ENDPROC(__error_p)

__error:
#ifdef CONFIG_ARCH_RPC
/*
 * Turn the screen red on a error - RiscPC only.
 */
	mov	r0, #0x02000000
	mov	r3, #0x11
	orr	r3, r3, r3, lsl #8
	orr	r3, r3, r3, lsl #16
	str	r3, [r0], #4
	str	r3, [r0], #4
	str	r3, [r0], #4
	str	r3, [r0], #4
#endif
1:	mov	r0, r0
	b	1b
ENDPROC(__error)


code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 * XIP kernel .data segment decompressor
 *
 * Created by:	Nicolas Pitre, August 2017
 * Copyright:	(C) 2017  Linaro Limited
 */

#include <linux/init.h>
#include <linux/zutil.h>

/* for struct inflate_state */
#include "../../../lib/zlib_inflate/inftrees.h"
#include "../../../lib/zlib_inflate/inflate.h"
#include "../../../lib/zlib_inflate/infutil.h"

extern char __data_loc[];
extern char _edata_loc[];
extern char _sdata[];

/*
 * This code is called very early during the boot process to decompress
 * the .data segment stored compressed in ROM. Therefore none of the global
 * variables are valid yet, hence no kernel services such as memory
 * allocation is available. Everything must be allocated on the stack and
 * we must avoid any global data access. We use a temporary stack located
 * in the .bss area. The linker script makes sure the .bss is big enough
 * to hold our stack frame plus some room for called functions.
 *
 * We mimic the code in lib/decompress_inflate.c to use the smallest work
 * area possible. And because everything is statically allocated on the
 * stack then there is no need to clean up before returning.
 */

int __init __inflate_kernel_data(void)
{
	struct z_stream_s stream, *strm = &stream;
	struct inflate_state state;
	char *in = __data_loc;
	int rc;

	/* Check and skip gzip header (assume no filename) */
	if (in[0] != 0x1f || in[1] != 0x8b || in[2] != 0x08 || in[3] & ~3)
		return -1;
	in += 10;

	strm->workspace = &state;
	strm->next_in = in;
	strm->avail_in = _edata_loc - __data_loc;  /* upper bound */
	strm->next_out = _sdata;
	strm->avail_out = _edata_loc - __data_loc;
	zlib_inflateInit2(strm, -MAX_WBITS);
	WS(strm)->inflate_state.wsize = 0;
	WS(strm)->inflate_state.window = NULL;
	rc = zlib_inflate(strm, Z_FINISH);
	if (rc == Z_OK || rc == Z_STREAM_END)
		rc = strm->avail_out;  /* should be 0 */
	return rc;
}


code:
/* SPDX-License-Identifier: GPL-2.0-only */
/*
 *  linux/arch/arm/kernel/head-nommu.S
 *
 *  Copyright (C) 1994-2002 Russell King
 *  Copyright (C) 2003-2006 Hyok S. Choi
 *
 *  Common kernel startup code (non-paged MM)
 */
#include <linux/linkage.h>
#include <linux/init.h>
#include <linux/errno.h>

#include <asm/assembler.h>
#include <asm/ptrace.h>
#include <asm/asm-offsets.h>
#include <asm/memory.h>
#include <asm/cp15.h>
#include <asm/thread_info.h>
#include <asm/v7m.h>
#include <asm/mpu.h>
#include <asm/page.h>

/*
 * Kernel startup entry point.
 * ---------------------------
 *
 * This is normally called from the decompressor code.  The requirements
 * are: MMU = off, D-cache = off, I-cache = dont care, r0 = 0,
 * r1 = machine nr.
 *
 * See linux/arch/arm/tools/mach-types for the complete list of machine
 * numbers for r1.
 *
 */

	__HEAD

#ifdef CONFIG_CPU_THUMBONLY
	.thumb
ENTRY(stext)
#else
	.arm
ENTRY(stext)

 THUMB(	badr	r9, 1f		)	@ Kernel is always entered in ARM.
 THUMB(	bx	r9		)	@ If this is a Thumb-2 kernel,
 THUMB(	.thumb			)	@ switch to Thumb now.
 THUMB(1:			)
#endif

#ifdef CONFIG_ARM_VIRT_EXT
	bl	__hyp_stub_install
#endif
	@ ensure svc mode and all interrupts masked
	safe_svcmode_maskall r9
						@ and irqs disabled
#if defined(CONFIG_CPU_CP15)
	mrc	p15, 0, r9, c0, c0		@ get processor id
#elif defined(CONFIG_CPU_V7M)
	ldr	r9, =BASEADDR_V7M_SCB
	ldr	r9, [r9, V7M_SCB_CPUID]
#else
	ldr	r9, =CONFIG_PROCESSOR_ID
#endif
	bl	__lookup_processor_type		@ r5=procinfo r9=cpuid
	movs	r10, r5				@ invalid processor (r5=0)?
	beq	__error_p				@ yes, error 'p'

#ifdef CONFIG_ARM_MPU
	bl	__setup_mpu
#endif

	badr	lr, 1f				@ return (PIC) address
	ldr	r12, [r10, #PROCINFO_INITFUNC]
	add	r12, r12, r10
	ret	r12
1:	ldr	lr, =__mmap_switched
	b	__after_proc_init
ENDPROC(stext)

#ifdef CONFIG_SMP
	.text
ENTRY(secondary_startup)
	/*
	 * Common entry point for secondary CPUs.
	 *
	 * Ensure that we're in SVC mode, and IRQs are disabled.  Lookup
	 * the processor type - there is no need to check the machine type
	 * as it has already been validated by the primary processor.
	 */
#ifdef CONFIG_ARM_VIRT_EXT
	bl	__hyp_stub_install_secondary
#endif
	safe_svcmode_maskall r9

#ifndef CONFIG_CPU_CP15
	ldr	r9, =CONFIG_PROCESSOR_ID
#else
	mrc	p15, 0, r9, c0, c0		@ get processor id
#endif
	bl	__lookup_processor_type		@ r5=procinfo r9=cpuid
	movs	r10, r5				@ invalid processor?
	beq	__error_p			@ yes, error 'p'

	ldr	r7, __secondary_data

#ifdef CONFIG_ARM_MPU
	bl      __secondary_setup_mpu		@ Initialize the MPU
#endif

	badr	lr, 1f				@ return (PIC) address
	ldr	r12, [r10, #PROCINFO_INITFUNC]
	add	r12, r12, r10
	ret	r12
1:	bl	__after_proc_init
	ldr	r7, __secondary_data		@ reload r7
	ldr	sp, [r7, #12]			@ set up the stack pointer
	ldr	r0, [r7, #16]			@ set up task pointer
	mov	fp, #0
	b	secondary_start_kernel
ENDPROC(secondary_startup)

	.type	__secondary_data, %object
__secondary_data:
	.long	secondary_data
#endif /* CONFIG_SMP */

/*
 * Set the Control Register and Read the process ID.
 */
	.text
__after_proc_init:
M_CLASS(movw	r12, #:lower16:BASEADDR_V7M_SCB)
M_CLASS(movt	r12, #:upper16:BASEADDR_V7M_SCB)
#ifdef CONFIG_ARM_MPU
M_CLASS(ldr	r3, [r12, 0x50])
AR_CLASS(mrc	p15, 0, r3, c0, c1, 4)          @ Read ID_MMFR0
	and	r3, r3, #(MMFR0_PMSA)           @ PMSA field
	teq	r3, #(MMFR0_PMSAv7)             @ PMSA v7
	beq	1f
	teq	r3, #(MMFR0_PMSAv8)		@ PMSA v8
	/*
	 * Memory region attributes for PMSAv8:
	 *
	 *   n = AttrIndx[2:0]
	 *                      n       MAIR
	 *   DEVICE_nGnRnE      000     00000000
	 *   NORMAL             001     11111111
	 */
	ldreq	r3, =PMSAv8_MAIR(0x00, PMSAv8_RGN_DEVICE_nGnRnE) | \
		     PMSAv8_MAIR(0xff, PMSAv8_RGN_NORMAL)
AR_CLASS(mcreq	p15, 0, r3, c10, c2, 0)		@ MAIR 0
M_CLASS(streq	r3, [r12, #PMSAv8_MAIR0])
	moveq	r3, #0
AR_CLASS(mcreq	p15, 0, r3, c10, c2, 1)		@ MAIR 1
M_CLASS(streq	r3, [r12, #PMSAv8_MAIR1])

1:
#endif
#ifdef CONFIG_CPU_CP15
	/*
	 * CP15 system control register value returned in r0 from
	 * the CPU init function.
	 */

#ifdef CONFIG_ARM_MPU
	biceq	r0, r0, #CR_BR			@ Disable the 'default mem-map'
	orreq	r0, r0, #CR_M			@ Set SCTRL.M (MPU on)
#endif
#if defined(CONFIG_ALIGNMENT_TRAP) && __LINUX_ARM_ARCH__ < 6
	orr	r0, r0, #CR_A
#else
	bic	r0, r0, #CR_A
#endif
#ifdef CONFIG_CPU_DCACHE_DISABLE
	bic	r0, r0, #CR_C
#endif
#ifdef CONFIG_CPU_BPREDICT_DISABLE
	bic	r0, r0, #CR_Z
#endif
#ifdef CONFIG_CPU_ICACHE_DISABLE
	bic	r0, r0, #CR_I
#endif
	mcr	p15, 0, r0, c1, c0, 0		@ write control reg
	instr_sync
#elif defined (CONFIG_CPU_V7M)
#ifdef CONFIG_ARM_MPU
	ldreq	r3, [r12, MPU_CTRL]
	biceq	r3, #MPU_CTRL_PRIVDEFENA
	orreq	r3, #MPU_CTRL_ENABLE
	streq	r3, [r12, MPU_CTRL]
	isb
#endif
	/* For V7M systems we want to modify the CCR similarly to the SCTLR */
#ifdef CONFIG_CPU_DCACHE_DISABLE
	bic	r0, r0, #V7M_SCB_CCR_DC
#endif
#ifdef CONFIG_CPU_BPREDICT_DISABLE
	bic	r0, r0, #V7M_SCB_CCR_BP
#endif
#ifdef CONFIG_CPU_ICACHE_DISABLE
	bic	r0, r0, #V7M_SCB_CCR_IC
#endif
	str	r0, [r12, V7M_SCB_CCR]
	/* Pass exc_ret to __mmap_switched */
	mov	r0, r10
#endif /* CONFIG_CPU_CP15 elif CONFIG_CPU_V7M */
	ret	lr
ENDPROC(__after_proc_init)
	.ltorg

#ifdef CONFIG_ARM_MPU

#ifndef CONFIG_CPU_V7M
/* Set which MPU region should be programmed */
.macro set_region_nr tmp, rgnr, unused
	mov	\tmp, \rgnr			@ Use static region numbers
	mcr	p15, 0, \tmp, c6, c2, 0		@ Write RGNR
.endm

/* Setup a single MPU region, either D or I side (D-side for unified) */
.macro setup_region bar, acr, sr, side = PMSAv7_DATA_SIDE, unused
	mcr	p15, 0, \bar, c6, c1, (0 + \side)	@ I/DRBAR
	mcr	p15, 0, \acr, c6, c1, (4 + \side)	@ I/DRACR
	mcr	p15, 0, \sr, c6, c1, (2 + \side)		@ I/DRSR
.endm
#else
.macro set_region_nr tmp, rgnr, base
	mov	\tmp, \rgnr
	str     \tmp, [\base, #PMSAv7_RNR]
.endm

.macro setup_region bar, acr, sr, unused, base
	lsl     \acr, \acr, #16
	orr     \acr, \acr, \sr
	str     \bar, [\base, #PMSAv7_RBAR]
	str     \acr, [\base, #PMSAv7_RASR]
.endm

#endif
/*
 * Setup the MPU and initial MPU Regions. We create the following regions:
 * Region 0: Use this for probing the MPU details, so leave disabled.
 * Region 1: Background region - covers the whole of RAM as strongly ordered
 * Region 2: Normal, Shared, cacheable for RAM. From PHYS_OFFSET, size from r6
 * Region 3: Normal, shared, inaccessible from PL0 to protect the vectors page
 *
 * r6: Value to be written to DRSR (and IRSR if required) for PMSAv7_RAM_REGION
*/
	__HEAD

ENTRY(__setup_mpu)

	/* Probe for v7 PMSA compliance */
M_CLASS(movw	r12, #:lower16:BASEADDR_V7M_SCB)
M_CLASS(movt	r12, #:upper16:BASEADDR_V7M_SCB)

AR_CLASS(mrc	p15, 0, r0, c0, c1, 4)		@ Read ID_MMFR0
M_CLASS(ldr	r0, [r12, 0x50])
	and	r0, r0, #(MMFR0_PMSA)		@ PMSA field
	teq	r0, #(MMFR0_PMSAv7)		@ PMSA v7
	beq	__setup_pmsa_v7
	teq	r0, #(MMFR0_PMSAv8)		@ PMSA v8
	beq	__setup_pmsa_v8

	ret	lr
ENDPROC(__setup_mpu)

ENTRY(__setup_pmsa_v7)
	/* Calculate the size of a region covering just the kernel */
	ldr	r5, =PLAT_PHYS_OFFSET		@ Region start: PHYS_OFFSET
	ldr     r6, =(_end)			@ Cover whole kernel
	sub	r6, r6, r5			@ Minimum size of region to map
	clz	r6, r6				@ Region size must be 2^N...
	rsb	r6, r6, #31			@ ...so round up region size
	lsl	r6, r6, #PMSAv7_RSR_SZ		@ Put size in right field
	orr	r6, r6, #(1 << PMSAv7_RSR_EN)	@ Set region enabled bit

	/* Determine whether the D/I-side memory map is unified. We set the
	 * flags here and continue to use them for the rest of this function */
AR_CLASS(mrc	p15, 0, r0, c0, c0, 4)		@ MPUIR
M_CLASS(ldr    r0, [r12, #MPU_TYPE])
	ands	r5, r0, #MPUIR_DREGION_SZMASK	@ 0 size d region => No MPU
	bxeq	lr
	tst	r0, #MPUIR_nU			@ MPUIR_nU = 0 for unified

	/* Setup second region first to free up r6 */
	set_region_nr r0, #PMSAv7_RAM_REGION, r12
	isb
	/* Full access from PL0, PL1, shared for CONFIG_SMP, cacheable */
	ldr	r0, =PLAT_PHYS_OFFSET		@ RAM starts at PHYS_OFFSET
	ldr	r5,=(PMSAv7_AP_PL1RW_PL0RW | PMSAv7_RGN_NORMAL)

	setup_region r0, r5, r6, PMSAv7_DATA_SIDE, r12	@ PHYS_OFFSET, shared, enabled
	beq	1f					@ Memory-map not unified
	setup_region r0, r5, r6, PMSAv7_INSTR_SIDE, r12	@ PHYS_OFFSET, shared, enabled
1:	isb

	/* First/background region */
	set_region_nr r0, #PMSAv7_BG_REGION, r12
	isb
	/* Execute Never,  strongly ordered, inaccessible to PL0, rw PL1  */
	mov	r0, #0				@ BG region starts at 0x0
	ldr	r5,=(PMSAv7_ACR_XN | PMSAv7_RGN_STRONGLY_ORDERED | PMSAv7_AP_PL1RW_PL0NA)
	mov	r6, #PMSAv7_RSR_ALL_MEM		@ 4GB region, enabled

	setup_region r0, r5, r6, PMSAv7_DATA_SIDE, r12	@ 0x0, BG region, enabled
	beq	2f					@ Memory-map not unified
	setup_region r0, r5, r6, PMSAv7_INSTR_SIDE r12	@ 0x0, BG region, enabled
2:	isb

#ifdef CONFIG_XIP_KERNEL
	set_region_nr r0, #PMSAv7_ROM_REGION, r12
	isb

	ldr	r5,=(PMSAv7_AP_PL1RO_PL0NA | PMSAv7_RGN_NORMAL)

	ldr	r0, =CONFIG_XIP_PHYS_ADDR		@ ROM start
	ldr     r6, =(_exiprom)				@ ROM end
	sub	r6, r6, r0				@ Minimum size of region to map
	clz	r6, r6					@ Region size must be 2^N...
	rsb	r6, r6, #31				@ ...so round up region size
	lsl	r6, r6, #PMSAv7_RSR_SZ			@ Put size in right field
	orr	r6, r6, #(1 << PMSAv7_RSR_EN)		@ Set region enabled bit

	setup_region r0, r5, r6, PMSAv7_DATA_SIDE, r12	@ XIP_PHYS_ADDR, shared, enabled
	beq	3f					@ Memory-map not unified
	setup_region r0, r5, r6, PMSAv7_INSTR_SIDE, r12	@ XIP_PHYS_ADDR, shared, enabled
3:	isb
#endif
	ret	lr
ENDPROC(__setup_pmsa_v7)

ENTRY(__setup_pmsa_v8)
	mov	r0, #0
AR_CLASS(mcr	p15, 0, r0, c6, c2, 1)		@ PRSEL
M_CLASS(str	r0, [r12, #PMSAv8_RNR])
	isb

#ifdef CONFIG_XIP_KERNEL
	ldr	r5, =CONFIG_XIP_PHYS_ADDR		@ ROM start
	ldr     r6, =(_exiprom)				@ ROM end
	sub	r6, r6, #1
	bic	r6, r6, #(PMSAv8_MINALIGN - 1)

	orr	r5, r5, #(PMSAv8_AP_PL1RW_PL0NA | PMSAv8_RGN_SHARED)
	orr	r6, r6, #(PMSAv8_LAR_IDX(PMSAv8_RGN_NORMAL) | PMSAv8_LAR_EN)

AR_CLASS(mcr	p15, 0, r5, c6, c8, 0)			@ PRBAR0
AR_CLASS(mcr	p15, 0, r6, c6, c8, 1)			@ PRLAR0
M_CLASS(str	r5, [r12, #PMSAv8_RBAR_A(0)])
M_CLASS(str	r6, [r12, #PMSAv8_RLAR_A(0)])
#endif

	ldr	r5, =KERNEL_START
	ldr	r6, =KERNEL_END
	sub	r6, r6, #1
	bic	r6, r6, #(PMSAv8_MINALIGN - 1)

	orr	r5, r5, #(PMSAv8_AP_PL1RW_PL0NA | PMSAv8_RGN_SHARED)
	orr	r6, r6, #(PMSAv8_LAR_IDX(PMSAv8_RGN_NORMAL) | PMSAv8_LAR_EN)

AR_CLASS(mcr	p15, 0, r5, c6, c8, 4)			@ PRBAR1
AR_CLASS(mcr	p15, 0, r6, c6, c8, 5)			@ PRLAR1
M_CLASS(str	r5, [r12, #PMSAv8_RBAR_A(1)])
M_CLASS(str	r6, [r12, #PMSAv8_RLAR_A(1)])

	/* Setup Background: 0x0 - min(KERNEL_START, XIP_PHYS_ADDR) */
#ifdef CONFIG_XIP_KERNEL
	ldr	r6, =KERNEL_START
	ldr	r5, =CONFIG_XIP_PHYS_ADDR
	cmp	r6, r5
	movcs	r6, r5
#else
	ldr	r6, =KERNEL_START
#endif
	cmp	r6, #0
	beq	1f

	mov	r5, #0
	sub	r6, r6, #1
	bic	r6, r6, #(PMSAv8_MINALIGN - 1)

	orr	r5, r5, #(PMSAv8_AP_PL1RW_PL0NA | PMSAv8_RGN_SHARED | PMSAv8_BAR_XN)
	orr	r6, r6, #(PMSAv8_LAR_IDX(PMSAv8_RGN_DEVICE_nGnRnE) | PMSAv8_LAR_EN)

AR_CLASS(mcr	p15, 0, r5, c6, c9, 0)			@ PRBAR2
AR_CLASS(mcr	p15, 0, r6, c6, c9, 1)			@ PRLAR2
M_CLASS(str	r5, [r12, #PMSAv8_RBAR_A(2)])
M_CLASS(str	r6, [r12, #PMSAv8_RLAR_A(2)])

1:
	/* Setup Background: max(KERNEL_END, _exiprom) - 0xffffffff */
#ifdef CONFIG_XIP_KERNEL
	ldr	r5, =KERNEL_END
	ldr	r6, =(_exiprom)
	cmp	r5, r6
	movcc	r5, r6
#else
	ldr	r5, =KERNEL_END
#endif
	mov	r6, #0xffffffff
	bic	r6, r6, #(PMSAv8_MINALIGN - 1)

	orr	r5, r5, #(PMSAv8_AP_PL1RW_PL0NA | PMSAv8_RGN_SHARED | PMSAv8_BAR_XN)
	orr	r6, r6, #(PMSAv8_LAR_IDX(PMSAv8_RGN_DEVICE_nGnRnE) | PMSAv8_LAR_EN)

AR_CLASS(mcr	p15, 0, r5, c6, c9, 4)			@ PRBAR3
AR_CLASS(mcr	p15, 0, r6, c6, c9, 5)			@ PRLAR3
M_CLASS(str	r5, [r12, #PMSAv8_RBAR_A(3)])
M_CLASS(str	r6, [r12, #PMSAv8_RLAR_A(3)])

#ifdef CONFIG_XIP_KERNEL
	/* Setup Background: min(_exiprom, KERNEL_END) - max(KERNEL_START, XIP_PHYS_ADDR) */
	ldr	r5, =(_exiprom)
	ldr	r6, =KERNEL_END
	cmp	r5, r6
	movcs	r5, r6

	ldr	r6, =KERNEL_START
	ldr	r0, =CONFIG_XIP_PHYS_ADDR
	cmp	r6, r0
	movcc	r6, r0

	sub	r6, r6, #1
	bic	r6, r6, #(PMSAv8_MINALIGN - 1)

	orr	r5, r5, #(PMSAv8_AP_PL1RW_PL0NA | PMSAv8_RGN_SHARED | PMSAv8_BAR_XN)
	orr	r6, r6, #(PMSAv8_LAR_IDX(PMSAv8_RGN_DEVICE_nGnRnE) | PMSAv8_LAR_EN)

#ifdef CONFIG_CPU_V7M
	/* There is no alias for n == 4 */
	mov	r0, #4
	str	r0, [r12, #PMSAv8_RNR]			@ PRSEL
	isb

	str	r5, [r12, #PMSAv8_RBAR_A(0)]
	str	r6, [r12, #PMSAv8_RLAR_A(0)]
#else
	mcr	p15, 0, r5, c6, c10, 0			@ PRBAR4
	mcr	p15, 0, r6, c6, c10, 1			@ PRLAR4
#endif
#endif
	ret	lr
ENDPROC(__setup_pmsa_v8)

#ifdef CONFIG_SMP
/*
 * r6: pointer at mpu_rgn_info
 */

	.text
ENTRY(__secondary_setup_mpu)
	/* Use MPU region info supplied by __cpu_up */
	ldr	r6, [r7]			@ get secondary_data.mpu_rgn_info

	/* Probe for v7 PMSA compliance */
	mrc	p15, 0, r0, c0, c1, 4		@ Read ID_MMFR0
	and	r0, r0, #(MMFR0_PMSA)		@ PMSA field
	teq	r0, #(MMFR0_PMSAv7)		@ PMSA v7
	beq	__secondary_setup_pmsa_v7
	teq	r0, #(MMFR0_PMSAv8)		@ PMSA v8
	beq	__secondary_setup_pmsa_v8
	b	__error_p
ENDPROC(__secondary_setup_mpu)

/*
 * r6: pointer at mpu_rgn_info
 */
ENTRY(__secondary_setup_pmsa_v7)
	/* Determine whether the D/I-side memory map is unified. We set the
	 * flags here and continue to use them for the rest of this function */
	mrc	p15, 0, r0, c0, c0, 4		@ MPUIR
	ands	r5, r0, #MPUIR_DREGION_SZMASK	@ 0 size d region => No MPU
	beq	__error_p

	ldr	r4, [r6, #MPU_RNG_INFO_USED]
	mov	r5, #MPU_RNG_SIZE
	add	r3, r6, #MPU_RNG_INFO_RNGS
	mla	r3, r4, r5, r3

1:
	tst	r0, #MPUIR_nU			@ MPUIR_nU = 0 for unified
	sub	r3, r3, #MPU_RNG_SIZE
	sub	r4, r4, #1

	set_region_nr r0, r4
	isb

	ldr	r0, [r3, #MPU_RGN_DRBAR]
	ldr	r6, [r3, #MPU_RGN_DRSR]
	ldr	r5, [r3, #MPU_RGN_DRACR]

	setup_region r0, r5, r6, PMSAv7_DATA_SIDE
	beq	2f
	setup_region r0, r5, r6, PMSAv7_INSTR_SIDE
2:	isb

	mrc	p15, 0, r0, c0, c0, 4		@ Reevaluate the MPUIR
	cmp	r4, #0
	bgt	1b

	ret	lr
ENDPROC(__secondary_setup_pmsa_v7)

ENTRY(__secondary_setup_pmsa_v8)
	ldr	r4, [r6, #MPU_RNG_INFO_USED]
#ifndef CONFIG_XIP_KERNEL
	add	r4, r4, #1
#endif
	mov	r5, #MPU_RNG_SIZE
	add	r3, r6, #MPU_RNG_INFO_RNGS
	mla	r3, r4, r5, r3

1:
	sub	r3, r3, #MPU_RNG_SIZE
	sub	r4, r4, #1

	mcr	p15, 0, r4, c6, c2, 1		@ PRSEL
	isb

	ldr	r5, [r3, #MPU_RGN_PRBAR]
	ldr	r6, [r3, #MPU_RGN_PRLAR]

	mcr	p15, 0, r5, c6, c3, 0		@ PRBAR
	mcr	p15, 0, r6, c6, c3, 1           @ PRLAR

	cmp	r4, #0
	bgt	1b

	ret	lr
ENDPROC(__secondary_setup_pmsa_v8)
#endif /* CONFIG_SMP */
#endif /* CONFIG_ARM_MPU */
#include "head-common.S"


code:
/* SPDX-License-Identifier: GPL-2.0-only */
/*
 *  linux/arch/arm/kernel/head.S
 *
 *  Copyright (C) 1994-2002 Russell King
 *  Copyright (c) 2003 ARM Limited
 *  All Rights Reserved
 *
 *  Kernel startup code for all 32-bit CPUs
 */
#include <linux/linkage.h>
#include <linux/init.h>
#include <linux/pgtable.h>

#include <asm/assembler.h>
#include <asm/cp15.h>
#include <asm/domain.h>
#include <asm/ptrace.h>
#include <asm/asm-offsets.h>
#include <asm/memory.h>
#include <asm/thread_info.h>

#if defined(CONFIG_DEBUG_LL) && !defined(CONFIG_DEBUG_SEMIHOSTING)
#include CONFIG_DEBUG_LL_INCLUDE
#endif
/*
 * swapper_pg_dir is the virtual address of the initial page table.
 * We place the page tables 16K below KERNEL_RAM_VADDR.  Therefore, we must
 * make sure that KERNEL_RAM_VADDR is correctly set.  Currently, we expect
 * the least significant 16 bits to be 0x8000, but we could probably
 * relax this restriction to KERNEL_RAM_VADDR >= PAGE_OFFSET + 0x4000.
 */
#define KERNEL_RAM_VADDR	(KERNEL_OFFSET + TEXT_OFFSET)
#if (KERNEL_RAM_VADDR & 0xffff) != 0x8000
#error KERNEL_RAM_VADDR must start at 0xXXXX8000
#endif

#ifdef CONFIG_ARM_LPAE
	/* LPAE requires an additional page for the PGD */
#define PG_DIR_SIZE	0x5000
#define PMD_ENTRY_ORDER	3	/* PMD entry size is 2^PMD_ENTRY_ORDER */
#else
#define PG_DIR_SIZE	0x4000
#define PMD_ENTRY_ORDER	2
#endif

	.globl	swapper_pg_dir
	.equ	swapper_pg_dir, KERNEL_RAM_VADDR - PG_DIR_SIZE

	/*
	 * This needs to be assigned at runtime when the linker symbols are
	 * resolved. These are unsigned 64bit really, but in this assembly code
	 * We store them as 32bit.
	 */
	.pushsection .data
	.align	2
	.globl	kernel_sec_start
	.globl	kernel_sec_end
kernel_sec_start:
	.long	0
	.long	0
kernel_sec_end:
	.long	0
	.long	0
	.popsection

	.macro	pgtbl, rd, phys
	add	\rd, \phys, #TEXT_OFFSET
	sub	\rd, \rd, #PG_DIR_SIZE
	.endm

/*
 * Kernel startup entry point.
 * ---------------------------
 *
 * This is normally called from the decompressor code.  The requirements
 * are: MMU = off, D-cache = off, I-cache = dont care, r0 = 0,
 * r1 = machine nr, r2 = atags or dtb pointer.
 *
 * This code is mostly position independent, so if you link the kernel at
 * 0xc0008000, you call this at __pa(0xc0008000).
 *
 * See linux/arch/arm/tools/mach-types for the complete list of machine
 * numbers for r1.
 *
 * We're trying to keep crap to a minimum; DO NOT add any machine specific
 * crap here - that's what the boot loader (or in extreme, well justified
 * circumstances, zImage) is for.
 */
	.arm

	__HEAD
ENTRY(stext)
 ARM_BE8(setend	be )			@ ensure we are in BE8 mode

 THUMB(	badr	r9, 1f		)	@ Kernel is always entered in ARM.
 THUMB(	bx	r9		)	@ If this is a Thumb-2 kernel,
 THUMB(	.thumb			)	@ switch to Thumb now.
 THUMB(1:			)

#ifdef CONFIG_ARM_VIRT_EXT
	bl	__hyp_stub_install
#endif
	@ ensure svc mode and all interrupts masked
	safe_svcmode_maskall r9

	mrc	p15, 0, r9, c0, c0		@ get processor id
	bl	__lookup_processor_type		@ r5=procinfo r9=cpuid
	movs	r10, r5				@ invalid processor (r5=0)?
 THUMB( it	eq )		@ force fixup-able long branch encoding
	beq	__error_p			@ yes, error 'p'

#ifdef CONFIG_ARM_LPAE
	mrc	p15, 0, r3, c0, c1, 4		@ read ID_MMFR0
	and	r3, r3, #0xf			@ extract VMSA support
	cmp	r3, #5				@ long-descriptor translation table format?
 THUMB( it	lo )				@ force fixup-able long branch encoding
	blo	__error_lpae			@ only classic page table format
#endif

#ifndef CONFIG_XIP_KERNEL
	adr_l	r8, _text			@ __pa(_text)
	sub	r8, r8, #TEXT_OFFSET		@ PHYS_OFFSET
#else
	ldr	r8, =PLAT_PHYS_OFFSET		@ always constant in this case
#endif

	/*
	 * r1 = machine no, r2 = atags or dtb,
	 * r8 = phys_offset, r9 = cpuid, r10 = procinfo
	 */
	bl	__vet_atags
#ifdef CONFIG_SMP_ON_UP
	bl	__fixup_smp
#endif
#ifdef CONFIG_ARM_PATCH_PHYS_VIRT
	bl	__fixup_pv_table
#endif
	bl	__create_page_tables

	/*
	 * The following calls CPU specific code in a position independent
	 * manner.  See arch/arm/mm/proc-*.S for details.  r10 = base of
	 * xxx_proc_info structure selected by __lookup_processor_type
	 * above.
	 *
	 * The processor init function will be called with:
	 *  r1 - machine type
	 *  r2 - boot data (atags/dt) pointer
	 *  r4 - translation table base (low word)
	 *  r5 - translation table base (high word, if LPAE)
	 *  r8 - translation table base 1 (pfn if LPAE)
	 *  r9 - cpuid
	 *  r13 - virtual address for __enable_mmu -> __turn_mmu_on
	 *
	 * On return, the CPU will be ready for the MMU to be turned on,
	 * r0 will hold the CPU control register value, r1, r2, r4, and
	 * r9 will be preserved.  r5 will also be preserved if LPAE.
	 */
	ldr	r13, =__mmap_switched		@ address to jump to after
						@ mmu has been enabled
	badr	lr, 1f				@ return (PIC) address
#ifdef CONFIG_ARM_LPAE
	mov	r5, #0				@ high TTBR0
	mov	r8, r4, lsr #12			@ TTBR1 is swapper_pg_dir pfn
#else
	mov	r8, r4				@ set TTBR1 to swapper_pg_dir
#endif
	ldr	r12, [r10, #PROCINFO_INITFUNC]
	add	r12, r12, r10
	ret	r12
1:	b	__enable_mmu
ENDPROC(stext)
	.ltorg

/*
 * Setup the initial page tables.  We only setup the barest
 * amount which are required to get the kernel running, which
 * generally means mapping in the kernel code.
 *
 * r8 = phys_offset, r9 = cpuid, r10 = procinfo
 *
 * Returns:
 *  r0, r3, r5-r7 corrupted
 *  r4 = physical page table address
 */
__create_page_tables:
	pgtbl	r4, r8				@ page table address

	/*
	 * Clear the swapper page table
	 */
	mov	r0, r4
	mov	r3, #0
	add	r6, r0, #PG_DIR_SIZE
1:	str	r3, [r0], #4
	str	r3, [r0], #4
	str	r3, [r0], #4
	str	r3, [r0], #4
	teq	r0, r6
	bne	1b

#ifdef CONFIG_ARM_LPAE
	/*
	 * Build the PGD table (first level) to point to the PMD table. A PGD
	 * entry is 64-bit wide.
	 */
	mov	r0, r4
	add	r3, r4, #0x1000			@ first PMD table address
	orr	r3, r3, #3			@ PGD block type
	mov	r6, #4				@ PTRS_PER_PGD
	mov	r7, #1 << (55 - 32)		@ L_PGD_SWAPPER
1:
#ifdef CONFIG_CPU_ENDIAN_BE8
	str	r7, [r0], #4			@ set top PGD entry bits
	str	r3, [r0], #4			@ set bottom PGD entry bits
#else
	str	r3, [r0], #4			@ set bottom PGD entry bits
	str	r7, [r0], #4			@ set top PGD entry bits
#endif
	add	r3, r3, #0x1000			@ next PMD table
	subs	r6, r6, #1
	bne	1b

	add	r4, r4, #0x1000			@ point to the PMD tables
#ifdef CONFIG_CPU_ENDIAN_BE8
	add	r4, r4, #4			@ we only write the bottom word
#endif
#endif

	ldr	r7, [r10, #PROCINFO_MM_MMUFLAGS] @ mm_mmuflags

	/*
	 * Create identity mapping to cater for __enable_mmu.
	 * This identity mapping will be removed by paging_init().
	 */
	adr_l	r5, __turn_mmu_on		@ _pa(__turn_mmu_on)
	adr_l	r6, __turn_mmu_on_end		@ _pa(__turn_mmu_on_end)
	mov	r5, r5, lsr #SECTION_SHIFT
	mov	r6, r6, lsr #SECTION_SHIFT

1:	orr	r3, r7, r5, lsl #SECTION_SHIFT	@ flags + kernel base
	str	r3, [r4, r5, lsl #PMD_ENTRY_ORDER]	@ identity mapping
	cmp	r5, r6
	addlo	r5, r5, #1			@ next section
	blo	1b

	/*
	 * The main matter: map in the kernel using section mappings, and
	 * set two variables to indicate the physical start and end of the
	 * kernel.
	 */
	add	r0, r4, #KERNEL_OFFSET >> (SECTION_SHIFT - PMD_ENTRY_ORDER)
	ldr	r6, =(_end - 1)
	adr_l	r5, kernel_sec_start		@ _pa(kernel_sec_start)
#if defined CONFIG_CPU_ENDIAN_BE8 || defined CONFIG_CPU_ENDIAN_BE32
	str	r8, [r5, #4]			@ Save physical start of kernel (BE)
#else
	str	r8, [r5]			@ Save physical start of kernel (LE)
#endif
	orr	r3, r8, r7			@ Add the MMU flags
	add	r6, r4, r6, lsr #(SECTION_SHIFT - PMD_ENTRY_ORDER)
1:	str	r3, [r0], #1 << PMD_ENTRY_ORDER
	add	r3, r3, #1 << SECTION_SHIFT
	cmp	r0, r6
	bls	1b
	eor	r3, r3, r7			@ Remove the MMU flags
	adr_l	r5, kernel_sec_end		@ _pa(kernel_sec_end)
#if defined CONFIG_CPU_ENDIAN_BE8 || defined CONFIG_CPU_ENDIAN_BE32
	str	r3, [r5, #4]			@ Save physical end of kernel (BE)
#else
	str	r3, [r5]			@ Save physical end of kernel (LE)
#endif

#ifdef CONFIG_XIP_KERNEL
	/*
	 * Map the kernel image separately as it is not located in RAM.
	 */
#define XIP_START XIP_VIRT_ADDR(CONFIG_XIP_PHYS_ADDR)
	mov	r3, pc
	mov	r3, r3, lsr #SECTION_SHIFT
	orr	r3, r7, r3, lsl #SECTION_SHIFT
	add	r0, r4,  #(XIP_START & 0xff000000) >> (SECTION_SHIFT - PMD_ENTRY_ORDER)
	str	r3, [r0, #((XIP_START & 0x00f00000) >> SECTION_SHIFT) << PMD_ENTRY_ORDER]!
	ldr	r6, =(_edata_loc - 1)
	add	r0, r0, #1 << PMD_ENTRY_ORDER
	add	r6, r4, r6, lsr #(SECTION_SHIFT - PMD_ENTRY_ORDER)
1:	cmp	r0, r6
	add	r3, r3, #1 << SECTION_SHIFT
	strls	r3, [r0], #1 << PMD_ENTRY_ORDER
	bls	1b
#endif

	/*
	 * Then map boot params address in r2 if specified.
	 * We map 2 sections in case the ATAGs/DTB crosses a section boundary.
	 */
	mov	r0, r2, lsr #SECTION_SHIFT
	cmp	r2, #0
	ldrne	r3, =FDT_FIXED_BASE >> (SECTION_SHIFT - PMD_ENTRY_ORDER)
	addne	r3, r3, r4
	orrne	r6, r7, r0, lsl #SECTION_SHIFT
	strne	r6, [r3], #1 << PMD_ENTRY_ORDER
	addne	r6, r6, #1 << SECTION_SHIFT
	strne	r6, [r3]

#if defined(CONFIG_ARM_LPAE) && defined(CONFIG_CPU_ENDIAN_BE8)
	sub	r4, r4, #4			@ Fixup page table pointer
						@ for 64-bit descriptors
#endif

#ifdef CONFIG_DEBUG_LL
#if !defined(CONFIG_DEBUG_ICEDCC) && !defined(CONFIG_DEBUG_SEMIHOSTING)
	/*
	 * Map in IO space for serial debugging.
	 * This allows debug messages to be output
	 * via a serial console before paging_init.
	 */
	addruart r7, r3, r0

	mov	r3, r3, lsr #SECTION_SHIFT
	mov	r3, r3, lsl #PMD_ENTRY_ORDER

	add	r0, r4, r3
	mov	r3, r7, lsr #SECTION_SHIFT
	ldr	r7, [r10, #PROCINFO_IO_MMUFLAGS] @ io_mmuflags
	orr	r3, r7, r3, lsl #SECTION_SHIFT
#ifdef CONFIG_ARM_LPAE
	mov	r7, #1 << (54 - 32)		@ XN
#ifdef CONFIG_CPU_ENDIAN_BE8
	str	r7, [r0], #4
	str	r3, [r0], #4
#else
	str	r3, [r0], #4
	str	r7, [r0], #4
#endif
#else
	orr	r3, r3, #PMD_SECT_XN
	str	r3, [r0], #4
#endif

#else /* CONFIG_DEBUG_ICEDCC || CONFIG_DEBUG_SEMIHOSTING */
	/* we don't need any serial debugging mappings */
	ldr	r7, [r10, #PROCINFO_IO_MMUFLAGS] @ io_mmuflags
#endif

#if defined(CONFIG_ARCH_NETWINDER) || defined(CONFIG_ARCH_CATS)
	/*
	 * If we're using the NetWinder or CATS, we also need to map
	 * in the 16550-type serial port for the debug messages
	 */
	add	r0, r4, #0xff000000 >> (SECTION_SHIFT - PMD_ENTRY_ORDER)
	orr	r3, r7, #0x7c000000
	str	r3, [r0]
#endif
#ifdef CONFIG_ARCH_RPC
	/*
	 * Map in screen at 0x02000000 & SCREEN2_BASE
	 * Similar reasons here - for debug.  This is
	 * only for Acorn RiscPC architectures.
	 */
	add	r0, r4, #0x02000000 >> (SECTION_SHIFT - PMD_ENTRY_ORDER)
	orr	r3, r7, #0x02000000
	str	r3, [r0]
	add	r0, r4, #0xd8000000 >> (SECTION_SHIFT - PMD_ENTRY_ORDER)
	str	r3, [r0]
#endif
#endif
#ifdef CONFIG_ARM_LPAE
	sub	r4, r4, #0x1000		@ point to the PGD table
#endif
	ret	lr
ENDPROC(__create_page_tables)
	.ltorg

#if defined(CONFIG_SMP)
	.text
	.arm
ENTRY(secondary_startup_arm)
 THUMB(	badr	r9, 1f		)	@ Kernel is entered in ARM.
 THUMB(	bx	r9		)	@ If this is a Thumb-2 kernel,
 THUMB(	.thumb			)	@ switch to Thumb now.
 THUMB(1:			)
ENTRY(secondary_startup)
	/*
	 * Common entry point for secondary CPUs.
	 *
	 * Ensure that we're in SVC mode, and IRQs are disabled.  Lookup
	 * the processor type - there is no need to check the machine type
	 * as it has already been validated by the primary processor.
	 */

 ARM_BE8(setend	be)				@ ensure we are in BE8 mode

#ifdef CONFIG_ARM_VIRT_EXT
	bl	__hyp_stub_install_secondary
#endif
	safe_svcmode_maskall r9

	mrc	p15, 0, r9, c0, c0		@ get processor id
	bl	__lookup_processor_type
	movs	r10, r5				@ invalid processor?
	moveq	r0, #'p'			@ yes, error 'p'
 THUMB( it	eq )		@ force fixup-able long branch encoding
	beq	__error_p

	/*
	 * Use the page tables supplied from  __cpu_up.
	 */
	adr_l	r3, secondary_data
	mov_l	r12, __secondary_switched
	ldrd	r4, r5, [r3, #0]		@ get secondary_data.pgdir
ARM_BE8(eor	r4, r4, r5)			@ Swap r5 and r4 in BE:
ARM_BE8(eor	r5, r4, r5)			@ it can be done in 3 steps
ARM_BE8(eor	r4, r4, r5)			@ without using a temp reg.
	ldr	r8, [r3, #8]			@ get secondary_data.swapper_pg_dir
	badr	lr, __enable_mmu		@ return address
	mov	r13, r12			@ __secondary_switched address
	ldr	r12, [r10, #PROCINFO_INITFUNC]
	add	r12, r12, r10			@ initialise processor
						@ (return control reg)
	ret	r12
ENDPROC(secondary_startup)
ENDPROC(secondary_startup_arm)

ENTRY(__secondary_switched)
#if defined(CONFIG_VMAP_STACK) && !defined(CONFIG_ARM_LPAE)
	@ Before using the vmap'ed stack, we have to switch to swapper_pg_dir
	@ as the ID map does not cover the vmalloc region.
	mrc	p15, 0, ip, c2, c0, 1	@ read TTBR1
	mcr	p15, 0, ip, c2, c0, 0	@ set TTBR0
	instr_sync
#endif
	adr_l	r7, secondary_data + 12		@ get secondary_data.stack
	ldr	sp, [r7]
	ldr	r0, [r7, #4]			@ get secondary_data.task
	mov	fp, #0
	b	secondary_start_kernel
ENDPROC(__secondary_switched)

#endif /* defined(CONFIG_SMP) */

/*
 * Setup common bits before finally enabling the MMU.  Essentially
 * this is just loading the page table pointer and domain access
 * registers.  All these registers need to be preserved by the
 * processor setup function (or set in the case of r0)
 *
 *  r0  = cp#15 control register
 *  r1  = machine ID
 *  r2  = atags or dtb pointer
 *  r4  = TTBR pointer (low word)
 *  r5  = TTBR pointer (high word if LPAE)
 *  r9  = processor ID
 *  r13 = *virtual* address to jump to upon completion
 */
__enable_mmu:
#if defined(CONFIG_ALIGNMENT_TRAP) && __LINUX_ARM_ARCH__ < 6
	orr	r0, r0, #CR_A
#else
	bic	r0, r0, #CR_A
#endif
#ifdef CONFIG_CPU_DCACHE_DISABLE
	bic	r0, r0, #CR_C
#endif
#ifdef CONFIG_CPU_BPREDICT_DISABLE
	bic	r0, r0, #CR_Z
#endif
#ifdef CONFIG_CPU_ICACHE_DISABLE
	bic	r0, r0, #CR_I
#endif
#ifdef CONFIG_ARM_LPAE
	mcrr	p15, 0, r4, r5, c2		@ load TTBR0
#else
	mov	r5, #DACR_INIT
	mcr	p15, 0, r5, c3, c0, 0		@ load domain access register
	mcr	p15, 0, r4, c2, c0, 0		@ load page table pointer
#endif
	b	__turn_mmu_on
ENDPROC(__enable_mmu)

/*
 * Enable the MMU.  This completely changes the structure of the visible
 * memory space.  You will not be able to trace execution through this.
 * If you have an enquiry about this, *please* check the linux-arm-kernel
 * mailing list archives BEFORE sending another post to the list.
 *
 *  r0  = cp#15 control register
 *  r1  = machine ID
 *  r2  = atags or dtb pointer
 *  r9  = processor ID
 *  r13 = *virtual* address to jump to upon completion
 *
 * other registers depend on the function called upon completion
 */
	.align	5
	.pushsection	.idmap.text, "ax"
ENTRY(__turn_mmu_on)
	mov	r0, r0
	instr_sync
	mcr	p15, 0, r0, c1, c0, 0		@ write control reg
	mrc	p15, 0, r3, c0, c0, 0		@ read id reg
	instr_sync
	mov	r3, r3
	mov	r3, r13
	ret	r3
__turn_mmu_on_end:
ENDPROC(__turn_mmu_on)
	.popsection

#ifdef CONFIG_SMP_ON_UP
	__HEAD
__fixup_smp:
	and	r3, r9, #0x000f0000	@ architecture version
	teq	r3, #0x000f0000		@ CPU ID supported?
	bne	__fixup_smp_on_up	@ no, assume UP

	bic	r3, r9, #0x00ff0000
	bic	r3, r3, #0x0000000f	@ mask 0xff00fff0
	mov	r4, #0x41000000
	orr	r4, r4, #0x0000b000
	orr	r4, r4, #0x00000020	@ val 0x4100b020
	teq	r3, r4			@ ARM 11MPCore?
	reteq	lr			@ yes, assume SMP

	mrc	p15, 0, r0, c0, c0, 5	@ read MPIDR
	and	r0, r0, #0xc0000000	@ multiprocessing extensions and
	teq	r0, #0x80000000		@ not part of a uniprocessor system?
	bne    __fixup_smp_on_up	@ no, assume UP

	@ Core indicates it is SMP. Check for Aegis SOC where a single
	@ Cortex-A9 CPU is present but SMP operations fault.
	mov	r4, #0x41000000
	orr	r4, r4, #0x0000c000
	orr	r4, r4, #0x00000090
	teq	r3, r4			@ Check for ARM Cortex-A9
	retne	lr			@ Not ARM Cortex-A9,

	@ If a future SoC *does* use 0x0 as the PERIPH_BASE, then the
	@ below address check will need to be #ifdef'd or equivalent
	@ for the Aegis platform.
	mrc	p15, 4, r0, c15, c0	@ get SCU base address
	teq	r0, #0x0		@ '0' on actual UP A9 hardware
	beq	__fixup_smp_on_up	@ So its an A9 UP
	ldr	r0, [r0, #4]		@ read SCU Config
ARM_BE8(rev	r0, r0)			@ byteswap if big endian
	and	r0, r0, #0x3		@ number of CPUs
	teq	r0, #0x0		@ is 1?
	retne	lr

__fixup_smp_on_up:
	adr_l	r4, __smpalt_begin
	adr_l	r5, __smpalt_end
	b	__do_fixup_smp_on_up
ENDPROC(__fixup_smp)

	.pushsection .data
	.align	2
	.globl	smp_on_up
smp_on_up:
	ALT_SMP(.long	1)
	ALT_UP(.long	0)
	.popsection
#endif

	.text
__do_fixup_smp_on_up:
	cmp	r4, r5
	reths	lr
	ldmia	r4, {r0, r6}
 ARM(	str	r6, [r0, r4]	)
 THUMB(	add	r0, r0, r4	)
	add	r4, r4, #8
#ifdef __ARMEB__
 THUMB(	mov	r6, r6, ror #16	)	@ Convert word order for big-endian.
#endif
 THUMB(	strh	r6, [r0], #2	)	@ For Thumb-2, store as two halfwords
 THUMB(	mov	r6, r6, lsr #16	)	@ to be robust against misaligned r0.
 THUMB(	strh	r6, [r0]	)
	b	__do_fixup_smp_on_up
ENDPROC(__do_fixup_smp_on_up)

ENTRY(fixup_smp)
	stmfd	sp!, {r4 - r6, lr}
	mov	r4, r0
	add	r5, r0, r1
	bl	__do_fixup_smp_on_up
	ldmfd	sp!, {r4 - r6, pc}
ENDPROC(fixup_smp)

#include "head-common.S"


code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 * Hibernation support specific for ARM
 *
 * Derived from work on ARM hibernation support by:
 *
 * Ubuntu project, hibernation support for mach-dove
 * Copyright (C) 2010 Nokia Corporation (Hiroshi Doyu)
 * Copyright (C) 2010 Texas Instruments, Inc. (Teerth Reddy et al.)
 *  https://lkml.org/lkml/2010/6/18/4
 *  https://lists.linux-foundation.org/pipermail/linux-pm/2010-June/027422.html
 *  https://patchwork.kernel.org/patch/96442/
 *
 * Copyright (C) 2006 Rafael J. Wysocki <rjw@sisk.pl>
 */

#include <linux/mm.h>
#include <linux/suspend.h>
#include <asm/system_misc.h>
#include <asm/idmap.h>
#include <asm/suspend.h>
#include <asm/memory.h>
#include <asm/sections.h>
#include "reboot.h"

int pfn_is_nosave(unsigned long pfn)
{
	unsigned long nosave_begin_pfn = virt_to_pfn(&__nosave_begin);
	unsigned long nosave_end_pfn = virt_to_pfn(&__nosave_end - 1);

	return (pfn >= nosave_begin_pfn) && (pfn <= nosave_end_pfn);
}

void notrace save_processor_state(void)
{
	WARN_ON(num_online_cpus() != 1);
	local_fiq_disable();
}

void notrace restore_processor_state(void)
{
	local_fiq_enable();
}

/*
 * Snapshot kernel memory and reset the system.
 *
 * swsusp_save() is executed in the suspend finisher so that the CPU
 * context pointer and memory are part of the saved image, which is
 * required by the resume kernel image to restart execution from
 * swsusp_arch_suspend().
 *
 * soft_restart is not technically needed, but is used to get success
 * returned from cpu_suspend.
 *
 * When soft reboot completes, the hibernation snapshot is written out.
 */
static int notrace arch_save_image(unsigned long unused)
{
	int ret;

	ret = swsusp_save();
	if (ret == 0)
		_soft_restart(virt_to_idmap(cpu_resume), false);
	return ret;
}

/*
 * Save the current CPU state before suspend / poweroff.
 */
int notrace swsusp_arch_suspend(void)
{
	return cpu_suspend(0, arch_save_image);
}

/*
 * Restore page contents for physical pages that were in use during loading
 * hibernation image.  Switch to idmap_pgd so the physical page tables
 * are overwritten with the same contents.
 */
static void notrace arch_restore_image(void *unused)
{
	struct pbe *pbe;

	cpu_switch_mm(idmap_pgd, &init_mm);
	for (pbe = restore_pblist; pbe; pbe = pbe->next)
		copy_page(pbe->orig_address, pbe->address);

	_soft_restart(virt_to_idmap(cpu_resume), false);
}

static u64 resume_stack[PAGE_SIZE/2/sizeof(u64)] __nosavedata;

/*
 * Resume from the hibernation image.
 * Due to the kernel heap / data restore, stack contents change underneath
 * and that would make function calls impossible; switch to a temporary
 * stack within the nosave region to avoid that problem.
 */
int swsusp_arch_resume(void)
{
	call_with_stack(arch_restore_image, 0,
		resume_stack + ARRAY_SIZE(resume_stack));
	return 0;
}