[GitHub/mt8127/android_kernel_alcatel_ttab.git] / arch / avr32 / kernel / entry-avr32b.S

/*
 * Copyright (C) 2004-2006 Atmel Corporation
 *
 * 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.
 */

/*
 * This file contains the low-level entry-points into the kernel, that is,
 * exception handlers, debug trap handlers, interrupt handlers and the
 * system call handler.
 */
#include <linux/errno.h>

#include <asm/asm.h>
#include <asm/hardirq.h>
#include <asm/irq.h>
#include <asm/ocd.h>
#include <asm/page.h>
#include <asm/pgtable.h>
#include <asm/ptrace.h>
#include <asm/sysreg.h>
#include <asm/thread_info.h>
#include <asm/unistd.h>

#ifdef CONFIG_PREEMPT
# define preempt_stop		mask_interrupts
#else
# define preempt_stop
# define fault_resume_kernel	fault_restore_all
#endif

#define __MASK(x)	((1 << (x)) - 1)
#define IRQ_MASK	((__MASK(SOFTIRQ_BITS) << SOFTIRQ_SHIFT) | \
			 (__MASK(HARDIRQ_BITS) << HARDIRQ_SHIFT))

	.section .ex.text,"ax",@progbits
	.align	2
exception_vectors:
	bral	handle_critical
	.align	2
	bral	handle_critical
	.align	2
	bral	do_bus_error_write
	.align	2
	bral	do_bus_error_read
	.align	2
	bral	do_nmi_ll
	.align	2
	bral	handle_address_fault
	.align	2
	bral	handle_protection_fault
	.align	2
	bral	handle_debug
	.align	2
	bral	do_illegal_opcode_ll
	.align	2
	bral	do_illegal_opcode_ll
	.align	2
	bral	do_illegal_opcode_ll
	.align	2
	bral	do_fpe_ll
	.align	2
	bral	do_illegal_opcode_ll
	.align	2
	bral	handle_address_fault
	.align	2
	bral	handle_address_fault
	.align	2
	bral	handle_protection_fault
	.align	2
	bral	handle_protection_fault
	.align	2
	bral	do_dtlb_modified

	/*
	 * r0 :	PGD/PT/PTE
	 * r1 : Offending address
	 * r2 : Scratch register
	 * r3 : Cause (5, 12 or 13)
	 */
#define	tlbmiss_save	pushm	r0-r3
#define tlbmiss_restore	popm	r0-r3

	.section .tlbx.ex.text,"ax",@progbits
	.global	itlb_miss
itlb_miss:
	tlbmiss_save
	rjmp	tlb_miss_common

	.section .tlbr.ex.text,"ax",@progbits
dtlb_miss_read:
	tlbmiss_save
	rjmp	tlb_miss_common

	.section .tlbw.ex.text,"ax",@progbits
dtlb_miss_write:
	tlbmiss_save

	.global	tlb_miss_common
tlb_miss_common:
	mfsr	r0, SYSREG_TLBEAR
	mfsr	r1, SYSREG_PTBR

	/* Is it the vmalloc space? */
	bld	r0, 31
	brcs	handle_vmalloc_miss

	/* First level lookup */
pgtbl_lookup:
	lsr	r2, r0, PGDIR_SHIFT
	ld.w	r3, r1[r2 << 2]
	bfextu	r1, r0, PAGE_SHIFT, PGDIR_SHIFT - PAGE_SHIFT
	bld	r3, _PAGE_BIT_PRESENT
	brcc	page_table_not_present

	/* Translate to virtual address in P1. */
	andl	r3, 0xf000
	sbr	r3, 31

	/* Second level lookup */
	ld.w	r2, r3[r1 << 2]
	mfsr	r0, SYSREG_TLBARLO
	bld	r2, _PAGE_BIT_PRESENT
	brcc	page_not_present

	/* Mark the page as accessed */
	sbr	r2, _PAGE_BIT_ACCESSED
	st.w	r3[r1 << 2], r2

	/* Drop software flags */
	andl	r2, _PAGE_FLAGS_HARDWARE_MASK & 0xffff
	mtsr	SYSREG_TLBELO, r2

	/* Figure out which entry we want to replace */
	mfsr	r1, SYSREG_MMUCR
	clz	r2, r0
	brcc	1f
	mov	r3, -1			/* All entries have been accessed, */
	mov	r2, 0			/* so start at 0 */
	mtsr	SYSREG_TLBARLO, r3	/* and reset TLBAR */

1:	bfins	r1, r2, SYSREG_DRP_OFFSET, SYSREG_DRP_SIZE
	mtsr	SYSREG_MMUCR, r1
	tlbw

	tlbmiss_restore
	rete

handle_vmalloc_miss:
	/* Simply do the lookup in init's page table */
	mov	r1, lo(swapper_pg_dir)
	orh	r1, hi(swapper_pg_dir)
	rjmp	pgtbl_lookup


	/* ---                    System Call                    --- */

	.section .scall.text,"ax",@progbits
system_call:
#ifdef CONFIG_PREEMPT
	mask_interrupts
#endif
	pushm	r12		/* r12_orig */
	stmts	--sp, r0-lr

	mfsr	r0, SYSREG_RAR_SUP
	mfsr	r1, SYSREG_RSR_SUP
#ifdef CONFIG_PREEMPT
	unmask_interrupts
#endif
	zero_fp
	stm	--sp, r0-r1

	/* check for syscall tracing */
	get_thread_info r0
	ld.w	r1, r0[TI_flags]
	bld	r1, TIF_SYSCALL_TRACE
	brcs	syscall_trace_enter

syscall_trace_cont:
	cp.w	r8, NR_syscalls
	brhs	syscall_badsys

	lddpc   lr, syscall_table_addr
	ld.w    lr, lr[r8 << 2]
	mov	r8, r5		/* 5th argument (6th is pushed by stub) */
	icall   lr

	.global	syscall_return
syscall_return:
	get_thread_info r0
	mask_interrupts		/* make sure we don't miss an interrupt
				   setting need_resched or sigpending
				   between sampling and the rets */

	/* Store the return value so that the correct value is loaded below */
	stdsp   sp[REG_R12], r12

	ld.w	r1, r0[TI_flags]
	andl	r1, _TIF_ALLWORK_MASK, COH
	brne	syscall_exit_work

syscall_exit_cont:
	popm	r8-r9
	mtsr	SYSREG_RAR_SUP, r8
	mtsr	SYSREG_RSR_SUP, r9
	ldmts	sp++, r0-lr
	sub	sp, -4		/* r12_orig */
	rets

	.align	2
syscall_table_addr:
	.long   sys_call_table

syscall_badsys:
	mov	r12, -ENOSYS
	rjmp	syscall_return

	.global ret_from_fork
ret_from_fork:
	rcall   schedule_tail

	/* check for syscall tracing */
	get_thread_info r0
	ld.w	r1, r0[TI_flags]
	andl	r1, _TIF_ALLWORK_MASK, COH
	brne	syscall_exit_work
	rjmp    syscall_exit_cont

