trap.c   [plain text]

/*
 * Copyright (c) 2007 Apple Inc. All rights reserved.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
 *
 * This file contains Original Code and/or Modifications of Original Code
 * as defined in and that are subject to the Apple Public Source License
 * Version 2.0 (the 'License'). You may not use this file except in
 * compliance with the License. The rights granted to you under the License
 * may not be used to create, or enable the creation or redistribution of,
 * unlawful or unlicensed copies of an Apple operating system, or to
 * circumvent, violate, or enable the circumvention or violation of, any
 * terms of an Apple operating system software license agreement.
 *
 * Please obtain a copy of the License at
 * http://www.opensource.apple.com/apsl/ and read it before using this file.
 *
 * The Original Code and all software distributed under the License are
 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
 * Please see the License for the specific language governing rights and
 * limitations under the License.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
 */
#include <kern/debug.h>
#include <mach_kdp.h>
#include <machine/endian.h>
#include <mach/mach_types.h>
#include <mach/boolean.h>
#include <mach/vm_prot.h>
#include <mach/vm_types.h>
#include <mach/mach_traps.h>

#include <mach/exception.h>
#include <mach/kern_return.h>
#include <mach/vm_param.h>
#include <mach/message.h>
#include <mach/machine/thread_status.h>

#include <vm/vm_page.h>
#include <vm/pmap.h>
#include <vm/vm_fault.h>
#include <vm/vm_kern.h>

#include <kern/ast.h>
#include <kern/thread.h>
#include <kern/task.h>
#include <kern/sched_prim.h>

#include <sys/kdebug.h>

#include <arm/trap.h>
#include <arm/caches_internal.h>
#include <arm/cpu_data_internal.h>
#include <arm/machdep_call.h>
#include <arm/machine_routines.h>
#include <arm/misc_protos.h>
#include <arm/setjmp.h>
#include <arm/proc_reg.h>

/*
 * External function prototypes.
 */
#include <kern/syscall_sw.h>
#include <kern/host.h>
#include <kern/processor.h>


#if CONFIG_DTRACE
extern kern_return_t dtrace_user_probe(arm_saved_state_t* regs, unsigned int instr);
extern boolean_t dtrace_tally_fault(user_addr_t);

/* Traps for userland processing. Can't include bsd/sys/fasttrap_isa.h, so copy and paste the trap instructions
   over from that file. Need to keep these in sync! */
#define FASTTRAP_ARM_INSTR 0xe7ffdefc
#define FASTTRAP_THUMB_INSTR 0xdefc

#define FASTTRAP_ARM_RET_INSTR 0xe7ffdefb
#define FASTTRAP_THUMB_RET_INSTR 0xdefb

/* See <rdar://problem/4613924> */
perfCallback tempDTraceTrapHook = NULL; /* Pointer to DTrace fbt trap hook routine */
#endif

#define COPYIN(dst, src, size)					\
	((regs->cpsr & PSR_MODE_MASK) != PSR_USER_MODE) ?	\
		copyin_kern(dst, src, size)			\
	:							\
		copyin(dst, src, size)

#define COPYOUT(src, dst, size)					\
	((regs->cpsr & PSR_MODE_MASK) != PSR_USER_MODE) ?	\
		copyout_kern(src, dst, size)			\
	:							\
		copyout(src, dst, size)

/* Second-level exception handlers forward declarations */
void            sleh_undef(struct arm_saved_state *, struct arm_vfpsaved_state *);
void            sleh_abort(struct arm_saved_state *, int);
static kern_return_t sleh_alignment(struct arm_saved_state *);
static void 	panic_with_thread_kernel_state(const char *msg, arm_saved_state_t *regs);


volatile perfCallback    perfTrapHook = NULL;	/* Pointer to CHUD trap hook routine */

int             sleh_alignment_count = 0;
int             trap_on_alignment_fault = 0;

