entry.s

linux-2.6.15.6

Lfiqmsg:	.ascii	"*** Unexpected FIQ\n\0"
		.align

.LCfiq:		.word	__temp_fiq
.LCirq:		.word	__temp_irq

/*=============================================================================
 * Undefined instruction handler
 *-----------------------------------------------------------------------------
 * Handles floating point instructions
 */
vector_undefinstr:
		tst	lr, #MODE_SVC26          @ did we come from a non-user mode?
		bne	__und_svc                @ yes - deal with it.
/* Otherwise, fall through for the user-space (common) case. */
		save_user_regs
		zero_fp                                 @ zero frame pointer
		teqp	pc, #PSR_I_BIT | MODE_SVC26     @ disable IRQs
.Lbug_undef:
		ldr	r4, .LC2
                ldr     pc, [r4]         @ Call FP module entry point
/* FIXME - should we trap for a null pointer here? */

/* The SVC mode case */
__und_svc:	save_svc_regs                           @ Non-user mode
                mask_pc r0, lr
                and     r2, lr, #3
                sub     r0, r0, #4
                mov     r1, sp
                bl      do_undefinstr
                restore_svc_regs

/* We get here if the FP emulator doesnt handle the undef instr.
 * If the insn WAS handled, the emulator jumps to ret_from_exception by itself/
 */
		.globl	fpundefinstr 
fpundefinstr:
		mov	r0, lr
		mov	r1, sp
		teqp	pc, #MODE_SVC26
		bl	do_undefinstr
		b	ret_from_exception		@ Normal FP exit

#if defined CONFIG_FPE_NWFPE || defined CONFIG_FPE_FASTFPE
		/* The FPE is always present */
		.equ	fpe_not_present, 0
#else
/* We get here if an undefined instruction happens and the floating
 * point emulator is not present.  If the offending instruction was
 * a WFS, we just perform a normal return as if we had emulated the
 * operation.  This is a hack to allow some basic userland binaries
 * to run so that the emulator module proper can be loaded. --philb
 * FIXME - probably a broken useless hack...
 */
