[PATCH v2 0/5] MIPS: relocate: Add automatic relocation to CONFIG_RELOCATABLE

[PATCH v2 0/5] MIPS: relocate: Add automatic relocation to CONFIG_RELOCATABLE

[Jinyang He]

1, To get the address the kernel is linked to, use PC-region(static) +
   offset(dynamic) way. See detail at [PATCH v2 3/5]
2, To deal with smp cpu in kernel entry, e.g. cavium-octeon platform,
   move smp cpu handle later than jump to the address kernel linked to.
   See detail at [PATCH v2 4/5]

Jinyang He (5):
  MIPS: relocate: Only compile relocs when CONFIG_RELOCATABLE is enabled
  MIPS: relocate: Use CONFIG_RANDOMIZE_BASE to enable kaslr
  MIPS: Jump to kernel linked address by PC relative way
  MIPS: cavium-octeon: Execute the smp handle after jumping to kernel
    linked address
  MIPS: relocate: Add support to auto relocate kernel

 arch/mips/Makefile                                 |   2 +
 arch/mips/cavium-octeon/smp.c                      |   9 +-
 arch/mips/generic/init.c                           |   4 +-
 arch/mips/include/asm/bootinfo.h                   |   4 +-
 .../asm/mach-cavium-octeon/kernel-entry-init.h     |  24 ++-
 arch/mips/kernel/Makefile                          |   2 +-
 arch/mips/kernel/head.S                            | 185 ++++++++++++++++++++-
 arch/mips/kernel/{relocate.c => kaslr.c}           |  15 --
 8 files changed, 214 insertions(+), 31 deletions(-)
 rename arch/mips/kernel/{relocate.c => kaslr.c} (97%)

-- 
2.1.0

[PATCH v2 1/5] MIPS: relocate: Only compile relocs when CONFIG_RELOCATABLE is enabled

[Jinyang He]

Relocs is used for create relocate information, only CONFIG_RELOCATABLE
is enabled can it be used.

Signed-off-by: Jinyang He <hejinyang@loongson.cn>
---
 arch/mips/Makefile | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/arch/mips/Makefile b/arch/mips/Makefile
index e71d587..cb54d86 100644
--- a/arch/mips/Makefile
+++ b/arch/mips/Makefile
@@ -17,7 +17,9 @@ archscripts: scripts_basic
 ifeq ($(CONFIG_CPU_LOONGSON3_WORKAROUNDS),y)
 	$(Q)$(MAKE) $(build)=arch/mips/tools loongson3-llsc-check
 endif
+ifeq ($(CONFIG_RELOCATABLE),y)
 	$(Q)$(MAKE) $(build)=arch/mips/boot/tools relocs
+endif
 
 KBUILD_DEFCONFIG := 32r2el_defconfig
 KBUILD_DTBS      := dtbs
-- 
2.1.0

[PATCH v2 2/5] MIPS: relocate: Use CONFIG_RANDOMIZE_BASE to enable kaslr

[Jinyang He]

Currently, the difference between using CONFIG_RELOCATABLE and
CONFIG_RANDOMIZE_BASE is determine_relocation_address().
CONFIG_RANDOMIZE_BASE achieves randomization. CONFIG_RELOCATABLE
relocate the kernel is very limited, currently returns a constant.
Therefore, drop determine_relocation_address() if CONFIG_RELOCATABLE.
Rename relocate.c to kaslr.c and use CONFIG_RANDOMIZE_BASE to enable
the kaslr function.

Signed-off-by: Jinyang He <hejinyang@loongson.cn>
---
 arch/mips/cavium-octeon/smp.c                             |  8 ++++----
 arch/mips/generic/init.c                                  |  4 ++--
 arch/mips/include/asm/bootinfo.h                          |  4 ++--
 .../include/asm/mach-cavium-octeon/kernel-entry-init.h    |  4 ++--
 arch/mips/kernel/Makefile                                 |  2 +-
 arch/mips/kernel/head.S                                   |  6 +++---
 arch/mips/kernel/{relocate.c => kaslr.c}                  | 15 ---------------
 7 files changed, 14 insertions(+), 29 deletions(-)
 rename arch/mips/kernel/{relocate.c => kaslr.c} (97%)