/*
 *	Routine:        sleh_undef
 *	Function:       Second level exception handler for undefined exception
 */

void
sleh_undef(struct arm_saved_state * regs, struct arm_vfpsaved_state * vfp_ss __unused)
{
	exception_type_t exception = EXC_BAD_INSTRUCTION;
	mach_exception_data_type_t code[2] = {EXC_ARM_UNDEFINED};
	mach_msg_type_number_t codeCnt = 2;
	thread_t        thread = current_thread();
	vm_offset_t     recover;

	recover = thread->recover;
	thread->recover = 0;

	getCpuDatap()->cpu_stat.undef_ex_cnt++;

	/* Inherit the interrupt masks from previous */
	if (!(regs->cpsr & PSR_INTMASK))
		ml_set_interrupts_enabled(TRUE);

#if CONFIG_DTRACE
	if (tempDTraceTrapHook) {
		if (tempDTraceTrapHook(exception, regs, 0, 0) == KERN_SUCCESS) {
			/*
			 * If it succeeds, we are done...
			 */
			goto exit;
		}
	}

	/* Check to see if we've hit a userland probe */
	if ((regs->cpsr & PSR_MODE_MASK) == PSR_USER_MODE) {
		if (regs->cpsr & PSR_TF) {
			uint16_t instr;

			if(COPYIN((user_addr_t)(regs->pc), (char *)&instr,(vm_size_t)(sizeof(uint16_t))) != KERN_SUCCESS)
				goto exit;

			if (instr == FASTTRAP_THUMB_INSTR || instr == FASTTRAP_THUMB_RET_INSTR) {
				if (dtrace_user_probe(regs, instr) == KERN_SUCCESS)
					/* If it succeeds, we are done... */
					goto exit;
			}
		} else {
			uint32_t instr;

			if(COPYIN((user_addr_t)(regs->pc), (char *)&instr,(vm_size_t)(sizeof(uint32_t))) != KERN_SUCCESS)
				goto exit;

			if (instr == FASTTRAP_ARM_INSTR || instr == FASTTRAP_ARM_RET_INSTR) {
				if (dtrace_user_probe(regs, instr) == KERN_SUCCESS)
					/* If it succeeds, we are done... */
					goto exit;
			}
		}
	}
#endif /* CONFIG_DTRACE */


	if (regs->cpsr & PSR_TF) {
		unsigned short instr;

		if(COPYIN((user_addr_t)(regs->pc), (char *)&instr,(vm_size_t)(sizeof(unsigned short))) != KERN_SUCCESS)
			goto exit;

		if (IS_THUMB32(instr)) {
			unsigned int	instr32;

			instr32 = (instr<<16);

			if(COPYIN((user_addr_t)(((unsigned short *) (regs->pc))+1), (char *)&instr,(vm_size_t)(sizeof(unsigned short))) != KERN_SUCCESS)
				goto exit;

			instr32 |= instr;
			code[1] = instr32;

#if	__ARM_VFP__
			if (IS_THUMB_VFP(instr32)) {
				/* We no longer manage FPEXC beyond bootstrap, so verify that VFP is still enabled. */
				if (!get_vfp_enabled())
					panic("VFP was disabled (thumb); VFP should always be enabled");
			}
#endif
		} else {
			/* I don't believe we have any 16 bit VFP instructions, so just set code[1]. */
			code[1] = instr;

			if (IS_THUMB_GDB_TRAP(instr)) {
				exception = EXC_BREAKPOINT;
				code[0] = EXC_ARM_BREAKPOINT;
			}
		}
	} else {
		uint32_t instr;

		if(COPYIN((user_addr_t)(regs->pc), (char *)&instr,(vm_size_t)(sizeof(uint32_t))) != KERN_SUCCESS)
			goto exit;

		code[1] = instr;
#if	__ARM_VFP__
		if (IS_ARM_VFP(instr)) {
			/* We no longer manage FPEXC beyond bootstrap, so verify that VFP is still enabled. */
			if (!get_vfp_enabled())
				panic("VFP was disabled (arm); VFP should always be enabled");
		}
#endif

		if (IS_ARM_GDB_TRAP(instr)) {
			exception = EXC_BREAKPOINT;
			code[0] = EXC_ARM_BREAKPOINT;
		}
	}

	if (!((regs->cpsr & PSR_MODE_MASK) == PSR_USER_MODE)) {
		boolean_t	intr;

		intr = ml_set_interrupts_enabled(FALSE);

		if (exception == EXC_BREAKPOINT) {
			/* Save off the context here (so that the debug logic
			 * can see the original state of this thread).
			 */
			vm_offset_t kstackptr = current_thread()->machine.kstackptr;
			*((arm_saved_state_t *) kstackptr) = *regs;

			DebuggerCall(exception, regs);
			(void) ml_set_interrupts_enabled(intr);
			goto exit;
		}
		panic_context(exception, (void *)regs, "undefined kernel instruction\n"
		      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
		      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
		      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
		      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
		      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
		      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
		      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
		      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
		      regs->r[12], regs->sp, regs->lr, regs->pc,
		      regs->cpsr, regs->fsr, regs->far);

		(void) ml_set_interrupts_enabled(intr);

	} else {
		exception_triage(exception, code, codeCnt);
		/* NOTREACHED */
	}

exit:
	if (recover)
		thread->recover = recover;
}