syscall_trace_enter:
	pushm	r8-r12
	rcall	syscall_trace
	popm	r8-r12
	rjmp	syscall_trace_cont

syscall_exit_work:
	bld	r1, TIF_SYSCALL_TRACE
	brcc	1f
	unmask_interrupts
	rcall	syscall_trace
	mask_interrupts
	ld.w	r1, r0[TI_flags]

1:	bld	r1, TIF_NEED_RESCHED
	brcc	2f
	unmask_interrupts
	rcall	schedule
	mask_interrupts
	ld.w	r1, r0[TI_flags]
	rjmp	1b

2:	mov	r2, _TIF_SIGPENDING | _TIF_RESTORE_SIGMASK
	tst	r1, r2
	breq	3f
	unmask_interrupts
	mov	r12, sp
	mov	r11, r0
	rcall	do_notify_resume
	mask_interrupts
	ld.w	r1, r0[TI_flags]
	rjmp	1b

3:	bld	r1, TIF_BREAKPOINT
	brcc	syscall_exit_cont
	rjmp	enter_monitor_mode

	/* The slow path of the TLB miss handler */
page_table_not_present:
page_not_present:
	tlbmiss_restore
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	mfsr	r12, SYSREG_ECR
	mov	r11, sp
	rcall	do_page_fault
	rjmp	ret_from_exception

	/* This function expects to find offending PC in SYSREG_RAR_EX */
	.type	save_full_context_ex, @function
	.align	2
save_full_context_ex:
	mfsr	r11, SYSREG_RAR_EX
	sub	r9, pc, . - debug_trampoline
	mfsr	r8, SYSREG_RSR_EX
	cp.w	r9, r11
	breq	3f
	mov	r12, r8
	andh	r8, (MODE_MASK >> 16), COH
	brne	2f

1:	pushm	r11, r12	/* PC and SR */
	unmask_exceptions
	ret	r12

2:	sub	r10, sp, -(FRAME_SIZE_FULL - REG_LR)
	stdsp	sp[4], r10	/* replace saved SP */
	rjmp	1b

	/*
	 * The debug handler set up a trampoline to make us
	 * automatically enter monitor mode upon return, but since
	 * we're saving the full context, we must assume that the
	 * exception handler might want to alter the return address
	 * and/or status register. So we need to restore the original
	 * context and enter monitor mode manually after the exception
	 * has been handled.
	 */
3:	get_thread_info r8
	ld.w	r11, r8[TI_rar_saved]
	ld.w	r12, r8[TI_rsr_saved]
	rjmp	1b
	.size	save_full_context_ex, . - save_full_context_ex

	/* Low-level exception handlers */
handle_critical:
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	mfsr	r12, SYSREG_ECR
	mov	r11, sp
	rcall	do_critical_exception

	/* We should never get here... */
bad_return:
	sub	r12, pc, (. - 1f)
	bral	panic
	.align	2
1:	.asciz	"Return from critical exception!"

	.align	1
do_bus_error_write:
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	mov	r11, 1
	rjmp	1f

do_bus_error_read:
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	mov	r11, 0
1:	mfsr	r12, SYSREG_BEAR
	mov	r10, sp
	rcall	do_bus_error
	rjmp	ret_from_exception

	.align	1
do_nmi_ll:
	sub	sp, 4
	stmts	--sp, r0-lr
	mfsr	r9, SYSREG_RSR_NMI
	mfsr	r8, SYSREG_RAR_NMI
	bfextu	r0, r9, MODE_SHIFT, 3
	brne	2f

1:	pushm	r8, r9	/* PC and SR */
	mfsr	r12, SYSREG_ECR
	mov	r11, sp
	rcall	do_nmi
	popm	r8-r9
	mtsr	SYSREG_RAR_NMI, r8
	tst	r0, r0
	mtsr	SYSREG_RSR_NMI, r9
	brne	3f

	ldmts	sp++, r0-lr
	sub	sp, -4		/* skip r12_orig */
	rete

2:	sub	r10, sp, -(FRAME_SIZE_FULL - REG_LR)
	stdsp	sp[4], r10	/* replace saved SP */
	rjmp	1b

3:	popm	lr
	sub	sp, -4		/* skip sp */
	popm	r0-r12
	sub	sp, -4		/* skip r12_orig */
	rete

handle_address_fault:
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	mfsr	r12, SYSREG_ECR
	mov	r11, sp
	rcall	do_address_exception
	rjmp	ret_from_exception

handle_protection_fault:
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	mfsr	r12, SYSREG_ECR
	mov	r11, sp
	rcall	do_page_fault
	rjmp	ret_from_exception

	.align	1
do_illegal_opcode_ll:
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	mfsr	r12, SYSREG_ECR
	mov	r11, sp
	rcall	do_illegal_opcode
	rjmp	ret_from_exception

do_dtlb_modified:
	pushm	r0-r3
	mfsr	r1, SYSREG_TLBEAR
	mfsr	r0, SYSREG_PTBR
	lsr	r2, r1, PGDIR_SHIFT
	ld.w	r0, r0[r2 << 2]
	lsl	r1, (32 - PGDIR_SHIFT)
	lsr	r1, (32 - PGDIR_SHIFT) + PAGE_SHIFT

	/* Translate to virtual address in P1 */
	andl	r0, 0xf000
	sbr	r0, 31
	add	r2, r0, r1 << 2
	ld.w	r3, r2[0]
	sbr	r3, _PAGE_BIT_DIRTY
	mov	r0, r3
	st.w	r2[0], r3

	/* The page table is up-to-date. Update the TLB entry as well */
	andl	r0, lo(_PAGE_FLAGS_HARDWARE_MASK)
	mtsr	SYSREG_TLBELO, r0

	/* MMUCR[DRP] is updated automatically, so let's go... */
	tlbw

	popm	r0-r3
	rete

do_fpe_ll:
	sub	sp, 4
	stmts	--sp, r0-lr
	rcall	save_full_context_ex
	unmask_interrupts
	mov	r12, 26
	mov	r11, sp
	rcall	do_fpe
	rjmp	ret_from_exception

ret_from_exception:
	mask_interrupts
	lddsp	r4, sp[REG_SR]

	andh	r4, (MODE_MASK >> 16), COH
	brne	fault_resume_kernel

	get_thread_info r0
	ld.w	r1, r0[TI_flags]
	andl	r1, _TIF_WORK_MASK, COH
	brne	fault_exit_work

fault_resume_user:
	popm	r8-r9
	mask_exceptions
	mtsr	SYSREG_RAR_EX, r8
	mtsr	SYSREG_RSR_EX, r9
	ldmts	sp++, r0-lr
	sub	sp, -4
	rete

