WAIT vs. tickless kernel

WAIT vs. tickless kernel

[Atsushi Nemoto]

On some CPUs, there is a small window in the idle task which might
cause a large latency to wakeup a process.

http://www.linux-mips.org/archives/linux-mips/2005-11/msg00114.html

This can be avoided on some CPUs which can use xxx_wait_irqoff(), but
still there are many CPUs out of luck.

And now we have dyntick/tickless kernel.  On tickless kernel the
problem might become more serious.  We cannot know the worst latency
time.  Theoretically a task can lose wakeup-event forever.

Of course "nowait" kernel option will help, but are there any other
good solutions?

Just an idea: If we put an WAIT in hazard area of the MTC0 which
enables interrupts, can we accomplish something like
atomic-test-and-wait operation?

void r4k_wait_bulletproof(void)
{
	local_irq_disable();
	if (!need_resched())
		__asm__(
		"	.set	push		\n"
		"	.set	mips3		\n"
		"	.set	noat		\n"
		"	.align	4		\n" /* avoid stall on wait */
		"	mfc0	$1, $12		\n"
		"	ori	$1, 1		\n"
		"	mtc0	$1, $12		\n"
		"	wait			\n"
		"	xori	$1, 1		\n"
		"	mtc0	$1, $12		\n"
		"	.set	pop		\n");
	local_irq_enable();
}

If this work as expected?  Comments from pipeline gurus are welcome ;)

---
Atsushi Nemoto

Re: WAIT vs. tickless kernel

[Ralf Baechle]

On Thu, Nov 01, 2007 at 12:49:06AM +0900, Atsushi Nemoto wrote:

> On some CPUs, there is a small window in the idle task which might
> cause a large latency to wakeup a process.
> 
> http://www.linux-mips.org/archives/linux-mips/2005-11/msg00114.html
> 
> This can be avoided on some CPUs which can use xxx_wait_irqoff(), but
> still there are many CPUs out of luck.
> 
> And now we have dyntick/tickless kernel.  On tickless kernel the
> problem might become more serious.  We cannot know the worst latency
> time.  Theoretically a task can lose wakeup-event forever.
> 
> Of course "nowait" kernel option will help, but are there any other
> good solutions?
> 
> Just an idea: If we put an WAIT in hazard area of the MTC0 which
> enables interrupts, can we accomplish something like
> atomic-test-and-wait operation?
> 
> void r4k_wait_bulletproof(void)
> {
> 	local_irq_disable();
> 	if (!need_resched())
> 		__asm__(
> 		"	.set	push		\n"
> 		"	.set	mips3		\n"
> 		"	.set	noat		\n"
> 		"	.align	4		\n" /* avoid stall on wait */
> 		"	mfc0	$1, $12		\n"
> 		"	ori	$1, 1		\n"
> 		"	mtc0	$1, $12		\n"
> 		"	wait			\n"
> 		"	xori	$1, 1		\n"
> 		"	mtc0	$1, $12		\n"
> 		"	.set	pop		\n");
> 	local_irq_enable();
> }
> 
> If this work as expected?  Comments from pipeline gurus are welcome ;)

This one is definately playing with the fire.  Or alternatively requires
detailed knowledge of the pipeline and pipelines tend to change.  MIPS
Technologies does regular maintenance releases of its cores which also
add features and may change the pipelines in subtle way that may break
something like this.

The only safe but ugly workaround is to change the return from exception
code to detect if the EPC is in the range startin from the condition
check in the idle loop to including the WAIT instruction and if so to
patch the EPC to resume execution at the condition check or the
instruction following the WAIT.

  Ralf

Re: WAIT vs. tickless kernel

[Atsushi Nemoto]

On Wed, 31 Oct 2007 16:13:33 +0000, Ralf Baechle <ralf@linux-mips.org> wrote:
> This one is definately playing with the fire.  Or alternatively requires
> detailed knowledge of the pipeline and pipelines tend to change.  MIPS
> Technologies does regular maintenance releases of its cores which also
> add features and may change the pipelines in subtle way that may break
> something like this.

