--- a/packages/hal/cortexm/arch/current/src/vectors.S
+++ b/packages/hal/cortexm/arch/current/src/vectors.S
@@ -8,7 +8,7 @@
 // ####ECOSGPLCOPYRIGHTBEGIN####                                            
 // -------------------------------------------                              
 // This file is part of eCos, the Embedded Configurable Operating System.   
-// Copyright (C) 2008, 2011 Free Software Foundation, Inc.                        
+// Copyright (C) 2008, 2011, 2012 Free Software Foundation, Inc.                        
 //
 // eCos is free software; you can redistribute it and/or modify it under    
 // the terms of the GNU General Public License as published by the Free     
@@ -40,6 +40,7 @@
 //#####DESCRIPTIONBEGIN####
 //
 // Author(s):    nickg
+// Contributors(s): ilijak
 // Date:         2008-07-30
 // Description:  This file defines the code placed into the exception
 //               vectors. It also contains the first level default VSRs
@@ -57,6 +58,11 @@
 #include <pkgconf/kernel.h>
 #endif
 
+#include <cyg/hal/cortexm_fpu.h>
+#ifdef CYGHWR_HAL_CORTEXM_FPU
+# include <cyg/hal/hal_arch.inc>
+#endif
+
 #include <cyg/hal/variant.inc>
 
 //==========================================================================
@@ -162,6 +168,10 @@ hal_switch_state_vsr:
 hal_default_exception_vsr:
 
         mrs     r0,psp                  // Get process stack
+
+#ifdef  CYGSEM_HAL_DEBUG_FPU
+        hal_fpu_exc_push                // Save Floating Point Unit context
+#endif
         sub     r1,r0,#(4*12)           // Make space for saved state
         msr     psp,r1                  // Ensure PSP is up to date
 
@@ -169,13 +179,16 @@ hal_default_exception_vsr:
         mrs     r2,ipsr                 // R2 = vector number
         mrs     r3,basepri              // R3 = basepri
         stmfd   r0!,{r1-r11,lr}         // Push type, vector, basepri, r4-11
-
         mov     r4,r0                   // R4 = saved state pointer
 
         bl      hal_deliver_exception
 
         mov     r0,r4                   // R0 = state saved across call
         ldmfd   r0!,{r1-r11,lr}         // Pop type, vec, basepri, registers and LR
+
+#ifdef  CYGSEM_HAL_DEBUG_FPU
+        hal_fpu_exc_pop                 // Update Floating Point Unit context
+#endif
         msr     psp,r0                  // Restore PSP
         msr     basepri,r3              // Restore basepri
 
@@ -244,9 +257,12 @@ hal_default_interrupt_vsr:
 hal_pendable_svc_vsr:
 
         mrs     r12,psp                 // R12 = thread's PSP
-        sub     r0,r12,#0x20            // Make space for frame
+        sub     r0,r12,#HAL_SAVEDREG_AUTO_FRAME_SIZE            // Make space for frame
         msr     psp,r0                  // Put it back
 
+#ifdef CYGARC_CORTEXM_FPU_EXC_AUTOSAVE
+        hal_fpu_isr_fake_frame_push
+#endif // CYGARC_CORTEXM_FPU_EXC_AUTOSAVE
         ldr     r3,=0x01000000          // R3 = PSR = thumb bit set
         ldr     r2,=hal_interrupt_end   // R2 = PC = interrupt end entry point
         ldr     r1,=hal_interrupt_end_done // R1 = LR = restore code
@@ -273,8 +289,8 @@ hal_pendable_svc_vsr:
         .type   hal_interrupt_end_done, %function
 hal_interrupt_end_done:
 
-        ldr     r3,=hal_interrupt_end_vsr
-        swi 0
+        ldr  r3,=hal_interrupt_end_vsr
+        swi  0
 
 //==========================================================================
 // Interrupt end VSR
@@ -291,7 +307,7 @@ hal_interrupt_end_done:
 hal_interrupt_end_vsr:
 
         mrs     r12,psp                 // R12 = thread's PSP
-        add     r12,#32                 // Skip our saved state
+        add     r12,#HAL_SAVEDREG_AUTO_FRAME_SIZE                 // Skip our saved state
         msr     psp,r12                 // Restore thread's PSP
 
         bx      lr                      // And return
@@ -338,5 +354,40 @@ hal_default_svc_vsr:
 
         .pool
 
+#ifdef CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+//==========================================================================
+// Usage Fault VSR
+// Note: This VSR is also attached to HardFault vector.
+// At present this VSR is used to detect FPU usage for Lazy context switch.
+// after saving processor context on the process stack call
+// hal_deliver_usagefault_fpu_exception() to do the job and retun result in r0.
+// If result indicates no FPU activity then jump in hal_default_exception_vsr.
+        .global hal_usagefault_exception_vsr
+        .thumb
+        .thumb_func
+        .type   hal_usagefault_exception_vsr, %function
+hal_usagefault_exception_vsr:
+
+        mrs     r0,psp                  // Get process stack
+        sub     r1,r0,#(2*4)            // Make space for saved state
+        msr     psp,r1                  // Ensure PSP is up to date
+        stmfd   r0!,{r4,lr}             // save registers
+        mov     r4,r0                   // R4 = saved state pointer
+
+        bl      hal_deliver_usagefault_fpu_exception
+
+        mov     r1,r4                   // R0 = state saved across call
+        ldmfd   r1!,{r4,lr}             // Restore registers
+        msr     psp,r1                  // Restore PSP
+
+        cmp     r0,#0                   // Exception other than FPU?
+        bne     hal_default_exception_vsr // Y: - process it
+
+        bx      lr                      // N: Return
+
+        .pool
+
+#endif // CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+
 //==========================================================================
 // end of vectors.S