fault_resume_kernel:
#ifdef CONFIG_PREEMPT
	get_thread_info	r0
	ld.w	r2, r0[TI_preempt_count]
	cp.w	r2, 0
	brne	1f
	ld.w	r1, r0[TI_flags]
	bld	r1, TIF_NEED_RESCHED
	brcc	1f
	lddsp	r4, sp[REG_SR]
	bld	r4, SYSREG_GM_OFFSET
	brcs	1f
	rcall	preempt_schedule_irq
1:
#endif

	popm	r8-r9
	mask_exceptions
	mfsr	r1, SYSREG_SR
	mtsr	SYSREG_RAR_EX, r8
	mtsr	SYSREG_RSR_EX, r9
	popm	lr
	sub	sp, -4		/* ignore SP */
	popm	r0-r12
	sub	sp, -4		/* ignore r12_orig */
	rete

irq_exit_work:
	/* Switch to exception mode so that we can share the same code. */
	mfsr	r8, SYSREG_SR
	cbr	r8, SYSREG_M0_OFFSET
	orh	r8, hi(SYSREG_BIT(M1) | SYSREG_BIT(M2))
	mtsr	SYSREG_SR, r8
	sub	pc, -2
	get_thread_info r0
	ld.w	r1, r0[TI_flags]

fault_exit_work:
	bld	r1, TIF_NEED_RESCHED
	brcc	1f
	unmask_interrupts
	rcall	schedule
	mask_interrupts
	ld.w	r1, r0[TI_flags]
	rjmp	fault_exit_work

1:	mov	r2, _TIF_SIGPENDING | _TIF_RESTORE_SIGMASK
	tst	r1, r2
	breq	2f
	unmask_interrupts
	mov	r12, sp
	mov	r11, r0
	rcall	do_notify_resume
	mask_interrupts
	ld.w	r1, r0[TI_flags]
	rjmp	fault_exit_work

2:	bld	r1, TIF_BREAKPOINT
	brcc	fault_resume_user
	rjmp	enter_monitor_mode

	.section .kprobes.text, "ax", @progbits
	.type	handle_debug, @function
handle_debug:
	sub	sp, 4		/* r12_orig */
	stmts	--sp, r0-lr
	mfsr	r8, SYSREG_RAR_DBG
	mfsr	r9, SYSREG_RSR_DBG
	unmask_exceptions
	pushm	r8-r9
	bfextu	r9, r9, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
	brne	debug_fixup_regs

.Ldebug_fixup_cont:
#ifdef CONFIG_TRACE_IRQFLAGS
	rcall	trace_hardirqs_off
#endif
	mov	r12, sp
	rcall	do_debug
	mov	sp, r12

	lddsp	r2, sp[REG_SR]
	bfextu	r3, r2, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
	brne	debug_resume_kernel

	get_thread_info r0
	ld.w	r1, r0[TI_flags]
	mov	r2, _TIF_DBGWORK_MASK
	tst	r1, r2
	brne	debug_exit_work

	bld	r1, TIF_SINGLE_STEP
	brcc	1f
	mfdr	r4, OCD_DC
	sbr	r4, OCD_DC_SS_BIT
	mtdr	OCD_DC, r4

1:	popm	r10,r11
	mask_exceptions
	mtsr	SYSREG_RSR_DBG, r11
	mtsr	SYSREG_RAR_DBG, r10
#ifdef CONFIG_TRACE_IRQFLAGS
	rcall	trace_hardirqs_on
1:
#endif
	ldmts	sp++, r0-lr
	sub	sp, -4
	retd
	.size	handle_debug, . - handle_debug

	/* Mode of the trapped context is in r9 */
	.type	debug_fixup_regs, @function
debug_fixup_regs:
	mfsr	r8, SYSREG_SR
	mov	r10, r8
	bfins	r8, r9, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
	mtsr	SYSREG_SR, r8
	sub	pc, -2
	stdsp	sp[REG_LR], lr
	mtsr	SYSREG_SR, r10
	sub	pc, -2
	sub	r8, sp, -FRAME_SIZE_FULL
	stdsp	sp[REG_SP], r8
	rjmp	.Ldebug_fixup_cont
	.size	debug_fixup_regs, . - debug_fixup_regs

	.type	debug_resume_kernel, @function
debug_resume_kernel:
	mask_exceptions
	popm	r10, r11
	mtsr	SYSREG_RAR_DBG, r10
	mtsr	SYSREG_RSR_DBG, r11
#ifdef CONFIG_TRACE_IRQFLAGS
	bld	r11, SYSREG_GM_OFFSET
	brcc	1f
	rcall	trace_hardirqs_on
1:
#endif
	mfsr	r2, SYSREG_SR
	mov	r1, r2
	bfins	r2, r3, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
	mtsr	SYSREG_SR, r2
	sub	pc, -2
	popm	lr
	mtsr	SYSREG_SR, r1
	sub	pc, -2
	sub	sp, -4		/* skip SP */
	popm	r0-r12
	sub	sp, -4
	retd
	.size	debug_resume_kernel, . - debug_resume_kernel

	.type	debug_exit_work, @function
debug_exit_work:
	/*
	 * We must return from Monitor Mode using a retd, and we must
	 * not schedule since that involves the D bit in SR getting
	 * cleared by something other than the debug hardware. This
	 * may cause undefined behaviour according to the Architecture
	 * manual.
	 *
	 * So we fix up the return address and status and return to a
	 * stub below in Exception mode. From there, we can follow the
	 * normal exception return path.
	 *
	 * The real return address and status registers are stored on
	 * the stack in the way the exception return path understands,
	 * so no need to fix anything up there.
	 */
	sub	r8, pc, . - fault_exit_work
	mtsr	SYSREG_RAR_DBG, r8
	mov	r9, 0
	orh	r9, hi(SR_EM | SR_GM | MODE_EXCEPTION)
	mtsr	SYSREG_RSR_DBG, r9
	sub	pc, -2
	retd
	.size	debug_exit_work, . - debug_exit_work

	.set	rsr_int0,	SYSREG_RSR_INT0
	.set	rsr_int1,	SYSREG_RSR_INT1
	.set	rsr_int2,	SYSREG_RSR_INT2
	.set	rsr_int3,	SYSREG_RSR_INT3
	.set	rar_int0,	SYSREG_RAR_INT0
	.set	rar_int1,	SYSREG_RAR_INT1
	.set	rar_int2,	SYSREG_RAR_INT2
	.set	rar_int3,	SYSREG_RAR_INT3

	.macro	IRQ_LEVEL level
	.type	irq_level\level, @function
irq_level\level:
	sub	sp, 4		/* r12_orig */
	stmts	--sp,r0-lr
	mfsr	r8, rar_int\level
	mfsr	r9, rsr_int\level

#ifdef CONFIG_PREEMPT
	sub	r11, pc, (. - system_call)
	cp.w	r11, r8
	breq	4f
