diff --git a/trunk/bootrom/build/libraries/os/common/os_exception.c b/trunk/bootrom/build/libraries/os/common/os_exception.c
index c1fa31a..3419551 100644
--- a/trunk/bootrom/build/libraries/os/common/os_exception.c
+++ b/trunk/bootrom/build/libraries/os/common/os_exception.c
@@ -58,21 +58,29 @@ void osInitException( void )
  *---------------------------------------------------------------------------*/
 ASM void i_osUndefInstHandler( void )
 {
-        // change into previous mode
-        mrs     sp, spsr
-        orr     sp, sp, #HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
-        msr     cpsr_fsxc, sp
-
+        // change into system mode
+        msr     cpsr_fsxc, #HW_PSR_SYS_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
         stmfd   sp!, {r0, r12, lr, pc}      // push to previous mode stack (8byte alignment)
 
-        // get LR and SPSR into UNDEF mode
+        // get LR and SPSR of UNDEF mode
         msr     cpsr_fsxc, #HW_PSR_UNDEF_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
         mov     r12, lr
         mrs     r0,  spsr
 
-        // change into previous mode
-        orr     sp, r0, #HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
-        msr     cpsr_fsxc, sp
+        tst     r0, #HW_PSR_THUMB_STATE
+        bne     i_osUndefInstTerminate
+
+        tst     r0, #HW_PSR_JAVA_STATE
+        bne     i_osUndefInstTerminate
+
+        // permit for only system and user mode to use VFP
+        and     lr, r0, #HW_PSR_CPU_MODE_MASK
+        cmp     lr, #HW_PSR_SYS_MODE
+        cmpne   lr, #HW_PSR_USR_MODE
+        bne     i_osUndefInstTerminate
+
+        // change into system mode
+        msr     cpsr_fsxc, #HW_PSR_SYS_MODE | HW_PSR_IRQ_DISABLE | HW_PSR_FIQ_DISABLE
 
         tst     r0, #HW_PSR_THUMB_STATE
         subne   r12, r12, #2                // Thumb code return address
@@ -83,12 +91,6 @@ ASM void i_osUndefInstHandler( void )
 
         adr     lr, i_return
 
-        tst     r0, #HW_PSR_THUMB_STATE
-        bne     i_osUndefInstTerminate
-
-        tst     r0, #HW_PSR_JAVA_STATE
-        bne     i_osUndefInstTerminate
-
         ldr     r0,  [r12, #0]              // called from ARM
         mov     r12, r0, lsl #4
         sub     r12, r12, #0xC0000000
@@ -101,6 +103,7 @@ ASM void i_osUndefInstHandler( void )
 
 i_return
         ldmfd   sp!, {r0-r3}                // pop spsr
+        // restore into previous mode
         msr     cpsr_fsxc, r0
         ldmfd   sp!, {r0, r12, lr, pc}      // pop from USR stack