Yes, I never think this is robust or guaranteed...

> The only safe but ugly workaround is to change the return from exception
> code to detect if the EPC is in the range startin from the condition
> check in the idle loop to including the WAIT instruction and if so to
> patch the EPC to resume execution at the condition check or the
> instruction following the WAIT.

I'm also thinking of this approach.  Still wondering if it is worth to
implement.

---
Atsushi Nemoto

Re: WAIT vs. tickless kernel

[Ralf Baechle]

On Thu, Nov 01, 2007 at 01:31:24AM +0900, Atsushi Nemoto wrote:

> On Wed, 31 Oct 2007 16:13:33 +0000, Ralf Baechle <ralf@linux-mips.org> wrote:
> > This one is definately playing with the fire.  Or alternatively requires
> > detailed knowledge of the pipeline and pipelines tend to change.  MIPS
> > Technologies does regular maintenance releases of its cores which also
> > add features and may change the pipelines in subtle way that may break
> > something like this.
> 
> Yes, I never think this is robust or guaranteed...
> 
> > The only safe but ugly workaround is to change the return from exception
> > code to detect if the EPC is in the range startin from the condition
> > check in the idle loop to including the WAIT instruction and if so to
> > patch the EPC to resume execution at the condition check or the
> > instruction following the WAIT.
> 
> I'm also thinking of this approach.  Still wondering if it is worth to
> implement.

The tickless kernel is very interesting for the low power fraction.  And
it's especially those users who would suffer most the loss of the ability
to use the WAIT instruction.  For a system running from two AAA cells the
tradeoff is clear ...  So I think it's become a must.

  Ralf

Re: WAIT vs. tickless kernel

[Atsushi Nemoto]

On Wed, 31 Oct 2007 16:39:00 +0000, Ralf Baechle <ralf@linux-mips.org> wrote:
> > > The only safe but ugly workaround is to change the return from exception
> > > code to detect if the EPC is in the range startin from the condition
> > > check in the idle loop to including the WAIT instruction and if so to
> > > patch the EPC to resume execution at the condition check or the
> > > instruction following the WAIT.
> > 
> > I'm also thinking of this approach.  Still wondering if it is worth to
> > implement.
> 
> The tickless kernel is very interesting for the low power fraction.  And
> it's especially those users who would suffer most the loss of the ability
> to use the WAIT instruction.  For a system running from two AAA cells the
> tradeoff is clear ...  So I think it's become a must.

Then, something like this?  Selecting in build-time is not so good,
but there are some CPUs which do not need this hack at all.
Synthesizing the ret_from_irq() at runtime might satisfy everyone?

diff --git a/arch/mips/kernel/cpu-probe.c b/arch/mips/kernel/cpu-probe.c
index c8c47a2..621130c 100644
--- a/arch/mips/kernel/cpu-probe.c
+++ b/arch/mips/kernel/cpu-probe.c
@@ -51,12 +51,17 @@ static void r39xx_wait(void)
  * But it is implementation-dependent wheter the pipelie restarts when
  * a non-enabled interrupt is requested.
  */
+#ifdef CONFIG_ROLLBACK_CPU_WAIT
+extern void cpu_wait_rollback(void);
+#define r4k_wait cpu_wait_rollback
+#else
 static void r4k_wait(void)
 {
 	__asm__("	.set	mips3			\n"
 		"	wait				\n"
 		"	.set	mips0			\n");
 }