#endif

	pushm	r8-r9

	mov	r11, sp
	mov	r12, \level

	rcall	do_IRQ

	lddsp	r4, sp[REG_SR]
	bfextu	r4, r4, SYSREG_M0_OFFSET, 3
	cp.w	r4, MODE_SUPERVISOR >> SYSREG_M0_OFFSET
	breq	2f
	cp.w	r4, MODE_USER >> SYSREG_M0_OFFSET
#ifdef CONFIG_PREEMPT
	brne	3f
#else
	brne	1f
#endif

	get_thread_info	r0
	ld.w	r1, r0[TI_flags]
	andl	r1, _TIF_WORK_MASK, COH
	brne	irq_exit_work

1:
#ifdef CONFIG_TRACE_IRQFLAGS
	rcall	trace_hardirqs_on
#endif
	popm	r8-r9
	mtsr	rar_int\level, r8
	mtsr	rsr_int\level, r9
	ldmts	sp++,r0-lr
	sub	sp, -4		/* ignore r12_orig */
	rete

#ifdef CONFIG_PREEMPT
4:	mask_interrupts
	mfsr	r8, rsr_int\level
	sbr	r8, 16
	mtsr	rsr_int\level, r8
	ldmts	sp++, r0-lr
	sub	sp, -4		/* ignore r12_orig */
	rete
#endif

2:	get_thread_info	r0
	ld.w	r1, r0[TI_flags]
	bld	r1, TIF_CPU_GOING_TO_SLEEP
#ifdef CONFIG_PREEMPT
	brcc	3f
#else
	brcc	1b
#endif
	sub	r1, pc, . - cpu_idle_skip_sleep
	stdsp	sp[REG_PC], r1
#ifdef CONFIG_PREEMPT
3:	get_thread_info r0
	ld.w	r2, r0[TI_preempt_count]
	cp.w	r2, 0
	brne	1b
	ld.w	r1, r0[TI_flags]
	bld	r1, TIF_NEED_RESCHED
	brcc	1b
	lddsp	r4, sp[REG_SR]
	bld	r4, SYSREG_GM_OFFSET
	brcs	1b
	rcall	preempt_schedule_irq
#endif
	rjmp	1b
	.endm

	.section .irq.text,"ax",@progbits

	.global	irq_level0
	.global	irq_level1
	.global	irq_level2
	.global	irq_level3
	IRQ_LEVEL 0
	IRQ_LEVEL 1
	IRQ_LEVEL 2
	IRQ_LEVEL 3

	.section .kprobes.text, "ax", @progbits
	.type	enter_monitor_mode, @function
enter_monitor_mode:
	/*
	 * We need to enter monitor mode to do a single step. The
	 * monitor code will alter the return address so that we
	 * return directly to the user instead of returning here.
	 */
	breakpoint
	rjmp	breakpoint_failed

	.size	enter_monitor_mode, . - enter_monitor_mode

	.type	debug_trampoline, @function
	.global	debug_trampoline
debug_trampoline:
	/*
	 * Save the registers on the stack so that the monitor code
	 * can find them easily.
	 */
	sub	sp, 4		/* r12_orig */
	stmts	--sp, r0-lr
	get_thread_info	r0
	ld.w	r8, r0[TI_rar_saved]
	ld.w	r9, r0[TI_rsr_saved]
	pushm	r8-r9

	/*
	 * The monitor code will alter the return address so we don't
	 * return here.
	 */
	breakpoint
	rjmp	breakpoint_failed
	.size	debug_trampoline, . - debug_trampoline

	.type breakpoint_failed, @function
breakpoint_failed:
	/*
	 * Something went wrong. Perhaps the debug hardware isn't
	 * enabled?
	 */
	lda.w	r12, msg_breakpoint_failed
	mov	r11, sp
	mov	r10, 9		/* SIGKILL */
	call	die
1:	rjmp	1b

msg_breakpoint_failed:
	.asciz	"Failed to enter Debug Mode"
Commit	Line	Data
5f97f7f9 HS	1	/*
	2	* Copyright (C) 2004-2006 Atmel Corporation
	3	*
	4	* This program is free software; you can redistribute it and/or modify
	5	* it under the terms of the GNU General Public License version 2 as
	6	* published by the Free Software Foundation.
	7	*/
	8
	9	/*
	10	* This file contains the low-level entry-points into the kernel, that is,
	11	* exception handlers, debug trap handlers, interrupt handlers and the
	12	* system call handler.
	13	*/
	14	#include <linux/errno.h>
	15
	16	#include <asm/asm.h>
	17	#include <asm/hardirq.h>
	18	#include <asm/irq.h>
	19	#include <asm/ocd.h>
	20	#include <asm/page.h>
	21	#include <asm/pgtable.h>
	22	#include <asm/ptrace.h>
	23	#include <asm/sysreg.h>
	24	#include <asm/thread_info.h>
	25	#include <asm/unistd.h>
	26
	27	#ifdef CONFIG_PREEMPT
	28	# define preempt_stop mask_interrupts
	29	#else
	30	# define preempt_stop
	31	# define fault_resume_kernel fault_restore_all
	32	#endif
	33
	34	#define __MASK(x) ((1 << (x)) - 1)
	35	#define IRQ_MASK ((__MASK(SOFTIRQ_BITS) << SOFTIRQ_SHIFT) \| \
	36	(__MASK(HARDIRQ_BITS) << HARDIRQ_SHIFT))
	37
	38	.section .ex.text,"ax",@progbits
	39	.align 2
	40	exception_vectors:
	41	bral handle_critical
	42	.align 2
	43	bral handle_critical
	44	.align 2
	45	bral do_bus_error_write
	46	.align 2
	47	bral do_bus_error_read
	48	.align 2
	49	bral do_nmi_ll
	50	.align 2
	51	bral handle_address_fault
	52	.align 2
	53	bral handle_protection_fault
	54	.align 2
	55	bral handle_debug
	56	.align 2
	57	bral do_illegal_opcode_ll
	58	.align 2
	59	bral do_illegal_opcode_ll
	60	.align 2
	61	bral do_illegal_opcode_ll
	62	.align 2
	63	bral do_fpe_ll
	64	.align 2
65	bral do_illegal_opcode_ll
66	.align 2
67	bral handle_address_fault
68	.align 2
69	bral handle_address_fault
70	.align 2
71	bral handle_protection_fault
72	.align 2
73	bral handle_protection_fault
74	.align 2
75	bral do_dtlb_modified
76
77	/*
78	* r0 : PGD/PT/PTE
79	* r1 : Offending address
80	* r2 : Scratch register
81	* r3 : Cause (5, 12 or 13)
82	*/
83	#define tlbmiss_save pushm r0-r3
84	#define tlbmiss_restore popm r0-r3
85
86	.section .tlbx.ex.text,"ax",@progbits
87	.global itlb_miss
88	itlb_miss:
89	tlbmiss_save
90	rjmp tlb_miss_common
91
92	.section .tlbr.ex.text,"ax",@progbits
93	dtlb_miss_read:
94	tlbmiss_save
95	rjmp tlb_miss_common
96
97	.section .tlbw.ex.text,"ax",@progbits
98	dtlb_miss_write:
99	tlbmiss_save
100
101	.global tlb_miss_common
102	tlb_miss_common:
c0c3e816 HS	103	mfsr r0, SYSREG_TLBEAR
c0c3e816 HS	104	mfsr r1, SYSREG_PTBR
5f97f7f9 HS	105
5f97f7f9 HS	106	/* Is it the vmalloc space? */
c0c3e816	107	bld r0, 31
5f97f7f9 HS	108	brcs handle_vmalloc_miss
	109
	110	/* First level lookup */
	111	pgtbl_lookup:
