.global tlb_miss_common
tlb_miss_common:
- mfsr r0, SYSREG_PTBR
- mfsr r1, SYSREG_TLBEAR
+ mfsr r0, SYSREG_TLBEAR
+ mfsr r1, SYSREG_PTBR
/* Is it the vmalloc space? */
- bld r1, 31
+ bld r0, 31
brcs handle_vmalloc_miss
/* First level lookup */
pgtbl_lookup:
- lsr r2, r1, PGDIR_SHIFT
- ld.w r0, r0[r2 << 2]
- bld r0, _PAGE_BIT_PRESENT
+ 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
- /* TODO: Check access rights on page table if necessary */
-
/* Translate to virtual address in P1. */
- andl r0, 0xf000
- sbr r0, 31
+ andl r3, 0xf000
+ sbr r3, 31
/* Second level lookup */
- lsl r1, (32 - PGDIR_SHIFT)
- lsr r1, (32 - PGDIR_SHIFT) + PAGE_SHIFT
- add r2, r0, r1 << 2
- ld.w r1, r2[0]
- bld r1, _PAGE_BIT_PRESENT
+ 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 r1, _PAGE_BIT_ACCESSED
- st.w r2[0], r1
+ sbr r2, _PAGE_BIT_ACCESSED
+ st.w r3[r1 << 2], r2
/* Drop software flags */
- andl r1, _PAGE_FLAGS_HARDWARE_MASK & 0xffff
- mtsr SYSREG_TLBELO, r1
+ andl r2, _PAGE_FLAGS_HARDWARE_MASK & 0xffff
+ mtsr SYSREG_TLBELO, r2
/* Figure out which entry we want to replace */
- mfsr r0, SYSREG_TLBARLO
+ mfsr r1, SYSREG_MMUCR
clz r2, r0
brcc 1f
- mov r1, -1 /* All entries have been accessed, */
- mtsr SYSREG_TLBARLO, r1 /* so reset TLBAR */
- mov r2, 0 /* and start at 0 */
-1: mfsr r1, SYSREG_MMUCR
- lsl r2, 14
- andl r1, 0x3fff, COH
- or r1, r2
- mtsr SYSREG_MMUCR, r1
+ 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
handle_vmalloc_miss:
/* Simply do the lookup in init's page table */
- mov r0, lo(swapper_pg_dir)
- orh r0, hi(swapper_pg_dir)
+ mov r1, lo(swapper_pg_dir)
+ orh r1, hi(swapper_pg_dir)
rjmp pgtbl_lookup
.section .scall.text,"ax",@progbits
system_call:
+#ifdef CONFIG_PREEMPT
+ mask_interrupts
+#endif
pushm r12 /* r12_orig */
stmts --sp, r0-lr
- zero_fp
+
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 */
3: bld r1, TIF_BREAKPOINT
brcc syscall_exit_cont
- mfsr r3, SYSREG_TLBEHI
- lddsp r2, sp[REG_PC]
- andl r3, 0xff, COH
- lsl r3, 1
- sbr r3, 30
- sbr r3, 0
- mtdr DBGREG_BWA2A, r2
- mtdr DBGREG_BWC2A, r3
- rjmp syscall_exit_cont
-
+ rjmp enter_monitor_mode
/* The slow path of the TLB miss handler */
page_table_not_present:
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
- mfsr r11, SYSREG_RAR_EX
brne 2f
1: pushm r11, r12 /* PC and SR */
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:
- pushm r12
- pushm r0-r12
+ sub sp, 4
+ stmts --sp, r0-lr
rcall save_full_context_ex
mfsr r12, SYSREG_ECR
mov r11, sp
do_nmi_ll:
sub sp, 4
stmts --sp, r0-lr
- /* FIXME: Make sure RAR_NMI and RSR_NMI are pushed instead of *_EX */
- rcall save_full_context_ex
+ 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
- rjmp bad_return
+ 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
ret_from_exception:
mask_interrupts
lddsp r4, sp[REG_SR]
+
andh r4, (MODE_MASK >> 16), COH
brne fault_resume_kernel
2: bld r1, TIF_BREAKPOINT
brcc fault_resume_user
- mfsr r3, SYSREG_TLBEHI
- lddsp r2, sp[REG_PC]
- andl r3, 0xff, COH
- lsl r3, 1
- sbr r3, 30
- sbr r3, 0
- mtdr DBGREG_BWA2A, r2
- mtdr DBGREG_BWC2A, r3
- rjmp fault_resume_user
-
- /* If we get a debug trap from privileged context we end up here */
-handle_debug_priv:
- /* Fix up LR and SP in regs. r11 contains the mode we came from */
+ 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 r9, r8
- andh r8, hi(~MODE_MASK)
- or r8, r11
+ 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, r9
+ mtsr SYSREG_SR, r10
sub pc, -2
- sub r10, sp, -FRAME_SIZE_FULL
- stdsp sp[REG_SP], r10
- mov r12, sp
- rcall do_debug_priv
+ sub r8, sp, -FRAME_SIZE_FULL
+ stdsp sp[REG_SP], r8
+ rjmp .Ldebug_fixup_cont
+ .size debug_fixup_regs, . - debug_fixup_regs
- /* Now, put everything back */
- ssrf SR_EM_BIT
+ .type debug_resume_kernel, @function
+debug_resume_kernel:
+ mask_exceptions
popm r10, r11
mtsr SYSREG_RAR_DBG, r10
mtsr SYSREG_RSR_DBG, r11
- mfsr r8, SYSREG_SR
- mov r9, r8
- andh r8, hi(~MODE_MASK)
- andh r11, hi(MODE_MASK)
- or r8, r11
- mtsr SYSREG_SR, r8
+#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, r9
+ 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:
/*
- * At this point, everything is masked, that is, interrupts,
- * exceptions and debugging traps. We might get called from
- * interrupt or exception context in some rare cases, but this
- * will be taken care of by do_debug(), so we're not going to
- * do a 100% correct context save here.
+ * 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.
*/
-handle_debug:
- sub sp, 4 /* r12_orig */
- stmts --sp, r0-lr
- mfsr r10, SYSREG_RAR_DBG
- mfsr r11, SYSREG_RSR_DBG
- unmask_exceptions
- pushm r10,r11
- andh r11, (MODE_MASK >> 16), COH
- brne handle_debug_priv
-
- mov r12, sp
- rcall do_debug
-
- lddsp r10, sp[REG_SR]
- andh r10, (MODE_MASK >> 16), COH
- breq debug_resume_user
-
-debug_restore_all:
- popm r10,r11
- mask_exceptions
- mtsr SYSREG_RSR_DBG, r11
- mtsr SYSREG_RAR_DBG, r10
- ldmts sp++, r0-lr
- sub sp, -4
+ 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
-
-debug_resume_user:
- get_thread_info r0
- mask_interrupts
-
- ld.w r1, r0[TI_flags]
- andl r1, _TIF_DBGWORK_MASK, COH
- breq debug_restore_all
-
-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_SINGLE_STEP
- brcc debug_restore_all
- mfdr r2, DBGREG_DC
- sbr r2, DC_SS_BIT
- mtdr DBGREG_DC, r2
- rjmp debug_restore_all
+ .size debug_exit_work, . - debug_exit_work
.set rsr_int0, SYSREG_RSR_INT0
.set rsr_int1, SYSREG_RSR_INT1
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
rcall do_IRQ
lddsp r4, sp[REG_SR]
- andh r4, (MODE_MASK >> 16), COH
+ 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 2f
+ brne 3f
#else
brne 1f
#endif
andl r1, _TIF_WORK_MASK, COH
brne irq_exit_work
-1: popm r8-r9
+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
rete
#ifdef CONFIG_PREEMPT
-2:
- get_thread_info r0
+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
bld r4, SYSREG_GM_OFFSET
brcs 1b
rcall preempt_schedule_irq
- rjmp 1b
#endif
+ rjmp 1b
.endm
.section .irq.text,"ax",@progbits
+.global cpu_idle_sleep
+cpu_idle_sleep:
+ mask_interrupts
+ get_thread_info r8
+ ld.w r9, r8[TI_flags]
+ bld r9, TIF_NEED_RESCHED
+ brcs cpu_idle_enable_int_and_exit
+ sbr r9, TIF_CPU_GOING_TO_SLEEP
+ st.w r8[TI_flags], r9
+ unmask_interrupts
+ sleep 0
+cpu_idle_skip_sleep:
+ mask_interrupts
+ ld.w r9, r8[TI_flags]
+ cbr r9, TIF_CPU_GOING_TO_SLEEP
+ st.w r8[TI_flags], r9
+cpu_idle_enable_int_and_exit:
+ unmask_interrupts
+ retal r12
+
.global irq_level0
.global irq_level1
.global irq_level2
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"