[ARM PATCH] 1866/4: kernel support for iWMMXt present on some XScale cores

Patch from Nicolas Pitre

This is required for a system with user space binaries using iWMMXt 
instructions to even boot (revised again).

It also moves defines for thread_info offsets from  magic static
values to the auto generated asm/constants.h in order to better
cope with changing structure offsets and avoid user errors.

arch/arm/kernel/Makefile

@@ -19,6 +19,9 @@ obj-$(CONFIG_ARTHUR)		+= arthur.o
 obj-$(CONFIG_ISA_DMA)		+= dma-isa.o
 obj-$(CONFIG_PCI)		+= bios32.o
 
+obj-$(CONFIG_IWMMXT)		+= iwmmxt.o
+AFLAGS_iwmmxt.o			:= -Wa,-mcpu=iwmmxt
+
 ifneq ($(CONFIG_ARCH_EBSA110),y)
   obj-y		+= io.o
 endif

arch/arm/kernel/asm-offsets.c

@@ -13,6 +13,7 @@
 #include <linux/sched.h>
 #include <linux/mm.h>
 #include <asm/mach/arch.h>
+#include <asm/thread_info.h>
 
 /*
  * Make sure that the compiler and target are compatible.
@@ -48,10 +49,23 @@ int main(void)
 {
   DEFINE(TSK_ACTIVE_MM,		offsetof(struct task_struct, active_mm));
   BLANK();
+  DEFINE(TI_FLAGS,		offsetof(struct thread_info, flags));
+  DEFINE(TI_PREEMPT,		offsetof(struct thread_info, preempt_count));
+  DEFINE(TI_ADDR_LIMIT,		offsetof(struct thread_info, addr_limit));
+  DEFINE(TI_TASK,		offsetof(struct thread_info, task));
+  DEFINE(TI_EXEC_DOMAIN,	offsetof(struct thread_info, exec_domain));
+  DEFINE(TI_CPU,		offsetof(struct thread_info, cpu));
+  DEFINE(TI_CPU_DOMAIN,		offsetof(struct thread_info, cpu_domain));
+  DEFINE(TI_CPU_SAVE,		offsetof(struct thread_info, cpu_context));
+  DEFINE(TI_USED_CP,		offsetof(struct thread_info, used_cp));
+  DEFINE(TI_FPSTATE,		offsetof(struct thread_info, fpstate));
+  DEFINE(TI_VFPSTATE,		offsetof(struct thread_info, vfpstate));
+  DEFINE(TI_IWMMXT_STATE,	(offsetof(struct thread_info, fpstate)+4)&~7);
+  BLANK();
 #if __LINUX_ARM_ARCH__ >= 6
   DEFINE(MM_CONTEXT_ID,		offsetof(struct mm_struct, context.id));
-#endif
   BLANK();
+#endif
   DEFINE(VMA_VM_MM,		offsetof(struct vm_area_struct, vm_mm));
   DEFINE(VMA_VM_FLAGS,		offsetof(struct vm_area_struct, vm_flags));
   BLANK();

arch/arm/kernel/entry-armv.S

@@ -1174,7 +1174,7 @@ __und_usr:	sub	sp, sp, #S_FRAME_SIZE		@ Allocate frame size in one go
  *  r0  - instruction opcode.
  *  r10 - this threads thread_info structure.
  */
-call_fpe:	enable_irq r10				@ Enable interrupts
+call_fpe:
 		tst	r0, #0x08000000			@ only CDP/CPRT/LDC/STC have bit 27
 #if defined(CONFIG_CPU_ARM610) || defined(CONFIG_CPU_ARM710)
 		and	r8, r0, #0x0f000000		@ mask out op-code bits
