1 files changed, 125 insertions, 0 deletions
diff --git a/arch/arm/kernel/sleep.S b/arch/arm/kernel/sleep.S
new file mode 100644
index 00000000..baf4d282
--- /dev/null
+++ b/arch/arm/kernel/sleep.S
@@ -0,0 +1,125 @@
+#include <linux/linkage.h>
+#include <linux/threads.h>
+#include <asm/asm-offsets.h>
+#include <asm/assembler.h>
+#include <asm/glue-cache.h>
+#include <asm/glue-proc.h>
+#include "entry-header.S"
+	.text
+
+/*
+ * Save CPU state for a suspend.  This saves the CPU general purpose
+ * registers, and allocates space on the kernel stack to save the CPU
+ * specific registers and some other data for resume.
+ *  r0 = suspend function arg0
+ *  r1 = suspend function
+ */
+ENTRY(__cpu_suspend)
+	stmfd	sp!, {r4 - r11, lr}
+#ifdef MULTI_CPU
+	ldr	r10, =processor
+	ldr	r4, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
+#else
+	ldr	r4, =cpu_suspend_size
+#endif
+	mov	r5, sp			@ current virtual SP
+	add	r4, r4, #12		@ Space for pgd, virt sp, phys resume fn
+	sub	sp, sp, r4		@ allocate CPU state on stack
+	stmfd	sp!, {r0, r1}		@ save suspend func arg and pointer
+	add	r0, sp, #8		@ save pointer to save block
+	mov	r1, r4			@ size of save block
+	mov	r2, r5			@ virtual SP
+	ldr	r3, =sleep_save_sp
+#ifdef CONFIG_SMP
+	get_thread_info	r5
+	ldr	lr, [r5, #TI_CPU] 	@ cpu logical index
+	add	r3, r3, lr, lsl #2
+#endif
+	bl	__cpu_suspend_save
+	adr	lr, BSYM(cpu_suspend_abort)
+	ldmfd	sp!, {r0, pc}		@ call suspend fn
+ENDPROC(__cpu_suspend)
+	.ltorg
+
+cpu_suspend_abort:
+	ldmia	sp!, {r1 - r3}		@ pop phys pgd, virt SP, phys resume fn
+	teq	r0, #0
+	moveq	r0, #1			@ force non-zero value
+	mov	sp, r2
+	ldmfd	sp!, {r4 - r11, pc}
+ENDPROC(cpu_suspend_abort)
+
+/*
+ * r0 = control register value
+ */
+	.align	5
+	.pushsection	.idmap.text,"ax"
+ENTRY(cpu_resume_mmu)
+	ldr	r3, =cpu_resume_after_mmu
+	instr_sync
+	mcr	p15, 0, r0, c1, c0, 0	@ turn on MMU, I-cache, etc
+	mrc	p15, 0, r0, c0, c0, 0	@ read id reg
+	instr_sync
+	mov	r0, r0
+	mov	r0, r0
+	mov	pc, r3			@ jump to virtual address
+ENDPROC(cpu_resume_mmu)
+	.popsection
+cpu_resume_after_mmu:
+	bl	cpu_init		@ restore the und/abt/irq banked regs
+	mov	r0, #0			@ return zero on success
+	ldmfd	sp!, {r4 - r11, pc}
+ENDPROC(cpu_resume_after_mmu)
+
+/*
+ * Note: Yes, part of the following code is located into the .data section.
+ *       This is to allow sleep_save_sp to be accessed with a relative load
+ *       while we can't rely on any MMU translation.  We could have put
+ *       sleep_save_sp in the .text section as well, but some setups might
+ *       insist on it to be truly read-only.
+ */
+	.data
+	.align
+ENTRY(cpu_resume)
+ARM_BE8(setend be)			@ ensure we are in BE mode
+#ifdef CONFIG_SMP
+	mov	r1, #0			@ fall-back logical index for UP
+	ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
+	ALT_UP_B(1f)
+	bic	r0, #0xff000000
+	bl	cpu_logical_index 	@ return logical index in r1
+1:
+	adr	r0, sleep_save_sp
+	ldr	r0, [r0, r1, lsl #2]	@ stack phys addr
+#else
+	ldr	r0, sleep_save_sp	@ stack phys addr
+#endif
+	setmode	PSR_I_BIT | PSR_F_BIT | SVC_MODE, r1  @ set SVC, irqs off
+	@ load phys pgd, stack, resume fn
+  ARM(	ldmia	r0!, {r1, sp, pc}	)
+THUMB(	ldmia	r0!, {r1, r2, r3}	)
+THUMB(	mov	sp, r2			)
+THUMB(	bx	r3			)
+ENDPROC(cpu_resume)
+
+sleep_save_sp:
+	.rept	CONFIG_NR_CPUS
+	.long	0				@ preserve stack phys ptr here
+	.endr
+
+#ifdef CONFIG_SMP
+cpu_logical_index:
+	adr	r3, cpu_map_ptr
+	ldr	r2, [r3]
+	add	r3, r3, r2		@ virt_to_phys(__cpu_logical_map)
+	mov	r1, #0
+1:
+	ldr	r2, [r3, r1, lsl #2]
+	cmp	r2, r0
+	moveq	pc, lr
+	add	r1, r1, #1
+	b	1b
+
+cpu_map_ptr:
+	.long __cpu_logical_map - .
+#endif