Merge branch 'for-linus' of git://git.monstr.eu/linux-2.6-microblaze

* 'for-linus' of git://git.monstr.eu/linux-2.6-microblaze: (49 commits)
  microblaze: Add KGDB support
  microblaze: Support brki rX, 0x18 for user application debugging
  microblaze: Remove nop after MSRCLR/SET, MTS, MFS instructions
  microblaze: Simplify syscall rutine
  microblaze: Move PT_MODE saving to delay slot
  microblaze: Fix _interrupt function
  microblaze: Fix _user_exception function
  microblaze: Put together addik instructions
  microblaze: Use delay slot in syscall macros
  microblaze: Save kernel mode in delay slot
  microblaze: Do not mix register saving and mode setting
  microblaze: Move SAVE_STATE upward
  microblaze: entry.S: Macro optimization
  microblaze: Optimize hw exception rutine
  microblaze: Implement clear_ums macro and fix SAVE_STATE macro
  microblaze: Remove additional setup for kernel_mode
  microblaze: Optimize SAVE_STATE macro
  microblaze: Remove additional loading
  microblaze: Completely remove working with R11 register
  microblaze: Do not setup BIP in _debug_exception
  ...

34 files changed, 977 insertions, 544 deletions

diff --git a/arch/microblaze/Kconfig b/arch/microblaze/Kconfig
index 505a0859242..be3855250db 100644
--- a/arch/microblaze/Kconfig
+++ b/arch/microblaze/Kconfig
@@ -14,6 +14,7 @@ config MICROBLAZE
 	select USB_ARCH_HAS_EHCI
 	select ARCH_WANT_OPTIONAL_GPIOLIB
 	select HAVE_OPROFILE
+	select HAVE_ARCH_KGDB
 	select HAVE_DMA_ATTRS
 	select HAVE_DMA_API_DEBUG
 	select TRACING_SUPPORT
@@ -223,6 +224,36 @@ config TASK_SIZE
 	hex "Size of user task space" if TASK_SIZE_BOOL
 	default "0x80000000"
 
+choice
+	prompt "Page size"
+	default MICROBLAZE_4K_PAGES
+	depends on ADVANCED_OPTIONS && !MMU
+	help
+	  Select the kernel logical page size. Increasing the page size
+	  will reduce software overhead at each page boundary, allow
+	  hardware prefetch mechanisms to be more effective, and allow
+	  larger dma transfers increasing IO efficiency and reducing
+	  overhead. However the utilization of memory will increase.
+	  For example, each cached file will using a multiple of the
+	  page size to hold its contents and the difference between the
+	  end of file and the end of page is wasted.
+
+	  If unsure, choose 4K_PAGES.
+
+config MICROBLAZE_4K_PAGES
+	bool "4k page size"
+
+config MICROBLAZE_8K_PAGES
+	bool "8k page size"
+
+config MICROBLAZE_16K_PAGES
+	bool "16k page size"
+
+config MICROBLAZE_32K_PAGES
+	bool "32k page size"
+
+endchoice
+
 endmenu
 
 source "mm/Kconfig"
diff --git a/arch/microblaze/Kconfig.debug b/arch/microblaze/Kconfig.debug
index 9dc708a7f70..e6e5e0da28c 100644
--- a/arch/microblaze/Kconfig.debug
+++ b/arch/microblaze/Kconfig.debug
@@ -10,6 +10,7 @@ source "lib/Kconfig.debug"
 
 config EARLY_PRINTK
 	bool "Early printk function for kernel"
+	depends on SERIAL_UARTLITE_CONSOLE
 	default n
 	help
 	  This option turns on/off early printk messages to console.
diff --git a/arch/microblaze/boot/Makefile b/arch/microblaze/boot/Makefile
index 57f50c2371c..be01d78750d 100644
--- a/arch/microblaze/boot/Makefile
+++ b/arch/microblaze/boot/Makefile
@@ -35,13 +35,14 @@ quiet_cmd_cp = CP      $< $@$2
 	cmd_cp = cat $< >$@$2 || (rm -f $@ && echo false)
 
 quiet_cmd_strip = STRIP   $@
-      cmd_strip = $(STRIP) -K _start -K _end -K __log_buf -K _fdt_start vmlinux -o $@
+	cmd_strip = $(STRIP) -K microblaze_start -K _end -K __log_buf \
+				-K _fdt_start vmlinux -o $@
 
 quiet_cmd_uimage = UIMAGE  $@.ub