c0c3e816 HS	112	lsr r2, r0, PGDIR_SHIFT
	113	ld.w r3, r1[r2 << 2]
	114	bfextu r1, r0, PAGE_SHIFT, PGDIR_SHIFT - PAGE_SHIFT
	115	bld r3, _PAGE_BIT_PRESENT
5f97f7f9 HS	116	brcc page_table_not_present
5f97f7f9 HS	117
5f97f7f9	118	/* Translate to virtual address in P1. */
c0c3e816 HS	119	andl r3, 0xf000
c0c3e816 HS	120	sbr r3, 31
5f97f7f9 HS	121
5f97f7f9 HS	122	/* Second level lookup */
c0c3e816 HS	123	ld.w r2, r3[r1 << 2]
	124	mfsr r0, SYSREG_TLBARLO
	125	bld r2, _PAGE_BIT_PRESENT
5f97f7f9 HS	126	brcc page_not_present
	127
	128	/* Mark the page as accessed */
c0c3e816 HS	129	sbr r2, _PAGE_BIT_ACCESSED
c0c3e816 HS	130	st.w r3[r1 << 2], r2
5f97f7f9 HS	131
5f97f7f9 HS	132	/* Drop software flags */
c0c3e816 HS	133	andl r2, _PAGE_FLAGS_HARDWARE_MASK & 0xffff
c0c3e816 HS	134	mtsr SYSREG_TLBELO, r2
5f97f7f9 HS	135
5f97f7f9 HS	136	/* Figure out which entry we want to replace */
c0c3e816	137	mfsr r1, SYSREG_MMUCR
5f97f7f9 HS	138	clz r2, r0
5f97f7f9 HS	139	brcc 1f
c0c3e816 HS	140	mov r3, -1 /* All entries have been accessed, */
	141	mov r2, 0 /* so start at 0 */
	142	mtsr SYSREG_TLBARLO, r3 /* and reset TLBAR */
5f97f7f9	143
c0c3e816 HS	144	1: bfins r1, r2, SYSREG_DRP_OFFSET, SYSREG_DRP_SIZE
c0c3e816 HS	145	mtsr SYSREG_MMUCR, r1
5f97f7f9 HS	146	tlbw
	147
	148	tlbmiss_restore
	149	rete
	150
	151	handle_vmalloc_miss:
	152	/* Simply do the lookup in init's page table */
c0c3e816 HS	153	mov r1, lo(swapper_pg_dir)
c0c3e816 HS	154	orh r1, hi(swapper_pg_dir)
5f97f7f9 HS	155	rjmp pgtbl_lookup
	156
	157
	158	/* --- System Call --- */
	159
	160	.section .scall.text,"ax",@progbits
	161	system_call:
a7e30b8d PR	162	#ifdef CONFIG_PREEMPT
	163	mask_interrupts
	164	#endif
5f97f7f9 HS	165	pushm r12 /* r12_orig */
5f97f7f9 HS	166	stmts --sp, r0-lr
a7e30b8d	167
5f97f7f9 HS	168	mfsr r0, SYSREG_RAR_SUP
5f97f7f9 HS	169	mfsr r1, SYSREG_RSR_SUP
a7e30b8d PR	170	#ifdef CONFIG_PREEMPT
	171	unmask_interrupts
	172	#endif
	173	zero_fp
5f97f7f9 HS	174	stm --sp, r0-r1
	175
	176	/* check for syscall tracing */
	177	get_thread_info r0
	178	ld.w r1, r0[TI_flags]
	179	bld r1, TIF_SYSCALL_TRACE
	180	brcs syscall_trace_enter
	181
	182	syscall_trace_cont:
	183	cp.w r8, NR_syscalls
	184	brhs syscall_badsys
	185
	186	lddpc lr, syscall_table_addr
	187	ld.w lr, lr[r8 << 2]
	188	mov r8, r5 /* 5th argument (6th is pushed by stub) */
	189	icall lr
	190
	191	.global syscall_return
	192	syscall_return:
	193	get_thread_info r0
	194	mask_interrupts /* make sure we don't miss an interrupt
	195	setting need_resched or sigpending
	196	between sampling and the rets */
	197
	198	/* Store the return value so that the correct value is loaded below */
	199	stdsp sp[REG_R12], r12
	200
	201	ld.w r1, r0[TI_flags]
	202	andl r1, _TIF_ALLWORK_MASK, COH
	203	brne syscall_exit_work
	204
	205	syscall_exit_cont:
	206	popm r8-r9
	207	mtsr SYSREG_RAR_SUP, r8
	208	mtsr SYSREG_RSR_SUP, r9
	209	ldmts sp++, r0-lr
	210	sub sp, -4 /* r12_orig */
	211	rets
	212
	213	.align 2
	214	syscall_table_addr:
	215	.long sys_call_table
	216
	217	syscall_badsys:
	218	mov r12, -ENOSYS
	219	rjmp syscall_return
	220
	221	.global ret_from_fork
	222	ret_from_fork:
	223	rcall schedule_tail
	224
	225	/* check for syscall tracing */
	226	get_thread_info r0
	227	ld.w r1, r0[TI_flags]
	228	andl r1, _TIF_ALLWORK_MASK, COH
	229	brne syscall_exit_work
	230	rjmp syscall_exit_cont
	231
	232	syscall_trace_enter:
	233	pushm r8-r12
	234	rcall syscall_trace
	235	popm r8-r12
	236	rjmp syscall_trace_cont
	237
238	syscall_exit_work:
239	bld r1, TIF_SYSCALL_TRACE
240	brcc 1f
241	unmask_interrupts
242	rcall syscall_trace
243	mask_interrupts
244	ld.w r1, r0[TI_flags]
245
246	1: bld r1, TIF_NEED_RESCHED
247	brcc 2f
248	unmask_interrupts
249	rcall schedule
250	mask_interrupts
251	ld.w r1, r0[TI_flags]
252	rjmp 1b
253
254	2: mov r2, _TIF_SIGPENDING \| _TIF_RESTORE_SIGMASK
255	tst r1, r2
256	breq 3f
257	unmask_interrupts
258	mov r12, sp
259	mov r11, r0
260	rcall do_notify_resume
261	mask_interrupts
262	ld.w r1, r0[TI_flags]
263	rjmp 1b
264
265	3: bld r1, TIF_BREAKPOINT
266	brcc syscall_exit_cont
2507bc13	267	rjmp enter_monitor_mode
5f97f7f9 HS	268
	269	/* The slow path of the TLB miss handler */
	270	page_table_not_present:
	271	page_not_present:
	272	tlbmiss_restore
	273	sub sp, 4
	274	stmts --sp, r0-lr
	275	rcall save_full_context_ex
	276	mfsr r12, SYSREG_ECR
	277	mov r11, sp
	278	rcall do_page_fault
	279	rjmp ret_from_exception
	280
	281	/* This function expects to find offending PC in SYSREG_RAR_EX */