/*
 *	Routine:	sleh_abort
 *	Function:	Second level exception handler for abort(Pref/Data)
 */

void
sleh_abort(struct arm_saved_state * regs, int type)
{
	int             status; 
	int		debug_status=0;
	int             spsr;
	int             exc;
	mach_exception_data_type_t codes[2];
	vm_map_t        map;
	vm_map_address_t vaddr;
	vm_map_address_t fault_addr;
	vm_prot_t       fault_type;
	kern_return_t   result;
	vm_offset_t     recover;
	thread_t        thread = current_thread();
	boolean_t		intr;

	recover = thread->recover;
	thread->recover = 0;

	status = regs->fsr & FSR_MASK;
	spsr = regs->cpsr;

	/* The DSFR/IFSR.ExT bit indicates "IMPLEMENTATION DEFINED" classification.
	 * Allow a platform-level error handler to decode it.
	 */
	if ((regs->fsr) & FSR_EXT) {
		cpu_data_t	*cdp = getCpuDatap();

		if (cdp->platform_error_handler != (platform_error_handler_t) NULL) {
			(*(platform_error_handler_t)cdp->platform_error_handler) (cdp->cpu_id, 0);
			/* If a platform error handler is registered, expect it to panic, not fall through */
			panic("Unexpected return from platform_error_handler");
		}
	}

	/* Done with asynchronous handling; re-enable here so that subsequent aborts are taken as early as possible. */
	reenable_async_aborts();

	if (ml_at_interrupt_context())
		panic_with_thread_kernel_state("sleh_abort at interrupt context", regs);

	fault_addr = vaddr = regs->far;

	if (type == T_DATA_ABT) {
		getCpuDatap()->cpu_stat.data_ex_cnt++;
	} else { /* T_PREFETCH_ABT */
		getCpuDatap()->cpu_stat.instr_ex_cnt++;
		fault_type = VM_PROT_READ | VM_PROT_EXECUTE;
	}

	if (status == FSR_DEBUG)
	    debug_status = arm_debug_read_dscr() & ARM_DBGDSCR_MOE_MASK;

	/* Inherit the interrupt masks from previous */
	if (!(spsr & PSR_INTMASK))
		ml_set_interrupts_enabled(TRUE);

	if (type == T_DATA_ABT) {
		/*
		 * Now that interrupts are reenabled, we can perform any needed
		 * copyin operations.
		 *
		 * Because we have reenabled interrupts, any instruction copy
		 * must be a copyin, even on UP systems.
		 */

		if (regs->fsr & DFSR_WRITE) {
			fault_type = (VM_PROT_READ | VM_PROT_WRITE);
			/* Cache operations report faults as write access, change these to read access */
			/* Cache operations are invoked from arm mode for now */
			if (!(regs->cpsr & PSR_TF)) {
				unsigned int    ins;

				if(COPYIN((user_addr_t)(regs->pc), (char *)&ins,(vm_size_t)(sizeof(unsigned int))) != KERN_SUCCESS)
					goto exit;

				if (arm_mcr_cp15(ins) || arm_mcrr_cp15(ins))
					fault_type = VM_PROT_READ;
			}
		} else {
			fault_type = VM_PROT_READ;
			/*
			 * DFSR is not getting the "write" bit set
			 * when a swp instruction is encountered (even when it is
			 * a write fault.
			 */
			if (!(regs->cpsr & PSR_TF)) {
				unsigned int    ins;

				if(COPYIN((user_addr_t)(regs->pc), (char *)&ins,(vm_size_t)(sizeof(unsigned int))) != KERN_SUCCESS)
					goto exit;

				if ((ins & ARM_SWP_MASK) == ARM_SWP)
					fault_type = VM_PROT_WRITE;
			}
		}
	}

	if ((spsr & PSR_MODE_MASK) != PSR_USER_MODE) {
		/* Fault in kernel mode */

		if ((status == FSR_DEBUG)
		    && ((debug_status == ARM_DBGDSCR_MOE_ASYNC_WATCHPOINT) || (debug_status == ARM_DBGDSCR_MOE_SYNC_WATCHPOINT))
		    && (recover != 0) && (getCpuDatap()->cpu_user_debug != 0)) {
			/* If we hit a watchpoint in kernel mode, probably in a copyin/copyout which we don't want to
			 * abort.  Turn off watchpoints and keep going; we'll turn them back on in load_and_go_user.
			 */
			arm_debug_set(NULL);
			goto exit;
		}

		if ((type == T_PREFETCH_ABT) || (status == FSR_DEBUG)) {

			intr = ml_set_interrupts_enabled(FALSE);
			if (status == FSR_DEBUG) {
				DebuggerCall(EXC_BREAKPOINT, regs);
				(void) ml_set_interrupts_enabled(intr);
				goto exit;
			}
			panic_context(EXC_BAD_ACCESS, (void*)regs, "sleh_abort: prefetch abort in kernel mode: fault_addr=0x%x\n"
			      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
			      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
			      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
			      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
			      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
			      fault_addr,
			      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
			      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
			      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
			      regs->r[12], regs->sp, regs->lr, regs->pc,
			      regs->cpsr, regs->fsr, regs->far);

			(void) ml_set_interrupts_enabled(intr);

		} else if (TEST_FSR_VMFAULT(status)) {

#if CONFIG_DTRACE
			if (thread->options & TH_OPT_DTRACE) {	/* Executing under dtrace_probe? */
				if (dtrace_tally_fault(fault_addr)) { /* Should a fault under dtrace be ignored? */
					/* Point to next instruction */
					regs->pc += ((regs->cpsr & PSR_TF) && !IS_THUMB32(*((uint16_t*) (regs->pc)))) ? 2 : 4;
					goto exit;
				} else {
					intr = ml_set_interrupts_enabled(FALSE);
					panic_context(EXC_BAD_ACCESS, (void *)regs, "Unexpected page fault under dtrace_probe"
					      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
					      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
					      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
					      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
					      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
					      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
					      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
					      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
					      regs->r[12], regs->sp, regs->lr, regs->pc,
					      regs->cpsr, regs->fsr, regs->far);

					(void) ml_set_interrupts_enabled(intr);

					goto exit;
				}
			}
#endif

			if (VM_KERNEL_ADDRESS(vaddr) || thread == THREAD_NULL)
				map = kernel_map;
			else
				map = thread->map;

			/* check to see if it is just a pmap ref/modify fault */
			result = arm_fast_fault(map->pmap, trunc_page(fault_addr), fault_type, FALSE);
			if (result == KERN_SUCCESS)
				goto exit;

			/*
			 *  We have to "fault" the page in.
			 */
			result = vm_fault(map, fault_addr,
					  fault_type,
					  FALSE /* change_wiring */, VM_KERN_MEMORY_NONE,
					  (map == kernel_map) ? THREAD_UNINT : THREAD_ABORTSAFE, NULL, 0);

			if (result == KERN_SUCCESS) {
				goto exit;
			} else {
				/*
				 *  If we have a recover handler, invoke it now.
				 */
				if (recover != 0) {
					regs->pc = (register_t) (recover & ~0x1);
					regs->cpsr = (regs->cpsr & ~PSR_TF) | ((recover & 0x1) << PSR_TFb);
					goto exit;
				}
			}
		} else if ((status & FSR_ALIGN_MASK) == FSR_ALIGN) {
			result = sleh_alignment(regs);
			if (result == KERN_SUCCESS) {
				goto exit;
			} else {
				intr = ml_set_interrupts_enabled(FALSE);

				panic_context(EXC_BAD_ACCESS, (void *)regs, "unaligned kernel data access: pc=0x%08x fault_addr=0x%x\n"
				      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
				      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
				      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
				      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
				      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
				      regs->pc, fault_addr,
				      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
				      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
				      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
				      regs->r[12], regs->sp, regs->lr, regs->pc,
				      regs->cpsr, regs->fsr, regs->far);

				(void) ml_set_interrupts_enabled(intr);

				goto exit;
			}

		}
		intr = ml_set_interrupts_enabled(FALSE);

		panic_context(EXC_BAD_ACCESS, (void *)regs, "kernel abort type %d: fault_type=0x%x, fault_addr=0x%x\n"
		      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
		      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
		      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
		      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
		      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
		      type, fault_type, fault_addr,
		      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
		      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
		      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
		      regs->r[12], regs->sp, regs->lr, regs->pc,
		      regs->cpsr, regs->fsr, regs->far);

		(void) ml_set_interrupts_enabled(intr);

		goto exit;
	}
	/* Fault in user mode */

	if (TEST_FSR_VMFAULT(status)) {
		map = thread->map;

#if CONFIG_DTRACE
		if (thread->options & TH_OPT_DTRACE) {	/* Executing under dtrace_probe? */
			if (dtrace_tally_fault(fault_addr)) { /* Should a user mode fault under dtrace be ignored? */
				if (recover) {
					regs->pc = recover;
				} else {
					intr = ml_set_interrupts_enabled(FALSE);

					panic_context(EXC_BAD_ACCESS, (void *)regs, "copyin/out has no recovery point"
					      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
					      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
					      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
					      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
					      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
					      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
					      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
					      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
					      regs->r[12], regs->sp, regs->lr, regs->pc,
					      regs->cpsr, regs->fsr, regs->far);

					(void) ml_set_interrupts_enabled(intr);
				}
				goto exit;
			} else {
				intr = ml_set_interrupts_enabled(FALSE);

				panic_context(EXC_BAD_ACCESS, (void*)regs, "Unexpected UMW page fault under dtrace_probe"
				      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
				      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
				      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
				      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
				      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
				      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
				      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
				      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
				      regs->r[12], regs->sp, regs->lr, regs->pc,
				      regs->cpsr, regs->fsr, regs->far);

				(void) ml_set_interrupts_enabled(intr);

				goto exit;
			}
		}
#endif

		/* check to see if it is just a pmap ref/modify fault */
		result = arm_fast_fault(map->pmap, trunc_page(fault_addr), fault_type, TRUE);
		if (result != KERN_SUCCESS) {
			/*
			 * We have to "fault" the page in.
			 */
			result = vm_fault(map, fault_addr, fault_type,
					  FALSE /* change_wiring */, VM_KERN_MEMORY_NONE,
					  THREAD_ABORTSAFE, NULL, 0);
		}
		if (result == KERN_SUCCESS || result == KERN_ABORTED) {
			goto exception_return;
		}
		exc = EXC_BAD_ACCESS;
		codes[0] = result;
	} else if ((status & FSR_ALIGN_MASK) == FSR_ALIGN) {
		if (sleh_alignment(regs) == KERN_SUCCESS) {
			goto exception_return;
		}
		exc = EXC_BAD_ACCESS;
		codes[0] = EXC_ARM_DA_ALIGN;
	} else if (status == FSR_DEBUG) {
		exc = EXC_BREAKPOINT;
		codes[0] = EXC_ARM_DA_DEBUG;
	} else if ((status == FSR_SDOM) || (status == FSR_PDOM)) {
		exc = EXC_BAD_ACCESS;
		codes[0] = KERN_INVALID_ADDRESS;
	} else {
		exc = EXC_BAD_ACCESS;
		codes[0] = KERN_FAILURE;
	}

	codes[1] = vaddr;
	exception_triage(exc, codes, 2);
	/* NOTREACHED */

exception_return:
	if (recover)
		thread->recover = recover;
	thread_exception_return();
	/* NOTREACHED */

exit:
	if (recover)
		thread->recover = recover;
	return;
}