diff --git a/arch/mips/cavium-octeon/smp.c b/arch/mips/cavium-octeon/smp.c
index 66ce552..dfdbc79 100644
--- a/arch/mips/cavium-octeon/smp.c
+++ b/arch/mips/cavium-octeon/smp.c
@@ -28,9 +28,9 @@
 volatile unsigned long octeon_processor_boot = 0xff;
 volatile unsigned long octeon_processor_sp;
 volatile unsigned long octeon_processor_gp;
-#ifdef CONFIG_RELOCATABLE
+#ifdef CONFIG_RANDOMIZE_BASE
 volatile unsigned long octeon_processor_relocated_kernel_entry;
-#endif /* CONFIG_RELOCATABLE */
+#endif /* CONFIG_RANDOMIZE_BASE */
 
 #ifdef CONFIG_HOTPLUG_CPU
 uint64_t octeon_bootloader_entry_addr;
@@ -190,7 +190,7 @@ static void __init octeon_smp_setup(void)
 }
 
 
-#ifdef CONFIG_RELOCATABLE
+#ifdef CONFIG_RANDOMIZE_BASE
 int plat_post_relocation(long offset)
 {
 	unsigned long entry = (unsigned long)kernel_entry;
@@ -200,7 +200,7 @@ int plat_post_relocation(long offset)
 
 	return 0;
 }
-#endif /* CONFIG_RELOCATABLE */
+#endif /* CONFIG_RANDOMIZE_BASE */
 
 /**
  * Firmware CPU startup hook
diff --git a/arch/mips/generic/init.c b/arch/mips/generic/init.c
index 1842cdd..f7a4e29 100644
--- a/arch/mips/generic/init.c
+++ b/arch/mips/generic/init.c
@@ -82,7 +82,7 @@ void __init *plat_get_fdt(void)
 	return (void *)fdt;
 }
 
-#ifdef CONFIG_RELOCATABLE
+#ifdef CONFIG_RANDOMIZE_BASE
 
 void __init plat_fdt_relocated(void *new_location)
 {
@@ -97,7 +97,7 @@ void __init plat_fdt_relocated(void *new_location)
 		fw_arg1 = (unsigned long)new_location;
 }
 
-#endif /* CONFIG_RELOCATABLE */
+#endif /* CONFIG_RANDOMIZE_BASE */
 
 void __init plat_mem_setup(void)
 {
diff --git a/arch/mips/include/asm/bootinfo.h b/arch/mips/include/asm/bootinfo.h
index 5be10ece..6b37663 100644
--- a/arch/mips/include/asm/bootinfo.h
+++ b/arch/mips/include/asm/bootinfo.h
@@ -169,7 +169,7 @@ static inline void plat_swiotlb_setup(void) {}
  */
 extern void *plat_get_fdt(void);
 
-#ifdef CONFIG_RELOCATABLE
+#ifdef CONFIG_RANDOMIZE_BASE
 
 /**
  * plat_fdt_relocated() - Update platform's information about relocated dtb
@@ -180,7 +180,7 @@ extern void *plat_get_fdt(void);
  */
 void plat_fdt_relocated(void *new_location);
 
-#endif /* CONFIG_RELOCATABLE */
+#endif /* CONFIG_RANDOMIZE_BASE */
 #endif /* CONFIG_USE_OF */
 
 #endif /* _ASM_BOOTINFO_H */
diff --git a/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h b/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
index c38b38c..e66767d 100644
--- a/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
+++ b/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
@@ -100,7 +100,7 @@
 	#
 
 octeon_spin_wait_boot:
-#ifdef CONFIG_RELOCATABLE
+#ifdef CONFIG_RANDOMIZE_BASE
 	PTR_LA	t0, octeon_processor_relocated_kernel_entry
 	LONG_L	t0, (t0)
 	beq	zero, t0, 1f
@@ -109,7 +109,7 @@
 	jr	t0
 	nop
 1:
-#endif /* CONFIG_RELOCATABLE */
+#endif /* CONFIG_RANDOMIZE_BASE */
 
 	# This is the variable where the next core to boot is stored
 	PTR_LA	t0, octeon_processor_boot
diff --git a/arch/mips/kernel/Makefile b/arch/mips/kernel/Makefile
index b4a57f1..bf7ac86 100644
--- a/arch/mips/kernel/Makefile
+++ b/arch/mips/kernel/Makefile
@@ -92,7 +92,7 @@ obj-$(CONFIG_I8253)		+= i8253.o
 
 obj-$(CONFIG_GPIO_TXX9)		+= gpio_txx9.o
 
-obj-$(CONFIG_RELOCATABLE)	+= relocate.o
+obj-$(CONFIG_RANDOMIZE_BASE)	+= kaslr.o
 
 obj-$(CONFIG_KEXEC)		+= machine_kexec.o relocate_kernel.o crash.o
 obj-$(CONFIG_CRASH_DUMP)	+= crash_dump.o
diff --git a/arch/mips/kernel/head.S b/arch/mips/kernel/head.S
index b825ed4..a25af1d 100644
--- a/arch/mips/kernel/head.S
+++ b/arch/mips/kernel/head.S
@@ -118,7 +118,7 @@ NESTED(kernel_entry, 16, sp)			# kernel entry point
 	set_saved_sp	sp, t0, t1
 	PTR_SUBU	sp, 4 * SZREG		# init stack pointer
 
-#ifdef CONFIG_RELOCATABLE
+#ifdef CONFIG_RANDOMIZE_BASE
 	/* Copy kernel and apply the relocations */
 	jal		relocate_kernel
 
@@ -136,9 +136,9 @@ NESTED(kernel_entry, 16, sp)			# kernel entry point
 	 * newly sync'd icache.
 	 */
 	jr.hb		v0
-#else  /* !CONFIG_RELOCATABLE */
+#else  /* !CONFIG_RANDOMIZE_BASE */
 	j		start_kernel
-#endif /* !CONFIG_RELOCATABLE */
+#endif /* !CONFIG_RANDOMIZE_BASE */
 	END(kernel_entry)
 
 #ifdef CONFIG_SMP
diff --git a/arch/mips/kernel/relocate.c b/arch/mips/kernel/kaslr.c
similarity index 97%
rename from arch/mips/kernel/relocate.c
rename to arch/mips/kernel/kaslr.c
index 499a535..6742d58 100644
--- a/arch/mips/kernel/relocate.c
+++ b/arch/mips/kernel/kaslr.c
@@ -188,8 +188,6 @@ static int __init relocate_exception_table(long offset)
 	return 0;
 }
 