2507bc13 HS	282	.type save_full_context_ex, @function
2507bc13 HS	283	.align 2
5f97f7f9	284	save_full_context_ex:
2507bc13 HS	285	mfsr r11, SYSREG_RAR_EX
2507bc13 HS	286	sub r9, pc, . - debug_trampoline
5f97f7f9	287	mfsr r8, SYSREG_RSR_EX
2507bc13 HS	288	cp.w r9, r11
2507bc13 HS	289	breq 3f
5f97f7f9 HS	290	mov r12, r8
5f97f7f9 HS	291	andh r8, (MODE_MASK >> 16), COH
5f97f7f9 HS	292	brne 2f
	293
	294	1: pushm r11, r12 /* PC and SR */
	295	unmask_exceptions
	296	ret r12
	297
	298	2: sub r10, sp, -(FRAME_SIZE_FULL - REG_LR)
	299	stdsp sp[4], r10 /* replace saved SP */
	300	rjmp 1b
	301
2507bc13 HS	302	/*
	303	* The debug handler set up a trampoline to make us
	304	* automatically enter monitor mode upon return, but since
	305	* we're saving the full context, we must assume that the
	306	* exception handler might want to alter the return address
	307	* and/or status register. So we need to restore the original
	308	* context and enter monitor mode manually after the exception
	309	* has been handled.
	310	*/
	311	3: get_thread_info r8
	312	ld.w r11, r8[TI_rar_saved]
	313	ld.w r12, r8[TI_rsr_saved]
	314	rjmp 1b
	315	.size save_full_context_ex, . - save_full_context_ex
	316
5f97f7f9 HS	317	/* Low-level exception handlers */
5f97f7f9 HS	318	handle_critical:
5998a3cf HS	319	sub sp, 4
5998a3cf HS	320	stmts --sp, r0-lr
5f97f7f9 HS	321	rcall save_full_context_ex
	322	mfsr r12, SYSREG_ECR
	323	mov r11, sp
	324	rcall do_critical_exception
	325
	326	/* We should never get here... */
	327	bad_return:
	328	sub r12, pc, (. - 1f)
	329	bral panic
	330	.align 2
	331	1: .asciz "Return from critical exception!"
	332
	333	.align 1
	334	do_bus_error_write:
	335	sub sp, 4
	336	stmts --sp, r0-lr
	337	rcall save_full_context_ex
	338	mov r11, 1
	339	rjmp 1f
	340
	341	do_bus_error_read:
	342	sub sp, 4
	343	stmts --sp, r0-lr
	344	rcall save_full_context_ex
	345	mov r11, 0
	346	1: mfsr r12, SYSREG_BEAR
	347	mov r10, sp
	348	rcall do_bus_error
	349	rjmp ret_from_exception
	350
	351	.align 1
	352	do_nmi_ll:
	353	sub sp, 4
	354	stmts --sp, r0-lr
92b728c1 HS	355	mfsr r9, SYSREG_RSR_NMI
	356	mfsr r8, SYSREG_RAR_NMI
	357	bfextu r0, r9, MODE_SHIFT, 3
	358	brne 2f
	359
	360	1: pushm r8, r9 /* PC and SR */
5f97f7f9 HS	361	mfsr r12, SYSREG_ECR
	362	mov r11, sp
	363	rcall do_nmi
92b728c1 HS	364	popm r8-r9
	365	mtsr SYSREG_RAR_NMI, r8
	366	tst r0, r0
	367	mtsr SYSREG_RSR_NMI, r9
	368	brne 3f
	369
	370	ldmts sp++, r0-lr
	371	sub sp, -4 /* skip r12_orig */
	372	rete
	373
	374	2: sub r10, sp, -(FRAME_SIZE_FULL - REG_LR)
	375	stdsp sp[4], r10 /* replace saved SP */
	376	rjmp 1b
	377
	378	3: popm lr
	379	sub sp, -4 /* skip sp */
	380	popm r0-r12
	381	sub sp, -4 /* skip r12_orig */
	382	rete
5f97f7f9 HS	383
	384	handle_address_fault:
	385	sub sp, 4
	386	stmts --sp, r0-lr
	387	rcall save_full_context_ex
	388	mfsr r12, SYSREG_ECR
	389	mov r11, sp
	390	rcall do_address_exception
	391	rjmp ret_from_exception
	392
	393	handle_protection_fault:
	394	sub sp, 4
	395	stmts --sp, r0-lr
	396	rcall save_full_context_ex
	397	mfsr r12, SYSREG_ECR
	398	mov r11, sp
	399	rcall do_page_fault
	400	rjmp ret_from_exception
	401
	402	.align 1
	403	do_illegal_opcode_ll:
	404	sub sp, 4
	405	stmts --sp, r0-lr
	406	rcall save_full_context_ex
	407	mfsr r12, SYSREG_ECR
	408	mov r11, sp
	409	rcall do_illegal_opcode
	410	rjmp ret_from_exception
	411
	412	do_dtlb_modified:
	413	pushm r0-r3
	414	mfsr r1, SYSREG_TLBEAR
	415	mfsr r0, SYSREG_PTBR
	416	lsr r2, r1, PGDIR_SHIFT
	417	ld.w r0, r0[r2 << 2]
	418	lsl r1, (32 - PGDIR_SHIFT)
	419	lsr r1, (32 - PGDIR_SHIFT) + PAGE_SHIFT
	420
	421	/* Translate to virtual address in P1 */
	422	andl r0, 0xf000
	423	sbr r0, 31
	424	add r2, r0, r1 << 2
	425	ld.w r3, r2[0]
	426	sbr r3, _PAGE_BIT_DIRTY
	427	mov r0, r3
	428	st.w r2[0], r3
	429
	430	/* The page table is up-to-date. Update the TLB entry as well */
	431	andl r0, lo(_PAGE_FLAGS_HARDWARE_MASK)
	432	mtsr SYSREG_TLBELO, r0
	433
	434	/* MMUCR[DRP] is updated automatically, so let's go... */
	435	tlbw
	436
	437	popm r0-r3
	438	rete
	439
	440	do_fpe_ll:
	441	sub sp, 4
	442	stmts --sp, r0-lr
	443	rcall save_full_context_ex
	444	unmask_interrupts
	445	mov r12, 26
	446	mov r11, sp