/*
 *	Routine:        sleh_alignment
 *	Function:       Second level exception handler for alignment data fault
 */

static kern_return_t
sleh_alignment(struct arm_saved_state * regs)
{
	unsigned int    status;
	unsigned int    ins;
	unsigned int    rd_index;
	unsigned int    base_index;
	unsigned int    paddr;
	void           *src;
	unsigned int    reg_list;
	unsigned int    pre;
	unsigned int    up;
	unsigned int    write_back;
	kern_return_t   rc = KERN_SUCCESS;

	getCpuDatap()->cpu_stat.unaligned_cnt++;

	/* Do not try to emulate in modified execution states */
	if (regs->cpsr & (PSR_EF | PSR_JF))
		return KERN_NOT_SUPPORTED;

	/* Disallow emulation of kernel instructions */
	if ((regs->cpsr & PSR_MODE_MASK) != PSR_USER_MODE)
		return KERN_NOT_SUPPORTED;
		

#define ALIGN_THRESHOLD 1024
	if ((sleh_alignment_count++ & (ALIGN_THRESHOLD - 1)) ==
	    (ALIGN_THRESHOLD - 1))
		kprintf("sleh_alignment: %d more alignment faults: %d total\n",
			ALIGN_THRESHOLD, sleh_alignment_count);

	if ((trap_on_alignment_fault != 0)
	    && (sleh_alignment_count % trap_on_alignment_fault == 0))
		return KERN_NOT_SUPPORTED;

	status = regs->fsr;
	paddr = regs->far;

	if (regs->cpsr & PSR_TF) {
		 unsigned short  ins16;

		/* Get aborted instruction */
#if	__ARM_SMP__ || __ARM_USER_PROTECT__
		if(COPYIN((user_addr_t)(regs->pc), (char *)&ins16,(vm_size_t)(sizeof(uint16_t))) != KERN_SUCCESS) {
			/* Failed to fetch instruction, return success to re-drive the exception */
			return KERN_SUCCESS;
		}
#else
		ins16 = *(unsigned short *) (regs->pc);
#endif

		/*
		 * Map multi-word Thumb loads and stores to their ARM
		 * equivalents.
		 * Don't worry about single-word instructions, since those are
		 * handled in hardware.
		 */

		reg_list = ins16 & 0xff;
		if (reg_list == 0)
			return KERN_NOT_SUPPORTED;

		if (((ins16 & THUMB_STR_1_MASK) == THUMB_LDMIA) ||
		    ((ins16 & THUMB_STR_1_MASK) == THUMB_STMIA)) {
			base_index = (ins16 >> 8) & 0x7;
			ins = 0xE8800000 | (base_index << 16) | reg_list;
			if ((ins16 & THUMB_STR_1_MASK) == THUMB_LDMIA)
				ins |= (1 << 20);
			if (((ins16 & THUMB_STR_1_MASK) == THUMB_STMIA) ||
			    !(reg_list & (1 << base_index)))
				ins |= (1 << 21);
		} else if ((ins16 & THUMB_PUSH_MASK) == THUMB_POP) {
			unsigned int    r = (ins16 >> 8) & 1;
			ins = 0xE8BD0000 | (r << 15) | reg_list;
		} else if ((ins16 & THUMB_PUSH_MASK) == THUMB_PUSH) {
			unsigned int    r = (ins16 >> 8) & 1;
			ins = 0xE92D0000 | (r << 14) | reg_list;
		} else {
			return KERN_NOT_SUPPORTED;
		}
	} else {
		/* Get aborted instruction */
#if	__ARM_SMP__ || __ARM_USER_PROTECT__
		if(COPYIN((user_addr_t)(regs->pc), (char *)&ins,(vm_size_t)(sizeof(unsigned int))) != KERN_SUCCESS) {
			/* Failed to fetch instruction, return success to re-drive the exception */
			return KERN_SUCCESS;
		}
#else
		ins = *(unsigned int *) (regs->pc);
#endif
	}

	/* Don't try to emulate unconditional instructions */
	if ((ins & 0xF0000000) == 0xF0000000)
		return KERN_NOT_SUPPORTED;

	pre = (ins >> 24) & 1;
	up = (ins >> 23) & 1;
	reg_list = ins & 0xffff;
	write_back = (ins >> 21) & 1;
	base_index = (ins >> 16) & 0xf;

	if ((ins & ARM_BLK_MASK) == ARM_STM) {	/* STM or LDM */
		int             reg_count = 0;
		int             waddr;

		for (rd_index = 0; rd_index < 16; rd_index++) {
			if (reg_list & (1 << rd_index))
				reg_count++;
		}

		paddr = regs->r[base_index];

		switch (ins & (ARM_POST_INDEXING | ARM_INCREMENT)) {
			/* Increment after */
		case ARM_INCREMENT:
			waddr = paddr + reg_count * 4;
			break;

			/* Increment before */
		case ARM_POST_INDEXING | ARM_INCREMENT:
			waddr = paddr + reg_count * 4;
			paddr += 4;
			break;

			/* Decrement after */
		case 0:
			waddr = paddr - reg_count * 4;
			paddr = waddr + 4;
			break;

			/* Decrement before */
		case ARM_POST_INDEXING:
			waddr = paddr - reg_count * 4;
			paddr = waddr;
			break;

		default:
			waddr = 0;
		}

		for (rd_index = 0; rd_index < 16; rd_index++) {
			if (reg_list & (1 << rd_index)) {
				src = &regs->r[rd_index];

				if ((ins & (1 << 20)) == 0)	/* STM */
					rc = COPYOUT(src, paddr, 4);
				else	/* LDM */
					rc = COPYIN(paddr, src, 4);

				if (rc != KERN_SUCCESS)
					break;

				paddr += 4;
			}
		}

		paddr = waddr;
	} else {
		rc = 1;
	}

	if (rc == KERN_SUCCESS) {
		if (regs->cpsr & PSR_TF)
			regs->pc += 2;
		else
			regs->pc += 4;

		if (write_back)
			regs->r[base_index] = paddr;
	}
	return (rc);
}