+#endif
 
 /*
  * This variant is preferable as it allows testing need_resched and going to
diff --git a/arch/mips/kernel/entry.S b/arch/mips/kernel/entry.S
index e29598a..ffa043c 100644
--- a/arch/mips/kernel/entry.S
+++ b/arch/mips/kernel/entry.S
@@ -27,6 +27,20 @@
 #endif
 
 	.text
+#ifdef CONFIG_ROLLBACK_CPU_WAIT
+	.align	6
+FEXPORT(cpu_wait_rollback)
+	LONG_L	t0, TI_FLAGS($28)
+	andi	t0, _TIF_NEED_RESCHED
+	bnez	t0, 1f
+	.set	mips3
+	wait
+	.set	mips0
+1:
+	jr	ra
+	.align	6
+cpu_wait_rollback_end:
+#endif
 	.align	5
 #ifndef CONFIG_PREEMPT
 FEXPORT(ret_from_exception)
@@ -35,6 +49,14 @@ FEXPORT(ret_from_exception)
 #endif
 FEXPORT(ret_from_irq)
 	LONG_S	s0, TI_REGS($28)
+#ifdef CONFIG_ROLLBACK_CPU_WAIT
+	LONG_L	t0, PT_EPC(sp)
+	ori	t0, 0x3f
+	xori	t0, 0x3f
+	PTR_LA	t1, cpu_wait_rollback
+	bne	t0, t1, __ret_from_irq
+	LONG_S	t0, PT_EPC(sp)			# return to cpu_wait_rollback
+#endif
 FEXPORT(__ret_from_irq)
 	LONG_L	t0, PT_STATUS(sp)		# returning to kernel mode?
 	andi	t0, t0, KU_USER

Re: WAIT vs. tickless kernel

[Atsushi Nemoto]

On Sat, 03 Nov 2007 01:46:49 +0900 (JST), Atsushi Nemoto <anemo@mba.ocn.ne.jp> wrote:
> > The tickless kernel is very interesting for the low power fraction.  And
> > it's especially those users who would suffer most the loss of the ability
> > to use the WAIT instruction.  For a system running from two AAA cells the
> > tradeoff is clear ...  So I think it's become a must.
> 
> Then, something like this?  Selecting in build-time is not so good,
> but there are some CPUs which do not need this hack at all.
> Synthesizing the ret_from_irq() at runtime might satisfy everyone?

Revised.  

As Ralf said on IRC, the adjustment can be done at beginning of
handler, instead of ret_from_irq.  So we can enable this hack at
runtime.  I introduced BUILD_ROLLBACK_PROLOGUE macro to build prologue
code for handle_int and except_vec_vi.  I'm not sure except_vec4 needs
the prologue or not.

And if the EPC was just after WAIT (i.e. normal wakeup from WAIT) the
rollback is not needed.  So I arranged r4k_wait so that the rollback
region exactly fit to 32 byte.

How about this?

------------------------------------------------------------------------
Subject: Fix potential latency problem due to non-atomic cpu_wait.

If an interrupt happened between checking of NEED_RESCHED and WAIT
instruction, adjust EPC to restart from checking of NEED_RESCHED.

Signed-off-by: Atsushi Nemoto <anemo@mba.ocn.ne.jp>
---
 arch/mips/kernel/cpu-probe.c |   16 ++--------------
 arch/mips/kernel/genex.S     |   38 ++++++++++++++++++++++++++++++++++++++
 arch/mips/kernel/traps.c     |   22 ++++++++++++++++------
 3 files changed, 56 insertions(+), 20 deletions(-)

diff --git a/arch/mips/kernel/cpu-probe.c b/arch/mips/kernel/cpu-probe.c
index c8c47a2..c745b91 100644
--- a/arch/mips/kernel/cpu-probe.c
+++ b/arch/mips/kernel/cpu-probe.c
@@ -45,18 +45,7 @@ static void r39xx_wait(void)
 	local_irq_enable();
 }
 
-/*
- * There is a race when WAIT instruction executed with interrupt
- * enabled.
- * But it is implementation-dependent wheter the pipelie restarts when
- * a non-enabled interrupt is requested.
- */
-static void r4k_wait(void)
-{
-	__asm__("	.set	mips3			\n"
-		"	wait				\n"
-		"	.set	mips0			\n");
-}
+extern void r4k_wait(void);
 
 /*
  * This variant is preferable as it allows testing need_resched and going to
@@ -128,7 +117,7 @@ static int __init wait_disable(char *s)
 
 __setup("nowait", wait_disable);
 
-static inline void check_wait(void)
+void __init check_wait(void)
 {
 	struct cpuinfo_mips *c = &current_cpu_data;
 
@@ -239,7 +228,6 @@ static inline void check_errata(void)
 
 void __init check_bugs32(void)
 {
-	check_wait();
 	check_errata();
 }
 
diff --git a/arch/mips/kernel/genex.S b/arch/mips/kernel/genex.S
index c0f19d6..bb72c3a 100644
--- a/arch/mips/kernel/genex.S
+++ b/arch/mips/kernel/genex.S
@@ -20,6 +20,7 @@
 #include <asm/stackframe.h>
 #include <asm/war.h>
 #include <asm/page.h>
+#include <asm/thread_info.h>
 
 #define PANIC_PIC(msg)					\
 		.set push;				\
@@ -126,7 +127,43 @@ handle_vcei:
 
 	__FINIT
 
+	.align	5	/* 32 byte rollback region */
+LEAF(r4k_wait)
+	.set	push
+	.set	noreorder
+	/* start of rollback region */
+	LONG_L	t0, TI_FLAGS($28)
+	nop
+	andi	t0, _TIF_NEED_RESCHED
+	bnez	t0, 1f
+	 nop
+	nop
+	nop
+	.set	mips3
+	wait
+	.set	mips0
+	/* end of rollback region (the region size must be power of two) */
+	.set	pop
+1:
+	jr	ra
+	END(r4k_wait)
+
+	.macro	BUILD_ROLLBACK_PROLOGUE handler
+	FEXPORT(rollback_\handler)
+	.set	push
+	.set	noat
+	MFC0	k0, CP0_EPC
+	ori	k0, 0x1f	/* 32 byte rollback region */
+	xori	k0, 0x1f
+	PTR_LA	k1, r4k_wait
+	bne	k0, k1, 9f
+	MTC0	k0, CP0_EPC
+9:
+	.set pop
+	.endm
+
 	.align  5