447	rcall do_fpe
448	rjmp ret_from_exception
449
450	ret_from_exception:
451	mask_interrupts
452	lddsp r4, sp[REG_SR]
2507bc13	453
5f97f7f9 HS	454	andh r4, (MODE_MASK >> 16), COH
	455	brne fault_resume_kernel
	456
	457	get_thread_info r0
	458	ld.w r1, r0[TI_flags]
	459	andl r1, _TIF_WORK_MASK, COH
	460	brne fault_exit_work
	461
	462	fault_resume_user:
	463	popm r8-r9
	464	mask_exceptions
	465	mtsr SYSREG_RAR_EX, r8
	466	mtsr SYSREG_RSR_EX, r9
	467	ldmts sp++, r0-lr
	468	sub sp, -4
	469	rete
	470
	471	fault_resume_kernel:
	472	#ifdef CONFIG_PREEMPT
	473	get_thread_info r0
	474	ld.w r2, r0[TI_preempt_count]
	475	cp.w r2, 0
	476	brne 1f
	477	ld.w r1, r0[TI_flags]
	478	bld r1, TIF_NEED_RESCHED
	479	brcc 1f
	480	lddsp r4, sp[REG_SR]
	481	bld r4, SYSREG_GM_OFFSET
	482	brcs 1f
	483	rcall preempt_schedule_irq
	484	1:
	485	#endif
	486
	487	popm r8-r9
	488	mask_exceptions
	489	mfsr r1, SYSREG_SR
	490	mtsr SYSREG_RAR_EX, r8
	491	mtsr SYSREG_RSR_EX, r9
	492	popm lr
	493	sub sp, -4 /* ignore SP */
	494	popm r0-r12
	495	sub sp, -4 /* ignore r12_orig */
	496	rete
	497
	498	irq_exit_work:
	499	/* Switch to exception mode so that we can share the same code. */
	500	mfsr r8, SYSREG_SR
	501	cbr r8, SYSREG_M0_OFFSET
	502	orh r8, hi(SYSREG_BIT(M1) \| SYSREG_BIT(M2))
	503	mtsr SYSREG_SR, r8
	504	sub pc, -2
	505	get_thread_info r0
	506	ld.w r1, r0[TI_flags]
	507
	508	fault_exit_work:
	509	bld r1, TIF_NEED_RESCHED
	510	brcc 1f
	511	unmask_interrupts
	512	rcall schedule
	513	mask_interrupts
	514	ld.w r1, r0[TI_flags]
	515	rjmp fault_exit_work
	516
	517	1: mov r2, _TIF_SIGPENDING \| _TIF_RESTORE_SIGMASK
518	tst r1, r2
519	breq 2f
520	unmask_interrupts
521	mov r12, sp
522	mov r11, r0
523	rcall do_notify_resume
524	mask_interrupts
525	ld.w r1, r0[TI_flags]
526	rjmp fault_exit_work
527
528	2: bld r1, TIF_BREAKPOINT
529	brcc fault_resume_user
2507bc13 HS	530	rjmp enter_monitor_mode
	531
	532	.section .kprobes.text, "ax", @progbits
	533	.type handle_debug, @function
	534	handle_debug:
	535	sub sp, 4 /* r12_orig */
	536	stmts --sp, r0-lr
	537	mfsr r8, SYSREG_RAR_DBG
	538	mfsr r9, SYSREG_RSR_DBG
	539	unmask_exceptions
	540	pushm r8-r9
	541	bfextu r9, r9, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
	542	brne debug_fixup_regs
	543
	544	.Ldebug_fixup_cont:
	545	#ifdef CONFIG_TRACE_IRQFLAGS
	546	rcall trace_hardirqs_off
	547	#endif
	548	mov r12, sp
	549	rcall do_debug
	550	mov sp, r12
	551
	552	lddsp r2, sp[REG_SR]
	553	bfextu r3, r2, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
	554	brne debug_resume_kernel
	555
	556	get_thread_info r0
	557	ld.w r1, r0[TI_flags]
	558	mov r2, _TIF_DBGWORK_MASK
	559	tst r1, r2
	560	brne debug_exit_work
	561
	562	bld r1, TIF_SINGLE_STEP
	563	brcc 1f
	564	mfdr r4, OCD_DC
	565	sbr r4, OCD_DC_SS_BIT
	566	mtdr OCD_DC, r4
	567
	568	1: popm r10,r11
	569	mask_exceptions
	570	mtsr SYSREG_RSR_DBG, r11
	571	mtsr SYSREG_RAR_DBG, r10
	572	#ifdef CONFIG_TRACE_IRQFLAGS
	573	rcall trace_hardirqs_on
	574	1:
	575	#endif
	576	ldmts sp++, r0-lr
	577	sub sp, -4
	578	retd
	579	.size handle_debug, . - handle_debug
	580
	581	/* Mode of the trapped context is in r9 */
	582	.type debug_fixup_regs, @function
	583	debug_fixup_regs:
	584	mfsr r8, SYSREG_SR
	585	mov r10, r8
	586	bfins r8, r9, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
	587	mtsr SYSREG_SR, r8
5f97f7f9 HS	588	sub pc, -2
5f97f7f9 HS	589	stdsp sp[REG_LR], lr
2507bc13	590	mtsr SYSREG_SR, r10
5f97f7f9	591	sub pc, -2
2507bc13 HS	592	sub r8, sp, -FRAME_SIZE_FULL
	593	stdsp sp[REG_SP], r8
	594	rjmp .Ldebug_fixup_cont
	595	.size debug_fixup_regs, . - debug_fixup_regs
5f97f7f9	596
2507bc13 HS	597	.type debug_resume_kernel, @function
	598	debug_resume_kernel:
	599	mask_exceptions
5f97f7f9 HS	600	popm r10, r11
	601	mtsr SYSREG_RAR_DBG, r10
	602	mtsr SYSREG_RSR_DBG, r11
320516b7 HS	603	#ifdef CONFIG_TRACE_IRQFLAGS
	604	bld r11, SYSREG_GM_OFFSET
	605	brcc 1f
	606	rcall trace_hardirqs_on
	607	1:
	608	#endif
	609	mfsr r2, SYSREG_SR
2507bc13 HS	610	mov r1, r2
2507bc13 HS	611	bfins r2, r3, SYSREG_MODE_OFFSET, SYSREG_MODE_SIZE
320516b7	612	mtsr SYSREG_SR, r2
5f97f7f9 HS	613	sub pc, -2
5f97f7f9 HS	614	popm lr
2507bc13	615	mtsr SYSREG_SR, r1
5f97f7f9 HS	616	sub pc, -2
	617	sub sp, -4 /* skip SP */
	618	popm r0-r12
	619	sub sp, -4
	620	retd