-      cmd_uimage = $(CONFIG_SHELL) $(MKIMAGE) -A microblaze -O linux -T kernel \
-                   -C none -n 'Linux-$(KERNELRELEASE)' \
-                   -a $(CONFIG_KERNEL_BASE_ADDR) -e $(CONFIG_KERNEL_BASE_ADDR) \
-                   -d $@ $@.ub
+	cmd_uimage = $(CONFIG_SHELL) $(MKIMAGE) -A microblaze -O linux -T kernel \
+		-C none -n 'Linux-$(KERNELRELEASE)' \
+		-a $(CONFIG_KERNEL_BASE_ADDR) -e $(CONFIG_KERNEL_BASE_ADDR) \
+		-d $@ $@.ub
 
 $(obj)/simpleImage.%: vmlinux FORCE
 	$(call if_changed,cp,.unstrip)
diff --git a/arch/microblaze/include/asm/cacheflush.h b/arch/microblaze/include/asm/cacheflush.h
index a6edd356cd0..7ebd955460d 100644
--- a/arch/microblaze/include/asm/cacheflush.h
+++ b/arch/microblaze/include/asm/cacheflush.h
@@ -17,6 +17,7 @@
 
 /* Somebody depends on this; sigh... */
 #include <linux/mm.h>
+#include <linux/io.h>
 
 /* Look at Documentation/cachetlb.txt */
 
@@ -60,7 +61,6 @@ void microblaze_cache_init(void);
 #define invalidate_icache()				mbc->iin();
 #define invalidate_icache_range(start, end)		mbc->iinr(start, end);
 
-
 #define flush_icache_user_range(vma, pg, adr, len)	flush_icache();
 #define flush_icache_page(vma, pg)			do { } while (0)
 
@@ -72,9 +72,15 @@ void microblaze_cache_init(void);
 #define flush_dcache()					mbc->dfl();
 #define flush_dcache_range(start, end)			mbc->dflr(start, end);
 
-#define ARCH_IMPLEMENTS_FLUSH_DCACHE_PAGE 0
-/* D-cache aliasing problem can't happen - cache is between MMU and ram */
-#define flush_dcache_page(page)			do { } while (0)
+#define ARCH_IMPLEMENTS_FLUSH_DCACHE_PAGE 1
+/* MS: We have to implement it because of rootfs-jffs2 issue on WB */
+#define flush_dcache_page(page) \
+do { \
+	unsigned long addr = (unsigned long) page_address(page); /* virtual */ \
+	addr = (u32)virt_to_phys((void *)addr); \
+	flush_dcache_range((unsigned) (addr), (unsigned) (addr) + PAGE_SIZE); \
+} while (0);
+
 #define flush_dcache_mmap_lock(mapping)		do { } while (0)
 #define flush_dcache_mmap_unlock(mapping)	do { } while (0)
 
@@ -97,8 +103,10 @@ void microblaze_cache_init(void);
 
 #define copy_to_user_page(vma, page, vaddr, dst, src, len)		\
 do {									\
+	u32 addr = virt_to_phys(dst);					\
+	invalidate_icache_range((unsigned) (addr), (unsigned) (addr) + (len));\
 	memcpy((dst), (src), (len));					\
-	flush_icache_range((unsigned) (dst), (unsigned) (dst) + (len));	\
+	flush_dcache_range((unsigned) (addr), (unsigned) (addr) + (len));\
 } while (0)
 
 #define copy_from_user_page(vma, page, vaddr, dst, src, len)		\
diff --git a/arch/microblaze/include/asm/dma-mapping.h b/arch/microblaze/include/asm/dma-mapping.h
index 18b3731c850..50738958070 100644
--- a/arch/microblaze/include/asm/dma-mapping.h
+++ b/arch/microblaze/include/asm/dma-mapping.h
@@ -79,12 +79,6 @@ static inline int dma_supported(struct device *dev, u64 mask)
 	return ops->dma_supported(dev, mask);
 }
 