#ifndef	NO_KDEBUG
/* XXX quell warnings */
void            syscall_trace(struct arm_saved_state * regs);
void            syscall_trace_exit(unsigned int, unsigned int);
void            mach_syscall_trace(struct arm_saved_state * regs, unsigned int call_number);
void            mach_syscall_trace_exit(unsigned int retval, unsigned int call_number);
void            interrupt_trace(struct arm_saved_state * regs);
void            interrupt_trace_exit(void);

/* called from the fleh_swi handler, if TRACE_SYSCALL is enabled */
void
syscall_trace(
	      struct arm_saved_state * regs)
{
	kprintf("syscall: %d\n", regs->r[12]);
}

void
syscall_trace_exit(
		   unsigned int r0,
		   unsigned int r1)
{
	kprintf("syscall exit: 0x%x 0x%x\n", r0, r1);
}

void
mach_syscall_trace(
		   struct arm_saved_state * regs,
		   unsigned int call_number)
{
	int             i, argc;
	int             kdarg[3] = {0, 0, 0};

	argc = mach_trap_table[call_number].mach_trap_arg_count;

	if (argc > 3)
		argc = 3;

	for (i = 0; i < argc; i++)
		kdarg[i] = (int) regs->r[i];

	KERNEL_DEBUG_CONSTANT_IST(KDEBUG_TRACE,
		MACHDBG_CODE(DBG_MACH_EXCP_SC, (call_number)) | DBG_FUNC_START,
		kdarg[0], kdarg[1], kdarg[2], 0, 0);

}