@@ -1186,6 +1186,14 @@ call_fpe:	enable_irq r10				@ Enable interrupts
 		mov	r7, #1
 		add	r6, r10, #TI_USED_CP
 		strb	r7, [r6, r8, lsr #8]		@ set appropriate used_cp[]
+#ifdef CONFIG_IWMMXT
+		@ Test if we need to give access to iWMMXt coprocessors
+		ldr	r5, [r10, #TI_FLAGS]
+		rsbs	r7, r8, #(1 << 8)		@ CP 0 or 1 only
+		movcss	r7, r5, lsr #(TIF_USING_IWMMXT + 1)
+		bcs	iwmmxt_task_enable
+#endif
+		enable_irq r7
 		add	pc, pc, r8, lsr #6
 		mov	r0, r0
 
@@ -1264,7 +1272,11 @@ ENTRY(ret_from_exception)
 ENTRY(__switch_to)
 		add	ip, r1, #TI_CPU_SAVE
 		ldr	r3, [r2, #TI_CPU_DOMAIN]!
-		stmia	ip, {r4 - sl, fp, sp, lr}	@ Store most regs on stack
+		stmia	ip!, {r4 - sl, fp, sp, lr}	@ Store most regs on stack
+#if defined(CONFIG_CPU_XSCALE) && !defined(CONFIG_IWMMXT)
+		mra	r4, r5, acc0
+		stmia   ip, {r4, r5}
+#endif
 		mcr	p15, 0, r3, c3, c0, 0		@ Set domain register
 #ifdef CONFIG_VFP
 		@ Always disable VFP so we can lazily save/restore the old
@@ -1272,6 +1284,13 @@ ENTRY(__switch_to)
 		VFPFMRX	r4, FPEXC
 		bic	r4, r4, #FPEXC_ENABLE
 		VFPFMXR	FPEXC, r4
+#endif
+#if defined(CONFIG_IWMMXT)
+		bl	iwmmxt_task_switch
+#elif defined(CONFIG_CPU_XSCALE)
+		add	r4, r2, #40			@ cpu_context_save->extra
+		ldmib	r4, {r4, r5}
+		mar	acc0, r4, r5
 #endif
 		ldmib	r2, {r4 - sl, fp, sp, pc}	@ Load all regs saved previously
 

arch/arm/kernel/iwmmxt.S

+/*
+ *  linux/arch/arm/kernel/iwmmxt.S
+ *
+ *  XScale iWMMXt (Concan) context switching and handling
+ *
+ *  Initial code:
+ *  Copyright (c) 2003, Intel Corporation
+ *
+ *  Full lazy switching support, optimizations and more, by Nicolas Pitre
+*   Copyright (c) 2003-2004, MontaVista Software, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/linkage.h>
+#include <asm/ptrace.h>
+#include <asm/thread_info.h>
+#include <asm/constants.h>
+
+#define MMX_WR0		 	(0x00)
+#define MMX_WR1		 	(0x08)
+#define MMX_WR2		 	(0x10)
+#define MMX_WR3			(0x18)
+#define MMX_WR4		 	(0x20)
+#define MMX_WR5		 	(0x28)
+#define MMX_WR6		 	(0x30)
+#define MMX_WR7		 	(0x38)
+#define MMX_WR8		 	(0x40)
+#define MMX_WR9		 	(0x48)
+#define MMX_WR10		(0x50)
+#define MMX_WR11		(0x58)
+#define MMX_WR12		(0x60)
+#define MMX_WR13		(0x68)
+#define MMX_WR14		(0x70)
+#define MMX_WR15		(0x78)
+#define MMX_WCSSF		(0x80)
+#define MMX_WCASF		(0x84)
+#define MMX_WCGR0		(0x88)
+#define MMX_WCGR1		(0x8C)
+#define MMX_WCGR2		(0x90)
+#define MMX_WCGR3		(0x94)
+
+#define MMX_SIZE		(0x98)
+
+	.text
+
+/*
+ * Lazy switching of Concan coprocessor context
+ *
+ * r10 = struct thread_info pointer
+ * r9  = ret_from_exception
+ * lr  = undefined instr exit
+ *
+ * called from prefetch exception handler with interrupts disabled
+ */
+
+ENTRY(iwmmxt_task_enable)
+
+	mrc	p15, 0, r2, c15, c1, 0
+	tst	r2, #0x3			@ CP0 and CP1 accessible?
+	movne	pc, lr				@ if so no business here
+	orr	r2, r2, #0x3			@ enable access to CP0 and CP1
+	mcr	p15, 0, r2, c15, c1, 0
+
+	ldr	r3, =concan_owner
+	add	r0, r10, #TI_IWMMXT_STATE	@ get task Concan save area
+	ldr	r2, [sp, #60]			@ current task pc value
+	ldr	r1, [r3]			@ get current Concan owner
+	str	r0, [r3]			@ this task now owns Concan regs
+	sub	r2, r2, #4			@ adjust pc back
+	str	r2, [sp, #60]
+
+	mrc	p15, 0, r2, c2, c0, 0
+	mov	r2, r2				@ cpwait
+
+	teq	r1, #0				@ test for last ownership
+	mov	lr, r9				@ normal exit from exception
+	beq	concan_load			@ no owner, skip save
+
+concan_save:
+
+	tmrc	r2, wCon
+
+	@ CUP? wCx
+	tst	r2, #0x1
+	beq 	1f
+
+concan_dump:
+
+	wstrw	wCSSF, [r1, #MMX_WCSSF]
+	wstrw	wCASF, [r1, #MMX_WCASF]
+	wstrw	wCGR0, [r1, #MMX_WCGR0]
+	wstrw	wCGR1, [r1, #MMX_WCGR1]
+	wstrw	wCGR2, [r1, #MMX_WCGR2]
+	wstrw	wCGR3, [r1, #MMX_WCGR3]
+
+1:	@ MUP? wRn
+	tst	r2, #0x2
+	beq	2f
+
+	wstrd	wR0,  [r1, #MMX_WR0]
+	wstrd	wR1,  [r1, #MMX_WR1]
+	wstrd	wR2,  [r1, #MMX_WR2]
+	wstrd	wR3,  [r1, #MMX_WR3]
+	wstrd	wR4,  [r1, #MMX_WR4]
+	wstrd	wR5,  [r1, #MMX_WR5]
+	wstrd	wR6,  [r1, #MMX_WR6]
+	wstrd	wR7,  [r1, #MMX_WR7]
+	wstrd	wR8,  [r1, #MMX_WR8]
+	wstrd	wR9,  [r1, #MMX_WR9]
+	wstrd	wR10, [r1, #MMX_WR10]
+	wstrd	wR11, [r1, #MMX_WR11]
+	wstrd	wR12, [r1, #MMX_WR12]
+	wstrd	wR13, [r1, #MMX_WR13]
+	wstrd	wR14, [r1, #MMX_WR14]
+	wstrd	wR15, [r1, #MMX_WR15]
+
+2:	teq	r0, #0				@ anything to load?
+	moveq	pc, lr
+
+concan_load:
+
+	@ Load wRn
+	wldrd	wR0,  [r0, #MMX_WR0]
+	wldrd	wR1,  [r0, #MMX_WR1]
+	wldrd	wR2,  [r0, #MMX_WR2]
+	wldrd	wR3,  [r0, #MMX_WR3]
+	wldrd	wR4,  [r0, #MMX_WR4]
+	wldrd	wR5,  [r0, #MMX_WR5]
+	wldrd	wR6,  [r0, #MMX_WR6]
+	wldrd	wR7,  [r0, #MMX_WR7]
+	wldrd	wR8,  [r0, #MMX_WR8]
+	wldrd	wR9,  [r0, #MMX_WR9]
+	wldrd	wR10, [r0, #MMX_WR10]
+	wldrd	wR11, [r0, #MMX_WR11]
+	wldrd	wR12, [r0, #MMX_WR12]
+	wldrd	wR13, [r0, #MMX_WR13]
+	wldrd	wR14, [r0, #MMX_WR14]
+	wldrd	wR15, [r0, #MMX_WR15]
+
+	@ Load wCx
+	wldrw	wCSSF, [r0, #MMX_WCSSF]
+	wldrw	wCASF, [r0, #MMX_WCASF]
+	wldrw	wCGR0, [r0, #MMX_WCGR0]
+	wldrw	wCGR1, [r0, #MMX_WCGR1]
+	wldrw	wCGR2, [r0, #MMX_WCGR2]
+	wldrw	wCGR3, [r0, #MMX_WCGR3]
+
+	@ clear CUP/MUP (only if r1 != 0)
+	teq	r1, #0
+	mov 	r2, #0
+	moveq	pc, lr
+	tmcr	wCon, r2
+	mov	pc, lr
+
+/*
+ * Back up Concan regs to save area and disable access to them
+ * (mainly for gdb or sleep mode usage)
+ *
+ * r0 = struct thread_info pointer of target task or NULL for any
+ */
+
+ENTRY(iwmmxt_task_disable)
+
+	stmfd	sp!, {r4, lr}
+
+	mrs	ip, cpsr
+	orr	r2, ip, #PSR_I_BIT		@ disable interrupts
+	msr	cpsr_c, r2
+
+	ldr	r3, =concan_owner
+	add	r2, r0, #TI_IWMMXT_STATE	@ get task Concan save area
+	ldr	r1, [r3]			@ get current Concan owner
+	teq	r1, #0				@ any current owner?
+	beq	1f				@ no: quit
+	teq	r0, #0				@ any owner?
+	teqne	r1, r2				@ or specified one?
+	bne	1f				@ no: quit
+
+	mrc	p15, 0, r4, c15, c1, 0
+	orr	r4, r4, #0x3			@ enable access to CP0 and CP1
+	mcr	p15, 0, r4, c15, c1, 0
+	mov	r0, #0				@ nothing to load
+	str	r0, [r3]			@ no more current owner
+	mrc	p15, 0, r2, c2, c0, 0
+	mov	r2, r2				@ cpwait
+	bl	concan_save
+
+	bic	r4, r4, #0x3			@ disable access to CP0 and CP1
+	mcr	p15, 0, r4, c15, c1, 0
+	mrc	p15, 0, r2, c2, c0, 0
+	mov	r2, r2				@ cpwait
+
+1:	msr	cpsr_c, ip			@ restore interrupt mode
+	ldmfd	sp!, {r4, pc}
+
+/*
+ * Copy Concan state to given memory address
+ *
+ * r0 = struct thread_info pointer of target task
+ * r1 = memory address where to store Concan state
+ *
+ * this is called mainly in the creation of signal stack frames
+ */
+
+ENTRY(iwmmxt_task_copy)
+
+	mrs	ip, cpsr
+	orr	r2, ip, #PSR_I_BIT		@ disable interrupts
+	msr	cpsr_c, r2
+
+	ldr	r3, =concan_owner
+	add	r2, r0, #TI_IWMMXT_STATE	@ get task Concan save area
+	ldr	r3, [r3]			@ get current Concan owner
+	teq	r2, r3				@ does this task own it...
+	beq	1f
+
+	@ current Concan values are in the task save area
+	msr	cpsr_c, ip			@ restore interrupt mode
+	mov	r0, r1
+	mov	r1, r2
+	mov	r2, #MMX_SIZE
+	b	memcpy
+
+1:	@ this task owns Concan regs -- grab a copy from there
+	mov	r0, #0				@ nothing to load
+	mov	r2, #3				@ save all regs
+	mov	r3, lr				@ preserve return address
+	bl	concan_dump
+	msr	cpsr_c, ip			@ restore interrupt mode
+	mov	pc, r3
+
+/*
+ * Restore Concan state from given memory address
+ *
+ * r0 = struct thread_info pointer of target task
+ * r1 = memory address where to get Concan state from
+ *
+ * this is used to restore Concan state when unwinding a signal stack frame
+ */
+
+ENTRY(iwmmxt_task_restore)
+
+	mrs	ip, cpsr
+	orr	r2, ip, #PSR_I_BIT		@ disable interrupts
+	msr	cpsr_c, r2
+
+	ldr	r3, =concan_owner
+	add	r2, r0, #TI_IWMMXT_STATE	@ get task Concan save area
+	ldr	r3, [r3]			@ get current Concan owner
+	bic	r2, r2, #0x7			@ 64-bit alignment
+	teq	r2, r3				@ does this task own it...
+	beq	1f
+
+	@ this task doesn't own Concan regs -- use its save area
+	msr	cpsr_c, ip			@ restore interrupt mode
+	mov	r0, r2
+	mov	r2, #MMX_SIZE
+	b	memcpy
+
+1:	@ this task owns Concan regs -- load them directly
+	mov	r0, r1
+	mov	r1, #0				@ don't clear CUP/MUP
+	mov	r3, lr				@ preserve return address
+	bl	concan_load
+	msr	cpsr_c, ip			@ restore interrupt mode
+	mov	pc, r3
+
+/*
+ * Concan handling on task switch
+ *
+ * r0 = previous task_struct pointer (must be preserved)
+ * r1 = previous thread_info pointer
+ * r2 = next thread_info.cpu_domain pointer (must be preserved)
+ *
+ * Called only from __switch_to with task preemption disabled.
+ * No need to care about preserving r4 and above.
+ */
+ENTRY(iwmmxt_task_switch)
+
+	mrc	p15, 0, r4, c15, c1, 0
+	tst	r4, #0x3			@ CP0 and CP1 accessible?
+	bne	1f				@ yes: block them for next task
+
+	ldr	r5, =concan_owner
+	add	r6, r2, #(TI_IWMMXT_STATE - TI_CPU_DOMAIN) @ get next task Concan save area
+	ldr	r5, [r5]			@ get current Concan owner
+	teq	r5, r6				@ next task owns it?
+	movne	pc, lr				@ no: leave Concan disabled
+
+1:	eor	r4, r4, #3			@ flip Concan access
+	mcr	p15, 0, r4, c15, c1, 0
+
+	mrc	p15, 0, r4, c2, c0, 0
+	sub	pc, lr, r4, lsr #32		@ cpwait and return
+
+/*
+ * Remove Concan ownership of given task
+ *
+ * r0 = struct thread_info pointer
+ */
+ENTRY(iwmmxt_task_release)
+
+	mrs	r2, cpsr
+	orr	ip, r2, #PSR_I_BIT		@ disable interrupts
+	msr	cpsr_c, ip
+	ldr	r3, =concan_owner
+	add	r0, r0, #TI_IWMMXT_STATE	@ get task Concan save area
+	ldr	r1, [r3]			@ get current Concan owner
+	eors	r0, r0, r1			@ if equal...
+	streq	r0, [r3]			@ then clear ownership
+	msr	cpsr_c, r2			@ restore interrupts
+	mov	pc, lr
+
+	.data
+concan_owner:
+	.word	0
+

arch/arm/kernel/process.c

@@ -313,6 +313,9 @@ void flush_thread(void)
 
 	memset(thread->used_cp, 0, sizeof(thread->used_cp));
 	memset(&tsk->thread.debug, 0, sizeof(struct debug_info));
+#if defined(CONFIG_IWMMXT)
+	iwmmxt_task_release(thread);
+#endif
 	fp_init(&thread->fpstate);
 #if defined(CONFIG_VFP)
 	vfp_flush_thread(&thread->vfpstate);
@@ -324,6 +327,9 @@ void release_thread(struct task_struct *dead_task)
 #if defined(CONFIG_VFP)
 	vfp_release_thread(&dead_task->thread_info->vfpstate);
 #endif
+#if defined(CONFIG_IWMMXT)
+	iwmmxt_task_release(dead_task->thread_info);
+#endif
 }
 
 asmlinkage void ret_from_fork(void) __asm__("ret_from_fork");

arch/arm/kernel/signal.c

@@ -145,6 +145,100 @@ struct rt_sigframe
 	unsigned long retcode;
 };
 
+#ifdef CONFIG_IWMMXT
+
+/* iwmmxt_area is 0x98 bytes long, preceeded by 8 bytes of signature */
+#define IWMMXT_STORAGE_SIZE	(0x98 + 8)
+#define IWMMXT_MAGIC0		0x12ef842a
+#define IWMMXT_MAGIC1		0x1c07ca71
+
+static int page_present(struct mm_struct *mm, unsigned long addr, int wr)
+{
+	pgd_t *pgd = pgd_offset(mm, addr);
+	if (pgd_present(*pgd)) {
+		pmd_t *pmd = pmd_offset(pgd, addr);
+		if (pmd_present(*pmd)) {
+			pte_t *pte = pte_offset_map(pmd, addr);
+			return (pte_present(*pte) && (!wr || pte_write(*pte)));
+		}
+	}
+	return 0;
+}
+
+static int
+preserve_iwmmxt_context(void *iwmmxt_save_area)
+{
+	int err = 0;
+
+	/* the iWMMXt context must be 64 bit aligned */
+	long *iwmmxt_storage = (long *)(((long)iwmmxt_save_area + 4) & ~7);
+
+again:
+	__put_user_error(IWMMXT_MAGIC0, iwmmxt_storage+0, err);
+	__put_user_error(IWMMXT_MAGIC1, iwmmxt_storage+1, err);
+	/*
+	 * iwmmxt_task_copy() doesn't check user permissions.
+	 * Let's do a dummy write on the upper boundary to ensure
+	 * access to user mem is OK all way up.
+	 */
+	__put_user_error(0, iwmmxt_storage+IWMMXT_STORAGE_SIZE/4-1, err);
+	if (!err) {
+		/* Let's make sure the user mapping won't disappear under us */
+		struct mm_struct *mm = current->mm;
+		unsigned long addr = (unsigned long)iwmmxt_storage;
+		spin_lock(&mm->page_table_lock);
+		if ( !page_present(mm, addr, 1) ||
+		     !page_present(mm, addr+IWMMXT_STORAGE_SIZE-1, 1) ) {
+			/* our user area has gone before grabbing the lock */
+			spin_unlock(&mm->page_table_lock);
+			goto again;
+		}
+		iwmmxt_task_copy(current_thread_info(), iwmmxt_storage+2);
+		spin_unlock(&mm->page_table_lock);
+		return 0;
+	}
+	return err;
+}
+
+static int
+restore_iwmmxt_context(void *iwmmxt_save_area)
+{
+	int err = 0;
+	long *iwmmxt_storage, magic0, magic1, dummy;
+
+	/* the iWMMXt context is 64 bit aligned */
+	iwmmxt_storage = (long *)(((long)iwmmxt_save_area + 4) & ~7);
+
+	/*
+	 * Validate iWMMXt context signature.
+	 * Also, iwmmxt_task_restore() doesn't check user permissions.
+	 * Let's do a dummy write on the upper boundary to ensure
+	 * access to user mem is OK all way up.
+	 */
+again:
+	__get_user_error(magic0, iwmmxt_storage+0, err);
+	__get_user_error(magic1, iwmmxt_storage+1, err);
+	if (!err && magic0 == IWMMXT_MAGIC0 && magic1 == IWMMXT_MAGIC1 &&
+	    !__get_user(dummy, iwmmxt_storage+IWMMXT_STORAGE_SIZE/4-1)) {
+		/* Let's make sure the user mapping won't disappear under us */
+		struct mm_struct *mm = current->mm;
+		unsigned long addr = (unsigned long)iwmmxt_storage;
+		spin_lock(&mm->page_table_lock);
+		if ( !page_present(mm, addr, 0) ||
+		     !page_present(mm, addr+IWMMXT_STORAGE_SIZE-1, 0) ) {
+			/* our user area has gone before grabbing the lock */
+			spin_unlock(&mm->page_table_lock);
+			goto again;
+		}
+		iwmmxt_task_restore(current_thread_info(), iwmmxt_storage+2);
+		spin_unlock(&mm->page_table_lock);
+		return 0;
+	}
+	return -1;
+}
+
+#endif
+
 static int
 restore_sigcontext(struct pt_regs *regs, struct sigcontext __user *sc)
 {
@@ -208,6 +302,11 @@ asmlinkage int sys_sigreturn(struct pt_regs *regs)
 	if (restore_sigcontext(regs, &frame->sc))
 		goto badframe;
 
+#ifdef CONFIG_IWMMXT
+	if (test_thread_flag(TIF_USING_IWMMXT) && restore_iwmmxt_context(frame+1))
+		goto badframe;
+#endif
+
 	/* Send SIGTRAP if we're single-stepping */
 	if (current->ptrace & PT_SINGLESTEP) {
 		ptrace_cancel_bpt(current);
@@ -256,6 +355,11 @@ asmlinkage int sys_rt_sigreturn(struct pt_regs *regs)
 	if (do_sigaltstack(&frame->uc.uc_stack, NULL, regs->ARM_sp) == -EFAULT)
 		goto badframe;
 
+#ifdef CONFIG_IWMMXT
+	if (test_thread_flag(TIF_USING_IWMMXT) && restore_iwmmxt_context(frame+1))
+		goto badframe;
+#endif
+
 	/* Send SIGTRAP if we're single-stepping */
 	if (current->ptrace & PT_SINGLESTEP) {
 		ptrace_cancel_bpt(current);
@@ -306,6 +410,11 @@ get_sigframe(struct k_sigaction *ka, struct pt_regs *regs, int framesize)
 {
 	unsigned long sp = regs->ARM_sp;
 
+#ifdef CONFIG_IWMMXT
+	if (test_thread_flag(TIF_USING_IWMMXT))
+		framesize = (framesize + 4 + IWMMXT_STORAGE_SIZE) & ~7;
+#endif
+
 	/*
 	 * This is the X/Open sanctioned signal stack switching.
 	 */
@@ -394,6 +503,11 @@ setup_frame(int usig, struct k_sigaction *ka, sigset_t *set, struct pt_regs *reg
 				      sizeof(frame->extramask));
 	}
 
+#ifdef CONFIG_IWMMXT
+	if (test_thread_flag(TIF_USING_IWMMXT))
+		err |= preserve_iwmmxt_context(frame+1);
+#endif
+
 	if (err == 0)
 		err = setup_return(regs, ka, &frame->retcode, frame, usig);
 
@@ -428,6 +542,11 @@ setup_rt_frame(int usig, struct k_sigaction *ka, siginfo_t *info,
 				regs, set->sig[0]);
 	err |= __copy_to_user(&frame->uc.uc_sigmask, set, sizeof(*set));
 
+#ifdef CONFIG_IWMMXT
+	if (test_thread_flag(TIF_USING_IWMMXT))
+		err |= preserve_iwmmxt_context(frame+1);
+#endif
+
 	if (err == 0)
 		err = setup_return(regs, ka, &frame->retcode, frame, usig);
 

arch/arm/mach-pxa/Kconfig

@@ -12,7 +12,7 @@ config ARCH_LUBBOCK
 config MACH_MAINSTONE
 	bool "Intel HCDDBBVA0 Development Platform"
 	select PXA27x
-	#select IWMMXT
+	select IWMMXT
 
 config ARCH_PXA_IDP
 	bool "Accelent Xscale IDP"
@@ -32,4 +32,9 @@ config PXA27x
 	help
 	  Select code specific to PXA27x variants
 
+config IWMMXT
+	bool
+	help
+	  Enable support for iWMMXt
+
 endif

arch/arm/mm/proc-xscale.S

@@ -585,7 +585,12 @@ __xscale_setup:
 	mcr	p15, 0, r4, c2, c0, 0		@ load page table pointer
 	mov	r0, #0x1f			@ Domains 0, 1 = client
 	mcr	p15, 0, r0, c3, c0, 0		@ load domain access register
-	mov	r0, #1				@ Allow access to CP0 and CP13
+#ifdef CONFIG_IWMMXT
+	mov	r0, #0				@ initially disallow access to CP0/CP1
+#else
+	mov	r0, #1				@ Allow access to CP0
+#endif
+	orr     r0, r0, #1 << 6			@ cp6 for IOP3xx and Bulverde
 	orr	r0, r0, #1 << 13		@ Its undefined whether this
 	mcr	p15, 0, r0, c15, c1, 0		@ affects USR or SVC modes
 	mrc	p15, 0, r0, c1, c0, 0		@ get control register

include/asm-arm/elf.h

 #ifndef __ASMARM_ELF_H
 #define __ASMARM_ELF_H
 
+#include <linux/config.h>
+
 /*
  * ELF register definitions..
  */
@@ -14,6 +16,7 @@ typedef unsigned long elf_freg_t[3];
 
 #define EM_ARM	40
 #define EF_ARM_APCS26 0x08
+#define EF_ARM_SOFT_FLOAT 0x200
 
 #define R_ARM_NONE	0
 #define R_ARM_PC24	1
@@ -91,6 +94,8 @@ extern char elf_platform[];
 	(( (elf_hwcap & HWCAP_26BIT) && (x)->e_flags & EF_ARM_APCS26) || \
 	  ((x)->e_flags & EF_ARM_APCS26) == 0)
 
+#ifndef CONFIG_IWMMXT
+
 /* Old NetWinder binaries were compiled in such a way that the iBCS
    heuristic always trips on them.  Until these binaries become uncommon
    enough not to care, don't trust the `ibcs' flag here.  In any case
@@ -99,6 +104,28 @@ extern char elf_platform[];
 #define SET_PERSONALITY(ex,ibcs2) \
 	set_personality(((ex).e_flags&EF_ARM_APCS26 ?PER_LINUX :PER_LINUX_32BIT))
 
+#else
+
+/*
+ * All iWMMXt capable CPUs don't support 26-bit mode.  Yet they can run
+ * legacy binaries which used to contain FPA11 floating point instructions
+ * that have always been emulated by the kernel.  PFA11 and iWMMXt overlap
+ * on coprocessor 1 space though.  We therefore must decide if given task
+ * is allowed to use CP 0 and 1 for iWMMXt, or if they should be blocked
+ * at all times for the prefetch exception handler to catch FPA11 opcodes
+ * and emulate them.  The best indication to discriminate those two cases
+ * is the SOFT_FLOAT flag in the ELF header.
+ */
+
+#define SET_PERSONALITY(ex,ibcs2) \
+do { \
+	set_personality(PER_LINUX_32BIT); \
+	if ((ex).e_flags & EF_ARM_SOFT_FLOAT) \
+		set_thread_flag(TIF_USING_IWMMXT); \
+} while (0)
+
+#endif
+
 #endif
 
 #endif

include/asm-arm/fpstate.h

@@ -11,7 +11,7 @@
 #ifndef __ASM_ARM_FPSTATE_H
 #define __ASM_ARM_FPSTATE_H
 
-#define FP_SIZE 35
+#include <linux/config.h>
 
 #ifndef __ASSEMBLY__
 
@@ -43,19 +43,32 @@ union vfp_state {
 extern void vfp_flush_thread(union vfp_state *);
 extern void vfp_release_thread(union vfp_state *);
 
+#define FP_HARD_SIZE 35
+
 struct fp_hard_struct {
-	unsigned int save[FP_SIZE];		/* as yet undefined */
+	unsigned int save[FP_HARD_SIZE];		/* as yet undefined */
 };
 
+#define FP_SOFT_SIZE 35
+
 struct fp_soft_struct {
-	unsigned int save[FP_SIZE];		/* undefined information */
+	unsigned int save[FP_SOFT_SIZE];		/* undefined information */
+};
+
+struct iwmmxt_struct {
+	unsigned int save[0x98/sizeof(int) + 1];
 };
 
 union fp_state {
 	struct fp_hard_struct	hard;
 	struct fp_soft_struct	soft;
+#ifdef CONFIG_IWMMXT
+	struct iwmmxt_struct	iwmmxt;
+#endif
 };
 
+#define FP_SIZE (sizeof(union fp_state) / sizeof(int))
+
 #endif
 
 #endif

include/asm-arm/thread_info.h

@@ -100,19 +100,10 @@ extern void free_thread_info(struct thread_info *);
 #define thread_saved_fp(tsk)	\
 	((unsigned long)((tsk)->thread_info->cpu_context.fp))
 
-#else /* !__ASSEMBLY__ */
-
-#define TI_FLAGS	0
-#define TI_PREEMPT	4
-#define TI_ADDR_LIMIT	8
-#define TI_TASK		12
-#define TI_EXEC_DOMAIN	16
-#define TI_CPU		20
-#define TI_CPU_DOMAIN	24
-#define TI_CPU_SAVE	28
-#define TI_USED_CP	76
-#define TI_FPSTATE	(TI_USED_CP+16)
-#define TI_VFPSTATE	(TI_FPSTATE+FP_SIZE*4)
+extern void iwmmxt_task_disable(struct thread_info *);
+extern void iwmmxt_task_copy(struct thread_info *, void *);
+extern void iwmmxt_task_restore(struct thread_info *, void *);
+extern void iwmmxt_task_release(struct thread_info *);
 
 #endif
 
@@ -133,6 +124,7 @@ extern void free_thread_info(struct thread_info *);
 #define TIF_SYSCALL_TRACE	8
 #define TIF_USED_FPU		16
 #define TIF_POLLING_NRFLAG	17
+#define TIF_USING_IWMMXT	18
 
 #define _TIF_NOTIFY_RESUME	(1 << TIF_NOTIFY_RESUME)
 #define _TIF_SIGPENDING		(1 << TIF_SIGPENDING)
@@ -140,6 +132,7 @@ extern void free_thread_info(struct thread_info *);
 #define _TIF_SYSCALL_TRACE	(1 << TIF_SYSCALL_TRACE)
 #define _TIF_USED_FPU		(1 << TIF_USED_FPU)
 #define _TIF_POLLING_NRFLAG	(1 << TIF_POLLING_NRFLAG)
+#define _TIF_USING_IWMMXT	(1 << TIF_USING_IWMMXT)
 
 /*
  * Change these and you break ASM code in entry-common.S