-#ifdef CONFIG_PCI
-/* We have our own implementation of pci_set_dma_mask() */
-#define HAVE_ARCH_PCI_SET_DMA_MASK
-
-#endif
-
 static inline int dma_set_mask(struct device *dev, u64 dma_mask)
 {
 	struct dma_map_ops *ops = get_dma_ops(dev);
diff --git a/arch/microblaze/include/asm/elf.h b/arch/microblaze/include/asm/elf.h
index 7d4acf2b278..732caf1be74 100644
--- a/arch/microblaze/include/asm/elf.h
+++ b/arch/microblaze/include/asm/elf.h
@@ -77,7 +77,7 @@ typedef elf_fpreg_t elf_fpregset_t[ELF_NFPREG];
 #define ELF_DATA	ELFDATA2MSB
 #endif
 
-#define ELF_EXEC_PAGESIZE	4096
+#define ELF_EXEC_PAGESIZE	PAGE_SIZE
 
 
 #define ELF_CORE_COPY_REGS(_dest, _regs)			\
diff --git a/arch/microblaze/include/asm/exceptions.h b/arch/microblaze/include/asm/exceptions.h
index 4c7b5d037c8..6479097b802 100644
--- a/arch/microblaze/include/asm/exceptions.h
+++ b/arch/microblaze/include/asm/exceptions.h
@@ -14,6 +14,11 @@
 #define _ASM_MICROBLAZE_EXCEPTIONS_H
 
 #ifdef __KERNEL__
+
+#ifndef CONFIG_MMU
+#define EX_HANDLER_STACK_SIZ	(4*19)
+#endif
+
 #ifndef __ASSEMBLY__
 
 /* Macros to enable and disable HW exceptions in the MSR */
@@ -64,22 +69,6 @@ asmlinkage void full_exception(struct pt_regs *regs, unsigned int type,
 void die(const char *str, struct pt_regs *fp, long err);
 void _exception(int signr, struct pt_regs *regs, int code, unsigned long addr);
 
-#if defined(CONFIG_KGDB)
-void (*debugger)(struct pt_regs *regs);
-int (*debugger_bpt)(struct pt_regs *regs);
-int (*debugger_sstep)(struct pt_regs *regs);
-int (*debugger_iabr_match)(struct pt_regs *regs);
-int (*debugger_dabr_match)(struct pt_regs *regs);
-void (*debugger_fault_handler)(struct pt_regs *regs);
-#else
-#define debugger(regs)			do { } while (0)
-#define debugger_bpt(regs)		0
-#define debugger_sstep(regs)		0
-#define debugger_iabr_match(regs)	0
-#define debugger_dabr_match(regs)	0
-#define debugger_fault_handler		((void (*)(struct pt_regs *))0)
-#endif
-
 #endif /*__ASSEMBLY__ */
 #endif /* __KERNEL__ */
 #endif /* _ASM_MICROBLAZE_EXCEPTIONS_H */
diff --git a/arch/microblaze/include/asm/kgdb.h b/arch/microblaze/include/asm/kgdb.h
new file mode 100644
index 00000000000..78b17d40b23
--- /dev/null
+++ b/arch/microblaze/include/asm/kgdb.h
@@ -0,0 +1,28 @@
+#ifdef __KERNEL__
+#ifndef __MICROBLAZE_KGDB_H__
+#define __MICROBLAZE_KGDB_H__
+
+#ifndef __ASSEMBLY__
+
+#define CACHE_FLUSH_IS_SAFE	1
+#define BUFMAX			2048
+
+/*
+ * 32 32-bit general purpose registers (r0-r31)
+ *  6 32-bit special registers (pc, msr, ear, esr, fsr, btr)
+ * 12 32-bit PVR
+ *   7 32-bit MMU Regs (redr, rpid, rzpr, rtlbx, rtlbsx, rtlblo, rtlbhi)
+ * ------
+ *  57 registers
+ */
+#define NUMREGBYTES	(57 * 4)
+
+#define BREAK_INSTR_SIZE	4
+static inline void arch_kgdb_breakpoint(void)
+{
+	__asm__ __volatile__("brki r16, 0x18;");
+}
+
+#endif /* __ASSEMBLY__ */
+#endif /* __MICROBLAZE_KGDB_H__ */
+#endif /* __KERNEL__ */
diff --git a/arch/microblaze/include/asm/page.h b/arch/microblaze/include/asm/page.h
index 464ff32bee3..c12c6dfafd9 100644
--- a/arch/microblaze/include/asm/page.h
+++ b/arch/microblaze/include/asm/page.h
@@ -23,8 +23,16 @@
 #ifdef __KERNEL__
 
 /* PAGE_SHIFT determines the page size */
-#define PAGE_SHIFT	(12)
-#define PAGE_SIZE	(_AC(1, UL) << PAGE_SHIFT)
+#if defined(CONFIG_MICROBLAZE_32K_PAGES)
+#define PAGE_SHIFT		15
+#elif defined(CONFIG_MICROBLAZE_16K_PAGES)
+#define PAGE_SHIFT		14
+#elif defined(CONFIG_MICROBLAZE_8K_PAGES)
+#define PAGE_SHIFT		13
+#else
+#define PAGE_SHIFT		12
+#endif
+#define PAGE_SIZE	(ASM_CONST(1) << PAGE_SHIFT)
 #define PAGE_MASK	(~(PAGE_SIZE-1))
 
 #define LOAD_OFFSET	ASM_CONST((CONFIG_KERNEL_START-CONFIG_KERNEL_BASE_ADDR))
diff --git a/arch/microblaze/include/asm/pvr.h b/arch/microblaze/include/asm/pvr.h
index e38abc7714b..9578666e98b 100644
--- a/arch/microblaze/include/asm/pvr.h
+++ b/arch/microblaze/include/asm/pvr.h
@@ -16,7 +16,7 @@
 #define PVR_MSR_BIT 0x400
 
 struct pvr_s {
-	unsigned pvr[16];
+	unsigned pvr[12];
 };
 
 /* The following taken from Xilinx's standalone BSP pvr.h */
diff --git a/arch/microblaze/include/asm/setup.h b/arch/microblaze/include/asm/setup.h
index 7f31394985e..782b5c89248 100644
--- a/arch/microblaze/include/asm/setup.h
+++ b/arch/microblaze/include/asm/setup.h
@@ -28,8 +28,6 @@ void disable_early_printk(void);
 void heartbeat(void);
 void setup_heartbeat(void);
 
-unsigned long long sched_clock(void);
-
 #   ifdef CONFIG_MMU
 extern void mmu_reset(void);
 extern void early_console_reg_tlb_alloc(unsigned int addr);
diff --git a/arch/microblaze/include/asm/system.h b/arch/microblaze/include/asm/system.h
index 81e1f7d5b4c..e6a2284571d 100644
--- a/arch/microblaze/include/asm/system.h
+++ b/arch/microblaze/include/asm/system.h
@@ -45,7 +45,6 @@ extern struct task_struct *_switch_to(struct thread_info *prev,
 #define smp_rmb()		rmb()
 #define smp_wmb()		wmb()
 
-void show_trace(struct task_struct *task, unsigned long *stack);
 void __bad_xchg(volatile void *ptr, int size);
 
 static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
diff --git a/arch/microblaze/include/asm/uaccess.h b/arch/microblaze/include/asm/uaccess.h
index 26460d15b33..d840f4a2d3c 100644
--- a/arch/microblaze/include/asm/uaccess.h
+++ b/arch/microblaze/include/asm/uaccess.h
@@ -359,7 +359,7 @@ extern long __user_bad(void);
 	__copy_tofrom_user((__force void __user *)(to), \
 				(void __user *)(from), (n))
 #define __copy_from_user_inatomic(to, from, n) \
-		copy_from_user((to), (from), (n))
+		__copy_from_user((to), (from), (n))
 
 static inline long copy_from_user(void *to,
 		const void __user *from, unsigned long n)
@@ -373,7 +373,7 @@ static inline long copy_from_user(void *to,
 #define __copy_to_user(to, from, n)	\
 		__copy_tofrom_user((void __user *)(to), \
 			(__force const void __user *)(from), (n))
-#define __copy_to_user_inatomic(to, from, n)	copy_to_user((to), (from), (n))
+#define __copy_to_user_inatomic(to, from, n) __copy_to_user((to), (from), (n))
 
 static inline long copy_to_user(void __user *to,
 		const void *from, unsigned long n)
diff --git a/arch/microblaze/include/asm/unwind.h b/arch/microblaze/include/asm/unwind.h
new file mode 100644
index 00000000000..d248b7de4b1
--- /dev/null
+++ b/arch/microblaze/include/asm/unwind.h
@@ -0,0 +1,29 @@
+/*
+ * Backtrace support for Microblaze
+ *
+ * Copyright (C) 2010  Digital Design Corporation
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#ifndef __MICROBLAZE_UNWIND_H
+#define __MICROBLAZE_UNWIND_H
+
+struct stack_trace;
+
+struct trap_handler_info {
+	unsigned long start_addr;
+	unsigned long end_addr;
+	const char    *trap_name;
+};
+extern struct trap_handler_info microblaze_trap_handlers;
+
+extern const char _hw_exception_handler;
+extern const char ex_handler_unhandled;
+
+void microblaze_unwind(struct task_struct *task, struct stack_trace *trace);
+
+#endif	/* __MICROBLAZE_UNWIND_H */
+
diff --git a/arch/microblaze/kernel/Makefile b/arch/microblaze/kernel/Makefile
index e51bc152082..5eecc9f1fbd 100644
--- a/arch/microblaze/kernel/Makefile
+++ b/arch/microblaze/kernel/Makefile
@@ -17,7 +17,7 @@ extra-y := head.o vmlinux.lds
 obj-y += dma.o exceptions.o \
 	hw_exception_handler.o init_task.o intc.o irq.o of_device.o \
 	of_platform.o process.o prom.o prom_parse.o ptrace.o \
-	setup.o signal.o sys_microblaze.o timer.o traps.o reset.o
+	reset.o setup.o signal.o sys_microblaze.o timer.o traps.o unwind.o
 
 obj-y += cpu/
 
@@ -28,5 +28,6 @@ obj-$(CONFIG_MODULES)		+= microblaze_ksyms.o module.o
 obj-$(CONFIG_MMU)		+= misc.o
 obj-$(CONFIG_STACKTRACE)	+= stacktrace.o
 obj-$(CONFIG_FUNCTION_TRACER)	+= ftrace.o mcount.o
+obj-$(CONFIG_KGDB)		+= kgdb.o
 
 obj-y	+= entry$(MMU).o
diff --git a/arch/microblaze/kernel/cpu/mb.c b/arch/microblaze/kernel/cpu/mb.c
index 4216eb1eaa3..7086e356428 100644
--- a/arch/microblaze/kernel/cpu/mb.c
+++ b/arch/microblaze/kernel/cpu/mb.c
@@ -126,6 +126,7 @@ static int show_cpuinfo(struct seq_file *m, void *v)
 			cpuinfo.pvr_user1,
 			cpuinfo.pvr_user2);
 
+	count += seq_printf(m, "Page size:\t%lu\n", PAGE_SIZE);
 	return 0;
 }
 
diff --git a/arch/microblaze/kernel/entry-nommu.S b/arch/microblaze/kernel/entry-nommu.S
index 8cc18cd2cce..ca84368570b 100644
--- a/arch/microblaze/kernel/entry-nommu.S
+++ b/arch/microblaze/kernel/entry-nommu.S
@@ -588,3 +588,31 @@ sys_rt_sigsuspend_wrapper:
 #include "syscall_table.S"
 
 syscall_table_size=(.-sys_call_table)
+
+type_SYSCALL:
+	.ascii "SYSCALL\0"
+type_IRQ:
+	.ascii "IRQ\0"
+type_IRQ_PREEMPT:
+	.ascii "IRQ (PREEMPTED)\0"
+type_SYSCALL_PREEMPT:
+	.ascii " SYSCALL (PREEMPTED)\0"
+
+	/*
+	 * Trap decoding for stack unwinder
+	 * Tuples are (start addr, end addr, string)
+	 * If return address lies on [start addr, end addr],
+	 * unwinder displays 'string'
+	 */
+
+	.align 4
+.global microblaze_trap_handlers
+microblaze_trap_handlers:
+	/* Exact matches come first */
+	.word ret_to_user  ; .word ret_to_user    ; .word type_SYSCALL
+	.word ret_from_intr; .word ret_from_intr  ; .word type_IRQ
+	/* Fuzzy matches go here */
+	.word ret_from_intr; .word no_intr_resched; .word type_IRQ_PREEMPT
+	.word work_pending ; .word no_work_pending; .word type_SYSCALL_PREEMPT
+	/* End of table */
+	.word 0             ; .word 0               ; .word 0
diff --git a/arch/microblaze/kernel/entry.S b/arch/microblaze/kernel/entry.S
index c0ede25c5b9..304882e5645 100644
--- a/arch/microblaze/kernel/entry.S
+++ b/arch/microblaze/kernel/entry.S
@@ -48,128 +48,107 @@
  */
 #if CONFIG_XILINX_MICROBLAZE0_USE_MSR_INSTR
 	.macro	clear_bip
-	msrclr	r11, MSR_BIP
-	nop
+	msrclr	r0, MSR_BIP
 	.endm
 
 	.macro	set_bip
-	msrset	r11, MSR_BIP
-	nop
+	msrset	r0, MSR_BIP
 	.endm
 
 	.macro	clear_eip
-	msrclr	r11, MSR_EIP
-	nop
+	msrclr	r0, MSR_EIP
 	.endm
 
 	.macro	set_ee
-	msrset	r11, MSR_EE
-	nop
+	msrset	r0, MSR_EE
 	.endm
 
 	.macro	disable_irq
-	msrclr	r11, MSR_IE
-	nop
+	msrclr	r0, MSR_IE
 	.endm
 
 	.macro	enable_irq
-	msrset	r11, MSR_IE
-	nop
+	msrset	r0, MSR_IE
 	.endm
 
 	.macro	set_ums
-	msrset	r11, MSR_UMS
-	nop
-	msrclr	r11, MSR_VMS
-	nop
+	msrset	r0, MSR_UMS
+	msrclr	r0, MSR_VMS
 	.endm
 
 	.macro	set_vms
-	msrclr	r11, MSR_UMS
-	nop
-	msrset	r11, MSR_VMS
-	nop
+	msrclr	r0, MSR_UMS
+	msrset	r0, MSR_VMS
+	.endm
+
+	.macro	clear_ums
+	msrclr	r0, MSR_UMS
 	.endm
 
 	.macro	clear_vms_ums
-	msrclr	r11, MSR_VMS
-	nop
-	msrclr	r11, MSR_UMS
-	nop
+	msrclr	r0, MSR_VMS | MSR_UMS
 	.endm
 #else
 	.macro	clear_bip
 	mfs	r11, rmsr
-	nop
 	andi	r11, r11, ~MSR_BIP
 	mts	rmsr, r11
-	nop
 	.endm
 
 	.macro	set_bip
 	mfs	r11, rmsr
-	nop
 	ori	r11, r11, MSR_BIP
 	mts	rmsr, r11
-	nop
 	.endm
 
 	.macro	clear_eip
 	mfs	r11, rmsr
-	nop
 	andi	r11, r11, ~MSR_EIP
 	mts	rmsr, r11
-	nop
 	.endm
 
 	.macro	set_ee
 	mfs	r11, rmsr
-	nop
 	ori	r11, r11, MSR_EE
 	mts	rmsr, r11
-	nop
 	.endm
 
 	.macro	disable_irq
 	mfs	r11, rmsr
-	nop
 	andi	r11, r11, ~MSR_IE
 	mts	rmsr, r11
-	nop
 	.endm
 
 	.macro	enable_irq
 	mfs	r11, rmsr
-	nop
 	ori	r11, r11, MSR_IE
 	mts	rmsr, r11
-	nop
 	.endm
 
 	.macro set_ums
 	mfs	r11, rmsr
-	nop
 	ori	r11, r11, MSR_VMS
 	andni	r11, r11, MSR_UMS
 	mts	rmsr, r11
-	nop
 	.endm
 
 	.macro	set_vms
 	mfs	r11, rmsr
-	nop
 	ori	r11, r11, MSR_VMS
 	andni	r11, r11, MSR_UMS
 	mts	rmsr, r11
-	nop
+	.endm
+
+	.macro	clear_ums
+	mfs	r11, rmsr
+	andni	r11, r11, MSR_UMS
+	mts	rmsr,r11
 	.endm
 
 	.macro	clear_vms_ums
 	mfs	r11, rmsr
-	nop
 	andni	r11, r11, (MSR_VMS|MSR_UMS)
 	mts	rmsr,r11
-	nop
 	.endm
 #endif
 
@@ -180,18 +159,22 @@
 
 /* turn on virtual protected mode save */
 #define VM_ON		\
-	set_ums;		\
+	set_ums;	\
 	rted	r0, 2f;	\
-2: nop;
+	nop; \
+2:
 
 /* turn off virtual protected mode save and user mode save*/
 #define VM_OFF			\
-	clear_vms_ums;			\
+	clear_vms_ums;		\
 	rted	r0, TOPHYS(1f);	\
-1: nop;
+	nop; \
+1:
 
 #define SAVE_REGS \
 	swi	r2, r1, PTO+PT_R2;	/* Save SDA */			\
+	swi	r3, r1, PTO+PT_R3;					\
+	swi	r4, r1, PTO+PT_R4;					\
 	swi	r5, r1, PTO+PT_R5;					\
 	swi	r6, r1, PTO+PT_R6;					\
 	swi	r7, r1, PTO+PT_R7;					\
@@ -218,14 +201,14 @@
 	swi	r30, r1, PTO+PT_R30;					\
 	swi	r31, r1, PTO+PT_R31;	/* Save current task reg */	\
 	mfs	r11, rmsr;		/* save MSR */			\
-	nop;								\
 	swi	r11, r1, PTO+PT_MSR;
 
 #define RESTORE_REGS \
 	lwi	r11, r1, PTO+PT_MSR;					\
 	mts	rmsr , r11;						\
-	nop;								\
 	lwi	r2, r1, PTO+PT_R2;	/* restore SDA */		\
+	lwi	r3, r1, PTO+PT_R3;					\
+	lwi	r4, r1, PTO+PT_R4;					\
 	lwi	r5, r1, PTO+PT_R5;					\
 	lwi	r6, r1, PTO+PT_R6;					\
 	lwi	r7, r1, PTO+PT_R7;					\
@@ -252,6 +235,39 @@
 	lwi	r30, r1, PTO+PT_R30;					\
 	lwi	r31, r1, PTO+PT_R31;	/* Restore cur task reg */
 
+#define SAVE_STATE	\
+	swi	r1, r0, TOPHYS(PER_CPU(ENTRY_SP)); /* save stack */	\
+	/* See if already in kernel mode.*/				\
+	mfs	r1, rmsr;						\
+	andi	r1, r1, MSR_UMS;					\
+	bnei	r1, 1f;						\
+	/* Kernel-mode state save.  */					\
+	/* Reload kernel stack-ptr. */					\
+	lwi	r1, r0, TOPHYS(PER_CPU(ENTRY_SP));			\
+	/* FIXME: I can add these two lines to one */			\
+	/* tophys(r1,r1); */						\
+	/* addik	r1, r1, -STATE_SAVE_SIZE; */			\
+	addik	r1, r1, CONFIG_KERNEL_BASE_ADDR - CONFIG_KERNEL_START - STATE_SAVE_SIZE; \
+	SAVE_REGS							\
+	brid	2f;							\
+	swi	r1, r1, PTO+PT_MODE; 	 				\
+1:	/* User-mode state save.  */					\
+	lwi	r1, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); /* get saved current */\
+	tophys(r1,r1);							\
+	lwi	r1, r1, TS_THREAD_INFO;	/* get the thread info */	\
+	/* MS these three instructions can be added to one */		\
+	/* addik	r1, r1, THREAD_SIZE; */				\
+	/* tophys(r1,r1); */						\
+	/* addik	r1, r1, -STATE_SAVE_SIZE; */			\
+	addik r1, r1, THREAD_SIZE + CONFIG_KERNEL_BASE_ADDR - CONFIG_KERNEL_START - STATE_SAVE_SIZE; \
+	SAVE_REGS							\
+	lwi	r11, r0, TOPHYS(PER_CPU(ENTRY_SP));			\
+	swi	r11, r1, PTO+PT_R1; /* Store user SP.  */		\
+	swi	r0, r1, PTO + PT_MODE; /* Was in user-mode.  */		\
+	/* MS: I am clearing UMS even in case when I come from kernel space */ \
+	clear_ums; 							\
+2:	lwi	CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE));
+
 .text
 
 /*
@@ -267,45 +283,23 @@
  * are masked. This is nice, means we don't have to CLI before state save
  */
 C_ENTRY(_user_exception):
