(unsigned long) reboot_code_buffer + KEXEC_CONTROL_PAGE_SIZE);
        printk(KERN_INFO "Bye!\n");
 
-       cpu_proc_fin();
+       local_irq_disable();
+       local_fiq_disable();
        setup_mm_for_reboot(0); /* mode is not used, so just pass 0*/
+       flush_cache_all();
+       cpu_proc_fin();
+       flush_cache_all();
        cpu_reset(reboot_code_buffer_phys);
 }
 
 #include <linux/utsname.h>
 #include <linux/uaccess.h>
 
+#include <asm/cacheflush.h>
 #include <asm/leds.h>
 #include <asm/processor.h>
 #include <asm/system.h>
 
 void arm_machine_restart(char mode, const char *cmd)
 {
-       /*
-        * Clean and disable cache, and turn off interrupts
-        */
-       cpu_proc_fin();
+       /* Disable interrupts first */
+       local_irq_disable();
+       local_fiq_disable();
 
        /*
         * Tell the mm system that we are going to reboot -
         */
        setup_mm_for_reboot(mode);
 
+       /* Clean and invalidate caches */
+       flush_cache_all();
+
+       /* Turn off caching */
+       cpu_proc_fin();
+
+       /* Push out any further dirty data, and ensure cache is empty */
+       flush_cache_all();
+
        /*
         * Now call the architecture specific reboot code.
         */
 
  * cpu_arm1020_proc_fin()
  */
 ENTRY(cpu_arm1020_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm1020_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm1020_reset(loc)
 
  * cpu_arm1020e_proc_fin()
  */
 ENTRY(cpu_arm1020e_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm1020e_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm1020e_reset(loc)
 
  * cpu_arm1022_proc_fin()
  */
 ENTRY(cpu_arm1022_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm1022_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm1022_reset(loc)
 
  * cpu_arm1026_proc_fin()
  */
 ENTRY(cpu_arm1026_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm1026_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm1026_reset(loc)
 
 
 ENTRY(cpu_arm6_proc_fin)
 ENTRY(cpu_arm7_proc_fin)
-               mov     r0, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-               msr     cpsr_c, r0
                mov     r0, #0x31                       @ ....S..DP...M
                mcr     p15, 0, r0, c1, c0, 0           @ disable caches
                mov     pc, lr
 
                mov     pc, lr
 
 ENTRY(cpu_arm720_proc_fin)
-               stmfd   sp!, {lr}
-               mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-               msr     cpsr_c, ip
                mrc     p15, 0, r0, c1, c0, 0
                bic     r0, r0, #0x1000                 @ ...i............
                bic     r0, r0, #0x000e                 @ ............wca.
                mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-               mcr     p15, 0, r1, c7, c7, 0           @ invalidate cache
-               ldmfd   sp!, {pc}
+               mov     pc, lr
 
 /*
  * Function: arm720_proc_do_idle(void)
 
  * cpu_arm740_proc_fin()
  */
 ENTRY(cpu_arm740_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
        mrc     p15, 0, r0, c1, c0, 0
        bic     r0, r0, #0x3f000000             @ bank/f/lock/s
        bic     r0, r0, #0x0000000c             @ w-buffer/cache
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       mcr     p15, 0, r0, c7, c0, 0           @ invalidate cache
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm740_reset(loc)
 
  * cpu_arm7tdmi_proc_fin()
  */
 ENTRY(cpu_arm7tdmi_proc_fin)
-               mov     r0, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-               msr     cpsr_c, r0
                mov     pc, lr
 
 /*
 
  * cpu_arm920_proc_fin()
  */
 ENTRY(cpu_arm920_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-#ifndef CONFIG_CPU_DCACHE_WRITETHROUGH
-       bl      arm920_flush_kern_cache_all
-#else
-       bl      v4wt_flush_kern_cache_all
-#endif
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm920_reset(loc)
 
  * cpu_arm922_proc_fin()
  */
 ENTRY(cpu_arm922_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-#ifndef CONFIG_CPU_DCACHE_WRITETHROUGH
-       bl      arm922_flush_kern_cache_all
-#else
-       bl      v4wt_flush_kern_cache_all
-#endif
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm922_reset(loc)
 
  * cpu_arm925_proc_fin()
  */
 ENTRY(cpu_arm925_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm925_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm925_reset(loc)
 
  * cpu_arm926_proc_fin()
  */
 ENTRY(cpu_arm926_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm926_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm926_reset(loc)
 
  * cpu_arm940_proc_fin()
  */
 ENTRY(cpu_arm940_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm940_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x00001000             @ i-cache
        bic     r0, r0, #0x00000004             @ d-cache
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm940_reset(loc)
 
  * cpu_arm946_proc_fin()
  */
 ENTRY(cpu_arm946_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      arm946_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x00001000             @ i-cache
        bic     r0, r0, #0x00000004             @ d-cache
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_arm946_reset(loc)
 
  * cpu_arm9tdmi_proc_fin()
  */
 ENTRY(cpu_arm9tdmi_proc_fin)
-               mov     r0, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-               msr     cpsr_c, r0
                mov     pc, lr
 
 /*
 
  * cpu_fa526_proc_fin()
  */
 ENTRY(cpu_fa526_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      fa_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
        nop
        nop
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_fa526_reset(loc)
 
  * cpu_feroceon_proc_fin()
  */
 ENTRY(cpu_feroceon_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      feroceon_flush_kern_cache_all
-
 #if defined(CONFIG_CACHE_FEROCEON_L2) && \
        !defined(CONFIG_CACHE_FEROCEON_L2_WRITETHROUGH)
        mov     r0, #0
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_feroceon_reset(loc)
 
  * cpu_mohawk_proc_fin()
  */
 ENTRY(cpu_mohawk_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      mohawk_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1800                 @ ...iz...........
        bic     r0, r0, #0x0006                 @ .............ca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_mohawk_reset(loc)
 
  * cpu_sa110_proc_fin()
  */
 ENTRY(cpu_sa110_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      v4wb_flush_kern_cache_all       @ clean caches
-1:     mov     r0, #0
+       mov     r0, #0
        mcr     p15, 0, r0, c15, c2, 2          @ Disable clock switching
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_sa110_reset(loc)
 
  *  - Clean and turn off caches.
  */
 ENTRY(cpu_sa1100_proc_fin)
-       stmfd   sp!, {lr}
-       mov     ip, #PSR_F_BIT | PSR_I_BIT | SVC_MODE
-       msr     cpsr_c, ip
-       bl      v4wb_flush_kern_cache_all
        mcr     p15, 0, ip, c15, c2, 2          @ Disable clock switching
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x000e                 @ ............wca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  * cpu_sa1100_reset(loc)
 
        mov     pc, lr
 
 ENTRY(cpu_v6_proc_fin)
-       stmfd   sp!, {lr}
-       cpsid   if                              @ disable interrupts
-       bl      v6_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x0006                 @ .............ca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 
 /*
  *     cpu_v6_reset(loc)
 
 ENDPROC(cpu_v7_proc_init)
 
 ENTRY(cpu_v7_proc_fin)
-       stmfd   sp!, {lr}
-       cpsid   if                              @ disable interrupts
-       bl      v7_flush_kern_cache_all
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1000                 @ ...i............
        bic     r0, r0, #0x0006                 @ .............ca.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldmfd   sp!, {pc}
+       mov     pc, lr
 ENDPROC(cpu_v7_proc_fin)
 
 /*
 
  * cpu_xsc3_proc_fin()
  */
 ENTRY(cpu_xsc3_proc_fin)
-       str     lr, [sp, #-4]!
-       mov     r0, #PSR_F_BIT|PSR_I_BIT|SVC_MODE
-       msr     cpsr_c, r0
-       bl      xsc3_flush_kern_cache_all       @ clean caches
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1800                 @ ...IZ...........
        bic     r0, r0, #0x0006                 @ .............CA.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldr     pc, [sp], #4
+       mov     pc, lr
 
 /*
  * cpu_xsc3_reset(loc)
 
  * cpu_xscale_proc_fin()
  */
 ENTRY(cpu_xscale_proc_fin)
-       str     lr, [sp, #-4]!
-       mov     r0, #PSR_F_BIT|PSR_I_BIT|SVC_MODE
-       msr     cpsr_c, r0
-       bl      xscale_flush_kern_cache_all     @ clean caches
        mrc     p15, 0, r0, c1, c0, 0           @ ctrl register
        bic     r0, r0, #0x1800                 @ ...IZ...........
        bic     r0, r0, #0x0006                 @ .............CA.
        mcr     p15, 0, r0, c1, c0, 0           @ disable caches
-       ldr     pc, [sp], #4
+       mov     pc, lr
 
 /*
  * cpu_xscale_reset(loc)