-#ifdef CONFIG_RANDOMIZE_BASE
-
 static inline __init unsigned long rotate_xor(unsigned long hash,
 					      const void *area, size_t size)
 {
@@ -280,19 +278,6 @@ static inline void __init *determine_relocation_address(void)
 	return RELOCATED(dest);
 }
 
-#else
-
-static inline void __init *determine_relocation_address(void)
-{
-	/*
-	 * Choose a new address for the kernel
-	 * For now we'll hard code the destination
-	 */
-	return (void *)0xffffffff81000000;
-}
-
-#endif
-
 static inline int __init relocation_addr_valid(void *loc_new)
 {
 	if ((unsigned long)loc_new & 0x0000ffff) {
-- 
2.1.0

[PATCH v2 3/5] MIPS: Jump to kernel linked address by PC relative way

[Jinyang He]

Commit 15ad838d281b ("[MIPS] Always do the ARC64_TWIDDLE_PC thing.")
gives the kernel a chance to jump to the address the kernel is linked to.
It can be seen from the old records that this situation is usually a jump
from XPHYS to KSEG0. For example, kdump operation, kexec_start_address is
usually in XPHYS on Loongson64 platform. Jumping from XPHYS to KSEG0 is a
big jump. With the relocation, there may be small jumps. As follows,

			+-----------+
			|           |
			|___________|
			|           |
			|  _kernel  |
			|           |
			|_\0:       |
			|   jr t0   |---+
			| la t0, \0 |   |
			|___________|   |
			|           |   |
			|           |   |
			|___________|   |
			|           |   |
			|  kernel   |   |
			|           |   |
			|\0:        |<--+
			|   jr t0   |
			| la t0, \0 |
			|           |
			|___________|
			|           |
			+-----------+

_kernel is same as kernel, only the loaded address is different. They are
all in KSEG0. For kernel, `jr t0` is right operation and PC will jump to
\0. For _kernel, `jr t0` is dangerous operation and PC will not jump to
_\0 because `la t0, \0` target address is not PC relative.

This patch get the PC address by fixed `1:` (t0) and dynamic ra at
`1:` (t1). t0 determines the PC Region of the address where the kernel is
linked to. t1 determines the offset relative to the PC Region.

Signed-off-by: Jinyang He <hejinyang@loongson.cn>
---
 arch/mips/kernel/head.S | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/arch/mips/kernel/head.S b/arch/mips/kernel/head.S
index a25af1d..0186285 100644
--- a/arch/mips/kernel/head.S
+++ b/arch/mips/kernel/head.S
@@ -79,6 +79,9 @@ FEXPORT(__kernel_entry)
 	j	kernel_entry
 #endif /* CONFIG_BOOT_RAW */
 
+#define PC_REGION_SHIFT 28
+#define PC_REGION_MASK (~(1 << PC_REGION_SHIFT - 1))
+
 	__REF
 
 NESTED(kernel_entry, 16, sp)			# kernel entry point
@@ -89,9 +92,17 @@ NESTED(kernel_entry, 16, sp)			# kernel entry point
 
 	/* We might not get launched at the address the kernel is linked to,
 	   so we jump there.  */
-	PTR_LA	t0, 0f
-	jr	t0
-0:
+	PTR_LA	t0, 1f
+	bal		1f
+1:	move	t1, ra
+	PTR_LI	t2, PC_REGION_MASK
+	and		t0, t2
+	PTR_LI	t2, ~PC_REGION_MASK
+	and		t1, t2
+	or		t0, t1
+	PTR_ADDIU	t0, 2f-1b
+	jr		t0
+2:
 
 	PTR_LA		t0, __bss_start		# clear .bss
 	LONG_S		zero, (t0)
-- 
2.1.0

[PATCH v2 4/5] MIPS: cavium-octeon: Execute the smp handle after jumping to kernel linked address

[Jinyang He]

Once relocation is required, some fixed addresses in `smp handle` will be
affected. For example, octeon_processor_relocated_kernel_entry may cause
the problem of loading wrong memory at old address.

This patch makes smp cpu do two more things, one is to set ST0_KX, another
is to jump to the kernel linked address. They will not destroy the
cavium-octeon platform. This patch adds two platform-related macros,
SMP_IN_KERNEL_ENTRY and smp_in_kernel_handle, which can be used on other
platforms to deal with the SMP CPU in the kernel_entry, if the smp CPU
entered the kernel entry.

Signed-off-by: Jinyang He <hejinyang@loongson.cn>
---
 arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h | 7 +++++++
 arch/mips/kernel/head.S                                      | 4 ++++
 2 files changed, 11 insertions(+)

diff --git a/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h b/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
index e66767d..21b9854 100644
--- a/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
+++ b/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
@@ -86,6 +86,13 @@
 	dli	v0, 0x27
 	dmtc0	v0, CP0_DCACHE_ERR_REG
 1:
+	.set pop
+.endm
+
+#define SMP_IN_KERNEL_ENTRY
+.macro	smp_in_kernel_entry_handle
+	.set push
+	.set arch=octeon
 	# Get my core id
 	rdhwr	v0, $0
 	# Jump the master to kernel_entry
diff --git a/arch/mips/kernel/head.S b/arch/mips/kernel/head.S
index 0186285..6cfe23e 100644
--- a/arch/mips/kernel/head.S
+++ b/arch/mips/kernel/head.S
@@ -104,6 +104,10 @@ NESTED(kernel_entry, 16, sp)			# kernel entry point
 	jr		t0
 2:
 
+#ifdef SMP_IN_KERNEL_ENTRY
+	smp_in_kernel_entry_handle
+#endif
+
 	PTR_LA		t0, __bss_start		# clear .bss
 	LONG_S		zero, (t0)
 	PTR_LA		t1, __bss_stop - LONGSIZE
-- 
2.1.0

[PATCH v2 5/5] MIPS: relocate: Add support to auto relocate kernel

[Jinyang He]

These codes are base on arch/mips/kernel/kaslr.c. Once do_relocate_kernel
occurs, the kernel can hardly work normally, unless do_relocate_kernel
success. Follows are what boot cpu does in do_relocate_kernel.

1, Save args
2, Whether the PC meets expectation
   Determine whether the kernel needs to be relocated. At this time, a0
   is the difference between the PC and the fixed address in the kernel.
3, Whether offset 64KB aligned
4, Whether current _text and _end are in the PC region
   Makesure RELOCATED(_text/_end) in same PC region, otherwise fail.
   Because we used lots of j/jal in kernel.
5, get current __start/stop___ex_table address
   Get RELOCATED(__start/stop___ex_table) first, and relocate them.
6, get current _relocation_start address
   Get RELOCATED(_relocation_start address) and do relocation by info in
   relocation table. 964 is based on apply_r_mips_64_rel, 932 is based on
   apply_r_mips_32_rel, 926 is based on apply_r_mips_26_rel and 916 is
   based on apply_r_mips_hi16_rel.
   R_MIPS_26: 0~25bit: ((insn & 0x03ffffff) << 2 + offset) >> 2
                     = (insn & 0x03ffffff) + offset >> 2
   R_MIPS_HI16: 0~15bit: ((insn & 0x0000ffff) << 16 + offset) >> 16
                     = (insn & 0x0000ffff) + offset >> 16
7, Complete! And flush i-cache
8, Restore args

All relocate process did 3 things in short, relocate ex_table, relocate
insructions and flush i-cache.

Signed-off-by: Jinyang He <hejinyang@loongson.cn>
---
 arch/mips/cavium-octeon/smp.c                      |   5 +-
 .../asm/mach-cavium-octeon/kernel-entry-init.h     |  13 ++
 arch/mips/kernel/head.S                            | 158 +++++++++++++++++++++
 3 files changed, 175 insertions(+), 1 deletion(-)

diff --git a/arch/mips/cavium-octeon/smp.c b/arch/mips/cavium-octeon/smp.c
index dfdbc79..4201f08 100644
--- a/arch/mips/cavium-octeon/smp.c
+++ b/arch/mips/cavium-octeon/smp.c
@@ -28,9 +28,13 @@
 volatile unsigned long octeon_processor_boot = 0xff;
 volatile unsigned long octeon_processor_sp;
 volatile unsigned long octeon_processor_gp;
+
+#ifdef CONFIG_RELOCATABLE
+volatile unsigned long relocate_finished = 1;
 #ifdef CONFIG_RANDOMIZE_BASE
 volatile unsigned long octeon_processor_relocated_kernel_entry;
 #endif /* CONFIG_RANDOMIZE_BASE */
+#endif /* CONFIG_RELOCATABLE */
 
 #ifdef CONFIG_HOTPLUG_CPU
 uint64_t octeon_bootloader_entry_addr;
@@ -189,7 +193,6 @@ static void __init octeon_smp_setup(void)
 	octeon_smp_hotplug_setup();
 }
 
-
 #ifdef CONFIG_RANDOMIZE_BASE
 int plat_post_relocation(long offset)
 {
diff --git a/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h b/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
index 21b9854..14acc3a 100644
--- a/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
+++ b/arch/mips/include/asm/mach-cavium-octeon/kernel-entry-init.h
@@ -101,6 +101,19 @@
 
 #ifdef CONFIG_SMP
 
+#ifdef CONFIG_RELOCATABLE
+	# Wait main processor finished relocation
+	bal	1f
+1:	move	t0, ra
+	PTR_LA	t1, 1b
+	PTR_SUBU	t0, t1
+	PTR_LA	t1, relocate_finished
+	PTR_ADDU	t0, t1
+2:	nop
+	LONG_L	t1, 0(t0)
+	bnez	t1, 2b
+#endif /* CONFIG_RELOCATABLE */
+
 	#
 	# All cores other than the master need to wait here for SMP bootstrap
 	# to begin
diff --git a/arch/mips/kernel/head.S b/arch/mips/kernel/head.S
index 6cfe23e..c431d27 100644
--- a/arch/mips/kernel/head.S
+++ b/arch/mips/kernel/head.S
@@ -81,6 +81,160 @@ FEXPORT(__kernel_entry)
 
 #define PC_REGION_SHIFT 28
 #define PC_REGION_MASK (~(1 << PC_REGION_SHIFT - 1))
+#define R_MIPS_32	2
+#define R_MIPS_26	4
+#define R_MIPS_HI16	5
+#define R_MIPS_64	18
+
+	.macro	do_relocate_kernel
+	.set	push
+	/* Save args */
+	move		s0, a0
+	move		s1, a1
+	move		s2, a2
+	move		s3, a3
+
+	/* Whether the PC meets expectation */
+	bal		1f
+1:	move		a0, ra
+	PTR_LA		a1, 1b
+	PTR_SUB		a0, a0, a1
+	beqz		a0, 999f
+
+	/* Whether offset 64KB aligned */
+	and		t0, a0, 0xffff
+	bnez		t0, 999f
+
+	/* Whether current _text and _end are in the PC region */
+	PTR_LA		t0, _end
+	PTR_ADDU	t1, t0, a0
+	PTR_SRL		t1, PC_REGION_SHIFT
+	PTR_SRL		t0, PC_REGION_SHIFT
+	bne		t0, t1, 999f
+
+	PTR_LA		t0, _text
+	PTR_ADDU	t1, t0, a0
+	move		a3, t1
+	PTR_SRL		t1, PC_REGION_SHIFT
+	PTR_SRL		t0, PC_REGION_SHIFT
+	bne		t0, t1, 999f
+
+	/* get current __start/stop___ex_table address */
+	PTR_LA		a1, __start___ex_table
+	PTR_ADDU	a1, a0
+	PTR_LA		a2, __stop___ex_table
+	PTR_ADDU	a2, a0
+
+	/*
+	 * a0: offset
+	 * a1: current __start___ex_table
+	 * a2: current __stop___ex_table
+	 */
+1:	beq		a1, a2, 996f
+	PTR_L		t0, 0(a1)
+	PTR_ADDU	t0, a0
+	PTR_S		t0, 0(a1)
+	LONG_ADDIU	a1, PTRSIZE
+	b		1b
+
+996:
+	/* get current _relocation_start address */
+	PTR_LA		a1, _relocation_start
+	PTR_ADDU	a1, a0
+
+	/*
+	 * a0: offset
+	 * a1: _relocation_start[]
+	 * a2: offset >> 16, to relocate R_MIPS_HI16
+	 * a3: current _text
+	 * t0: to load _relocation_start[]
+	 * t1: relocation type
+	 * t2: current relocate position
+	 * t3: temporarily
+	 * t8: 0x00ffffff, mask, to get relocate position
+	 * t9: offset >> 2, to relocate R_MIPS_26
+	 */
+	PTR_LI		t8, 0xffffff
+	LONG_SRL 	t9, a0, 2
+	LONG_SRL 	a2, a0, 16
+1:	lw		t0, 0(a1)
+	beqz		t0, 997f
+	LONG_SRL	t1, t0, 24
+	and		t1, 0xff
+	and		t2, t0, t8
+	LONG_SLL	t2, 2
+	PTR_ADDU	t2, a3
+
+#ifdef CONFIG_64BIT
+	li		t3, R_MIPS_64
+	beq		t1, t3, 964f
+#endif
+	li		t3, R_MIPS_32
+	beq		t1, t3, 932f
+	li		t3, R_MIPS_26
+	beq		t1, t3, 926f
+	li		t3, R_MIPS_HI16
+	beq		t1, t3, 916f
+	b		999f
+
+#ifdef CONFIG_64BIT
+964:
+	ld		t3, 0(t2)
+	LONG_ADDU	t3, a0
+	sd		t3, 0(t2)
+	LONG_ADDIU	a1, 4
+	b		1b
+#endif
+
+932:
+	lw		t3, 0(t2)
+	LONG_ADDU	t3, a0
+	sw		t3, 0(t2)
+	LONG_ADDIU	a1, 4
+	b		1b
+
+926:
+	lw		t3, 0(t2)
+	LONG_ADDU	t3, t9
+	sw		t3, 0(t2)
+	LONG_ADDIU	a1, 4
+	b		1b
+
+916:
+	lw		t3, 0(t2)
+	LONG_ADDU	t3, a2
+	sw		t3, 0(t2)
+	LONG_ADDIU	a1, 4
+	b		1b
+
+997:
+	/* Complete! And flush i-cache */
+1:	PTR_LA		a0, _text
+	PTR_LA		a1, _end
+	synci		0(a0)
+	rdhwr		t0, $1
+	beqz		t0, 998f
+	PTR_ADDU	a0, t0
+	PTR_SUBU	t0, a1, a0
+	bgtz		t0, 1b
+
+998:
+	sync
+
+999:
+
+#ifdef SMP_IN_KERNEL_ENTRY
+	PTR_LA		a0, relocate_finished
+	LONG_S		zero, 0(a0)
+#endif
+
+	/* Restore args */
+	move		a0, s0
+	move		a1, s1
+	move		a2, s2
+	move		a3, s3
+	.set	pop
+	.endm
 
 	__REF
 
@@ -108,6 +262,10 @@ NESTED(kernel_entry, 16, sp)			# kernel entry point
 	smp_in_kernel_entry_handle
 #endif
 
+#ifdef CONFIG_RELOCATABLE
+	do_relocate_kernel
+#endif
+
 	PTR_LA		t0, __bss_start		# clear .bss
 	LONG_S		zero, (t0)
 	PTR_LA		t1, __bss_stop - LONGSIZE
-- 
2.1.0