-	swi	r1, r0, TOPHYS(PER_CPU(ENTRY_SP)) /* save stack */
 	addi	r14, r14, 4	/* return address is 4 byte after call */
-	swi	r11, r0, TOPHYS(r0_ram + PTO + PT_R11);	/* Save r11 */
-
-	lwi	r11, r0, TOPHYS(PER_CPU(KM));/* See if already in kernel mode.*/
-	beqi	r11, 1f;		/* Jump ahead if coming from user */
-/* Kernel-mode state save. */
-	lwi	r11, r0, TOPHYS(PER_CPU(ENTRY_SP)); /* Reload kernel stack-ptr*/
-	tophys(r1,r11);
-	swi	r11, r1, (PT_R1-PT_SIZE); /* Save original SP. */
-	lwi	r11, r0, TOPHYS(r0_ram + PTO + PT_R11); /* restore r11 */
-
-	addik	r1, r1, -STATE_SAVE_SIZE; /* Make room on the stack. */
-	SAVE_REGS
-
-	addi	r11, r0, 1; 		/* Was in kernel-mode. */
-	swi	r11, r1, PTO+PT_MODE; /* pt_regs -> kernel mode */
-	brid	2f;
-	nop;				/* Fill delay slot */
+	swi	r1, r0, TOPHYS(PER_CPU(ENTRY_SP)) /* save stack */
 
