changeset 3208:614e724c4b7a

Add: hardware floating point support for Cortex-M4F [ Bugzilla 1001607 ]
author vae
date Sat, 09 Mar 2013 17:13:06 +0000
parents 323cb5ba4487
children ea9c78f2cd61
files packages/hal/cortexm/arch/current/ChangeLog packages/hal/cortexm/arch/current/cdl/hal_cortexm.cdl packages/hal/cortexm/arch/current/cdl/hal_cortexm_fpu.cdl packages/hal/cortexm/arch/current/include/cortexm_core.h packages/hal/cortexm/arch/current/include/cortexm_fpu.h packages/hal/cortexm/arch/current/include/cortexm_regs.h packages/hal/cortexm/arch/current/include/cortexm_stub.h packages/hal/cortexm/arch/current/include/fpv4_sp_d16.h packages/hal/cortexm/arch/current/include/hal_arch.h packages/hal/cortexm/arch/current/include/hal_arch.inc packages/hal/cortexm/arch/current/include/hal_intr.h packages/hal/cortexm/arch/current/include/hal_io.h packages/hal/cortexm/arch/current/src/context.S packages/hal/cortexm/arch/current/src/cortexm_fpu.c packages/hal/cortexm/arch/current/src/cortexm_stub.c packages/hal/cortexm/arch/current/src/fpv4_sp_d16.c packages/hal/cortexm/arch/current/src/hal_misc.c packages/hal/cortexm/arch/current/src/vectors.S
diffstat 18 files changed, 1437 insertions(+), 104 deletions(-) [+]
line wrap: on
line diff
--- a/packages/hal/cortexm/arch/current/ChangeLog
+++ b/packages/hal/cortexm/arch/current/ChangeLog
@@ -1,3 +1,24 @@
+2013-02-10  Ilija Kocho  <ilijak@siva.com.mk>
+            Jonathan Larmour  <jifl@eCosCentric.com>
+
+	* cdl/hal_cortexm.cdl:
+	* cdl/hal_cortexm_fpu.cdl:   New
+	* include/cortexm_core.h:    New
+	* include/cortexm_fpu.h:     New
+	* include/cortexm_regs.h:
+	* include/cortexm_stub.h:
+	* include/fpv4_sp_d16.h:     New
+	* include/hal_arch.h:
+	* include/hal_arch.inc:      New
+	* include/hal_io.h:
+	* src/context.S:
+	* src/cortexm_fpu.c:         New
+	* src/cortexm_stub.c:
+	* src/fpv4_sp_d16.c:         New
+	* src/hal_misc.c:
+	* src/vectors.S:
+	Add: hardware floating point support for Cortex-M4F [ Bugzilla 1001607 ]
+
 2012-04-23  Ilija Kocho  <ilijak@siva.com.mk>
 
     * cdl/hal_cortexm.cdl:
--- a/packages/hal/cortexm/arch/current/cdl/hal_cortexm.cdl
+++ b/packages/hal/cortexm/arch/current/cdl/hal_cortexm.cdl
@@ -113,20 +113,23 @@ cdl_package CYGPKG_HAL_CORTEXM {
         flavor           data
         legal_values     { "EXTERNAL" "INTERNAL" }
         default_value    { "EXTERNAL" }
-        description      "
-             Select the Cortex-M system tick timer clock source."
+        description      "Select the Cortex-M system tick timer clock source."
     }
 
-    cdl_interface CYGINT_HAL_CORTEXM_FPU {
-        display "The platform and architecture contains floating point unit."
-    }
+    cdl_component CYGPKG_HAL_CORTEXM_FPU {
+        display        "Floating Point Support"
+        flavor         data
+        calculated     { !CYGHWR_HAL_CORTEXM_FPU ? "SOFT" :
+                          "HARD: " . CYGHWR_HAL_CORTEXM_FPU_SWITCH }
+        no_define
+        active_if      { CYGHWR_HAL_CORTEXM == "M4" }
+        description    "
+            Floating point arithmetics can be executed in software or,
+            on devices with floating point unit - FPU, in hardware.
+            FPU is optional on Cortex-M4 architecture, and is present
+            on Cortex-M4F cores."
 
-    cdl_option CYGHWR_HAL_CORTEXM_FPU {
-        display          "Floating Point Unit"
-        active_if        { CYGINT_HAL_CORTEXM_FPU != 0 }
-        default_value    CYGINT_HAL_CORTEXM_FPU
-        description      "
-            Use the Floating Point Unit."
+        script         hal_cortexm_fpu.cdl
     }
 
     cdl_option CYGIMP_HAL_ARCH_ENDIAN {
@@ -166,6 +169,92 @@ cdl_package CYGPKG_HAL_CORTEXM {
             description   "
                 This option determines the number of breakpoints supported by the HAL."
     }
+
+    cdl_component CYGBLD_ARCH_CPUFLAGS {
+        display      "Architecture related build flags"
+        flavor data
+        no_define
+
+        calculated { CYGHWR_HAL_CORTEXM == "M3" ? " -mcpu=cortex-m3" :
+                     CYGHWR_HAL_CORTEXM_FPU     ? " -mcpu=cortex-m4" : " " }
+
+        description "This component defines flags that code generation dependent on specific
+            CPU type or CPU features."
+
+        cdl_option CYGBLD_ARCH_CPUFLAGX_M3 {
+            display    "Exclude Cortex-M3 build flag"
+            flavor     data
+            no_define
+
+            calculated { CYGHWR_HAL_CORTEXM_FPU ? "-mcpu=cortex-m3" : "no_exclude" }
+        }
+
+        cdl_option CYGBLD_ARCH_CPUFLAGX_M4 {
+            display    "Exclude Cortex-M4 build flag"
+            flavor     data
+            no_define
+
+            calculated { (CYGHWR_HAL_CORTEXM != "M4") ? "-mcpu=cortex-m4" : "no_exclude" }
+        }
+
+        cdl_component CYGBLD_ARCH_CPUFLAG_FLOAT_ABI {
+            display    "Float ABI build flags"
+            flavor     data
+            no_define
+            calculated { CYGHWR_HAL_CORTEXM_FPU ? " -mfloat-abi=hard" : "" }
+
+            cdl_option CYGBLD_ARCH_CPUFLAGX_FLOAT_ABI {
+                display    "Float ABI flag to exclude"
+                flavor     data
+                no_define
+                calculated { CYGHWR_HAL_CORTEXM_FPU ? "-mfloat-abi=soft" : "-mfloat-abi=hard" }
+            }
+
+            cdl_option CYGBLD_ARCH_CPUFLAGX_HARDSOFT_FLOAT {
+                display    "Alt. float flag to exclude"
+                flavor     data
+                no_define
+                calculated { CYGHWR_HAL_CORTEXM_FPU ? "-msoft-float" : "-mhard-float" }
+            }
+
+            cdl_component CYGBLD_ARCH_CPUFLAG_FPV4SPD16 {
+                display    "FPv4-SP-D16 flags"
+                flavor     data
+                no_define
+                calculated { CYGHWR_HAL_FPV4_SP_D16 ? " -mfpu=fpv4-sp-d16" : "" }
+
+                cdl_option CYGBLD_ARCH_CPUFLAGX_FPV4SPD16 {
+                    display    "Exclude FPv4-SP-D16 flag"
+                    flavor     data
+                    no_define
+                    calculated { CYGHWR_HAL_FPV4_SP_D16 ? "do_not_exclude" : "-mfpu=fpv4-sp-d16" }
+                }
+            }
+        }
+
+        requires {
+            !is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAGX_M3)             &&
+            !is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAGX_M4)             &&
+            !is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAGX_FLOAT_ABI)      &&
+            !is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAGX_HARDSOFT_FLOAT) &&
+            !is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAGX_FPV4SPD16)      &&
+
+            is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAGS)                 &&
+            is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAG_FLOAT_ABI)        &&
+            is_substr(CYGBLD_GLOBAL_CFLAGS, CYGBLD_ARCH_CPUFLAG_FPV4SPD16)
+        }
+        requires {
+            !is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAGX_M3)             &&
+            !is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAGX_M4)             &&
+            !is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAGX_FLOAT_ABI)      &&
+            !is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAGX_HARDSOFT_FLOAT) &&
+            !is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAGX_FPV4SPD16)      &&
+
+            is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAGS)                 &&
+            is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAG_FLOAT_ABI)        &&
+            is_substr(CYGBLD_GLOBAL_LDFLAGS, CYGBLD_ARCH_CPUFLAG_FPV4SPD16)
+        }
+    }
 }
 
 # EOF hal_cortexm.cdl