fpe_not_present:
		adr	r10, wfs_mask_data
		ldmia	r10, {r4, r5, r6, r7, r8}
		ldr	r10, [sp, #S_PC]		@ Load PC
		sub	r10, r10, #4
		mask_pc	r10, r10
		ldrt	r10, [r10]			@ get instruction
		and	r5, r10, r5
		teq	r5, r4				@ Is it WFS?
		beq	ret_from_exception
		and	r5, r10, r8
		teq	r5, r6				@ Is it LDF/STF on sp or fp?
		teqne	r5, r7
		bne	fpundefinstr
		tst	r10, #0x00200000		@ Does it have WB
		beq	ret_from_exception
		and	r4, r10, #255			@ get offset
		and	r6, r10, #0x000f0000
		tst	r10, #0x00800000		@ +/-
		ldr	r5, [sp, r6, lsr #14]		@ Load reg
		rsbeq	r4, r4, #0
		add	r5, r5, r4, lsl #2
		str	r5, [sp, r6, lsr #14]		@ Save reg
		b	ret_from_exception

wfs_mask_data:	.word	0x0e200110			@ WFS/RFS
		.word	0x0fef0fff
		.word	0x0d0d0100			@ LDF [sp]/STF [sp]
		.word	0x0d0b0100			@ LDF [fp]/STF [fp]
		.word	0x0f0f0f00
#endif

.LC2:		.word	fp_enter

/*=============================================================================
 * Prefetch abort handler
 *-----------------------------------------------------------------------------
 */
#define DEBUG_UNDEF
/* remember: lr = USR pc */
vector_prefetch:
		sub	lr, lr, #4
		tst	lr, #MODE_SVC26
		bne	__pabt_invalid
		save_user_regs
		teqp	pc, #MODE_SVC26         @ Enable IRQs...
		mask_pc	r0, lr			@ Address of abort
		mov	r1, sp			@ Tasks registers
		bl	do_PrefetchAbort
		teq	r0, #0			@ If non-zero, we believe this abort..
		bne	ret_from_exception
#ifdef DEBUG_UNDEF
		adr	r0, t
		bl	printk
#endif
		ldr	lr, [sp,#S_PC]		@ FIXME program to test this on.  I think its
		b	.Lbug_undef		@ broken at the moment though!)

__pabt_invalid:	save_svc_regs
		mov	r0, sp			@ Prefetch aborts are definitely *not*
		mov	r1, #BAD_PREFETCH	@ allowed in non-user modes.  We cant
		and	r2, lr, #3		@ recover from this problem.
		b	bad_mode

#ifdef DEBUG_UNDEF
t:		.ascii "*** undef ***\r\n\0"
		.align
#endif

/*=============================================================================
 * Address exception handler
 *-----------------------------------------------------------------------------
 * These aren't too critical.
 * (they're not supposed to happen).
 * In order to debug the reason for address exceptions in non-user modes,
 * we have to obtain all the registers so that we can see what's going on.
 */

vector_addrexcptn:
		sub	lr, lr, #8
		tst	lr, #3
		bne	Laddrexcptn_not_user
		save_user_regs
		teq	pc, #MODE_SVC26
		mask_pc	r0, lr			@ Point to instruction
		mov	r1, sp			@ Point to registers
		mov	r2, #0x400
		mov	lr, pc
		bl	do_excpt
		b	ret_from_exception

Laddrexcptn_not_user:
		save_svc_regs
		and	r2, lr, #3
		teq	r2, #3
		bne	Laddrexcptn_illegal_mode
		teqp	pc, #MODE_SVC26
		mask_pc	r0, lr
		mov	r1, sp
		orr	r2, r2, #0x400
		bl	do_excpt
		ldmia	sp, {r0 - lr}		@ I cant remember the reason I changed this...
		add	sp, sp, #15*4
		movs	pc, lr

Laddrexcptn_illegal_mode:
		mov	r0, sp
		str	lr, [sp, #-4]!
		orr	r1, r2, #PSR_I_BIT | PSR_F_BIT
		teqp	r1, #0			@ change into mode (wont be user mode)
		mov	r0, r0
		mov	r1, r8			@ Any register from r8 - r14 can be banked
		mov	r2, r9
		mov	r3, r10
		mov	r4, r11
		mov	r5, r12
		mov	r6, r13
		mov	r7, r14
		teqp	pc, #PSR_F_BIT | MODE_SVC26 @ back to svc
		mov	r0, r0
		stmfd	sp!, {r1-r7}
		ldmia	r0, {r0-r7}
		stmfd	sp!, {r0-r7}
		mov	r0, sp
		mov	r1, #BAD_ADDREXCPTN
		b	bad_mode

/*=============================================================================
 * Interrupt (IRQ) handler
 *-----------------------------------------------------------------------------
 * Note: if the IRQ was taken whilst in user mode, then *no* kernel routine
 * is running, so do not have to save svc lr.
 *
 * Entered in IRQ mode.
 */

vector_IRQ:	ldr     sp, .LCirq         @ Setup some temporary stack
                sub     lr, lr, #4
                str     lr, [sp]           @ push return address

		tst     lr, #3
		bne	__irq_non_usr

__irq_usr:	teqp	pc, #PSR_I_BIT | MODE_SVC26     @ Enter SVC mode
		mov	r0, r0

		ldr	lr, .LCirq
		ldr	lr, [lr]           @ Restore lr for jump back to USR

		save_user_regs

		handle_irq

		mov	why, #0
		get_thread_info tsk
		b	ret_to_user

@ Place the IRQ priority table here so that the handle_irq macros above
@ and below here can access it.

		irq_prio_table

__irq_non_usr:	teqp	pc, #PSR_I_BIT | MODE_SVC26     @ Enter SVC mode
		mov	r0, r0

		save_svc_regs_irq

                and	r2, lr, #3
		teq	r2, #3
		bne	__irq_invalid                @ IRQ not from SVC mode

		handle_irq

		restore_svc_regs

__irq_invalid:	mov	r0, sp
		mov	r1, #BAD_IRQ
		b	bad_mode

/*=============================================================================
 * Data abort handler code
 *-----------------------------------------------------------------------------
 *
 * This handles both exceptions from user and SVC modes, computes the address
 *  range of the problem, and does any correction that is required.  It then
 *  calls the kernel data abort routine.
 *
 * This is where I wish that the ARM would tell you which address aborted.
 */

vector_data:	sub	lr, lr, #8		@ Correct lr
		tst	lr, #3
		bne	Ldata_not_user
		save_user_regs
		teqp	pc, #MODE_SVC26
		mask_pc	r0, lr
		bl	Ldata_do
		b	ret_from_exception

Ldata_not_user:
		save_svc_regs
		and	r2, lr, #3
		teq	r2, #3
		bne	Ldata_illegal_mode
		tst	lr, #PSR_I_BIT
		teqeqp	pc, #MODE_SVC26
		mask_pc	r0, lr
		bl	Ldata_do
		restore_svc_regs

Ldata_illegal_mode:
		mov	r0, sp
		mov	r1, #BAD_DATA
		b	bad_mode

Ldata_do:	mov	r3, sp
		ldr	r4, [r0]		@ Get instruction
		mov	r2, #0
		tst	r4, #1 << 20		@ Check to see if it is a write instruction
		orreq	r2, r2, #FAULT_CODE_WRITE @ Indicate write instruction
		mov	r1, r4, lsr #22		@ Now branch to the relevent processing routine
		and	r1, r1, #15 << 2
		add	pc, pc, r1
		movs	pc, lr
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_ldrstr_post	@ ldr	rd, [rn], #m
		b	Ldata_ldrstr_numindex	@ ldr	rd, [rn, #m]	@ RegVal
		b	Ldata_ldrstr_post	@ ldr	rd, [rn], rm
		b	Ldata_ldrstr_regindex	@ ldr	rd, [rn, rm]
		b	Ldata_ldmstm		@ ldm*a	rn, <rlist>
		b	Ldata_ldmstm		@ ldm*b	rn, <rlist>
		b	Ldata_unknown
		b	Ldata_unknown
		b	Ldata_ldrstr_post	@ ldc	rd, [rn], #m	@ Same as ldr	rd, [rn], #m
		b	Ldata_ldcstc_pre	@ ldc	rd, [rn, #m]
		b	Ldata_unknown
Ldata_unknown:	@ Part of jumptable
		mov	r0, r1
		mov	r1, r4
		mov	r2, r3
		b	baddataabort

Ldata_ldrstr_post:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		biceq	r0, r0, #PCMASK
		mov	r1, r0
#ifdef FAULT_CODE_LDRSTRPOST
		orr	r2, r2, #FAULT_CODE_LDRSTRPOST
#endif
		b	do_DataAbort

Ldata_ldrstr_numindex:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		mov	r1, r4, lsl #20
		biceq	r0, r0, #PCMASK
		tst	r4, #1 << 23
		addne	r0, r0, r1, lsr #20
		subeq	r0, r0, r1, lsr #20
		mov	r1, r0
#ifdef FAULT_CODE_LDRSTRPRE
		orr	r2, r2, #FAULT_CODE_LDRSTRPRE
#endif
		b	do_DataAbort

Ldata_ldrstr_regindex:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		and	r7, r4, #15
		biceq	r0, r0, #PCMASK
		teq	r7, #15			@ Check for PC
		ldr	r7, [r3, r7, lsl #2]	@ Get Rm
		and	r8, r4, #0x60		@ Get shift types
		biceq	r7, r7, #PCMASK
		mov	r9, r4, lsr #7		@ Get shift amount
		and	r9, r9, #31
		teq	r8, #0
		moveq	r7, r7, lsl r9
		teq	r8, #0x20		@ LSR shift
		moveq	r7, r7, lsr r9
		teq	r8, #0x40		@ ASR shift
		moveq	r7, r7, asr r9
		teq	r8, #0x60		@ ROR shift
		moveq	r7, r7, ror r9
		tst	r4, #1 << 23
		addne	r0, r0, r7
		subeq	r0, r0, r7		@ Apply correction
		mov	r1, r0
#ifdef FAULT_CODE_LDRSTRREG
		orr	r2, r2, #FAULT_CODE_LDRSTRREG
#endif
		b	do_DataAbort

Ldata_ldmstm:
		mov	r7, #0x11
		orr	r7, r7, r7, lsl #8
		and	r0, r4, r7
		and	r1, r4, r7, lsl #1
		add	r0, r0, r1, lsr #1
		and	r1, r4, r7, lsl #2
		add	r0, r0, r1, lsr #2
		and	r1, r4, r7, lsl #3
		add	r0, r0, r1, lsr #3
		add	r0, r0, r0, lsr #8
		add	r0, r0, r0, lsr #4
		and	r7, r0, #15		@ r7 = no. of registers to transfer.
		mov	r5, r4, lsr #14		@ Get Rn
		and	r5, r5, #15 << 2
		ldr	r0, [r3, r5]		@ Get reg
		eor	r6, r4, r4, lsl #2
		tst	r6, #1 << 23		@ Check inc/dec ^ writeback
		rsbeq	r7, r7, #0
		add	r7, r0, r7, lsl #2	@ Do correction (signed)
		subne	r1, r7, #1
		subeq	r1, r0, #1
		moveq	r0, r7
		tst	r4, #1 << 21		@ Check writeback
		strne	r7, [r3, r5]
		eor	r6, r4, r4, lsl #1
		tst	r6, #1 << 24		@ Check Pre/Post ^ inc/dec
		addeq	r0, r0, #4
		addeq	r1, r1, #4
		teq	r5, #15*4		@ CHECK FOR PC
		biceq	r1, r1, #PCMASK
		biceq	r0, r0, #PCMASK
#ifdef FAULT_CODE_LDMSTM
		orr	r2, r2, #FAULT_CODE_LDMSTM
#endif
		b	do_DataAbort

Ldata_ldcstc_pre:
		mov	r0, r4, lsr #14		@ Get Rn
		and	r0, r0, #15 << 2	@ Mask out reg.
		teq	r0, #15 << 2
		ldr	r0, [r3, r0]		@ Get register
		mov	r1, r4, lsl #24		@ Get offset
		biceq	r0, r0, #PCMASK
		tst	r4, #1 << 23
		addne	r0, r0, r1, lsr #24
		subeq	r0, r0, r1, lsr #24
		mov	r1, r0
#ifdef FAULT_CODE_LDCSTC
		orr	r2, r2, #FAULT_CODE_LDCSTC
#endif
		b	do_DataAbort


/*
 * This is the return code to user mode for abort handlers
 */
ENTRY(ret_from_exception)
		get_thread_info tsk
		mov	why, #0
		b	ret_to_user

		.data
ENTRY(fp_enter)
		.word	fpe_not_present
		.text
/*
 * Register switch for older 26-bit only ARMs
 */
ENTRY(__switch_to)
		add	r0, r0, #TI_CPU_SAVE
		stmia	r0, {r4 - sl, fp, sp, lr}
		add	r1, r1, #TI_CPU_SAVE
		ldmia	r1, {r4 - sl, fp, sp, pc}^

/*
 *=============================================================================
 *		Low-level interface code
 *-----------------------------------------------------------------------------
 *		Trap initialisation
 *-----------------------------------------------------------------------------
 *
 * Note - FIQ code has changed.  The default is a couple of words in 0x1c, 0x20
 * that call _unexp_fiq.  Nowever, we now copy the FIQ routine to 0x1c (removes
 * some excess cycles).
 *
 * What we need to put into 0-0x1c are branches to branch to the kernel.
 */

		.section ".init.text",#alloc,#execinstr

.Ljump_addresses:
		swi	SYS_ERROR0
		.word	vector_undefinstr	- 12
		.word	vector_swi		- 16
		.word	vector_prefetch		- 20
		.word	vector_data		- 24
		.word	vector_addrexcptn	- 28
		.word	vector_IRQ		- 32
		.word	_unexp_fiq		- 36
		b	. + 8
/*
 * initialise the trap system
 */
ENTRY(__trap_init)
		stmfd	sp!, {r4 - r7, lr}
		adr	r1, .Ljump_addresses
		ldmia	r1, {r1 - r7, ip, lr}
		orr	r2, lr, r2, lsr #2
		orr	r3, lr, r3, lsr #2
		orr	r4, lr, r4, lsr #2
		orr	r5, lr, r5, lsr #2
		orr	r6, lr, r6, lsr #2
		orr	r7, lr, r7, lsr #2
		orr	ip, lr, ip, lsr #2
		mov	r0, #0
		stmia	r0, {r1 - r7, ip}
		ldmfd	sp!, {r4 - r7, pc}^

		.bss
__temp_irq:	.space	4				@ saved lr_irq
__temp_fiq:	.space	128