-/* User-mode state save.  */
-1:
-	lwi	r11, r0, TOPHYS(r0_ram + PTO + PT_R11);	/* restore r11 */
 	lwi	r1, r0, TOPHYS(PER_CPU(CURRENT_SAVE)); /* get saved current */
 	tophys(r1,r1);
 	lwi	r1, r1, TS_THREAD_INFO;	/* get stack from task_struct */
-/* calculate kernel stack pointer from task struct 8k */
-	addik	r1, r1, THREAD_SIZE;
-	tophys(r1,r1);
-
-	addik	r1, r1, -STATE_SAVE_SIZE; /* Make room on the stack.  */
+	/* MS these three instructions can be added to one */
+	/* addik	r1, r1, THREAD_SIZE; */
+	/* tophys(r1,r1); */
+	/* addik	r1, r1, -STATE_SAVE_SIZE; */
+	addik r1, r1, THREAD_SIZE + CONFIG_KERNEL_BASE_ADDR - CONFIG_KERNEL_START - STATE_SAVE_SIZE;
 	SAVE_REGS
 
-	swi	r0, r1, PTO+PT_MODE;			/* Was in user-mode. */
 	lwi	r11, r0, TOPHYS(PER_CPU(ENTRY_SP));
 	swi	r11, r1, PTO+PT_R1;		/* Store user SP.  */