2507bc13	621	.size debug_resume_kernel, . - debug_resume_kernel
5f97f7f9	622
2507bc13 HS	623	.type debug_exit_work, @function
2507bc13 HS	624	debug_exit_work:
5f97f7f9	625	/*
2507bc13 HS	626	* We must return from Monitor Mode using a retd, and we must
	627	* not schedule since that involves the D bit in SR getting
	628	* cleared by something other than the debug hardware. This
	629	* may cause undefined behaviour according to the Architecture
	630	* manual.
	631	*
	632	* So we fix up the return address and status and return to a
	633	* stub below in Exception mode. From there, we can follow the
	634	* normal exception return path.
	635	*
	636	* The real return address and status registers are stored on
	637	* the stack in the way the exception return path understands,
	638	* so no need to fix anything up there.
5f97f7f9	639	*/
2507bc13 HS	640	sub r8, pc, . - fault_exit_work
	641	mtsr SYSREG_RAR_DBG, r8
	642	mov r9, 0
	643	orh r9, hi(SR_EM \| SR_GM \| MODE_EXCEPTION)
	644	mtsr SYSREG_RSR_DBG, r9
	645	sub pc, -2
5f97f7f9	646	retd
2507bc13	647	.size debug_exit_work, . - debug_exit_work
5f97f7f9 HS	648
	649	.set rsr_int0, SYSREG_RSR_INT0
	650	.set rsr_int1, SYSREG_RSR_INT1
	651	.set rsr_int2, SYSREG_RSR_INT2
	652	.set rsr_int3, SYSREG_RSR_INT3
	653	.set rar_int0, SYSREG_RAR_INT0
	654	.set rar_int1, SYSREG_RAR_INT1
	655	.set rar_int2, SYSREG_RAR_INT2
	656	.set rar_int3, SYSREG_RAR_INT3
	657
	658	.macro IRQ_LEVEL level
	659	.type irq_level\level, @function
	660	irq_level\level:
	661	sub sp, 4 /* r12_orig */
	662	stmts --sp,r0-lr
	663	mfsr r8, rar_int\level
	664	mfsr r9, rsr_int\level
a7e30b8d PR	665
	666	#ifdef CONFIG_PREEMPT
	667	sub r11, pc, (. - system_call)
	668	cp.w r11, r8
	669	breq 4f
	670	#endif
	671
5f97f7f9 HS	672	pushm r8-r9
	673
	674	mov r11, sp
	675	mov r12, \level
	676
	677	rcall do_IRQ
	678
	679	lddsp r4, sp[REG_SR]
19b7ce8b HCE	680	bfextu r4, r4, SYSREG_M0_OFFSET, 3
	681	cp.w r4, MODE_SUPERVISOR >> SYSREG_M0_OFFSET
	682	breq 2f
	683	cp.w r4, MODE_USER >> SYSREG_M0_OFFSET
5f97f7f9	684	#ifdef CONFIG_PREEMPT
19b7ce8b	685	brne 3f
5f97f7f9 HS	686	#else
	687	brne 1f
	688	#endif
	689
	690	get_thread_info r0
	691	ld.w r1, r0[TI_flags]
	692	andl r1, _TIF_WORK_MASK, COH
	693	brne irq_exit_work
	694
320516b7 HS	695	1:
	696	#ifdef CONFIG_TRACE_IRQFLAGS
	697	rcall trace_hardirqs_on
	698	#endif
	699	popm r8-r9
5f97f7f9 HS	700	mtsr rar_int\level, r8
	701	mtsr rsr_int\level, r9
	702	ldmts sp++,r0-lr
	703	sub sp, -4 /* ignore r12_orig */
	704	rete
	705
a7e30b8d PR	706	#ifdef CONFIG_PREEMPT
	707	4: mask_interrupts
	708	mfsr r8, rsr_int\level
	709	sbr r8, 16
	710	mtsr rsr_int\level, r8
	711	ldmts sp++, r0-lr
	712	sub sp, -4 /* ignore r12_orig */
	713	rete
	714	#endif
	715
19b7ce8b HCE	716	2: get_thread_info r0
	717	ld.w r1, r0[TI_flags]
	718	bld r1, TIF_CPU_GOING_TO_SLEEP
5f97f7f9	719	#ifdef CONFIG_PREEMPT
19b7ce8b HCE	720	brcc 3f
	721	#else
	722	brcc 1b
	723	#endif
	724	sub r1, pc, . - cpu_idle_skip_sleep
	725	stdsp sp[REG_PC], r1
	726	#ifdef CONFIG_PREEMPT
	727	3: get_thread_info r0
5f97f7f9 HS	728	ld.w r2, r0[TI_preempt_count]
	729	cp.w r2, 0
	730	brne 1b
	731	ld.w r1, r0[TI_flags]
	732	bld r1, TIF_NEED_RESCHED
	733	brcc 1b
	734	lddsp r4, sp[REG_SR]
	735	bld r4, SYSREG_GM_OFFSET
	736	brcs 1b
	737	rcall preempt_schedule_irq
5f97f7f9	738	#endif
19b7ce8b	739	rjmp 1b
5f97f7f9 HS	740	.endm
	741
	742	.section .irq.text,"ax",@progbits
	743
	744	.global irq_level0
	745	.global irq_level1
	746	.global irq_level2
	747	.global irq_level3
	748	IRQ_LEVEL 0
	749	IRQ_LEVEL 1
	750	IRQ_LEVEL 2
	751	IRQ_LEVEL 3
2507bc13 HS	752
	753	.section .kprobes.text, "ax", @progbits
	754	.type enter_monitor_mode, @function
	755	enter_monitor_mode:
	756	/*
	757	* We need to enter monitor mode to do a single step. The
	758	* monitor code will alter the return address so that we
	759	* return directly to the user instead of returning here.
	760	*/
	761	breakpoint
	762	rjmp breakpoint_failed
	763
	764	.size enter_monitor_mode, . - enter_monitor_mode
	765
	766	.type debug_trampoline, @function
	767	.global debug_trampoline
	768	debug_trampoline:
	769	/*
	770	* Save the registers on the stack so that the monitor code
	771	* can find them easily.
	772	*/
	773	sub sp, 4 /* r12_orig */
	774	stmts --sp, r0-lr
	775	get_thread_info r0
	776	ld.w r8, r0[TI_rar_saved]
	777	ld.w r9, r0[TI_rsr_saved]
	778	pushm r8-r9
	779
	780	/*
	781	* The monitor code will alter the return address so we don't
	782	* return here.
	783	*/
	784	breakpoint
	785	rjmp breakpoint_failed
	786	.size debug_trampoline, . - debug_trampoline
	787
	788	.type breakpoint_failed, @function
	789	breakpoint_failed:
	790	/*
	791	* Something went wrong. Perhaps the debug hardware isn't
	792	* enabled?
	793	*/
	794	lda.w r12, msg_breakpoint_failed
	795	mov r11, sp
	796	mov r10, 9 /* SIGKILL */
	797	call die
	798	1: rjmp 1b
	799
	800	msg_breakpoint_failed:
	801	.asciz "Failed to enter Debug Mode"