3 files changed, 24 insertions, 6 deletions

diff --git a/arch/microblaze/Kconfig b/arch/microblaze/Kconfig
index 050aca35aca2..922c4194c7bb 100644
--- a/arch/microblaze/Kconfig
+++ b/arch/microblaze/Kconfig
@@ -184,6 +184,17 @@ config LOWMEM_SIZE
         hex "Maximum low memory size (in bytes)" if LOWMEM_SIZE_BOOL
         default "0x30000000"
 
+config MANUAL_RESET_VECTOR
+        hex "Microblaze reset vector address setup"
+        default "0x0"
+        help
+          Set this option to have the kernel override the CPU Reset vector.
+          If zero, no change will be made to the MicroBlaze reset vector at
+          address 0x0.
+          If non-zero, a jump instruction to this address, will be written
+          to the reset vector at address 0x0.
+          If you are unsure, set it to default value 0x0.
+
 config KERNEL_START_BOOL
         bool "Set custom kernel base address"
         depends on ADVANCED_OPTIONS
diff --git a/arch/microblaze/kernel/entry.S b/arch/microblaze/kernel/entry.S
index 6b4052459d5a..ca15bc5c7449 100644
--- a/arch/microblaze/kernel/entry.S
+++ b/arch/microblaze/kernel/entry.S
@@ -993,11 +993,10 @@ ENTRY(_reset)
         /* These are compiled and loaded into high memory, then
          * copied into place in mach_early_setup */
         .section        .init.ivt, "ax"
+#if CONFIG_MANUAL_RESET_VECTOR
         .org    0x0
-        /* this is very important - here is the reset vector */
-        /* in current MMU branch you don't care what is here - it is
-         * used from bootloader site - but this is correct for FS-BOOT */
-        brai    0x70
+        brai    CONFIG_MANUAL_RESET_VECTOR
+#endif
         .org    0x8
         brai    TOPHYS(_user_exception); /* syscall handler */
         .org    0x10
diff --git a/arch/microblaze/kernel/setup.c b/arch/microblaze/kernel/setup.c
index 9312fbb37efd..8e2c09b7ff26 100644
--- a/arch/microblaze/kernel/setup.c
+++ b/arch/microblaze/kernel/setup.c
@@ -95,7 +95,8 @@ inline unsigned get_romfs_len(unsigned *addr)
 void __init machine_early_init(const char *cmdline, unsigned int ram,
                 unsigned int fdt, unsigned int msr)
 {
-        unsigned long *src, *dst = (unsigned long *)0x0;
+        unsigned long *src, *dst;
+        unsigned int offset = 0;
 
         /* If CONFIG_MTD_UCLINUX is defined, assume ROMFS is at the
          * end of kernel. There are two position which we want to check.
@@ -168,7 +169,14 @@ void __init machine_early_init(const char *cmdline, unsigned int ram,
                                 "CPU have it %x\n", msr);
 #endif
 
-        for (src = __ivt_start; src < __ivt_end; src++, dst++)
+        /* Do not copy reset vectors. offset = 0x2 means skip the first
+         * two instructions. dst is pointer to MB vectors which are placed
+         * in block ram. If you want to copy reset vector setup offset to 0x0 */
+#if !CONFIG_MANUAL_RESET_VECTOR
+        offset = 0x2;
+#endif
+        dst = (unsigned long *) (offset * sizeof(u32));
+        for (src = __ivt_start + offset; src < __ivt_end; src++, dst++)
                 *dst = *src;
 
         /* Initialize global data */