-	addi	r11, r0, 1;
-	swi	r11, r0, TOPHYS(PER_CPU(KM));	/* Now we're in kernel-mode.  */
-2:	lwi	CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE));
+	clear_ums;
+	lwi	CURRENT_TASK, r0, TOPHYS(PER_CPU(CURRENT_SAVE));
 	/* Save away the syscall number.  */
 	swi	r12, r1, PTO+PT_R0;
 	tovirt(r1,r1)
@@ -316,10 +310,8 @@ C_ENTRY(_user_exception):
  * register should point to the location where
  * the called function should return.  [note that MAKE_SYS_CALL uses label 1] */
 
-	# Step into virtual mode.
-	set_vms;
-	addik	r11, r0, 3f
-	rtid	r11, 0
+	/* Step into virtual mode */
+	rtbd	r0, 3f
 	nop
 3:
 	lwi	r11, CURRENT_TASK, TS_THREAD_INFO /* get thread info */
@@ -363,24 +355,17 @@ C_ENTRY(_user_exception):
 	# Find and jump into the syscall handler.
 	lwi	r12, r12, sys_call_table
 	/* where the trap should return need -8 to adjust for rtsd r15, 8 */
-	la	r15, r0, ret_from_trap-8
+	addi	r15, r0, ret_from_trap-8
 	bra	r12
 
 	/* The syscall number is invalid, return an error.  */
 5:
