+#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));
+