void
mach_syscall_trace_exit(
			unsigned int retval,
			unsigned int call_number)
{
	KERNEL_DEBUG_CONSTANT_IST(KDEBUG_TRACE,
		MACHDBG_CODE(DBG_MACH_EXCP_SC, (call_number)) | DBG_FUNC_END,
		retval, 0, 0, 0, 0);
}

void
interrupt_trace(
		struct arm_saved_state * regs)
{
#define	UMODE(rp)	(((rp)->cpsr & PSR_MODE_MASK) == PSR_USER_MODE)

	KERNEL_DEBUG_CONSTANT_IST(KDEBUG_TRACE,
		MACHDBG_CODE(DBG_MACH_EXCP_INTR, 0) | DBG_FUNC_START,
		0, UMODE(regs) ? regs->pc : VM_KERNEL_UNSLIDE(regs->pc),
		UMODE(regs), 0, 0);
}

void
interrupt_trace_exit(
		     void)
{
	KERNEL_DEBUG_CONSTANT_IST(KDEBUG_TRACE,
		MACHDBG_CODE(DBG_MACH_EXCP_INTR, 0) | DBG_FUNC_END,
		0, 0, 0, 0, 0);
}
#endif

/* XXX quell warnings */
void interrupt_stats(void);

/* This is called from locore.s directly. We only update per-processor interrupt counters in this function */
void
interrupt_stats(void)
{
	SCHED_STATS_INTERRUPT(current_processor());
}

static void 
panic_with_thread_kernel_state(const char *msg, struct arm_saved_state *regs)
{
		panic_context(0, (void*)regs, "%s (saved state:%p)\n"
			      "r0:   0x%08x  r1: 0x%08x  r2: 0x%08x  r3: 0x%08x\n"
			      "r4:   0x%08x  r5: 0x%08x  r6: 0x%08x  r7: 0x%08x\n"
			      "r8:   0x%08x  r9: 0x%08x r10: 0x%08x r11: 0x%08x\n"
			      "r12:  0x%08x  sp: 0x%08x  lr: 0x%08x  pc: 0x%08x\n"
			      "cpsr: 0x%08x fsr: 0x%08x far: 0x%08x\n",
				  msg, regs,
			      regs->r[0], regs->r[1], regs->r[2], regs->r[3],
			      regs->r[4], regs->r[5], regs->r[6], regs->r[7],
			      regs->r[8], regs->r[9], regs->r[10], regs->r[11],
			      regs->r[12], regs->sp, regs->lr, regs->pc,
			      regs->cpsr, regs->fsr, regs->far);

}