+BUILD_ROLLBACK_PROLOGUE handle_int
 NESTED(handle_int, PT_SIZE, sp)
 #ifdef CONFIG_TRACE_IRQFLAGS
 	/*
@@ -201,6 +238,7 @@ NESTED(except_vec_ejtag_debug, 0, sp)
  * This prototype is copied to ebase + n*IntCtl.VS and patched
  * to invoke the handler
  */
+BUILD_ROLLBACK_PROLOGUE except_vec_vi
 NESTED(except_vec_vi, 0, sp)
 	SAVE_SOME
 	SAVE_AT
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index fa50078..0b2cc58 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -43,6 +43,9 @@
 #include <asm/types.h>
 #include <asm/stacktrace.h>
 
+extern void check_wait(void);
+extern asmlinkage void r4k_wait(void);
+extern asmlinkage void rollback_handle_int(void);
 extern asmlinkage void handle_int(void);
 extern asmlinkage void handle_tlbm(void);
 extern asmlinkage void handle_tlbl(void);
@@ -1198,6 +1201,9 @@ static void *set_vi_srs_handler(int n, vi_handler_t addr, int srs)
 
 		extern char except_vec_vi, except_vec_vi_lui;
 		extern char except_vec_vi_ori, except_vec_vi_end;
+		extern char rollback_except_vec_vi;
+		char *vec_start = (cpu_wait == r4k_wait) ?
+			&rollback_except_vec_vi : &except_vec_vi;
 #ifdef CONFIG_MIPS_MT_SMTC
 		/*
 		 * We need to provide the SMTC vectored interrupt handler
@@ -1205,11 +1211,11 @@ static void *set_vi_srs_handler(int n, vi_handler_t addr, int srs)
 		 * Status.IM bit to be masked before going there.
 		 */
 		extern char except_vec_vi_mori;
