diff --git a/arch/sh/kernel/cpu/sh2/entry.S b/arch/sh/kernel/cpu/sh2/entry.S
index 16bde0e..1ee0a6e 100644
--- a/arch/sh/kernel/cpu/sh2/entry.S
+++ b/arch/sh/kernel/cpu/sh2/entry.S
@@ -47,6 +47,13 @@
 	mov.l	r3,@-sp
 	cli
 	mov.l	$cpu_mode,r2
+#ifdef CONFIG_SMP
+	mov.l	$cpuid,r3
+	mov.l	@r3,r3
+	mov.l	@r3,r3
+	shll2	r3
+	add	r3,r2
+#endif
 	mov.l	@r2,r0
 	mov.l	@(5*4,r15),r3	! previous SR
 	or	r0,r3		! set MD
@@ -57,6 +64,13 @@
 	mov.l	__md_bit,r0
 	mov.l	r0,@r2		! enter kernel mode
 	mov.l	$current_thread_info,r2
+#ifdef CONFIG_SMP
+	mov.l	$cpuid,r0
+	mov.l	@r0,r0
+	mov.l	@r0,r0
+	shll2	r0
+	add	r0,r2
+#endif
 	mov.l	@r2,r2
 	mov	#(THREAD_SIZE >> 8),r0
 	shll8	r0
@@ -265,6 +279,13 @@
 	lds.l	@r0+,macl
 	mov	r15,r0
 	mov.l	$cpu_mode,r2
+#ifdef CONFIG_SMP
+	mov.l	$cpuid,r3
+	mov.l	@r3,r3
+	mov.l	@r3,r3
+	shll2	r3
+	add	r3,r2
+#endif
 	mov	#OFF_SR,r3
 	mov.l	@(r0,r3),r1
 	mov.l	__md_bit,r3
@@ -281,6 +302,13 @@
 	mov.l	r1,@r2				! set pc
 	get_current_thread_info r0, r1
 	mov.l	$current_thread_info,r1
+#ifdef CONFIG_SMP
+	mov.l	$cpuid,r3
+	mov.l	@r3,r3
+	mov.l	@r3,r3
+	shll2	r3
+	add	r3,r1
+#endif
 	mov.l	r0,@r1
 	mov.l	@r15+,r0
 	mov.l	@r15+,r1
@@ -308,19 +336,41 @@
 	.long	__current_thread_info
 $cpu_mode:	
 	.long	__cpu_mode
+#ifdef CONFIG_SMP
+$cpuid:
+	.long sh2_cpuid_addr
+#endif
 		
 ! common exception handler
 #include "../../entry-common.S"
+
+#ifdef CONFIG_NR_CPUS
+#define NR_CPUS CONFIG_NR_CPUS
+#else
+#define NR_CPUS 1
+#endif
 	
 	.data
 ! cpu operation mode 
 ! bit30 = MD (compatible SH3/4)
 __cpu_mode:
+	.rept	NR_CPUS
 	.long	0x40000000
+	.endr
+
+#ifdef CONFIG_SMP
+.global sh2_cpuid_addr
+sh2_cpuid_addr:
+	.long	dummy_cpuid
+dummy_cpuid:
+	.long	0
+#endif
 		
 	.section	.bss
 __current_thread_info:
+	.rept	NR_CPUS
 	.long	0
+	.endr
 
 ENTRY(exception_handling_table)
 	.space	4*32