Merge with Linux 2.3.7.

WARNING:  2.3.7 is known to eat filesystems for breakfast and little
children for lunch, so if you try this on your machine make backups
first ...

1 files changed, 528 insertions, 430 deletions

diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index bcc938b32..9456abe33 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -61,8 +61,12 @@
 #define S_R1		4
 #define S_R0		0
 
+#define OFF_CR_ALIGNMENT(x)	cr_alignment - x
+
 #ifdef IOC_BASE
 /* IOC / IOMD based hardware */
+#include <asm/iomd.h>
+
 		.equ	ioc_base_high, IOC_BASE & 0xff000000
 		.equ	ioc_base_low, IOC_BASE & 0x00ff0000
 		.macro	disable_fiq
@@ -186,113 +190,109 @@ irq_prio_ebsa110:
 		.byte	 6, 6, 6, 6, 2, 2, 2, 2, 3, 3, 6, 6, 2, 2, 2, 2
 		.endm
 
-#elif defined(CONFIG_ARCH_EBSA285)
+#elif defined(CONFIG_HOST_FOOTBRIDGE) || defined(CONFIG_ADDIN_FOOTBRIDGE)
+#include <asm/dec21285.h>
 
 		.macro	disable_fiq
 		.endm
 
+		.equ	irq_mask_pci_err_high, IRQ_MASK_PCI_ERR & 0xff000000
+		.equ	irq_mask_pci_err_low,  IRQ_MASK_PCI_ERR & 0x00ffffff
+		.equ	dc21285_high, ARMCSR_BASE & 0xff000000
+		.equ	dc21285_low, ARMCSR_BASE & 0x00ffffff
+
 		.macro	get_irqnr_and_base, irqnr, irqstat, base