-		const int mori_offset = &except_vec_vi_mori - &except_vec_vi;
+		const int mori_offset = &except_vec_vi_mori - vec_start;
 #endif /* CONFIG_MIPS_MT_SMTC */
-		const int handler_len = &except_vec_vi_end - &except_vec_vi;
-		const int lui_offset = &except_vec_vi_lui - &except_vec_vi;
-		const int ori_offset = &except_vec_vi_ori - &except_vec_vi;
+		const int handler_len = &except_vec_vi_end - vec_start;
+		const int lui_offset = &except_vec_vi_lui - vec_start;
+		const int ori_offset = &except_vec_vi_ori - vec_start;
 
 		if (handler_len > VECTORSPACING) {
 			/*
@@ -1219,7 +1225,7 @@ static void *set_vi_srs_handler(int n, vi_handler_t addr, int srs)
 			panic("VECTORSPACING too small");
 		}
 
-		memcpy(b, &except_vec_vi, handler_len);
+		memcpy(b, vec_start, handler_len);
 #ifdef CONFIG_MIPS_MT_SMTC
 		BUG_ON(n > 7);	/* Vector index %d exceeds SMTC maximum. */
 
@@ -1497,6 +1503,10 @@ void __init trap_init(void)
 	extern char except_vec3_generic, except_vec3_r4000;
 	extern char except_vec4;
 	unsigned long i;
+	int rollback;
+
+	check_wait();
+	rollback = (cpu_wait == r4k_wait);
 
 	if (cpu_has_veic || cpu_has_vint)
 		ebase = (unsigned long) alloc_bootmem_low_pages(0x200 + VECTORSPACING*64);
@@ -1558,7 +1568,7 @@ void __init trap_init(void)
 	if (board_be_init)
 		board_be_init();
 
-	set_except_vector(0, handle_int);
+	set_except_vector(0, rollback ? rollback_handle_int : handle_int);
 	set_except_vector(1, handle_tlbm);
 	set_except_vector(2, handle_tlbl);
 	set_except_vector(3, handle_tlbs);

Re: WAIT vs. tickless kernel

[David Daney]

Atsushi Nemoto wrote:
> +LEAF(r4k_wait)
> +	.set	push
> +	.set	noreorder
> +	/* start of rollback region */
> +	LONG_L	t0, TI_FLAGS($28)
> +	nop
> +	andi	t0, _TIF_NEED_RESCHED
> +	bnez	t0, 1f
> +	 nop
> +	nop
> +	nop
> +	.set	mips3
> +	wait
> +	.set	mips0
> +	/* end of rollback region (the region size must be power of two) */
> +	.set	pop
>   

The .set mips0 is redundant as .set pop immediately follows.

David Daney

Re: WAIT vs. tickless kernel

[Atsushi Nemoto]

On Tue, 06 Nov 2007 07:58:46 -0800, David Daney <ddaney@avtrex.com> wrote:
> > +	.set	mips0
> > +	/* end of rollback region (the region size must be power of two) */
> > +	.set	pop
> >   
> 
> The .set mips0 is redundant as .set pop immediately follows.

Oh yes.  I'll drop it on next revision.  Thanks.

---
Atsushi Nemoto

Re: WAIT vs. tickless kernel

[Atsushi Nemoto]

On Wed, 07 Nov 2007 00:39:25 +0900 (JST), Atsushi Nemoto <anemo@mba.ocn.ne.jp> wrote:
> +	MFC0	k0, CP0_EPC
> +	ori	k0, 0x1f	/* 32 byte rollback region */
> +	xori	k0, 0x1f
> +	PTR_LA	k1, r4k_wait

Well, this part should be like this, for better pipelining.

	MFC0	k0, CP0_EPC
	PTR_LA	k1, r4k_wait
	ori	k0, 0x1f	/* 32 byte rollback region */
	xori	k0, 0x1f

> +	bne	k0, k1, 9f
> +	MTC0	k0, CP0_EPC
> +9:

And if we could assume branch-likely, this can be:

	.set	noreorder
	beql	k0, k1, 9f
	 MTC0	k0, CP0_EPC
9:

But not sure if it really have points.

>  	.align  5
> +BUILD_ROLLBACK_PROLOGUE handle_int
>  NESTED(handle_int, PT_SIZE, sp)

And one more question: should we put one more ".align 5" just befor
handle_int for CPUs do not need the rollback?

---
Atsushi Nemoto

Re: WAIT vs. tickless kernel

[Atsushi Nemoto]

Take 3.  Remove unnecessary .mips0 and make rollback_handler a bit
faster.

------------------------------------------------------------------------
Subject: Fix potential latency problem due to non-atomic cpu_wait.

If an interrupt happened between checking of NEED_RESCHED and WAIT
instruction, adjust EPC to restart from checking of NEED_RESCHED.

Signed-off-by: Atsushi Nemoto <anemo@mba.ocn.ne.jp>
---
 arch/mips/kernel/cpu-probe.c |   16 ++--------------
 arch/mips/kernel/genex.S     |   37 +++++++++++++++++++++++++++++++++++++
 arch/mips/kernel/traps.c     |   22 ++++++++++++++++------
 3 files changed, 55 insertions(+), 20 deletions(-)

diff --git a/arch/mips/kernel/cpu-probe.c b/arch/mips/kernel/cpu-probe.c
index 5c27943..1f71fec 100644
--- a/arch/mips/kernel/cpu-probe.c
+++ b/arch/mips/kernel/cpu-probe.c
@@ -45,18 +45,7 @@ static void r39xx_wait(void)
 	local_irq_enable();
 }
 