+	rtsd	r15, 8;		/* looks like a normal subroutine return */
 	addi	r3, r0, -ENOSYS;
-	rtsd	r15,8;		/* looks like a normal subroutine return */
-	or 	r0, r0, r0
-
 
 /* Entry point used to return from a syscall/trap */
 /* We re-enable BIP bit before state restore */
 C_ENTRY(ret_from_trap):
-	set_bip;			/*  Ints masked for state restore*/
-	lwi	r11, r1, PTO+PT_MODE;
-/* See if returning to kernel mode, if so, skip resched &c.  */
-	bnei	r11, 2f;
-
 	swi	r3, r1, PTO + PT_R3
 	swi	r4, r1, PTO + PT_R4
 
@@ -413,32 +398,19 @@ C_ENTRY(ret_from_trap):
 	andi	r11, r11, _TIF_SIGPENDING;
 	beqi	r11, 1f;		/* Signals to handle, handle them */
 
-	la	r5, r1, PTO;		/* Arg 1: struct pt_regs *regs */
+	addik	r5, r1, PTO;		/* Arg 1: struct pt_regs *regs */
 	addi	r7, r0, 1;		/* Arg 3: int in_syscall */
 	bralid	r15, do_signal;	/* Handle any signals */
 	add	r6, r0, r0;		/* Arg 2: sigset_t *oldset */
 
 /* Finally, return to user state.  */
-1:
-	lwi	r3, r1, PTO + PT_R3; /* restore syscall result */
-	lwi	r4, r1, PTO + PT_R4;
-
-	swi	r0, r0, PER_CPU(KM);	/* Now officially in user state. */
+1:	set_bip;			/*  Ints masked for state restore */
 	swi	CURRENT_TASK, r0, PER_CPU(CURRENT_SAVE); /* save current */
 	VM_OFF;
 	tophys(r1,r1);
 	RESTORE_REGS;
 	addik	r1, r1, STATE_SAVE_SIZE		/* Clean up stack space.  */
 	lwi	r1, r1, PT_R1 - PT_SIZE;/* Restore user stack pointer. */
-	bri	6f;
-
-/* Return to kernel state.  */
-2:	VM_OFF;
-	tophys