-		mov	r4, #0xfe000000
+		mov	r4, #dc21285_high
+		.if	dc21285_low
+		orr	r4, r4, #dc21285_low
+		.endif
 		ldr	\irqstat, [r4, #0x180]		@ get interrupts
-		mov	\irqnr, #0
-1001:		tst	\irqstat, #1
-		addeq	\irqnr, \irqnr, #1
-		moveq	\irqstat, \irqstat, lsr #1
-		tsteq	\irqnr, #32
-		beq	1001b
-		teq	\irqnr, #32
-		.endm
 
-		.macro	irq_prio_table
-		.endm
-
-#elif defined(CONFIG_ARCH_NEXUSPCI)
+		mov	\irqnr, #IRQ_SDRAMPARITY
+		tst	\irqstat, #IRQ_MASK_SDRAMPARITY
+		bne	1001f
 
-		.macro	disable_fiq
-		.endm
+		tst	\irqstat, #IRQ_MASK_UART_RX
+		movne	\irqnr, #IRQ_CONRX
+		bne	1001f
 
-		.macro	get_irqnr_and_base, irqnr, irqstat, base
-		ldr	r4, =0xffe00000
-		ldr	\irqstat, [r4, #0x180]		@ get interrupts
-		mov	\irqnr, #0
-1001:		tst	\irqstat, #1
-		addeq	\irqnr, \irqnr, #1
-		moveq	\irqstat, \irqstat, lsr #1
-		tsteq	\irqnr, #32
-		beq	1001b
-		teq	\irqnr, #32
-		.endm
+		tst	\irqstat, #IRQ_MASK_DMA1
+		movne	\irqnr, #IRQ_DMA1
+		bne	1001f
 
-		.macro	irq_prio_table
-		.endm
+		tst	\irqstat, #IRQ_MASK_DMA2
+		movne	\irqnr, #IRQ_DMA2
+		bne	1001f
 
-#elif defined(CONFIG_ARCH_VNC)
+		tst	\irqstat, #IRQ_MASK_IN0
+		movne	\irqnr, #IRQ_IN0
+		bne	1001f
 
-		.macro	disable_fiq
-		.endm
+		tst	\irqstat, #IRQ_MASK_IN1
+		movne	\irqnr, #IRQ_IN1
+		bne	1001f
 
-		.equ	pci_iack_high, PCI_IACK & 0xff000000
-		.equ	pci_iack_low,  PCI_IACK & 0x00ff0000
+		tst	\irqstat, #IRQ_MASK_IN2
+		movne	\irqnr, #IRQ_IN2
+		bne	1001f
 
-		.macro	get_irqnr_and_base, irqnr, irqstat, base
-		mov	r4, #IO_BASE_ARM_CSR
-		ldr	\irqstat, [r4, #CSR_IRQ_STATUS]	@ just show us the unmasked ones
+		tst	\irqstat, #IRQ_MASK_IN3
+		movne	\irqnr, #IRQ_IN3
+		bne	1001f
 
-		@ run through hard priorities
-		@ timer
-		tst	\irqstat, #IRQ_MASK_TIMER0
-		movne	\irqnr, #IRQ_TIMER0
+		tst	\irqstat, #IRQ_MASK_PCI
+		movne	\irqnr, #IRQ_PCI
 		bne	1001f
 
-		@ ether10
-		tst	\irqstat, #IRQ_MASK_ETHER10
-		movne	\irqnr, #IRQ_ETHER10
+		tst	\irqstat, #IRQ_MASK_I2OINPOST
+		movne	\irqnr, #IRQ_I2OINPOST
 		bne	1001f
 
-		@ ether100
-		tst	\irqstat, #IRQ_MASK_ETHER100
-		movne	\irqnr, #IRQ_ETHER100
+		tst	\irqstat, #IRQ_MASK_TIMER1
+		movne	\irqnr, #IRQ_TIMER1
 		bne	1001f
 
-		@ video compressor
-		tst	\irqstat, #IRQ_MASK_VIDCOMP
-		movne	\irqnr, #IRQ_VIDCOMP
+		tst	\irqstat, #IRQ_MASK_TIMER2
+		movne	\irqnr, #IRQ_TIMER2
 		bne	1001f
 
-		@ now try all the PIC sources
-		@ determine whether we have an irq
-		tst	\irqstat, #IRQ_MASK_EXTERN_IRQ
-		beq	1002f
-		mov	r4, #pci_iack_high
-		orr	r4, r4, #pci_iack_low
-		ldrb	\irqnr, [r4]			@ get the IACK byte
-		b	1001f
-
-1002:		@ PCI errors
-		tst	\irqstat, #IRQ_MASK_PCI_ERR
-		movne	\irqnr, #IRQ_PCI_ERR
+		tst	\irqstat, #IRQ_MASK_TIMER3
+		movne	\irqnr, #IRQ_TIMER3
 		bne	1001f
 
-		@ softint
-		tst	\irqstat, #IRQ_MASK_SOFTIRQ
-		movne	\irqnr, #IRQ_SOFTIRQ
+		tst	\irqstat, #IRQ_MASK_UART_TX
+		movne	\irqnr, #IRQ_CONTX
 		bne	1001f
 
-		@ debug uart
-		tst	\irqstat, #IRQ_MASK_UART_DEBUG
-		movne	\irqnr, #IRQ_CONRX
+		tst	\irqstat, #irq_mask_pci_err_high
+		tsteq	\irqstat, #irq_mask_pci_err_low
+		movne	\irqnr, #IRQ_PCI_ERR
 		bne	1001f
+1001:
+		.endm
 
-		@ watchdog
-		tst	\irqstat, #IRQ_MASK_WATCHDOG
-		movne	\irqnr, #IRQ_WATCHDOG
+		.macro	irq_prio_table
+		.endm
 
-1001:		@ If Z is set, then we will not enter an interrupt
+#elif defined(CONFIG_ARCH_NEXUSPCI)
+
+		.macro	disable_fiq
 		.endm
 
-		.macro	irq_prio_table
+		.macro	get_irqnr_and_base, irqnr, irqstat, base
+		ldr	r4, =0xffe00000
+		ldr	\irqstat, [r4, #0x180]		@ get interrupts
+		mov	\irqnr, #0
+1001:		tst	\irqstat, #1
+		addeq	\irqnr, \irqnr, #1
+		moveq	\irqstat, \irqstat, lsr #1
+		tsteq	\irqnr, #32
+		beq	1001b
+		teq	\irqnr, #32
 		.endm
 
+		.macro	irq_prio_table
+		.endm
 #else
 #error Unknown architecture
 #endif
@@ -306,22 +306,22 @@ irq_prio_ebsa110:
 		stmia	sp, {r0 - r12}			@ Calling r0 - r12
 		add	r8, sp, #S_PC
 		stmdb	r8, {sp, lr}^			@ Calling sp, lr
-		mov	r7, r0
+		str	lr, [r8, #0]			@ Save calling PC
 		mrs	r6, spsr
-		mov	r5, lr
-		stmia	r8, {r5, r6, r7}		@ Save calling PC, CPSR, OLD_R0
+		str	r6, [r8, #4]			@ Save CPSR
+		str	r0, [r8, #8]			@ Save OLD_R0
 		.endm
 
 		.macro	restore_user_regs
-		mrs	r0, cpsr			@ disable IRQs
-		orr	r0, r0, #I_BIT
-		msr	cpsr, r0
+		mrs	r1, cpsr			@ disable IRQs
+		orr	r1, r1, #I_BIT
 		ldr	r0, [sp, #S_PSR]		@ Get calling cpsr
+		msr	cpsr, r1
 		msr	spsr, r0			@ save in spsr_svc
 		ldmia	sp, {r0 - lr}^			@ Get calling r0 - lr
 		mov	r0, r0
-		add	sp, sp, #S_PC
-		ldr	lr, [sp], #S_FRAME_SIZE - S_PC	@ Get PC and jump over PC, PSR, OLD_R0
+		ldr	lr, [sp, #S_PC]			@ Get PC
+		add	sp, sp, #S_FRAME_SIZE
 		movs	pc, lr				@ return & move spsr_svc into cpsr
 		.endm
 
@@ -348,25 +348,6 @@ irq_prio_ebsa110:
 		msr	cpsr, \temp
 		.endm
 
-		.macro	initialise_traps_extra
-		mrs	r0, cpsr
-		bic	r0, r0, #31
-		orr	r0, r0, #0xd3
-		msr	cpsr, r0
-		.endm
-
-
-#ifndef __ARM_ARCH_4__
-.Larm700bug:	str	lr, [r8]
-		ldr	r0, [sp, #S_PSR]		@ Get calling cpsr
-		msr	spsr, r0
-		ldmia	sp, {r0 - lr}^			@ Get calling r0 - lr
-		mov	r0, r0
-		add	sp, sp, #S_PC
-		ldr	lr, [sp], #S_FRAME_SIZE - S_PC	@ Get PC and jump over PC, PSR, OLD_R0
-		movs	pc, lr
-#endif
-
 		.macro	get_current_task, rd
 		mov	\rd, sp, lsr #13
 		mov	\rd, \rd, lsl #13
@@ -379,231 +360,89 @@ irq_prio_ebsa110:
 		adr\cond	\reg, \label
 		.endm
 
-/*=============================================================================
- * Address exception handler
- *-----------------------------------------------------------------------------
- * These aren't too critical.
- * (they're not supposed to happen, and won't happen in 32-bit mode).
- */
-
-vector_addrexcptn:
-		b	vector_addrexcptn
-
-/*=============================================================================
- * Undefined FIQs
- *-----------------------------------------------------------------------------
- * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC
- * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg.
- * Basically to switch modes, we *HAVE* to clobber one register...  brain
- * damage alert!  I don't think that we can execute any code in here in any
- * other mode than FIQ...  Ok you can switch to another mode, but you can't
- * get out of that mode without clobbering one register.
- */
-_unexp_fiq:	disable_fiq
-		subs	pc, lr, #4
-
-/*=============================================================================
- * Interrupt entry dispatcher
- *-----------------------------------------------------------------------------
- * Enter in IRQ mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
- */
-vector_IRQ:	@
-		@ save mode specific registers
-		@
-		ldr	r13, .LCirq
-		sub	lr, lr, #4
-		str	lr, [r13]			@ save lr_IRQ
-		mrs	lr, spsr
-		str	lr, [r13, #4]			@ save spsr_IRQ
-		@
-		@ now branch to the relevent MODE handling routine
-		@
-		mrs	sp, cpsr			@ switch to SVC mode
-		bic	sp, sp, #31
-		orr	sp, sp, #0x13
-		msr	spsr, sp
-		and	lr, lr, #15
-		cmp	lr, #4
-		addlts	pc, pc, lr, lsl #2		@ Changes mode and branches
-		b	__irq_invalid			@  4 - 15
-		b	__irq_usr			@  0  (USR_26 / USR_32)
-		b	__irq_invalid			@  1  (FIQ_26 / FIQ_32)
-		b	__irq_invalid			@  2  (IRQ_26 / IRQ_32)
-		b	__irq_svc			@  3  (SVC_26 / SVC_32)
-/*
- *------------------------------------------------------------------------------------------------
- * Undef instr entry dispatcher - dispatches it to the correct handler for the processor mode
- *------------------------------------------------------------------------------------------------
- * Enter in UND mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
- */
-.LCirq:		.word	__temp_irq
-.LCund:		.word	__temp_und
-.LCabt:		.word	__temp_abt
-
-vector_undefinstr:
-		@
-		@ save mode specific registers
-		@
-		ldr	r13, [pc, #.LCund - . - 8]
-		str	lr, [r13]
-		mrs	lr, spsr
-		str	lr, [r13, #4]
-		@
-		@ now branch to the relevent MODE handling routine
-		@
-		mrs	sp, cpsr
-		bic	sp, sp, #31
-		orr	sp, sp, #0x13
-		msr	spsr, sp
-		and	lr, lr, #15
-		cmp	lr, #4
-		addlts	pc, pc, lr, lsl #2		@ Changes mode and branches
-		b	__und_invalid			@  4 - 15
-		b	__und_usr			@  0 (USR_26 / USR_32)
-		b	__und_invalid			@  1 (FIQ_26 / FIQ_32)
-		b	__und_invalid			@  2 (IRQ_26 / IRQ_32)
-		b	__und_svc			@  3 (SVC_26 / SVC_32)
-/*
- *------------------------------------------------------------------------------------------------
- * Prefetch abort dispatcher - dispatches it to the correct handler for the processor mode
- *------------------------------------------------------------------------------------------------
- * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
- */
-vector_prefetch:
-		@
-		@ save mode specific registers
-		@
-		sub	lr, lr, #4
-		ldr	r13, .LCabt
-		str	lr, [r13]
-		mrs	lr, spsr
-		str	lr, [r13, #4]
-		@
-		@ now branch to the relevent MODE handling routine
-		@
-		mrs	sp, cpsr
-		bic	sp, sp, #31
-		orr	sp, sp, #0x13
-		msr	spsr, sp
-		and	lr, lr, #15
-		adds	pc, pc, lr, lsl #2		@ Changes mode and branches
-		b	__pabt_invalid			@  4 - 15
-		b	__pabt_usr			@  0  (USR_26 / USR_32)
-		b	__pabt_invalid			@  1  (FIQ_26 / FIQ_32)
-		b	__pabt_invalid			@  2  (IRQ_26 / IRQ_32)
-		b	__pabt_invalid			@  3  (SVC_26 / SVC_32)
 /*
- *------------------------------------------------------------------------------------------------
- * Data abort dispatcher - dispatches it to the correct handler for the processor mode
- *------------------------------------------------------------------------------------------------
- * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
+ * Invalid mode handlers
  */
-vector_data:	@
-		@ save mode specific registers
-		@
-		sub	lr, lr, #8
-		ldr	r13, .LCabt
-		str	lr, [r13]
-		mrs	lr, spsr
-		str	lr, [r13, #4]
-		@
-		@ now branch to the relevent MODE handling routine
-		@
-		mrs	sp, cpsr
-		bic	sp, sp, #31
-		orr	sp, sp, #0x13
-		msr	spsr, sp
-		and	lr, lr, #15
-		cmp	lr, #4
-		addlts	pc, pc, lr, lsl #2		@ Changes mode & branches
-		b	__dabt_invalid			@  4 - 15
-		b	__dabt_usr			@  0  (USR_26 / USR_32)
-		b	__dabt_invalid			@  1  (FIQ_26 / FIQ_32)
-		b	__dabt_invalid			@  2  (IRQ_26 / IRQ_32)
-		b	__dabt_svc			@  3  (SVC_26 / SVC_32)
+__pabt_invalid:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
+		stmia	sp, {r0 - lr}			@ Save XXX r0 - lr
+		ldr	r4, .LCabt
+		mov	r1, #BAD_PREFETCH
+		b	1f
 
-/*=============================================================================
- * Prefetch abort handler
- *-----------------------------------------------------------------------------
- */
-pabtmsg:	.ascii	"Pabt: %08lX\n\0"
-		.align
-__pabt_usr:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
-		stmia	sp, {r0 - r12}			@ Save r0 - r12
-		add	r8, sp, #S_PC
-		stmdb	r8, {sp, lr}^			@ Save sp_usr lr_usr
+__dabt_invalid:	sub	sp, sp, #S_FRAME_SIZE
+		stmia	sp, {r0 - lr}			@ Save SVC r0 - lr [lr *should* be intact]
 		ldr	r4, .LCabt
-		ldmia	r4, {r5 - r7}			@ Get USR pc, cpsr
-		stmia	r8, {r5 - r7}			@ Save USR pc, cpsr, old_r0
+		mov	r1, #BAD_DATA
+		b	1f
 
-		mrs	r7, cpsr			@ Enable interrupts if they were
-		bic	r7, r7, #I_BIT			@ previously
-		msr	cpsr, r7
-		mov	r0, r5				@ address (pc)
-		mov	r1, sp				@ regs
-		bl	SYMBOL_NAME(do_PrefetchAbort)	@ call abort handler
-		teq	r0, #0				@ Does this still apply???
-		bne	ret_from_exception		@ Return from exception
-#ifdef DEBUG_UNDEF
-		adr	r0, t
-		bl	SYMBOL_NAME(printk)
-#endif
-		mov	r0, r5
-		mov	r1, sp
-		and	r2, r6, #31
-		bl	SYMBOL_NAME(do_undefinstr)
-		ldr	lr, [sp, #S_PSR]		@ Get USR cpsr
-		msr	spsr, lr
-		ldmia	sp, {r0 - pc}^			@ Restore USR registers
+__irq_invalid:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate space on stack for frame
+		stmfd	sp, {r0 - lr}			@ Save r0 - lr
+		ldr	r4, .LCirq
+		mov	r1, #BAD_IRQ
+		b	1f
 
-__pabt_invalid:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
-		stmia	sp, {r0 - lr}			@ Save XXX r0 - lr
-		mov	r7, r0				@ OLD R0
-		ldr	r4, .LCabt
-		ldmia	r4, {r5 - r7}			@ Get XXX pc, cpsr
+__und_invalid:	sub	sp, sp, #S_FRAME_SIZE
+		stmia	sp, {r0 - lr}
+		ldr	r4, .LCund
+		mov	r1, #BAD_UNDEFINSTR		@ int reason
+
+1:		mov	fp, #0
+		ldmia	r4, {r5 - r7}			@ Get XXX pc, cpsr, old_r0
 		add	r4, sp, #S_PC
 		stmia	r4, {r5 - r7}			@ Save XXX pc, cpsr, old_r0
-		mov	r0, sp				@ Prefetch aborts are definitely *not*
-		mov	r1, #BAD_PREFETCH		@ allowed in non-user modes.  We cant
-		and	r2, r6, #31			@ recover from this problem.
+		mov	r0, sp
+		and	r2, r6, #31			@ int mode
 		b	SYMBOL_NAME(bad_mode)
 
-#ifdef DEBUG_UNDEF
-t:		.ascii "*** undef ***\r\n\0"
-		.align
-#endif
 
-/*=============================================================================
- * Data abort handler code
- *-----------------------------------------------------------------------------
- */
-.LCprocfns:	.word	SYMBOL_NAME(processor)
+wfs_mask_data:	.word	0x0e200110			@ WFS/RFS
+		.word	0x0fef0fff
+		.word	0x0d0d0100			@ LDF [sp]/STF [sp]
+		.word	0x0d0b0100			@ LDF [fp]/STF [fp]
+		.word	0x0f0f0f00
 
-__dabt_usr:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
-		stmia	sp, {r0 - r12}			@ save r0 - r12
-		add	r3, sp, #S_PC
-		stmdb	r3, {sp, lr}^
-		ldr	r0, .LCabt
-		ldmia	r0, {r0 - r2}			@ Get USR pc, cpsr
-		stmia	r3, {r0 - r2}			@ Save USR pc, cpsr, old_r0
-		mov	fp, #0
-		mrs	r2, cpsr			@ Enable interrupts if they were
-		bic	r2, r2, #I_BIT			@ previously
-		msr	cpsr, r2
-		ldr	r2, .LCprocfns
-		mov	lr, pc
-		ldr	pc, [r2, #8]			@ call processor specific code
-		mov	r3, sp
-		bl	SYMBOL_NAME(do_DataAbort)
-		b	ret_from_sys_call
+/* 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
+ */
+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?
+		moveq	pc, r9
+		and	r5, r10, r8
+		teq	r5, r6				@ Is it LDF/STF on sp or fp?
+		teqne	r5, r7
+		movne	pc, lr
+		tst	r10, #0x00200000		@ Does it have WB
+		moveq	pc, r9
+		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
+		mov	pc, r9
 
+/*
+ * SVC mode handlers
+ */
+		.align	5
 __dabt_svc:	sub	sp, sp, #S_FRAME_SIZE
 		stmia	sp, {r0 - r12}			@ save r0 - r12
 		ldr	r2, .LCabt
 		add	r0, sp, #S_FRAME_SIZE
+		ldmia	r2, {r2 - r4}			@ get pc, cpsr
 		add	r5, sp, #S_SP
 		mov	r1, lr
-		ldmia	r2, {r2 - r4}			@ get pc, cpsr
 		stmia	r5, {r0 - r4}			@ save sp_SVC, lr_SVC, pc, cpsr, old_ro
 		tst	r3, #I_BIT
 		mrseq	r0, cpsr			@ Enable interrupts if they were
@@ -619,29 +458,15 @@ __dabt_svc:	sub	sp, sp, #S_FRAME_SIZE
 		msr	spsr, r0
 		ldmia	sp, {r0 - pc}^			@ load r0 - pc, cpsr
 
-__dabt_invalid:	sub	sp, sp, #S_FRAME_SIZE
-		stmia	sp, {r0 - lr}			@ Save SVC r0 - lr [lr *should* be intact]
-		mov	r7, r0
-		ldr	r4, .LCabt
-		ldmia	r4, {r5, r6}			@ Get SVC pc, cpsr
-		add	r4, sp, #S_PC
-		stmia	r4, {r5, r6, r7}		@ Save SVC pc, cpsr, old_r0
-		mov	r0, sp
-		mov	r1, #BAD_DATA
-		and	r2, r6, #31
-		b	SYMBOL_NAME(bad_mode)
-
-/*=============================================================================
- * Interrupt (IRQ) handler
- *-----------------------------------------------------------------------------
- */
-__irq_usr:	sub	sp, sp, #S_FRAME_SIZE
+		.align	5
+__irq_svc:	sub	sp, sp, #S_FRAME_SIZE
 		stmia	sp, {r0 - r12}			@ save r0 - r12
-		add	r8, sp, #S_PC
-		stmdb	r8, {sp, lr}^
-		ldr	r4, .LCirq
-		ldmia	r4, {r5 - r7}			@ get saved PC, SPSR
-		stmia	r8, {r5 - r7}			@ save pc, psr, old_r0
+		ldr	r7, .LCirq
+		add	r5, sp, #S_FRAME_SIZE
+		ldmia	r7, {r7 - r9}
+		add	r4, sp, #S_SP
+		mov	r6, lr
+		stmia	r4, {r5, r6, r7, r8, r9}	@ save sp_SVC, lr_SVC, pc, cpsr, old_ro
 1:		get_irqnr_and_base r0, r6, r5
 		movne	r1, sp
 		@
@@ -649,148 +474,414 @@ __irq_usr:	sub	sp, sp, #S_FRAME_SIZE
 		@
 		adrsvc	ne, lr, 1b
 		bne	do_IRQ
-		b	ret_with_reschedule
-
-		irq_prio_table
+		ldr	r0, [sp, #S_PSR]
+		msr	spsr, r0
+		ldmia	sp, {r0 - pc}^			@ load r0 - pc, cpsr
 
-__irq_svc:	sub	sp, sp, #S_FRAME_SIZE
+		.align	5
+__und_svc:	sub	sp, sp, #S_FRAME_SIZE
 		stmia	sp, {r0 - r12}			@ save r0 - r12
+		ldr	r7, .LCund
 		mov	r6, lr
-		ldr	r7, .LCirq
 		ldmia	r7, {r7 - r9}
 		add	r5, sp, #S_FRAME_SIZE
 		add	r4, sp, #S_SP
-		stmia	r4, {r5, r6, r7, r8, r9}	@ save sp_SVC, lr_SVC, pc, cpsr, old_ro
+		stmia	r4, {r5 - r9}			@ save sp_SVC, lr_SVC, pc, cpsr, old_ro
+
+		adrsvc	al, r9, 1f			@ r9  = normal FP return
+		bl	call_fpe			@ lr  = undefined instr return
+
+		mov	r0, r5				@ unsigned long pc
+		mov	r1, sp				@ struct pt_regs *regs
+		bl	SYMBOL_NAME(do_undefinstr)
+
+1:		ldr	lr, [sp, #S_PSR]		@ Get SVC cpsr
+		msr	spsr, lr
+		ldmia	sp, {r0 - pc}^			@ Restore SVC registers
+
+		.align	5
+.LCirq:		.word	__temp_irq
+.LCund:		.word	__temp_und
+.LCabt:		.word	__temp_abt
+.LCprocfns:	.word	SYMBOL_NAME(processor)
+.LCfp:		.word	SYMBOL_NAME(fp_enter)
+#ifdef CONFIG_ALIGNMENT_TRAP
+.LCswi:		.word	SYMBOL_NAME(cr_alignment)
+#endif
+
+		irq_prio_table
+
+/*
+ * User mode handlers
+ */
+#ifdef DEBUG_UNDEF
+t:		.ascii "Prefetch -> undefined instruction\n\0"
+		.align
+#endif
+		.align	5
+__dabt_usr:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
+		stmia	sp, {r0 - r12}			@ save r0 - r12
+		ldr	r4, .LCabt
+		add	r3, sp, #S_PC
+		ldmia	r4, {r0 - r2}			@ Get USR pc, cpsr
+		stmia	r3, {r0 - r2}			@ Save USR pc, cpsr, old_r0
+		stmdb	r3, {sp, lr}^
+
+#ifdef CONFIG_ALIGNMENT_TRAP
+		ldr	r7, [r4, #OFF_CR_ALIGNMENT(__temp_abt)]
+		mcr	p15, 0, r7, c1, c0
+#endif
+
+		mov	fp, #0
+		mrs	r2, cpsr			@ Enable interrupts if they were
+		bic	r2, r2, #I_BIT			@ previously
+		msr	cpsr, r2
+		ldr	r2, .LCprocfns
+		mov	lr, pc
+		ldr	pc, [r2, #8]			@ call processor specific code
+		mov	r3, sp
+		adrsvc	al, lr, ret_from_sys_call
+		b	SYMBOL_NAME(do_DataAbort)
+
+		.align	5
+__irq_usr:	sub	sp, sp, #S_FRAME_SIZE
+		stmia	sp, {r0 - r12}			@ save r0 - r12
+		ldr	r4, .LCirq
+		add	r8, sp, #S_PC
+		ldmia	r4, {r5 - r7}			@ get saved PC, SPSR
+		stmia	r8, {r5 - r7}			@ save pc, psr, old_r0
+		stmdb	r8, {sp, lr}^
+
+#ifdef CONFIG_ALIGNMENT_TRAP
+		ldr	r7, [r4, #OFF_CR_ALIGNMENT(__temp_irq)]
+		mcr	p15, 0, r7, c1, c0
+#endif
+
+		mov	fp, #0
 1:		get_irqnr_and_base r0, r6, r5
 		movne	r1, sp
+		adrsvc	ne, lr, 1b
 		@
 		@ routine called with r0 = irq number, r1 = struct pt_regs *
 		@
-		adrsvc	ne, lr, 1b
 		bne	do_IRQ
-		ldr	r0, [sp, #S_PSR]
-		msr	spsr, r0
-		ldmia	sp, {r0 - pc}^			@ load r0 - pc, cpsr
-
-__irq_invalid:	sub	sp, sp, #S_FRAME_SIZE	@ Allocate space on stack for frame
-		stmfd	sp, {r0 - lr}		@ Save r0 - lr
-		mov	r7, #-1
-		ldr	r4, .LCirq
-		ldmia	r4, {r5, r6}		@ get saved pc, psr
-		add	r4, sp, #S_PC
-		stmia	r4, {r5, r6, r7}
-		mov	fp, #0
-		mov	r0, sp
-		mov	r1, #BAD_IRQ
-		b	SYMBOL_NAME(bad_mode)
-
-/*=============================================================================
- * Undefined instruction handler
- *-----------------------------------------------------------------------------
- * Handles floating point instructions
- */
-.LC2:		.word	SYMBOL_NAME(fp_enter)
+		mov	r4, #0
+		b	ret_with_reschedule
 
+		.align	5
 __und_usr:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
 		stmia	sp, {r0 - r12}			@ Save r0 - r12
-		add	r8, sp, #S_PC
-		stmdb	r8, {sp, lr}^			@ Save user r0 - r12
 		ldr	r4, .LCund
+		add	r8, sp, #S_PC
 		ldmia	r4, {r5 - r7}
 		stmia	r8, {r5 - r7}			@ Save USR pc, cpsr, old_r0
-		mov	fp, #0
+		stmdb	r8, {sp, lr}^			@ Save user r0 - r12
 
-		adrsvc	al, r9, ret_from_exception	@ r9  = normal FP return
+#ifdef CONFIG_ALIGNMENT_TRAP
+		ldr	r7, [r4, #OFF_CR_ALIGNMENT(__temp_und)]
+		mcr	p15, 0, r7, c1, c0
+#endif
+
+		mov	fp, #0
+		adrsvc	al, r9, ret_from_sys_call	@ r9  = normal FP return
 		adrsvc	al, lr, fpundefinstr		@ lr  = undefined instr return
 
-1:		get_current_task r10
+call_fpe:	get_current_task r10
 		mov	r8, #1
 		strb	r8, [r10, #TSK_USED_MATH]	@ set current->used_math
+		ldr	r4, .LCfp
 		add	r10, r10, #TSS_FPESAVE		@ r10 = workspace
-		ldr	r4, .LC2
 		ldr	pc, [r4]			@ Call FP module USR entry point
 
-__und_svc:	sub	sp, sp, #S_FRAME_SIZE
-		stmia	sp, {r0 - r12}			@ save r0 - r12
-		mov	r6, lr
-		ldr	r7, .LCund
-		ldmia	r7, {r7 - r9}
-		add	r5, sp, #S_FRAME_SIZE
-		add	r4, sp, #S_SP
-		stmia	r4, {r5 - r9}			@ save sp_SVC, lr_SVC, pc, cpsr, old_ro
-
-		adrsvc	al, r9, 3f			@ r9  = normal FP return
-		bl	1b				@ lr  = undefined instr return
-
-		mov	r0, r5				@ unsigned long pc
-		mov	r1, sp				@ struct pt_regs *regs
-		bl	SYMBOL_NAME(do_undefinstr)
-
-3:		ldr	lr, [sp, #S_PSR]		@ Get SVC cpsr
-		msr	spsr, lr
-		ldmia	sp, {r0 - pc}^			@ Restore SVC registers
-
 fpundefinstr:	mov	r0, lr
 		mov	r1, sp
 		mrs	r4, cpsr			@ Enable interrupts
 		bic	r4, r4, #I_BIT
 		msr	cpsr, r4
-		adrsvc	al, lr, ret_from_exception
+		adrsvc	al, lr, ret_from_sys_call
 		b	SYMBOL_NAME(do_undefinstr)
 
-__und_invalid:	sub	sp, sp, #S_FRAME_SIZE
-		stmia	sp, {r0 - lr}
-		mov	r7, r0
-		ldr	r4, .LCund
-		ldmia	r4, {r5, r6}			@ Get UND/IRQ/FIQ/ABT pc, cpsr
-		add	r4, sp, #S_PC
-		stmia	r4, {r5, r6, r7}		@ Save UND/IRQ/FIQ/ABT pc, cpsr, old_r0
-		mov	r0, sp				@ struct pt_regs *regs
-		mov	r1, #BAD_UNDEFINSTR		@ int reason
-		and	r2, r6, #31			@ int mode
-		b	SYMBOL_NAME(bad_mode)		@ Does not ever return...
+		.align	5
+__pabt_usr:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
+		stmia	sp, {r0 - r12}			@ Save r0 - r12
+		ldr	r4, .LCabt
+		add	r8, sp, #S_PC
+		ldmia	r4, {r5 - r7}			@ Get USR pc, cpsr
+		stmia	r8, {r5 - r7}			@ Save USR pc, cpsr, old_r0
+		stmdb	r8, {sp, lr}^			@ Save sp_usr lr_usr
 
-/* 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
- */
-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?
-		moveq	pc, r9
-		and	r5, r10, r8
-		teq	r5, r6				@ Is it LDF/STF on sp or fp?
-		teqne	r5, r7
-		movne	pc, lr
-		tst	r10, #0x00200000		@ Does it have WB
-		moveq	pc, r9
-		and	r4, r10, #255			@ get offset
-		and	r6, r10, #0x000f0000
-		tst	r10, #0x00800000		@ +/-
-		rsbeq	r4, r4, #0
-		ldr	r5, [sp, r6, lsr #14]		@ Load reg
-		add	r5, r5, r4, lsl #2
-		str	r5, [sp, r6, lsr #14]		@ Save reg
-		mov	pc, r9
+#ifdef CONFIG_ALIGNMENT_TRAP
+		ldr	r7, [r4, #OFF_CR_ALIGNMENT(__temp_abt)]
+		mcr	p15, 0, r7, c1, c0
+#endif
 
-wfs_mask_data:	.word	0x0e200110			@ WFS
-		.word	0x0fff0fff
-		.word	0x0d0d0100			@ LDF [sp]/STF [sp]
-		.word	0x0d0b0100			@ LDF [fp]/STF [fp]
-		.word	0x0f0f0f00
+		mov	fp, #0
+		mrs	r7, cpsr			@ Enable interrupts if they were
+		bic	r7, r7, #I_BIT			@ previously
+		msr	cpsr, r7
+		mov	r0, r5				@ address (pc)
+		mov	r1, sp				@ regs
+		bl	SYMBOL_NAME(do_PrefetchAbort)	@ call abort handler
+		teq	r0, #0				@ Does this still apply???
+		bne	ret_from_sys_call		@ Return from exception
+#ifdef DEBUG_UNDEF
+		adr	r0, t
+		bl	SYMBOL_NAME(printk)
+#endif
+		mov	r0, r5
+		mov	r1, sp
+		and	r2, r6, #31
+		bl	SYMBOL_NAME(do_undefinstr)
+		ldr	lr, [sp, #S_PSR]		@ Get USR cpsr
+		msr	spsr, lr
+		ldmia	sp, {r0 - pc}^			@ Restore USR registers
 
 #include "entry-common.S"
 
+		.text
+
+#ifndef __ARM_ARCH_4__
+.Larm700bug:	ldr	r0, [sp, #S_PSR]		@ Get calling cpsr
+		str	lr, [r8]
+		msr	spsr, r0
+		ldmia	sp, {r0 - lr}^			@ Get calling r0 - lr
+		mov	r0, r0
+		ldr	lr, [sp, #S_PC]			@ Get PC
+		add	sp, sp, #S_FRAME_SIZE
+		movs	pc, lr
+#endif
+
+		.section ".text.init",#alloc,#execinstr
+/*
+ * Vector stubs.  NOTE that we only align 'vector_IRQ' to a cache line boundary,
+ * and we rely on each stub being exactly 48 (1.5 cache lines) in size.  This
+ * means that we only ever load two cache lines for this code, or one if we're
+ * lucky.  We also copy this code to 0x200 so that we can use branches in the
+ * vectors, rather than ldr's.
+ */
+		.align	5
+__stubs_start:
+/*
+ * Interrupt dispatcher
+ * Enter in IRQ mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
+ */
+vector_IRQ:	@
+		@ save mode specific registers
+		@
+		ldr	r13, .LCsirq
+		sub	lr, lr, #4
+		str	lr, [r13]			@ save lr_IRQ
+		mrs	lr, spsr
+		str	lr, [r13, #4]			@ save spsr_IRQ
+		@
+		@ now branch to the relevent MODE handling routine
+		@
+		bic	r13, lr, #63
+		orr	r13, r13, #0x93
+		msr	spsr, r13			@ switch to SVC_32 mode
+
+		and	lr, lr, #15
+		adr	r13, .LCtab_irq
+		ldr	lr, [r13, lr, lsl #2]
+		movs	pc, lr				@ Changes mode and branches
+/*
+ * Data abort dispatcher - dispatches it to the correct handler for the processor mode
+ * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
+ */
+vector_data:	@
+		@ save mode specific registers
+		@
+		ldr	r13, .LCsabt
+		sub	lr, lr, #8
+		str	lr, [r13]
+		mrs	lr, spsr
+		str	lr, [r13, #4]
+		@
+		@ now branch to the relevent MODE handling routine
+		@
+		bic	r13, lr, #63
+		orr	r13, r13, #0x93
+		msr	spsr, r13			@ switch to SVC_32 mode
+
+		and	lr, lr, #15
+		adr	r13, .LCtab_dabt
+		ldr	lr, [r13, lr, lsl #2]
+		movs	pc, lr				@ Changes mode and branches
+
+/*
+ * Prefetch abort dispatcher - dispatches it to the correct handler for the processor mode
+ * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
+ */
+vector_prefetch:
+		@
+		@ save mode specific registers
+		@
+		ldr	r13, .LCsabt
+		sub	lr, lr, #4
+		str	lr, [r13]			@ save lr_ABT
+		mrs	lr, spsr
+		str	lr, [r13, #4]			@ save spsr_ABT
+		@
+		@ now branch to the relevent MODE handling routine
+		@
+		bic	r13, lr, #63
+		orr	r13, r13, #0x93
+		msr	spsr, r13			@ switch to SVC_32 mode
+
+		ands	lr, lr, #15
+		ldreq	lr, .LCtab_pabt
+		ldrne	lr, .LCtab_pabt + 4
+		movs	pc, lr
+
+/*
+ * Undef instr entry dispatcher - dispatches it to the correct handler for the processor mode
+ * Enter in UND mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
+ */
+vector_undefinstr:
+		@
+		@ save mode specific registers
+		@
+		ldr	r13, .LCsund
+		str	lr, [r13]			@ save lr_UND
+		mrs	lr, spsr
+		str	lr, [r13, #4]			@ save spsr_UND
+		@
+		@ now branch to the relevent MODE handling routine
+		@
+		bic	r13, lr, #63
+		orr	r13, r13, #0x93
+		msr	spsr, r13			@ switch to SVC_32 mode
+
+		and	lr, lr, #15
+		adr	r13, .LCtab_und
+		ldr	lr, [r13, lr, lsl #2]
+		movs	pc, lr				@ Changes mode and branches
+
+/*=============================================================================
+ * Undefined FIQs
+ *-----------------------------------------------------------------------------
+ * Enter in FIQ mode, spsr = ANY CPSR, lr = ANY PC
+ * MUST PRESERVE SVC SPSR, but need to switch to SVC mode to show our msg.
+ * Basically to switch modes, we *HAVE* to clobber one register...  brain
+ * damage alert!  I don't think that we can execute any code in here in any
+ * other mode than FIQ...  Ok you can switch to another mode, but you can't
+ * get out of that mode without clobbering one register.
+ */
+vector_FIQ:	disable_fiq
+		subs	pc, lr, #4
+
+/*=============================================================================
+ * Address exception handler
+ *-----------------------------------------------------------------------------
+ * These aren't too critical.
+ * (they're not supposed to happen, and won't happen in 32-bit data mode).
+ */
+
+vector_addrexcptn:
+		b	vector_addrexcptn
+
+/*
+ * We group all the following data together to optimise
+ * for CPUs with separate I & D caches.
+ */
+		.align	5
+
+.LCtab_irq:	.word	__irq_usr			@  0  (USR_26 / USR_32)
+		.word	__irq_invalid			@  1  (FIQ_26 / FIQ_32)
+		.word	__irq_invalid			@  2  (IRQ_26 / IRQ_32)
+		.word	__irq_svc			@  3  (SVC_26 / SVC_32)
+		.word	__irq_invalid			@  4
+		.word	__irq_invalid			@  5
+		.word	__irq_invalid			@  6
+		.word	__irq_invalid			@  7
+		.word	__irq_invalid			@  8
+		.word	__irq_invalid			@  9
+		.word	__irq_invalid			@  a
+		.word	__irq_invalid			@  b
+		.word	__irq_invalid			@  c
+		.word	__irq_invalid			@  d
+		.word	__irq_invalid			@  e
+		.word	__irq_invalid			@  f
+
+.LCtab_und:	.word	__und_usr			@  0 (USR_26 / USR_32)
+		.word	__und_invalid			@  1 (FIQ_26 / FIQ_32)
+		.word	__und_invalid			@  2 (IRQ_26 / IRQ_32)
+		.word	__und_svc			@  3 (SVC_26 / SVC_32)
+		.word	__und_invalid			@  4
+		.word	__und_invalid			@  5
+		.word	__und_invalid			@  6
+		.word	__und_invalid			@  7
+		.word	__und_invalid			@  8
+		.word	__und_invalid			@  9
+		.word	__und_invalid			@  a
+		.word	__und_invalid			@  b
+		.word	__und_invalid			@  c
+		.word	__und_invalid			@  d
+		.word	__und_invalid			@  e
+		.word	__und_invalid			@  f
+
+.LCtab_dabt:	.word	__dabt_usr			@  0  (USR_26 / USR_32)
+		.word	__dabt_invalid			@  1  (FIQ_26 / FIQ_32)
+		.word	__dabt_invalid			@  2  (IRQ_26 / IRQ_32)
+		.word	__dabt_svc			@  3  (SVC_26 / SVC_32)
+		.word	__dabt_invalid			@  4
+		.word	__dabt_invalid			@  5
+		.word	__dabt_invalid			@  6
+		.word	__dabt_invalid			@  7
+		.word	__dabt_invalid			@  8
+		.word	__dabt_invalid			@  9
+		.word	__dabt_invalid			@  a
+		.word	__dabt_invalid			@  b
+		.word	__dabt_invalid			@  c
+		.word	__dabt_invalid			@  d
+		.word	__dabt_invalid			@  e
+		.word	__dabt_invalid			@  f
+
+.LCtab_pabt:	.word	__pabt_usr
+		.word	__pabt_invalid
+
+.LCvswi:	.word	vector_swi
+
+.LCsirq:	.word	__temp_irq
+.LCsund:	.word	__temp_und
+.LCsabt:	.word	__temp_abt
+
+__stubs_end:
+
+		.equ	__real_stubs_start, .LCvectors + 0x200
+
+.LCvectors:	swi	SYS_ERROR0
+		b	__real_stubs_start + (vector_undefinstr - __stubs_start)
+		ldr	pc, __real_stubs_start + (.LCvswi - __stubs_start)
+		b	__real_stubs_start + (vector_prefetch - __stubs_start)
+		b	__real_stubs_start + (vector_data - __stubs_start)
+		b	__real_stubs_start + (vector_addrexcptn - __stubs_start)
+		b	__real_stubs_start + (vector_IRQ - __stubs_start)
+		b	__real_stubs_start + (vector_FIQ - __stubs_start)
+
+ENTRY(trap_init)
+		stmfd	sp!, {r4 - r6, lr}
+
+		adr	r1, .LCvectors			@ set up the vectors
+		mov	r0, #0
+		ldmia	r1, {r1, r2, r3, r4, r5, r6, ip, lr}
+		stmia	r0, {r1, r2, r3, r4, r5, r6, ip, lr}
+
+		add	r2, r0, #0x200
+		adr	r0, __stubs_start		@ copy stubs to 0x200
+		adr	r1, __stubs_end
+1:		ldr	r3, [r0], #4
+		str	r3, [r2], #4
+		cmp	r0, r1
+		blt	1b
+		LOADREGS(fd, sp!, {r4 - r6, pc})
+
 		.data
 
+/*
+ * Do not reorder these, and do not insert extra data between...
+ */
+
 __temp_irq:	.word	0				@ saved lr_irq
 		.word	0				@ saved spsr_irq
 		.word	-1				@ old_r0
@@ -800,3 +891,10 @@ __temp_und:	.word	0				@ Saved lr_und
 __temp_abt:	.word	0				@ Saved lr_abt
 		.word	0				@ Saved spsr_abt
 		.word	-1				@ old_r0
+
+		.globl	SYMBOL_NAME(cr_alignment)
+		.globl	SYMBOL_NAME(cr_no_alignment)
+SYMBOL_NAME(cr_alignment):
+		.space	4
+SYMBOL_NAME(cr_no_alignment):
+		.space	4