-/*
- * There is a race when WAIT instruction executed with interrupt
- * enabled.
- * But it is implementation-dependent wheter the pipelie restarts when
- * a non-enabled interrupt is requested.
- */
-static void r4k_wait(void)
-{
-	__asm__("	.set	mips3			\n"
-		"	wait				\n"
-		"	.set	mips0			\n");
-}
+extern void r4k_wait(void);
 
 /*
  * This variant is preferable as it allows testing need_resched and going to
@@ -128,7 +117,7 @@ static int __init wait_disable(char *s)
 
 __setup("nowait", wait_disable);
 
-static inline void check_wait(void)
+void __init check_wait(void)
 {
 	struct cpuinfo_mips *c = &current_cpu_data;
 
@@ -239,7 +228,6 @@ static inline void check_errata(void)
 
 void __init check_bugs32(void)
 {
-	check_wait();
 	check_errata();
 }
 
diff --git a/arch/mips/kernel/genex.S b/arch/mips/kernel/genex.S
index e76a76b..96f46b3 100644
--- a/arch/mips/kernel/genex.S
+++ b/arch/mips/kernel/genex.S
@@ -20,6 +20,7 @@
 #include <asm/stackframe.h>
 #include <asm/war.h>
 #include <asm/page.h>
+#include <asm/thread_info.h>
 
 #define PANIC_PIC(msg)					\
 		.set push;				\
@@ -126,7 +127,42 @@ handle_vcei:
 
 	__FINIT
 
+	.align	5	/* 32 byte rollback region */
+LEAF(r4k_wait)
+	.set	push
+	.set	noreorder
+	/* start of rollback region */
+	LONG_L	t0, TI_FLAGS($28)
+	nop
+	andi	t0, _TIF_NEED_RESCHED
+	bnez	t0, 1f
+	 nop
+	nop
+	nop
+	.set	mips3
+	wait
+	/* end of rollback region (the region size must be power of two) */
+	.set	pop
+1:
+	jr	ra
+	END(r4k_wait)
+
+	.macro	BUILD_ROLLBACK_PROLOGUE handler
+	FEXPORT(rollback_\handler)
+	.set	push
+	.set	noat
+	MFC0	k0, CP0_EPC
+	PTR_LA	k1, r4k_wait
+	ori	k0, 0x1f	/* 32 byte rollback region */
+	xori	k0, 0x1f
+	bne	k0, k1, 9f
+	MTC0	k0, CP0_EPC
+9:
+	.set pop
+	.endm
+
 	.align  5