new file mode 100644
--- /dev/null
+++ b/packages/hal/cortexm/arch/current/cdl/hal_cortexm_fpu.cdl
@@ -0,0 +1,131 @@
+##==========================================================================
+##
+##      hal_cortexm_fpu.cdl
+##
+##      Cortex-M HAL Floating Point configuration data
+##
+##==========================================================================
+## ####ECOSGPLCOPYRIGHTBEGIN####                                            
+## -------------------------------------------                              
+## This file is part of eCos, the Embedded Configurable Operating System.   
+## Copyright (C) 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     
+## Software Foundation; either version 2 or (at your option) any later      
+## version.                                                                 
+##
+## eCos is distributed in the hope that it will be useful, but WITHOUT      
+## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
+## FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
+## for more details.                                                        
+##
+## You should have received a copy of the GNU General Public License        
+## along with eCos; if not, write to the Free Software Foundation, Inc.,    
+## 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
+##
+## As a special exception, if other files instantiate templates or use      
+## macros or inline functions from this file, or you compile this file      
+## and link it with other works to produce a work based on this file,       
+## this file does not by itself cause the resulting work to be covered by   
+## the GNU General Public License. However the source code for this file    
+## must still be made available in accordance with section (3) of the GNU   
+## General Public License v2.                                               
+##
+## This exception does not invalidate any other reasons why a work based    
+## on this file might be covered by the GNU General Public License.         
+## -------------------------------------------                              
+## ####ECOSGPLCOPYRIGHTEND####                                              
+##==========================================================================
+#######DESCRIPTIONBEGIN####
+##
+## Author(s):    ilijak
+## Date:         2012-03-03
+## Usage:        script hal_cortexm_fpu.cdl
+##
+######DESCRIPTIONEND####
+##
+##==========================================================================
+
+    #cdl_component CYGPKG_HAL_CORTEXM_FPU {
+        #display          "Floating Point Unit"
+        #flavor           none
+        #no_define
+
+        cdl_interface CYGINT_HAL_FPV4_SP_D16 {
+            display   "FPU is FPv4-SP-D16"
+            flavor     bool
+            implements CYGINT_HAL_CORTEXM_FPU
+        }
+
+        cdl_interface CYGINT_HAL_CORTEXM_FPU {
+            display  "FPU present."
+            flavor   data
+        }
+
+        cdl_component CYGHWR_HAL_CORTEXM_FPU {
+            display        "Use hardware FPU"
+            flavor         bool
+            active_if      { CYGINT_HAL_CORTEXM_FPU }
+            default_value  0
+            compile        cortexm_fpu.c
+
+            description "
+                Cortex-M4F cores have a single precision floating point unit.
+                This option enables FPU usage and provides related FPU control
+                options.
+                Hardware FPU enable, implies Cortex-M4 code generation, and
+                build flags shall be set accordingly, including -mcpu=cortex-m4.
+                As a side effect, the Cortex_M4 build flag will remain \(sticky\)
+                even if hardware FPU is subsequently disabled.
+                "
+
+            cdl_option CYGHWR_HAL_CORTEXM_FPU_SWITCH {
+                display       "FPU context switch"
+                flavor data
+                legal_values  { "ALL" "LAZY" "NONE" }
+                default_value { "LAZY" }
+                
+                requires { is_active (CYGPKG_KERNEL) implies 
+                    CYGTST_KERNEL_SKIP_MULTI_THREAD_FP_TEST ==
+                    (CYGHWR_HAL_CORTEXM_FPU_SWITCH == "NONE") }
+                
+                description   "
+                    This option selects the FPU context switching scheme.
+                    Straight-forward behaviour is to save and
+                    restore FPU state on every CPU context save/restore.
+                    While simple, robust and deterministic, this
+                    approach can be expensive if the FPU is used by
+                    few threads. The alternative schemes, available by this
+                    option, are to use hardware features that allow either:
+                    - LAZY: Save FPU context only if the thread makes use of
+                    the FPU. Where only few threads use FPU this should give
+                    shorter average context switching delay compared to ALL
+                    scheme. If more than one threads use FPU, the worst context
+                    switching time is typically worse than the one for ALL
+                    scheme.
+                    - ALL: Save FPU context for all threads. This is a simple
+                    scheme, which if all, or majority of threads use FPU may
+                    give better average context switching time than LAZY.
+                    This scheme also includes Lazy Stacking of FPU state
+                    for exceptions/interrupts.
+                    - NONE: No FPU state saving, this scheme adds no additional
+                    delay for saving of FPU state to context switching, but is
+                    only suitable if maximum one thread uses floating point."
+            }
+
+        cdl_option CYGHWR_HAL_FPV4_SP_D16 {
+            flavor bool
+            active_if { CYGINT_HAL_FPV4_SP_D16 }
+            calculated { CYGINT_HAL_FPV4_SP_D16 && CYGHWR_HAL_CORTEXM_FPU }
+            display     "FPv4-SP-D16"
+            no_define
+            compile     fpv4_sp_d16.c
+            description "
+                FPv4-SP-D16 is ARMv7 architecture single precision floating
+                point unit with 16 double precision / 32 single precision
+                registers. It is found on Cortex-M4F and Cortex-R5 cores."
+        }
+    #}
+
+# EOF hal_cortexm_fpu.cdl
new file mode 100644
--- /dev/null
+++ b/packages/hal/cortexm/arch/current/include/cortexm_core.h
@@ -0,0 +1,77 @@
+#ifndef CYGONCE_CORTEXM_CORE_H
+#define CYGONCE_CORTEXM_CORE_H
+//==========================================================================
+//
+//      cortexm_core.h
+//
+//      Cortex-M some core registers
+//
+//==========================================================================
+// ####ECOSGPLCOPYRIGHTBEGIN####                                            
+// -------------------------------------------                              
+// This file is part of eCos, the Embedded Configurable Operating System.   
+// Copyright (C) 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     
+// Software Foundation; either version 2 or (at your option) any later      
+// version.                                                                 
+//
+// eCos is distributed in the hope that it will be useful, but WITHOUT      
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
+// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
+// for more details.                                                        
+//
+// You should have received a copy of the GNU General Public License        
+// along with eCos; if not, write to the Free Software Foundation, Inc.,    
+// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
+//
+// As a special exception, if other files instantiate templates or use      
+// macros or inline functions from this file, or you compile this file      
+// and link it with other works to produce a work based on this file,       
+// this file does not by itself cause the resulting work to be covered by   
+// the GNU General Public License. However the source code for this file    
+// must still be made available in accordance with section (3) of the GNU   
+// General Public License v2.                                               
+//
+// This exception does not invalidate any other reasons why a work based    
+// on this file might be covered by the GNU General Public License.         
+// -------------------------------------------                              
+// ####ECOSGPLCOPYRIGHTEND####                                              
+//==========================================================================
+//#####DESCRIPTIONBEGIN####
+//
+// Author(s):      ilijak
+// Contributor(s):
+// Date:           2012-06-26
+// Description:    Some Cortex-M core register definitions
+//
+//####DESCRIPTIONEND####
+//
+//========================================================================
+
+
+// Coprocessor Access Control Register
+#define CYGARC_REG_FPU_CPACR 0xE000ED88
+
+#define CYGARC_REG_FPU_CPACR_ACCESS_DENIED     0
+#define CYGARC_REG_FPU_CPACR_ACCESS_PRIVILEGED 1
+#define CYGARC_REG_FPU_CPACR_ACCESS_RESERVED   2
+#define CYGARC_REG_FPU_CPACR_ACCESS_FULL       (CYGARC_REG_FPU_CPACR_ACCESS_PRIVILEGED | \
+                                               CYGARC_REG_FPU_CPACR_ACCESS_RESERVED)
+
+#define CYGARC_REG_FPU_CPACR_CP10(_access) ((_access) << 20)
+#define CYGARC_REG_FPU_CPACR_CP11(_access) ((_access) << 22)
+
+#define CYGARC_REG_FPU_CPACR_ENABLE                                       \
+           (CYGARC_REG_FPU_CPACR_CP10(CYGARC_REG_FPU_CPACR_ACCESS_FULL) | \
+            CYGARC_REG_FPU_CPACR_CP11(CYGARC_REG_FPU_CPACR_ACCESS_FULL))
+
+// CONTROL register
+// The CONTROL register is not memory mapped. Use CYGARC_MSR() and CYGARC_MRS().
+#define CYGARC_REG_CONTROL_PRIV_M       0x1
+#define CYGARC_REG_CONTROL_SPSEL_M      0x2
+#define CYGARC_REG_CONTROL_FPCA_M       0x4
+
+//==========================================================================
+#endif //CYGONCE_CORTEXM_CORE_H
new file mode 100644
--- /dev/null
+++ b/packages/hal/cortexm/arch/current/include/cortexm_fpu.h
@@ -0,0 +1,98 @@
+#ifndef CYGONCE_CORTEXM_FPU_H
+#define CYGONCE_CORTEXM_FPU_H
+//==========================================================================
+//
+//      cortexm_fpu.h
+//
+//      Cortex-M General Floating Point Unit definitions 
+//
+//==========================================================================
+// ####ECOSGPLCOPYRIGHTBEGIN####                                            
+// -------------------------------------------                              
+// This file is part of eCos, the Embedded Configurable Operating System.   
+// Copyright (C) 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     
+// Software Foundation; either version 2 or (at your option) any later      
+// version.                                                                 
+//
+// eCos is distributed in the hope that it will be useful, but WITHOUT      
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
+// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
+// for more details.                                                        
+//
+// You should have received a copy of the GNU General Public License        
+// along with eCos; if not, write to the Free Software Foundation, Inc.,    
+// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
+//
+// As a special exception, if other files instantiate templates or use      
+// macros or inline functions from this file, or you compile this file      
+// and link it with other works to produce a work based on this file,       
+// this file does not by itself cause the resulting work to be covered by   
+// the GNU General Public License. However the source code for this file    
+// must still be made available in accordance with section (3) of the GNU   
+// General Public License v2.                                               
+//
+// This exception does not invalidate any other reasons why a work based    
+// on this file might be covered by the GNU General Public License.         
+// -------------------------------------------                              
+// ####ECOSGPLCOPYRIGHTEND####                                              
+//==========================================================================
+//#####DESCRIPTIONBEGIN####
+//
+// Author(s):      ilijak
+// Contributor(s):
+// Date:           2012-04-25
+// Description:    Cortex-M4F General Floating Point Unit definitions and macros
+// Usage:          include <cyg/hal/cortexm_fpu.h>
+//
+//####DESCRIPTIONEND####
+//
+//========================================================================
+
+
+//===========================================================================
+// Floating Point Unit
+//
+// FPU is optional unit of Cortex-M4
+
+#include <pkgconf/hal.h>
+#include <pkgconf/hal_cortexm.h>
+#include <cyg/hal/cortexm_core.h>
+
+#ifdef CYGHWR_HAL_CORTEXM_FPU
+
+# if defined CYGSEM_HAL_ROM_MONITOR || defined CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS
+#  define CYGSEM_HAL_DEBUG_FPU
+# endif
+
+# ifdef CYGINT_HAL_FPV4_SP_D16
+#  include <cyg/hal/fpv4_sp_d16.h>
+# else
+#  error "Unknown FPU unit!"
+# endif
+
+#else // CYGHWR_HAL_CORTEXM_FPU
+
+# define CYGARC_CORTEXM_GDB_REG_FPA
+
+# define GDB_STUB_SAVEDREG_FRAME_TYPE(__type) (__type->u.type)
+
+# define HAL_SAVEDREG_AUTO_FRAME_SIZE (8*4)
+
+# define HAL_SAVEDREG_FPU_THREAD_S
+# define HAL_SAVEDREG_MAN_FPU_EXCEPTION_S
+# define HAL_SAVEDREG_AUTO_FPU_EXCEPTION_S
+# define HAL_THREAD_INIT_FPU_CONTEXT(__regs) CYG_EMPTY_STATEMENT
+
+# define GDB_STUB_SAVEDREG_FPU_THREAD_GET(__gdbreg,__regs) CYG_EMPTY_STATEMENT
+# define GDB_STUB_SAVEDREG_FPU_THREAD_SET(__gdbreg,__regs) CYG_EMPTY_STATEMENT
+# define GDB_STUB_SAVEDREG_FPU_EXCEPTION_GET(__gdbreg,__regs) CYG_EMPTY_STATEMENT
+# define GDB_STUB_SAVEDREG_FPU_EXCEPTION_SET(__gdbreg,__regs) CYG_EMPTY_STATEMENT
+
+#endif// CYGHWR_HAL_CORTEXM_FPU
+
+
+//==========================================================================
+#endif //CYGONCE_CORTEXM_FPU_H
--- a/packages/hal/cortexm/arch/current/include/cortexm_regs.h
+++ b/packages/hal/cortexm/arch/current/include/cortexm_regs.h
@@ -52,6 +52,11 @@
 
 #ifndef __ASSEMBLER__
 
