1 files changed, 35 insertions, 68 deletions

diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c
index 246c6a6b0261..a1c1cb5de913 100644
--- a/arch/mips/kernel/traps.c
+++ b/arch/mips/kernel/traps.c
@@ -2007,7 +2007,13 @@ unsigned long vi_handlers[64];
 
 void reserve_exception_space(phys_addr_t addr, unsigned long size)
 {
-	memblock_reserve(addr, size);
+	/*
+	 * reserve exception space on CPUs other than CPU0
+	 * is too late, since memblock is unavailable when APs
+	 * up
+	 */
+	if (smp_processor_id() == 0)
+		memblock_reserve(addr, size);
 }
 
 void __init *set_except_vector(int n, void *addr)
@@ -2055,110 +2061,71 @@ static void do_default_vi(void)
 	panic("Caught unexpected vectored interrupt.");
 }
 
-static void *set_vi_srs_handler(int n, vi_handler_t addr, int srs)
+void *set_vi_handler(int n, vi_handler_t addr)
 {
+	extern const u8 except_vec_vi[];
+	extern const u8 except_vec_vi_ori[], except_vec_vi_end[];
+	extern const u8 rollback_except_vec_vi[];
 	unsigned long handler;
 	unsigned long old_handler = vi_handlers[n];
 	int srssets = current_cpu_data.srsets;
 	u16 *h;
 	unsigned char *b;
+	const u8 *vec_start;
+	int ori_offset;
+	int handler_len;
 
 	BUG_ON(!cpu_has_veic && !cpu_has_vint);
 
 	if (addr == NULL) {
 		handler = (unsigned long) do_default_vi;
-		srs = 0;
 	} else
 		handler = (unsigned long) addr;
 	vi_handlers[n] = handler;
 
 	b = (unsigned char *)(ebase + 0x200 + n*VECTORSPACING);
 
-	if (srs >= srssets)
-		panic("Shadow register set %d not supported", srs);
-
 	if (cpu_has_veic) {
 		if (board_bind_eic_interrupt)
-			board_bind_eic_interrupt(n, srs);
+			board_bind_eic_interrupt(n, 0);
 	} else if (cpu_has_vint) {
 		/* SRSMap is only defined if shadow sets are implemented */
 		if (srssets > 1)
-			change_c0_srsmap(0xf << n*4, srs << n*4);
+			change_c0_srsmap(0xf << n*4, 0 << n*4);
 	}
 
-	if (srs == 0) {
-		/*
-		 * If no shadow set is selected then use the default handler
-		 * that does normal register saving and standard interrupt exit
-		 */
-		extern const u8 except_vec_vi[], except_vec_vi_lui[];
-		extern const u8 except_vec_vi_ori[], except_vec_vi_end[];
-		extern const u8 rollback_except_vec_vi[];
-		const u8 *vec_start = using_rollback_handler() ?
-				      rollback_except_vec_vi : except_vec_vi;
+	vec_start = using_rollback_handler() ? rollback_except_vec_vi :
+					       except_vec_vi;
 #if defined(CONFIG_CPU_MICROMIPS) || defined(CONFIG_CPU_BIG_ENDIAN)
-		const int lui_offset = except_vec_vi_lui - vec_start + 2;
-		const int ori_offset = except_vec_vi_ori - vec_start + 2;
+	ori_offset = except_vec_vi_ori - vec_start + 2;
 #else
-		const int lui_offset = except_vec_vi_lui - vec_start;
-		const int ori_offset = except_vec_vi_ori - vec_start;
+	ori_offset = except_vec_vi_ori - vec_start;
 #endif
-		const int handler_len = except_vec_vi_end - vec_start;
+	handler_len = except_vec_vi_end - vec_start;
 
-		if (handler_len > VECTORSPACING) {
-			/*
-			 * Sigh... panicing won't help as the console
-			 * is probably not configured :(
-			 */
-			panic("VECTORSPACING too small");
-		}
-
-		set_handler(((unsigned long)b - ebase), vec_start,
-#ifdef CONFIG_CPU_MICROMIPS
-				(handler_len - 1));
-#else
-				handler_len);
-#endif
-		h = (u16 *)(b + lui_offset);
-		*h = (handler >> 16) & 0xffff;
-		h = (u16 *)(b + ori_offset);
-		*h = (handler & 0xffff);
-		local_flush_icache_range((unsigned long)b,
-					 (unsigned long)(b+handler_len));
-	}
-	else {
+	if (handler_len > VECTORSPACING) {
 		/*
-		 * In other cases jump directly to the interrupt handler. It
-		 * is the handler's responsibility to save registers if required
-		 * (eg hi/lo) and return from the exception using "eret".
+		 * Sigh... panicing won't help as the console
+		 * is probably not configured :(
 		 */
-		u32 insn;
+		panic("VECTORSPACING too small");
+	}
 
-		h = (u16 *)b;
-		/* j handler */
+	set_handler(((unsigned long)b - ebase), vec_start,
 #ifdef CONFIG_CPU_MICROMIPS
-		insn = 0xd4000000 | (((u32)handler & 0x07ffffff) >> 1);
+			(handler_len - 1));
 #else
-		insn = 0x08000000 | (((u32)handler & 0x0fffffff) >> 2);
+			handler_len);
 #endif
-		h[0] = (insn >> 16) & 0xffff;
-		h[1] = insn & 0xffff;
-		h[2] = 0;
-		h[3] = 0;
-		local_flush_icache_range((unsigned long)b,
-					 (unsigned long)(b+8));
-	}
+	/* insert offset into vi_handlers[] */
+	h = (u16 *)(b + ori_offset);
+	*h = n * sizeof(handler);
+	local_flush_icache_range((unsigned long)b,
+				 (unsigned long)(b+handler_len));
 
 	return (void *)old_handler;
 }
 
-void *set_vi_handler(int n, vi_handler_t addr)
-{
-	return set_vi_srs_handler(n, addr, 0);
-}
-
-extern void tlb_init(void);
-
 /*
  * Timer interrupt
  */
@@ -2418,7 +2385,7 @@ void __init trap_init(void)
 		set_except_vector(i, handle_reserved);
 
 	/*
-	 * Copy the EJTAG debug exception vector handler code to it's final
+	 * Copy the EJTAG debug exception vector handler code to its final
 	 * destination.
 	 */
 	if (cpu_has_ejtag && board_ejtag_handler_setup)