+BUILD_ROLLBACK_PROLOGUE handle_int
 NESTED(handle_int, PT_SIZE, sp)
 #ifdef CONFIG_TRACE_IRQFLAGS
 	/*
@@ -201,6 +237,7 @@ NESTED(except_vec_ejtag_debug, 0, sp)
  * This prototype is copied to ebase + n*IntCtl.VS and patched
  * to invoke the handler
  */
+BUILD_ROLLBACK_PROLOGUE except_vec_vi
 NESTED(except_vec_vi, 0, sp)
 	SAVE_SOME
 	SAVE_AT
diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index 23e73d0..23807c6 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -43,6 +43,9 @@
 #include <asm/types.h>
 #include <asm/stacktrace.h>
 
+extern void check_wait(void);
+extern asmlinkage void r4k_wait(void);
+extern asmlinkage void rollback_handle_int(void);
 extern asmlinkage void handle_int(void);
 extern asmlinkage void handle_tlbm(void);
 extern asmlinkage void handle_tlbl(void);
@@ -1146,6 +1149,9 @@ static void *set_vi_srs_handler(int n, vi_handler_t addr, int srs)
 
 		extern char except_vec_vi, except_vec_vi_lui;
 		extern char except_vec_vi_ori, except_vec_vi_end;
+		extern char rollback_except_vec_vi;
+		char *vec_start = (cpu_wait == r4k_wait) ?
+			&rollback_except_vec_vi : &except_vec_vi;
 #ifdef CONFIG_MIPS_MT_SMTC
 		/*
 		 * We need to provide the SMTC vectored interrupt handler
@@ -1153,11 +1159,11 @@ static void *set_vi_srs_handler(int n, vi_handler_t addr, int srs)
 		 * Status.IM bit to be masked before going there.
 		 */
 		extern char except_vec_vi_mori;
-		const int mori_offset = &except_vec_vi_mori - &except_vec_vi;
+		const int mori_offset = &except_vec_vi_mori - vec_start;
 #endif /* CONFIG_MIPS_MT_SMTC */
-		const int handler_len = &except_vec_vi_end - &except_vec_vi;
-		const int lui_offset = &except_vec_vi_lui - &except_vec_vi;
-		const int ori_offset = &except_vec_vi_ori - &except_vec_vi;
+		const int handler_len = &except_vec_vi_end - vec_start;
+		const int lui_offset = &except_vec_vi_lui - vec_start;
+		const int ori_offset = &except_vec_vi_ori - vec_start;
 
 		if (handler_len > VECTORSPACING) {
 			/*
@@ -1167,7 +1173,7 @@ static void *set_vi_srs_handler(int n, vi_handler_t addr, int srs)
 			panic("VECTORSPACING too small");
 		}
 
-		memcpy(b, &except_vec_vi, handler_len);
+		memcpy(b, vec_start, handler_len);
 #ifdef CONFIG_MIPS_MT_SMTC
 		BUG_ON(n > 7);	/* Vector index %d exceeds SMTC maximum. */
 
@@ -1437,6 +1443,10 @@ void __init trap_init(void)
 	extern char except_vec3_generic, except_vec3_r4000;
 	extern char except_vec4;
 	unsigned long i;
+	int rollback;
+
+	check_wait();
+	rollback = (cpu_wait == r4k_wait);
 
 	if (cpu_has_veic || cpu_has_vint)
 		ebase = (unsigned long) alloc_bootmem_low_pages(0x200 + VECTORSPACING*64);
@@ -1496,7 +1506,7 @@ void __init trap_init(void)
 	if (board_be_init)
 		board_be_init();
 
-	set_except_vector(0, handle_int);
+	set_except_vector(0, rollback ? rollback_handle_int : handle_int);
 	set_except_vector(1, handle_tlbm);
 	set_except_vector(2, handle_tlbl);
 	set_except_vector(3, handle_tlbs);