+//--------------------------------------------------------------------------
+// No operation
+#define CYGARC_NOP() { __asm__ volatile( "nop" ); }
+
+
 //---------------------------------------------------------------------------
 // Change processor state instructions
 
@@ -76,6 +81,37 @@
 #define CYGARC_REVSH( _swapped_, _origin_ ) \
         __asm__ volatile ("revsh %0, %1\n" : "=r"(_swapped_) : "r"(_origin_))
 
+//------------------------------------------------------------------------
+// Barrier instructions
+// Data Synchronization Barrier
+#define CYGARC_DSB() __asm__ volatile( "dsb" )
+// Instruction Synchronization Barrier
+#define CYGARC_ISB() __asm__ volatile( "isb" )
+
+#define HAL_MEMORY_BARRIER() \
+CYG_MACRO_START              \
+    CYGARC_DSB();            \
+    CYGARC_ISB();            \
+CYG_MACRO_END
+
+//----------------------------------------------------------------------------
+// MSR instuctions
+// Special register instructions
+#define CYGARC_MSR(_reg_, _val_) \
+        __asm__ volatile ("msr " #_reg_", %0\n" : : "r"(_val_))
+
+#define CYGARC_MRS(_val_, _reg_) \
+        __asm__ volatile ("mrs %0," #_reg_ "\n" : "=r"(_val_) : )
+
+//----------------------------------------------------------------------------
+// VFP instuctions
+// Special floating point unit register instructions
+#define CYGARC_VMSR(_reg_, _val_) \
+        __asm__ volatile ("vmsr " #_reg_", %0\n" : : "r"(_val_))
+
+#define CYGARC_VMRS(_val_, _reg_) \
+        __asm__ volatile ("vmrs %0," #_reg_ "\n" : "=r"(_val_) : )
+
 #endif // __ASSEMBLER__
 
 //==========================================================================
--- a/packages/hal/cortexm/arch/current/include/cortexm_stub.h
+++ b/packages/hal/cortexm/arch/current/include/cortexm_stub.h
@@ -10,7 +10,7 @@
 // ####ECOSGPLCOPYRIGHTBEGIN####                                            
 // -------------------------------------------                              
 // This file is part of eCos, the Embedded Configurable Operating System.   
-// Copyright (C) 2008 Free Software Foundation, Inc.                        
+// Copyright (C) 2008, 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     
@@ -41,7 +41,8 @@
 //==========================================================================
 //#####DESCRIPTIONBEGIN####
 //
-// Author(s):    nickg
+// Author(s):      nickg
+// Contributor(s): ilijak
 // Date:         2008-07-30
 //
 //####DESCRIPTIONEND####
@@ -49,30 +50,33 @@
 //========================================================================
 */
 
+#include <pkgconf/system.h>
+#include <pkgconf/hal.h>
+
+#ifdef CYGHWR_HAL_CORTEXM_FPU
+#include <cyg/hal/cortexm_fpu.h>
+#endif
 
 #ifdef __cplusplus
 extern "C" {
 #endif
-
+#ifndef CYGHWR_HAL_CORTEXM_FPU
 // The ARM has float (and possibly other coprocessor) registers that are
 // larger than it can hold in a target_register_t.
-#define TARGET_HAS_LARGE_REGISTERS
+# define TARGET_HAS_LARGE_REGISTERS
 
 // ARM stub has special needs for register handling (not all regs are the
 // the same size), so special put_register and get_register are provided.
-#define CYGARC_STUB_REGISTER_ACCESS_DEFINED 1
+# define CYGARC_STUB_REGISTER_ACCESS_DEFINED 1
 
-#define NUMREGS    (16+8+2)  // 16 GPR, 8 FPR (unused), 2 PS
-
-#define REGSIZE( _x_ ) (((_x_) < F0 || (_x_) >= FPS) ? 4 : 12)
+# define NUMREGS    (16+8+2)  // 16 GPR, 8 FPR (unused), 2 PS
 
-// Comment out to allow for default gdb stub packet buffer which is larger.
-// #define NUMREGBYTES ((16*4)+(8*12)+(2*4))
+# define REGSIZE( _x_ ) (((_x_) < F0 || (_x_) >= FPS) ? 4 : 12)
 
-#ifndef TARGET_REGISTER_T_DEFINED
-#define TARGET_REGISTER_T_DEFINED
+# ifndef TARGET_REGISTER_T_DEFINED
+#  define TARGET_REGISTER_T_DEFINED
 typedef unsigned long target_register_t;
-#endif
+# endif
 
 enum regnames {
     R0, R1, R2, R3, R4, R5, R6, R7,
@@ -81,13 +85,15 @@ enum regnames {
     FPS, PS
 };
 
-#define HAL_STUB_REGISTERS_SIZE \
- ((sizeof(HAL_CORTEXM_GDB_Registers) + sizeof(target_register_t) - 1) / sizeof(target_register_t))
+#endif // CYGHWR_HAL_CORTEXM_FPU
+
+# define HAL_STUB_REGISTERS_SIZE \
+   ((sizeof(HAL_CORTEXM_GDB_Registers) + sizeof(target_register_t) - 1) / sizeof(target_register_t))
 
-#define PS_N 0x80000000
-#define PS_Z 0x40000000
-#define PS_C 0x20000000
-#define PS_V 0x10000000
+# define PS_N 0x80000000
+# define PS_Z 0x40000000
+# define PS_C 0x20000000
+# define PS_V 0x10000000
 
 typedef enum regnames regnames_t;
 
new file mode 100644
--- /dev/null
+++ b/packages/hal/cortexm/arch/current/include/fpv4_sp_d16.h
@@ -0,0 +1,285 @@
+#ifndef CYGONCE_FPV4_SP_D16_H
+#define CYGONCE_FPV4_SP_D16_H
+//==========================================================================
+//
+//      fpv4_sp_d16.h
+//
+//      FPv4spD16 Floating Point Unit definitions 
+//
+//==========================================================================
+// ####ECOSGPLCOPYRIGHTBEGIN####                                            
+// -------------------------------------------                              
+// This file is part of eCos, the Embedded Configurable Operating System.   
+// Copyright (C) 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     
+// Software Foundation; either version 2 or (at your option) any later      
+// version.                                                                 
+//
+// eCos is distributed in the hope that it will be useful, but WITHOUT      
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
+// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
+// for more details.                                                        
+//
+// You should have received a copy of the GNU General Public License        
+// along with eCos; if not, write to the Free Software Foundation, Inc.,    
+// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
+//
+// As a special exception, if other files instantiate templates or use      
+// macros or inline functions from this file, or you compile this file      
+// and link it with other works to produce a work based on this file,       
+// this file does not by itself cause the resulting work to be covered by   
+// the GNU General Public License. However the source code for this file    
+// must still be made available in accordance with section (3) of the GNU   
+// General Public License v2.                                               
+//
+// This exception does not invalidate any other reasons why a work based    
+// on this file might be covered by the GNU General Public License.         
+// -------------------------------------------                              
+// ####ECOSGPLCOPYRIGHTEND####                                              
+//==========================================================================
+//#####DESCRIPTIONBEGIN####
+//
+// Author(s):      ilijak
+// Contributor(s):
+// Date:           2012-04-25
+// Description:    FPv4spD16 Floating Point Unit definitions and macros
+// Usage:          include <cyg/hal/fpv4_sp_d16.h>
+//
+//####DESCRIPTIONEND####
+//
+//========================================================================
+
+#if defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL
+#define CYGARC_CORTEXM_FPU_EXC_AUTOSAVE
+#endif
+
+//===========================================================================
+// Floating-point Context Control Register
+#define CYGARC_REG_FPU_FPCCR 0xE000EF34
+
+#define CYGARC_REG_FPU_FPCCR_LSPACT   0x1
+#define CYGARC_REG_FPU_FPCCR_USER     0x2
+#define CYGARC_REG_FPU_FPCCR_THREAD   0x8
+#define CYGARC_REG_FPU_FPCCR_HFRDY    0x10
+#define CYGARC_REG_FPU_FPCCR_MMRDY    0x20
+#define CYGARC_REG_FPU_FPCCR_BFRDY    0x40
+#define CYGARC_REG_FPU_FPCCR_MONRDY   0x100
+#define CYGARC_REG_FPU_FPCCR_LSPEN    0x40000000
+#define CYGARC_REG_FPU_FPCCR_ASPEN    0x80000000
+
+#define HAL_CORTEXM_FPU_ENABLE()                      \
+CYG_MACRO_START                                       \
+    cyg_uint32 regval;                                \
+    HAL_READ_UINT32(CYGARC_REG_FPU_CPACR, regval);    \
+    regval |= CYGARC_REG_FPU_CPACR_ENABLE;            \
+    HAL_WRITE_UINT32(CYGARC_REG_FPU_CPACR, regval);   \
+    HAL_MEMORY_BARRIER();                             \
+CYG_MACRO_END
+
+#define HAL_CORTEXM_FPU_DISABLE()                     \
+CYG_MACRO_START                                       \
+    cyg_uint32 regval;                                \
+    HAL_READ_UINT32(CYGARC_REG_FPU_CPACR, regval);    \
+    regval &= ~CYGARC_REG_FPU_CPACR_ENABLE;           \
+    HAL_WRITE_UINT32(CYGARC_REG_FPU_CPACR, regval);   \
+    HAL_MEMORY_BARRIER();                             \
+CYG_MACRO_END
+
+#ifndef __ASSEMBLER__
+__externC void hal_init_fpu(void);
+#endif
+
+// Floating-point Context Address Register
+#define CYGARC_REG_FPU_FPCAR 0xE000EF38
+
+// Floating-point Default Status Control Register
+#define CYGARC_REG_FPU_FPDSCR 0xE000EF3C
+
+#define CYGARC_REG_FPU_FPDSCR_FZ            BIT_(24)
+#define CYGARC_REG_FPU_FPDSCR_DN            BIT_(25)
+#define CYGARC_REG_FPU_FPDSCR_AHP           BIT_(26)
+
+#define CYGARC_REG_FPU_FPDSCR_ROUND(__mode) VALUE_(22, (__mode))
+// where __mode is:
+#define CYGARC_REG_FPU_FPDSCR_ROUND_RN      0
+#define CYGARC_REG_FPU_FPDSCR_ROUND_RP      1
+#define CYGARC_REG_FPU_FPDSCR_ROUND_RM      2
+#define CYGARC_REG_FPU_FPDSCR_ROUND_RZ      3
+
+//==========================================================================
+// FPU Context
+#define HAL_SAVEDREGISTERS_WITH_FPU        0x80
+
+#define HAL_SAVEDREGISTERS_THREAD_FPU      (HAL_SAVEDREGISTERS_THREAD | \
+                                            HAL_SAVEDREGISTERS_WITH_FPU)
+
+#define HAL_SAVEDREGISTERS_EXCEPTION_FPU   (HAL_SAVEDREGISTERS_EXCEPTION | \
+                                            HAL_SAVEDREGISTERS_WITH_FPU)
+
+#ifndef CYGARC_CORTEXM_FPU_EXC_AUTOSAVE
+
+// Without automatic contex saving during exception or interrupt
+# define HAL_SAVEDREGISTERS_FPU_THREAD_CONTEXT_SIZE   (HAL_SAVEDREG_THREAD_FPU_N*4+4)
+# define HAL_SAVEDREG_AUTO_FRAME_SIZE                 (8*4)
+
+# define HAL_SAVEDREG_AUTO_FPU_EXCEPTION_S
+
+#else //  !CYGARC_CORTEXM_FPU_EXC_AUTOSAVE
+
+// With automatic contex saving during exception or interrupt enabled
+# if defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL
+
+#  define HAL_SAVEDREG_AUTO_EXCEPTION_FPU_N    16
+#  define HAL_SAVEDREG_AUTO_FRAME_SIZE         (8*4 + 16*4 + 4 + 4)
+
+// HAL_SavedRegisters entries for floating point registers
+//     see hal_arch.h for HAL_SavedRegisters definition.
+
+#  define HAL_SAVEDREG_AUTO_FPU_EXCEPTION_S                        \
+            cyg_uint32  s_auto[HAL_SAVEDREG_AUTO_EXCEPTION_FPU_N]; \
+            cyg_uint32  fpscr_auto;                                \
+            cyg_uint32  aligner
+
+# else // defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL
+#  error  "Automatic FPU context saving is not supported in LAZY and NONE modes."
+# endif // defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL
+#endif //  !CYGARC_CORTEXM_FPU_EXC_AUTOSAVE
+
+// Common for AUTOSAVE and non AUTOSAVE
+#if defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL || defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+
+// HAL_SavedRegisters entries for floating point registers
+//     see hal_arch.h for HAL_SavedRegisters definition.
+
+# define HAL_SAVEDREG_THREAD_FPU_N         32
+# define HAL_SAVEDREG_EXCEPTION_FPU_N      32
+
+# define HAL_SAVEDREG_FPU_THREAD_S                      \
+           cyg_uint32  fpscr;                           \
+           cyg_uint32  s[HAL_SAVEDREG_THREAD_FPU_N]
+
+# define HAL_SAVEDREG_FPU_EXCEPTION_S                   \
+           cyg_uint32  s[HAL_SAVEDREG_EXCEPTION_FPU_N]; \
+           cyg_uint32  fpscr;                           \
+           cyg_uint32  cpacr
+
+// Thread FP context initialization
+# define HAL_THREAD_INIT_FPU_REGS(__regs_p)                                 \
+CYG_MACRO_START                                                             \
+    int __reg_i;                                                            \
+    for(__reg_i = 0; __reg_i < HAL_SAVEDREG_THREAD_FPU_N; __reg_i++)    \
+        (__regs_p)->u.thread.s[__reg_i] = 0;                                \
+CYG_MACRO_END
+
+# define HAL_THREAD_INIT_FPU_CONTEXT(__regs_p)                              \
+CYG_MACRO_START                                                             \
+    HAL_THREAD_INIT_FPU_REGS(__regs_p);                                     \
+    (__regs_p)->u.thread.fpscr = 0;                                         \
+CYG_MACRO_END
+#else //defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL || defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+
+#  define HAL_SAVEDREG_FPU_THREAD_S
+#  define HAL_THREAD_INIT_FPU_CONTEXT(__regs) CYG_EMPTY_STATEMENT
+
+#endif //defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL || defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+//==========================================================================
+// hal_arch.h GDB stub support
+
+// Register layout expected by GDB VFP
+#ifndef __ASSEMBLER__
+typedef struct {
+    cyg_uint32 gpr[16];
+    cyg_uint32 xpsr;
+    cyg_uint32 s[32];
+    cyg_uint32 fpscr;
+} HAL_CORTEXM_GDB_Registers;
+#endif
+
+#if defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL || defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+
+# define GDB_STUB_SAVEDREG_FRAME_TYPE(__regs)                        \
+         ((__regs)->u.type & ~HAL_SAVEDREGISTERS_WITH_FPU)
+
+# define GDB_STUB_SAVEDREG_FPU_THREAD_GET(__gdbreg,__regs)           \
+CYG_MACRO_START                                                      \
+    cyg_uint32 reg_i;                                                \
+    for( reg_i = 0; reg_i < HAL_SAVEDREG_THREAD_FPU_N; reg_i++ ) \
+        (__gdbreg)->s[reg_i] = (__regs)->u.thread.s[reg_i];          \
+    (__gdbreg)->fpscr = (__regs)->u.thread.fpscr;                    \
+CYG_MACRO_END
+
+# define GDB_STUB_SAVEDREG_FPU_THREAD_SET(__gdbreg,__regs)           \
+CYG_MACRO_START                                                      \
+    cyg_uint32 reg_i;                                                \
+    for( reg_i = 0; reg_i < HAL_SAVEDREG_THREAD_FPU_N; reg_i++ ) \
+        (__regs)->u.thread.s[reg_i] = (__gdbreg)->s[reg_i];          \
+    (__regs)->u.thread.fpscr = (__gdbreg)->fpscr;                    \
+CYG_MACRO_END
+
+#else //  defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL || defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+
+# define GDB_STUB_SAVEDREG_FRAME_TYPE(__regs)              ((__regs)->u.type)
+# define GDB_STUB_SAVEDREG_FPU_THREAD_GET(__gdbreg,__regs) CYG_EMPTY_STATEMENT
+# define GDB_STUB_SAVEDREG_FPU_THREAD_SET(__gdbreg,__regs) CYG_EMPTY_STATEMENT
+
+#endif //  defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL || defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+
+#define GDB_STUB_SAVEDREG_FPU_EXCEPTION_GET(__gdbreg,__regs)        \
+CYG_MACRO_START                                                     \
+    cyg_uint32 reg_i;                                               \
+    for( reg_i = 0; reg_i < HAL_SAVEDREG_EXCEPTION_FPU_N; reg_i++ ) \
+        (__gdbreg)->s[reg_i] = (__regs)->u.exception.s[reg_i];      \
+    (__gdbreg)->fpscr = (__regs)->u.exception.fpscr;                \
+CYG_MACRO_END
+
+#define GDB_STUB_SAVEDREG_FPU_EXCEPTION_SET(__gdbreg,__regs)                      \
+CYG_MACRO_START                                                                   \
+    cyg_uint32 reg_i;                                                             \
+    for( reg_i = 0; reg_i < HAL_SAVEDREG_EXCEPTION_FPU_N; reg_i++ )               \
+        (__regs)->u.exception.s[reg_i] = (__gdbreg)->s[reg_i];                    \
+    (__regs)->u.exception.fpscr = (__gdbreg)->fpscr;                              \
+    if(*(cyg_uint32 *)CYGARC_REG_FPU_FPCCR & CYGARC_REG_FPU_FPCCR_ASPEN) {        \
+        for( reg_i = 0; reg_i < HAL_SAVEDREG_AUTO_EXCEPTION_FPU_N; reg_i++ )      \
+            (__regs)->u.exception.s_auto[reg_i] = (__regs)->u.exception.s[reg_i]; \
+        (__regs)->u.exception.fpscr_auto = (__regs)->u.exception.fpscr;           \
+    }                                                                             \
+CYG_MACRO_END
+
+//==========================================================================
+// hal_arch.h Minimal and sensible stack sizes:
+// Override value in hal_arch.h
+#define CYGNUM_HAL_STACK_CONTEXT_SIZE (4 * (20+32+4+4))
+
+// GDB stub ==================================================================
+// cortexm_stub.h definitions for FPV4-SP-D16
+
+// The Cortex-M4F double registers are larger then target_register_t.
+#define TARGET_HAS_LARGE_REGISTERS
+
+// Cortex-M4F stub register handling macros
+#define CYGARC_STUB_REGISTER_ACCESS_DEFINED 1
+#define NUMREGS    (FPSCR+1)  // 16 GPR, XPSR, 10 non existent, 16 VFP, FPSCR
+#define REGSIZE( _x_ ) (_x_ <= PC ? 4 :                         \
+                        (_x_ < XPSR ? 0 :                        \
+                         (_x_ == XPSR ? 4 :                      \
+                          (((_x_ >= VD0) && (_x_ <= VD15)) ? 8 : \
+                           (_x_ == FPSCR ? 4 : 0 )))))
+#ifndef __ASSEMBLER__
+# ifndef TARGET_REGISTER_T_DEFINED
+#  define TARGET_REGISTER_T_DEFINED
+typedef unsigned long target_register_t;
+# endif
+
+enum regnames {
+    R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, R10, R11, R12, SP, LR, PC,
+    XPSR = 25,
+    VD0 = 26, VD1, VD2, VD3, VD4, VD5, VD6, VD7,
+    VD8, VD9, VD10, VD11, VD12, VD13, VD14, VD15,
+    FPSCR
+};
+#endif // __ASSEMBLER__
+
+//==========================================================================
+#endif //CYGONCE_FPV4_SP_D16_H
--- a/packages/hal/cortexm/arch/current/include/hal_arch.h
+++ b/packages/hal/cortexm/arch/current/include/hal_arch.h
@@ -10,7 +10,7 @@
 // ####ECOSGPLCOPYRIGHTBEGIN####                                            
 // -------------------------------------------                              
 // This file is part of eCos, the Embedded Configurable Operating System.   
-// Copyright (C) 2008 Free Software Foundation, Inc.                        
+// Copyright (C) 2008, 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     
@@ -41,20 +41,28 @@
 //==========================================================================
 //#####DESCRIPTIONBEGIN####
 //
-// Author(s):    nickg
-// Date:         2008-07-30
-// Description:  Define architecture abstractions
+// Author(s):      nickg
+// Contributor(s): ilijak
+// Date:           2008-07-30
+// Description:    Define architecture abstractions
 //
 //####DESCRIPTIONEND####
 //
 //========================================================================
 */
 
+#ifndef __ASSEMBLER__
+
 #include <pkgconf/system.h>
 #include <pkgconf/hal.h>
 #include <cyg/infra/cyg_type.h>
 
 #include <cyg/hal/var_arch.h>
+#include <cyg/hal/cortexm_regs.h>
+
+#include <cyg/hal/cortexm_fpu.h>
+
+#endif //__ASSEMBLER__
 
 //==========================================================================
 // CPU save state
@@ -64,21 +72,32 @@
 // for each context. This makes the GDB state get/put slightly more
 // complex, but that is a suitable compromise.
 
+#define HAL_SAVEDREGISTERS_EXCEPTION    1
+#define HAL_SAVEDREGISTERS_THREAD       2
+#define HAL_SAVEDREGISTERS_INTERRUPT    3
+
+#ifndef __ASSEMBLER__
+
 typedef struct
 {
     union
     {
         cyg_uint32              type;           // State type
 
+        // Thread
         struct
         {
             cyg_uint32          type;           // State type
             cyg_uint32          basepri;        // BASEPRI
             cyg_uint32          sp;             // SP (R13)
+
+            HAL_SAVEDREG_FPU_THREAD_S;          // Floating Point Unit context
+
             cyg_uint32          r[13];          // R0..R12
             cyg_uint32          pc;             // PC/LR
         } thread;
 
+        // Exception
         struct
         {
             cyg_uint32          type;           // State type
@@ -87,7 +106,9 @@ typedef struct
 
             cyg_uint32          r4_11[8];       // Remaining CPU registers
             cyg_uint32          xlr;            // Exception return LR
-
+#ifdef CYGSEM_HAL_DEBUG_FPU
+            HAL_SAVEDREG_FPU_EXCEPTION_S;   // Floating Point Unit context
+#endif
             // The following are saved and restored automatically by the CPU
             // for exceptions or interrupts.
 
@@ -99,8 +120,11 @@ typedef struct
             cyg_uint32          lr;
             cyg_uint32          pc;
             cyg_uint32          psr;
+
+            HAL_SAVEDREG_AUTO_FPU_EXCEPTION_S; // Floating Point Unit context
         } exception;
 
+        // Interrupt
         struct
         {
             cyg_uint32          type;           // State type
@@ -117,20 +141,21 @@ typedef struct
             cyg_uint32          pc;
             cyg_uint32          psr;
 
+            HAL_SAVEDREG_AUTO_FPU_EXCEPTION_S; // Floating Point Unit context
         } interrupt;
     } u;
 
 } HAL_SavedRegisters;
 
-#define HAL_SAVEDREGISTERS_EXCEPTION    1
-#define HAL_SAVEDREGISTERS_THREAD       2
-#define HAL_SAVEDREGISTERS_INTERRUPT    3
-
 //==========================================================================
 // Thread context initialization
 
+#ifndef HAL_THREAD_INIT_FPU_CONTEXT
+#define HAL_THREAD_INIT_FPU_CONTEXT(__regs) CYG_EMPTY_STATEMENT
+#endif
+
 #define HAL_THREAD_INIT_CONTEXT( __sparg, __thread, __entry, __id )     \
-{                                                                       \
+CYG_MACRO_START                                                         \
     register CYG_WORD __sp = ((CYG_WORD)__sparg) & ~7;                  \
     register CYG_WORD *__ep = (CYG_WORD *)(__sp -= sizeof(CYG_WORD));   \
     register HAL_SavedRegisters *__regs;                                \
@@ -140,15 +165,16 @@ typedef struct
     __regs->u.type = HAL_SAVEDREGISTERS_THREAD;                         \
     for( __i = 1; __i < 13; __i++ )                                     \
         __regs->u.thread.r[__i] = 0;                                    \
+    HAL_THREAD_INIT_FPU_CONTEXT(__regs);                                \
     *__ep = (CYG_WORD)(__entry);                                        \
     __regs->u.thread.sp       = (CYG_WORD)(__sp);                       \
     __regs->u.thread.r[0]     = (CYG_WORD)(__thread);                   \
     __regs->u.thread.r[1]     = (CYG_WORD)(__id);                       \
-    __regs->u.thread.r[11]     = (CYG_WORD)(__ep);                      \
+    __regs->u.thread.r[11]    = (CYG_WORD)(__ep);                       \
     __regs->u.thread.pc       = (CYG_WORD)__entry;                      \
     __regs->u.thread.basepri  = 0;                                      \
     __sparg = (CYG_ADDRESS)__regs;                                      \
-}
+CYG_MACRO_END
 
 //==========================================================================
 // Context switch macros.
@@ -169,17 +195,30 @@ typedef struct
 
 //==========================================================================
 // Fetch PC from saved state
-
-#define CYGARC_HAL_GET_PC_REG(__regs,__val)                             \
-{                                                                       \
-    switch( (__regs)->u.type )                                          \
-    {                                                                   \
-    case HAL_SAVEDREGISTERS_THREAD   : (__val) = (__regs)->u.thread.pc; break; \
+#if defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL || \
+    defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+#define CYGARC_HAL_GET_PC_REG(__regs,__val)                                       \
+{                                                                                 \
+    switch(GDB_STUB_SAVEDREG_FRAME_TYPE(__regs))                                                    \
+    {                                                                             \
+    case HAL_SAVEDREGISTERS_THREAD:    (__val) = (__regs)->u.thread.pc; break;    \
     case HAL_SAVEDREGISTERS_EXCEPTION: (__val) = (__regs)->u.exception.pc; break; \
     case HAL_SAVEDREGISTERS_INTERRUPT: (__val) = (__regs)->u.interrupt.pc; break; \
-    }                                                                   \
+    default: (__val) = 0;                                                         \
+    }                                                                             \
 }
-
+#else
+#define CYGARC_HAL_GET_PC_REG(__regs,__val)                                       \
+{                                                                                 \
+    switch( (__regs)->u.type )                                                    \
+    {                                                                             \
+    case HAL_SAVEDREGISTERS_THREAD   : (__val) = (__regs)->u.thread.pc; break;    \
+    case HAL_SAVEDREGISTERS_EXCEPTION: (__val) = (__regs)->u.exception.pc; break; \
+    case HAL_SAVEDREGISTERS_INTERRUPT: (__val) = (__regs)->u.interrupt.pc; break; \
+    default: (__val) = 0;                                                         \
+    }                                                                             \
+}
+#endif
 //==========================================================================
 // Exception handling function
 // This function is defined by the kernel according to this prototype. It is
@@ -241,7 +280,8 @@ externC void cyg_hal_deliver_exception( 
 //==========================================================================
 // GDB support
 
-// Register layout expected by GDB
+#ifdef CYGARC_CORTEXM_GDB_REG_FPA
+// Register layout expected by GDB FPA
 typedef struct
 {
     cyg_uint32  gpr[16];
@@ -254,8 +294,9 @@ typedef struct
     cyg_uint32  f6[3];
     cyg_uint32  f7[3];
     cyg_uint32  fps;
-    cyg_uint32  ps;
+    cyg_uint32  xpsr;
 } HAL_CORTEXM_GDB_Registers;
+#endif
 
 // Translate a stack pointer as saved by the thread context macros
 // into a pointer to a HAL_SavedRegisters structure. On the Cortex-M
@@ -313,7 +354,9 @@ typedef cyg_uint32 hal_jmp_buf[CYGARC_JM
 #define CYGNUM_HAL_STACK_FRAME_SIZE (4 * 20)
 
 // Stack needed for a context switch
+#if !defined CYGNUM_HAL_STACK_CONTEXT_SIZE
 #define CYGNUM_HAL_STACK_CONTEXT_SIZE (4 * 20)
+#endif
 
 // Interrupt + call to ISR, interrupt_end() and the DSR
 #define CYGNUM_HAL_STACK_INTERRUPT_SIZE \
@@ -341,5 +384,7 @@ typedef cyg_uint32 hal_jmp_buf[CYGARC_JM
 #define CYGARC_HAL_SAVE_GP()
 #define CYGARC_HAL_RESTORE_GP()
 
+#endif // __ASSEMBLER__
+
 //==========================================================================
 #endif //CYGONCE_HAL_ARCH_H
new file mode 100644
--- /dev/null
+++ b/packages/hal/cortexm/arch/current/include/hal_arch.inc
@@ -0,0 +1,115 @@
+/*==========================================================================
+//
+//      hal_arch.inc
+//
+//      Cortex-M exception vector macros
+//
+//==========================================================================
+// ####ECOSGPLCOPYRIGHTBEGIN####                                            
+// -------------------------------------------                              
+// This file is part of eCos, the Embedded Configurable Operating System.   
+// Copyright (C) 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     
+// Software Foundation; either version 2 or (at your option) any later      
+// version.                                                                 
+//
+// eCos is distributed in the hope that it will be useful, but WITHOUT      
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
+// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
+// for more details.                                                        
+//
+// You should have received a copy of the GNU General Public License        
+// along with eCos; if not, write to the Free Software Foundation, Inc.,    
+// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
+//
+// As a special exception, if other files instantiate templates or use      
+// macros or inline functions from this file, or you compile this file      
+// and link it with other works to produce a work based on this file,       
+// this file does not by itself cause the resulting work to be covered by   
+// the GNU General Public License. However the source code for this file    
+// must still be made available in accordance with section (3) of the GNU   
+// General Public License v2.                                               
+//
+// This exception does not invalidate any other reasons why a work based    
+// on this file might be covered by the GNU General Public License.         
+// -------------------------------------------                              
+// ####ECOSGPLCOPYRIGHTEND####                                              
+//==========================================================================
+//#####DESCRIPTIONBEGIN####
+//
+// Author(s):       ilijak
+// Contributors(s):
+// Date:            2012-07-08
+// Description:     This file defines some GAS macros exception VSRs.
+//
+//####DESCRIPTIONEND####
+//
+//========================================================================
+*/
+
+#ifdef CYGHWR_HAL_CORTEXM_FPU
+
+# ifdef CYGINT_HAL_FPV4_SP_D16
+
+//============================================================================
+// LAZY context switching scheme keeps FPU disabled for the threads that
+// don't use floating point. We need to enable it before we save FPU context
+// in order to avoid Usage Fault exception.
+
+        .macro hal_fpu_enable
+        ldr    r1,=CYGARC_REG_FPU_CPACR
+        ldr    r2,[r1]
+        stmdb  r0!,{r2}           // Save thread's CPACR state
+        orr    r2,#CYGARC_REG_FPU_CPACR_ENABLE
+        str    r2,[r1]
+        .endm
+
+//============================================================================
+// Restore thread's FPU usage state.
+// undo hal_fpu_enable
+
+        .macro hal_fpu_undo_enable
+        ldmia  r0!,{r2}           // Retrieve previous thread's CPACR state
+        ldr    r1,=CYGARC_REG_FPU_CPACR
+        str    r2,[r1]
+        .endm
+
+//============================================================================
+// Store FPU context during exception if FPU was disabled then enamble it.
+
+        .macro hal_fpu_exc_push
+        hal_fpu_enable
+        vmrs         r1,fpscr
+        stmdb        r0!,{r1}
+        vstmdb.f64   r0!,{d0-d15}
+        .endm
+
+//============================================================================
+// Restore FPU context during exception and undo FPU enable.
+
+        .macro hal_fpu_exc_pop
+        vldmia.f64   r0!,{d0-d15}
+        ldmia        r0!,{r1}
+        vmsr         fpscr,r1
+        hal_fpu_undo_enable
+        .endm
+
+//============================================================================
+// Make fake fpu frame for hal_pendable_svc_vsr
+
+        .macro hal_fpu_isr_fake_frame_push
+        sub        r12,#4
+        vmrs       r1,fpscr
+        stmdb      r12!,{r1}
+        vstmdb.f32 r12!,{s0-s15}
+        .endm
+
+# else // CYGINT_HAL_FPV4_SP_D16
+#  error Unknown Floating Point Unit!
+# endif // CYGINT_HAL_FPV4_SP_D16
+
+#endif //CYGHWR_HAL_CORTEXM_FPU
+
+// end of hal_arch.inc
--- a/packages/hal/cortexm/arch/current/include/hal_intr.h
+++ b/packages/hal/cortexm/arch/current/include/hal_intr.h
@@ -368,6 +368,16 @@ typedef cyg_uint32  CYG_INTERRUPT_STATE;
     *(__pvalue) = __value;                                              \
 }
 
+#define HAL_CLOCK_READ_NS( __pvalue )                                              \
+CYG_MACRO_START                                                                                  \
+    cyg_uint32 __period, __value;                                                  \
+    HAL_READ_UINT32(CYGARC_REG_SYSTICK_BASE+CYGARC_REG_SYSTICK_RELOAD, __period ); \
+    HAL_READ_UINT32(CYGARC_REG_SYSTICK_BASE+CYGARC_REG_SYSTICK_VALUE, __value );   \
+    __value = (( __period + 1 ) - __value) * 1000;                                          \
+    __value /= (hal_cortexm_systick_clock / 1000000 );                  \
+    *(__pvalue) = __value;                                                         \
+CYG_MACRO_END
+
 #define HAL_CLOCK_LATENCY( __pvalue ) HAL_CLOCK_READ( __pvalue )
 
 #endif // CYGHWR_HAL_CLOCK_DEFINED
--- a/packages/hal/cortexm/arch/current/include/hal_io.h
+++ b/packages/hal/cortexm/arch/current/include/hal_io.h
@@ -216,6 +216,16 @@
 #define CYGARC_REG_NVIC_SHCSR_BUSFAULTACT       BIT_(1)
 #define CYGARC_REG_NVIC_SHCSR_MEMFAULTACT       BIT_(0)
 
+// Usage Fault register
+
+#define CYGARC_REG_UFSR                         0xE000ED2A
+#define CYGARC_REG_UFSR_DIVBYZERO               BIT_(9)
+#define CYGARC_REG_UFSR_UNALIGNED               BIT_(8)
+#define CYGARC_REG_UFSR_NOCP                    BIT_(3)
+#define CYGARC_REG_UFSR_INVPC                   BIT_(2)
+#define CYGARC_REG_UFSR_INVSTATE                BIT_(1)
+#define CYGARC_REG_UFSR_UNDEFINSTR              BIT_(0)
+
 #endif
 
 //==========================================================================
--- a/packages/hal/cortexm/arch/current/src/context.S
+++ b/packages/hal/cortexm/arch/current/src/context.S
@@ -8,7 +8,7 @@
 // ####ECOSGPLCOPYRIGHTBEGIN####                                            
 // -------------------------------------------                              
 // This file is part of eCos, the Embedded Configurable Operating System.   
-// Copyright (C) 2008 Free Software Foundation, Inc.                        
+// Copyright (C) 2008, 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     
@@ -39,9 +39,10 @@
 //==========================================================================
 //#####DESCRIPTIONBEGIN####
 //
-// Author(s):    nickg
-// Date:         2008-07-30
-// Description:  This file contains thread context switch code.
+// Author(s):      nickg, ilijak (FPU support)
+// Contributor(s):
+// Date:           2008-07-30
+// Description:    This file contains thread context switch code.
 //
 //####DESCRIPTIONEND####
 //
@@ -55,6 +56,12 @@
 #include <pkgconf/kernel.h>
 #endif
 
+#ifdef CYGHWR_HAL_CORTEXM_FPU
+#include <cyg/hal/hal_arch.h>
+#include <cyg/hal/cortexm_fpu.h>        // Optional Floating Point Unit
+
+#endif
+
 //==========================================================================
 
         .syntax unified
@@ -67,6 +74,132 @@
 // R0 contains a pointer to the SP of the thread to load, R1 contains
 // a pointer to the SP of the current thread.
 
+#ifdef CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+//==========================================================================
+// LAZY Context switch
+//
+// Save the FPU registers only for threads that use FPU.
+//
+
+        .globl  hal_thread_switch_context
+        .thumb
+        .thumb_func
+        .type   hal_thread_switch_context, %function
+hal_thread_switch_context:
+
+        push      {r0-r12,lr}                       // Push all savable register
+        ldr       r7,=CYGARC_REG_FPU_CPACR          //
+        ldr       r6,[r7]
+        tst       r6,#CYGARC_REG_FPU_CPACR_ENABLE   // Is FPU enabled?
+        beq       lazy_float_push                   // N: Be lazy.
+        vmrs      r5,fpscr                          // Y: Save FPU context
+        vpush.f64 {d0-d15}                          //   Push FPU registers
+        push      {r5}                              //   together with FPSCR
+        mov       r2,#HAL_SAVEDREGISTERS_THREAD_FPU // Set state type: thread with FPU
+        b         float_push_join
+   lazy_float_push:
+        mov       r2,#HAL_SAVEDREGISTERS_THREAD
+        sub       sp,#HAL_SAVEDREGISTERS_FPU_THREAD_CONTEXT_SIZE
+   float_push_join:
+        mrs       r3,basepri                        // Get priority base register
+        mov       r4,sp                             // Get SP (for info only)
+        push      {r2-r4}                           // Push them
+        str       sp,[r1]                           // Save SP
+        // Fall through
+
+//--------------------------------------------------------------------------
+// Load context
+//
+// This is used to load a thread context, abandoning the current one. Following
+// function part is the second half of hal_thread_switch_context.
+
+
+        .globl  hal_thread_load_context
+        .thumb
+        .thumb_func
+        .type   hal_thread_load_context, %function
+hal_thread_load_context_common:
+
+        ldr      sp,[r0]                // Load SP
+        pop      {r2-r4}                // Pop type, basepri, SP(discarded) and FPSCR
+        msr      basepri,r3             // Set BASEPRI
+        cmp      r2,#HAL_SAVEDREGISTERS_THREAD_FPU // Is state type a FP thread?
+        beq      real_float_pop // Y:
+                                // N: Be lazy
+        and      r6,#~CYGARC_REG_FPU_CPACR_ENABLE
+        add      sp,#HAL_SAVEDREGISTERS_FPU_THREAD_CONTEXT_SIZE
+        str      r6,[r7]
+        pop      {r0-r12,pc} // Pop all registers and return to an non-FP thread
+
+   real_float_pop: // Restore floating point context for FP thread.
+        orr      r6,#CYGARC_REG_FPU_CPACR_ENABLE
+        pop      {r5}           // Retrieve FPU status register
+        str      r6,[r7]
+        dsb
+        isb
+        vpop.f64 {d0-d15}       // Pestore FPU registers
+        vmsr     fpscr,r5       // Restore FPU status register
+        pop      {r0-r12,pc}    // Pop all registers and return to a FP thread.
+
+// Load context entry point
+// Load context initially needs CPACR register, so it is provided here.
+hal_thread_load_context:
+        ldr      r7,=CYGARC_REG_FPU_CPACR
+        ldr      r6,[r7]
+        b        hal_thread_load_context_common
+
+#elif defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL
+
+//==========================================================================
+// ALL Context switch
+//
+// Save the FPU registers for all threads.
+//
+
+        .globl  hal_thread_switch_context
+        .thumb
+        .thumb_func
+        .type   hal_thread_switch_context, %function
+hal_thread_switch_context:
+
+        push      {r0-r12,lr}                       // Push all savable register
+        vmrs      r5,fpscr
+        mov       r2,#HAL_SAVEDREGISTERS_THREAD_FPU // Set state type == thread
+        vpush.f64 {d0-d15}                          // Push FPU registers
+        sub       r4,sp,#4                          // Get SP (for info only)
+        mrs       r3,basepri                        // Get priority base register
+        push      {r2-r5}                           // Push them
+        str       sp,[r1]                           // Save SP
+        // Fall through
+
+//--------------------------------------------------------------------------
+// Load context
+//
+// This is used to load a thread context, abandoning the current one. This
+// function is also the second half of hal_thread_switch_context.
+
+
+        .globl  hal_thread_load_context
+        .thumb
+        .thumb_func
+        .type   hal_thread_load_context, %function
+hal_thread_load_context:
+
+        ldr      sp,[r0]                // Load SP
+        pop      {r2-r5}                // Pop type, basepri,SP(discarded) and FPSCR
+        msr      basepri,r3             // Set BASEPRI
+        vpop.f64 {d0-d15}               // Pop FPU registers
+        vmsr     fpscr,r5               // and FPU status register
+        pop      {r0-r12,pc}            // Pop all register and return
+
+#else
+
+//==========================================================================
+// NONE Context switch
+//
+// No FPU or NONE context saving scheme. Do not save FPU registers.
+//
+
         .globl  hal_thread_switch_context
         .thumb
         .thumb_func
@@ -100,6 +233,8 @@ hal_thread_load_context:
         msr     basepri,r3              // Set BASEPRI
         pop     {r0-r12,pc}             // Pop all register and return
 
+#endif
+
 //==========================================================================
 //  HAL longjmp, setjmp implementations
 //
new file mode 100644
--- /dev/null
+++ b/packages/hal/cortexm/arch/current/src/cortexm_fpu.c
@@ -0,0 +1,97 @@
+/*==========================================================================
+//
+//      cortexm_fpu.c
+//
+//      Cortex-M exception vectors
+//
+//==========================================================================
+// ####ECOSGPLCOPYRIGHTBEGIN####                                            
+// -------------------------------------------                              
+// This file is part of eCos, the Embedded Configurable Operating System.   
+// Copyright (C) 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     
+// Software Foundation; either version 2 or (at your option) any later      
+// version.                                                                 
+//
+// eCos is distributed in the hope that it will be useful, but WITHOUT      
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
+// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
+// for more details.                                                        
+//
+// You should have received a copy of the GNU General Public License        
+// along with eCos; if not, write to the Free Software Foundation, Inc.,    
+// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
+//
+// As a special exception, if other files instantiate templates or use      
+// macros or inline functions from this file, or you compile this file      
+// and link it with other works to produce a work based on this file,       
+// this file does not by itself cause the resulting work to be covered by   
+// the GNU General Public License. However the source code for this file    
+// must still be made available in accordance with section (3) of the GNU   
+// General Public License v2.                                               
+//
+// This exception does not invalidate any other reasons why a work based    
+// on this file might be covered by the GNU General Public License.         
+// -------------------------------------------                              
+// ####ECOSGPLCOPYRIGHTEND####                                              
+//==========================================================================
+//#####DESCRIPTIONBEGIN####
+//
+// Author(s):      ilijak
+// Contributor(s):
+// Date:           2012-05-30
+//
+//####DESCRIPTIONEND####
+//
+//========================================================================
+*/
+
+#include <pkgconf/hal.h>
+#include <pkgconf/hal_cortexm.h>
+
+#include <cyg/infra/diag.h>
+#include <cyg/infra/cyg_type.h>
+#include <cyg/infra/cyg_trac.h>         // tracing macros
+#include <cyg/infra/cyg_ass.h>          // assertion macros
+
+#include <cyg/hal/hal_arch.h>           // HAL header
+#include <cyg/hal/hal_intr.h>           // HAL header
+#include <cyg/hal/cortexm_regs.h>       // Special Cortex-M asm instructions
+
+#include <cyg/hal/cortexm_fpu.h>        // Optional Floating Point Unit
+
+#ifdef CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+//============================================================================
+// FPU Usage Fault VSR handler
+// Execution of Floating Point instruction when FPU is disabled
+// generates usage fault exception. In LAZY context switching scheme
+// it is used for detection of FPU usage by threads.
+//
+cyg_uint32 hal_deliver_usagefault_fpu_exception(void) {
+    cyg_uint32 regval;
+
+    HAL_READ_UINT32(CYGARC_REG_FPU_CPACR, regval);
+    if(!((regval & CYGARC_REG_FPU_CPACR_ENABLE) ^ CYGARC_REG_FPU_CPACR_ENABLE)){
+        CYG_FAIL("Usage fault exception other than FPU!!!");
+    } else {
+        HAL_READ_UINT32(CYGARC_REG_UFSR, regval);
+        if(regval & CYGARC_REG_UFSR_NOCP){
+            // Floating point instruction has occured
+            // Enable FPU
+            HAL_CORTEXM_FPU_ENABLE();
+            CYGARC_VMSR(fpscr, 0);
+            HAL_MEMORY_BARRIER();
+            HAL_WRITE_UINT32(CYGARC_REG_UFSR, CYGARC_REG_UFSR_NOCP);
+        } else {
+            CYG_FAIL("Usage fault exception other than FPU/NOCP!!!");
+        }
+    }
+    HAL_READ_UINT32(CYGARC_REG_UFSR, regval);
+    return regval;
+}
+#endif // CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+
+//==========================================================================
+// EOF cortexm_fpu.c
--- a/packages/hal/cortexm/arch/current/src/cortexm_stub.c
+++ b/packages/hal/cortexm/arch/current/src/cortexm_stub.c
@@ -8,7 +8,7 @@
 // ####ECOSGPLCOPYRIGHTBEGIN####                                            
 // -------------------------------------------                              
 // This file is part of eCos, the Embedded Configurable Operating System.   
-// Copyright (C) 2008 Free Software Foundation, Inc.                        
+// Copyright (C) 2008, 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     
@@ -39,8 +39,9 @@
 //==========================================================================
 //#####DESCRIPTIONBEGIN####
 //
-// Author(s):    nickg
-// Date:         2008-07-30
+// Author(s):      nickg
+// Contributor(s): ilijak, jifl
+// Date:           2008-07-30
 //
 //####DESCRIPTIONEND####
 //
@@ -85,6 +86,8 @@ int __computeSignal (unsigned int trap_n
     case CYGNUM_HAL_VECTOR_NMI:
     case CYGNUM_HAL_VECTOR_SYS_TICK:
         return SIGINT;
+    case CYGNUM_HAL_VECTOR_USAGE_FAULT:
+        return SIGFPE;
     default:
         return SIGTRAP;
     }
@@ -103,7 +106,7 @@ int __get_trap_number (void)
 
 
 //==========================================================================
-/* Set the currently-saved pc register value to PC. */
+// Set the currently-saved pc register value to PC.
 
 void set_pc (target_register_t pc)
 {
@@ -116,25 +119,16 @@ void set_pc (target_register_t pc)
 static int
 reg_offset(regnames_t reg)
 {
-    int base_offset;
-
-    if (reg < F0)
-        return reg * 4;
-
-    base_offset = 16 * 4;
+    int reg_i, offset = 0;
 
-    if (reg < FPS)
-        return base_offset + ((reg - F0) * 12);
-
-    base_offset += (8 * 12);
-
-    if (reg <= PS)
-        return base_offset + ((reg - FPS) * 4);
-
-    return -1;  // Should never happen!
+    for(reg_i = 0; reg_i < NUMREGS; reg_i++) {
+        if(reg_i == reg)
+            break;
+        offset += REGSIZE(reg_i);
+    }
+    return (NUMREGS == reg_i || 0 == REGSIZE(reg_i)) ? -1 : offset;
 }
 
-
 //==========================================================================
 // Return the currently-saved value corresponding to register REG of
 // the exception context.
@@ -288,9 +282,10 @@ int
 void __skipinst (void)
 {
     unsigned long pc = get_register(PC);
-
+    unsigned short a, b, *pc_p;
     pc += 2;
-
+    pc_p = (unsigned short *) pc; a=pc_p[0]; b=pc_p[1];
+    diag_printf("BP = 0x%04x 0x%04x\n", a, b);
     put_register(PC, pc);
 }
 
new file mode 100644
--- /dev/null
+++ b/packages/hal/cortexm/arch/current/src/fpv4_sp_d16.c
@@ -0,0 +1,105 @@
+//==========================================================================
+//
+//      fpv4_sp_d16.c
+//
+//      FPv4-SP-D16 support
+//
+//==========================================================================
+// ####ECOSGPLCOPYRIGHTBEGIN####                                            
+// -------------------------------------------                              
+// This file is part of eCos, the Embedded Configurable Operating System.   
+// Copyright (C) 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     
+// Software Foundation; either version 2 or (at your option) any later      
+// version.                                                                 
+//
+// eCos is distributed in the hope that it will be useful, but WITHOUT      
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or    
+// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License    
+// for more details.                                                        
+//
+// You should have received a copy of the GNU General Public License        
+// along with eCos; if not, write to the Free Software Foundation, Inc.,    
+// 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.            
+//
+// As a special exception, if other files instantiate templates or use      
+// macros or inline functions from this file, or you compile this file      
+// and link it with other works to produce a work based on this file,       
+// this file does not by itself cause the resulting work to be covered by   
+// the GNU General Public License. However the source code for this file    
+// must still be made available in accordance with section (3) of the GNU   
+// General Public License v2.                                               
+//
+// This exception does not invalidate any other reasons why a work based    
+// on this file might be covered by the GNU General Public License.         
+// -------------------------------------------                              
+// ####ECOSGPLCOPYRIGHTEND####                                              
+//==========================================================================
+//#####DESCRIPTIONBEGIN####
+//
+// Author(s):      ilijak
+// Contributor(s):
+// Date:           2012-05-30
+//
+//####DESCRIPTIONEND####
+//
+//========================================================================
+
+#include <pkgconf/hal.h>
+#include <pkgconf/hal_cortexm.h>
+
+#include <cyg/infra/diag.h>
+#include <cyg/infra/cyg_type.h>
+#include <cyg/infra/cyg_trac.h>         // tracing macros
+#include <cyg/infra/cyg_ass.h>          // assertion macros
+
+#include <cyg/hal/hal_arch.h>           // HAL header
+#include <cyg/hal/hal_intr.h>           // HAL header
+#include <cyg/hal/cortexm_regs.h>       // Special Cortex-M asm instructions
+
+#include <cyg/hal/cortexm_fpu.h>        // Optional Floating Point Unit
+
+
+#define CYGARC_REG_NVIC_ACTLR                   (CYGARC_REG_NVIC_BASE + 0x008)
+#define CYGARC_REG_NVIC_ACTLR_DISFCA            BIT_(8)
+//==========================================================================
+// FPU is disbled upon reset. Dependent on FPU context switching model it
+// may be enabled.
+
+void hal_init_fpu(void)
+{
+    cyg_uint32 regval;
+
+    // Initialize FPU according to context switch model.
+    // Disable FPU so we could access FPCCR
+    HAL_CORTEXM_FPU_DISABLE();
+
+#if defined CYGARC_CORTEXM_FPU_EXC_AUTOSAVE
+    // Enable automatic exception FPU context saving.
+    HAL_READ_UINT32(CYGARC_REG_FPU_FPCCR, regval);
+    regval |= CYGARC_REG_FPU_FPCCR_LSPEN | CYGARC_REG_FPU_FPCCR_ASPEN;
+    HAL_WRITE_UINT32(CYGARC_REG_FPU_FPCCR, regval);
+    HAL_MEMORY_BARRIER();
+#else
+    // Disable automatic exception FPU context saving.
+    CYGARC_MRS(regval, control);
+    regval &= ~CYGARC_REG_CONTROL_FPCA_M;
+    CYGARC_MSR(control, regval);
+    HAL_READ_UINT32(CYGARC_REG_FPU_FPCCR, regval);
+    regval &= ~(CYGARC_REG_FPU_FPCCR_LSPEN | CYGARC_REG_FPU_FPCCR_ASPEN);
+    HAL_WRITE_UINT32(CYGARC_REG_FPU_FPCCR, regval);
+    HAL_MEMORY_BARRIER();
+#endif
+
+#if defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_NONE || \
+    defined CYGHWR_HAL_CORTEXM_FPU_SWITCH_ALL
+    // Enable FPU
+    HAL_CORTEXM_FPU_ENABLE();
+    CYGARC_VMSR(fpscr, 0);
+#endif
+}
+
+//==========================================================================
+// EOF fpv4_sp_d16.c
--- a/packages/hal/cortexm/arch/current/src/hal_misc.c
+++ b/packages/hal/cortexm/arch/current/src/hal_misc.c
@@ -8,7 +8,7 @@
 // ####ECOSGPLCOPYRIGHTBEGIN####                                            
 // -------------------------------------------                              
 // This file is part of eCos, the Embedded Configurable Operating System.   
-// Copyright (C) 2008 Free Software Foundation, Inc.                        
+// Copyright (C) 2008, 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     
@@ -39,8 +39,9 @@
 //==========================================================================
 //#####DESCRIPTIONBEGIN####
 //
-// Author(s):    nickg
-// Date:         2008-07-30
+// Author(s):      nickg
+// Contributor(s): ilijak
+// Date:           2008-07-30
 //
 //####DESCRIPTIONEND####
 //
@@ -60,9 +61,12 @@
 
 #include <cyg/hal/hal_arch.h>           // HAL header
 #include <cyg/hal/hal_intr.h>           // HAL header
-
+#include <cyg/hal/cortexm_regs.h>       // Special Cortex-M asm instructions
 #include <cyg/hal/drv_api.h>
 
+#ifdef CYGHWR_HAL_CORTEXM_FPU
+#include <cyg/hal/cortexm_fpu.h>        // Optional Floating Point Unit
+#endif
 
 #if defined(CYGPKG_KERNEL_INSTRUMENT) &&        \
     defined(CYGDBG_KERNEL_INSTRUMENT_INTR)
@@ -82,6 +86,9 @@ typedef cyg_uint32 cyg_isr(cyg_uint32 ve
 __externC void hal_default_svc_vsr( void );
 __externC void hal_pendable_svc_vsr( void );
 __externC void hal_switch_state_vsr( void );
+#ifdef CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+__externC void hal_usagefault_exception_vsr( void );
+#endif
 
 // HAL and eCos functions
 __externC void hal_system_init( void );
@@ -131,6 +138,11 @@ void hal_reset_vsr( void )
     // the main initialization.
 
     hal_system_init();
+#if defined CYGHWR_HAL_CORTEXM_FPU
+    // Floating Point Unit is disabled after reset.
+    // Enable it unless for LAZY context switching scheme.
+    hal_init_fpu();
+#endif
 
     // Initialize vector table in base of SRAM.
     {
@@ -144,12 +156,16 @@ void hal_reset_vsr( void )
 
         for( i = 2; i < 15; i++ )
             hal_vsr_table[i] = (CYG_ADDRESS)hal_default_exception_vsr;
-
-#endif
+#endif // !defined(CYG_HAL_STARTUP_RAM)
         // Always point SVC and PENDSVC vectors to our local versions
 
         hal_vsr_table[CYGNUM_HAL_VECTOR_SERVICE] = (CYG_ADDRESS)hal_default_svc_vsr;
         hal_vsr_table[CYGNUM_HAL_VECTOR_PENDSV] = (CYG_ADDRESS)hal_pendable_svc_vsr;
+#ifdef CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
+        // Install UsageFault and HardFault to trap the FPU usage exceptions.
+        HAL_VSR_SET(CYGNUM_HAL_VECTOR_USAGE_FAULT, hal_usagefault_exception_vsr, NULL);
+        HAL_VSR_SET(CYGNUM_HAL_VECTOR_HARD_FAULT, hal_usagefault_exception_vsr, NULL);
+#endif // CYGHWR_HAL_CORTEXM_FPU_SWITCH_LAZY
 
         // For all startup type, redirect interrupt vectors to our VSR.
         for( i = CYGNUM_HAL_VECTOR_SYS_TICK ;
@@ -158,7 +174,6 @@ void hal_reset_vsr( void )
             hal_vsr_table[i] = (CYG_ADDRESS)hal_default_interrupt_vsr;
     }
 
-
 #if !defined(CYG_HAL_STARTUP_RAM)
 
     // Ensure that the CPU will use the vector table we have just set
@@ -316,6 +331,7 @@ void hal_deliver_exception( HAL_SavedReg
     // This is common in discovery code, e.g. checking for a particular
     // device which may generate an exception when probing if the
     // device is not present
+
 #ifdef CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS
     if (__mem_fault_handler )
     {
@@ -362,7 +378,6 @@ void hal_deliver_interrupt( cyg_uint32 v
     defined(CYGDBG_KERNEL_INSTRUMENT_INTR)
     CYG_INSTRUMENT_INTR(RAISE, vector, 0);
 #endif
-
     isr = (cyg_isr *)hal_interrupt_handlers[vector];
 
     // Call the ISR
@@ -586,9 +601,8 @@ hal_arch_default_isr(CYG_ADDRWORD vector
 __externC void hal_get_gdb_registers( HAL_CORTEXM_GDB_Registers *gdbreg, HAL_SavedRegisters *regs )
 {
     int i;
-    cyg_uint32 *p = gdbreg->f0;
 
-    switch( regs->u.type )
+    switch(GDB_STUB_SAVEDREG_FRAME_TYPE(regs))
     {
     case HAL_SAVEDREGISTERS_THREAD:
         for( i = 0; i < 13; i++ )
@@ -596,7 +610,9 @@ hal_arch_default_isr(CYG_ADDRWORD vector
         gdbreg->gpr[13] = regs->u.thread.sp;
         gdbreg->gpr[14] = regs->u.thread.pc;
         gdbreg->gpr[15] = regs->u.thread.pc;
-        gdbreg->ps = 0x01000000;
+        gdbreg->xpsr = 0x01000000;
+
+        GDB_STUB_SAVEDREG_FPU_THREAD_GET(gdbreg, regs);
         break;
 
     case HAL_SAVEDREGISTERS_EXCEPTION:
@@ -610,7 +626,10 @@ hal_arch_default_isr(CYG_ADDRWORD vector
         gdbreg->gpr[13] = ((cyg_uint32)regs)+sizeof(regs->u.exception);
         gdbreg->gpr[14] = regs->u.exception.lr;
         gdbreg->gpr[15] = regs->u.exception.pc;
-        gdbreg->ps = regs->u.exception.psr;
+        gdbreg->xpsr = regs->u.exception.psr;
+#ifdef  CYGSEM_HAL_DEBUG_FPU
+        GDB_STUB_SAVEDREG_FPU_EXCEPTION_GET(gdbreg, regs);
+#endif
         break;
 
     case HAL_SAVEDREGISTERS_INTERRUPT:
@@ -622,21 +641,24 @@ hal_arch_default_isr(CYG_ADDRWORD vector
         gdbreg->gpr[13] = ((cyg_uint32)regs)+sizeof(regs->u.interrupt);
         gdbreg->gpr[14] = regs->u.interrupt.lr;
         gdbreg->gpr[15] = regs->u.interrupt.pc;
-        gdbreg->ps = regs->u.interrupt.psr;
+        gdbreg->xpsr = regs->u.interrupt.psr;
         break;
     }
-
+#ifdef CYGARC_CORTEXM_GDB_REG_FPA
     // Clear FP state, which we don't use
-    for( i = 0; i < (8*3+1); i++ )
-        p[i] = 0;
-
+    {
+        cyg_uint32 *p = gdbreg->f0;
+        for( i = 0; i < (8*3+1); i++ )
+            p[i] = 0;
+    }
+#endif
 }
 
 __externC void hal_set_gdb_registers( HAL_CORTEXM_GDB_Registers *gdbreg, HAL_SavedRegisters *regs )
 {
     int i;
 
-    switch( regs->u.type )
+    switch(GDB_STUB_SAVEDREG_FRAME_TYPE(regs))
     {
     case HAL_SAVEDREGISTERS_THREAD:
         for( i = 0; i < 13; i++ )
@@ -644,6 +666,8 @@ hal_arch_default_isr(CYG_ADDRWORD vector
         regs->u.thread.sp = gdbreg->gpr[13];
         regs->u.thread.pc = gdbreg->gpr[14];
         regs->u.thread.pc = gdbreg->gpr[15];
+
+        GDB_STUB_SAVEDREG_FPU_THREAD_SET(gdbreg, regs);
         break;
 
     case HAL_SAVEDREGISTERS_EXCEPTION:
@@ -656,7 +680,10 @@ hal_arch_default_isr(CYG_ADDRWORD vector
         regs->u.exception.r12 = gdbreg->gpr[12];
         regs->u.exception.lr = gdbreg->gpr[14];
         regs->u.exception.pc = gdbreg->gpr[15];
-        regs->u.exception.psr = gdbreg->ps;
+        regs->u.exception.psr = gdbreg->xpsr;
+#ifdef  CYGSEM_HAL_DEBUG_FPU
+        GDB_STUB_SAVEDREG_FPU_EXCEPTION_SET(gdbreg, regs);
+#endif
         break;
 
     case HAL_SAVEDREGISTERS_INTERRUPT:
@@ -667,7 +694,7 @@ hal_arch_default_isr(CYG_ADDRWORD vector
         regs->u.interrupt.r12 = gdbreg->gpr[12];
         regs->u.interrupt.lr = gdbreg->gpr[14];
         regs->u.interrupt.pc = gdbreg->gpr[15];
-        regs->u.interrupt.psr = gdbreg->ps;
+        regs->u.interrupt.psr = gdbreg->xpsr;
         break;
     }
 }
--- 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