diff --git a/main.cpp b/main.cpp
index 74a32a0715507e3519ec176ee749f3501a28e5d3..1db1b55a3400dcc2265955135b0f46dfb5631557 100644
--- a/main.cpp
+++ b/main.cpp
@@ -7,8 +7,12 @@
 using namespace std;
 using namespace miosix;
 
-void ledThread(void *)
+int main()
 {
+    getchar();
+    
+    ElfProgram prog(reinterpret_cast<const unsigned int*>(main_elf),main_elf_len);
+    Process::create(prog);
     for(;;)
     {
         ledOn();
@@ -17,14 +21,3 @@ void ledThread(void *)
         Thread::sleep(200);
     }
 }
-
-int main()
-{
-    Thread::create(ledThread,STACK_MIN);
-    
-    getchar();
-    
-    ElfProgram prog(reinterpret_cast<const unsigned int*>(main_elf),main_elf_len);
-    Process::create(prog);
-    for(;;) Thread::sleep(1000);
-}
diff --git a/miosix/config/Makefile.inc b/miosix/config/Makefile.inc
index 4fdf4fdcefb7bb391cbec456c841c9789ebdb294..ef5b472f868c36c3e4a30fde3ee3f7bf09e44f1d 100644
--- a/miosix/config/Makefile.inc
+++ b/miosix/config/Makefile.inc
@@ -27,8 +27,8 @@ OPT_BOARD := stm32f207ig_stm3220g-eval
 ## -O2 is recomended otherwise, as it provides a good balance between code
 ## size and speed
 ##
-#OPT_OPTIMIZATION := -O0
-OPT_OPTIMIZATION := -O2
+OPT_OPTIMIZATION := -O0
+#OPT_OPTIMIZATION := -O2
 #OPT_OPTIMIZATION := -O3
 #OPT_OPTIMIZATION := -Os
 
diff --git a/miosix/config/miosix_settings.h b/miosix/config/miosix_settings.h
index 42a80ac79e88eb4ecad03ce71b516d5e139bb538..655c53101ee0f09c166eeb9e2a6dfbd7b1385695 100644
--- a/miosix/config/miosix_settings.h
+++ b/miosix/config/miosix_settings.h
@@ -130,7 +130,7 @@ const unsigned char MAX_OPEN_FILES=8;
  * mode, so to use debugging it is necessary to disble sleep in the idle thread.
  * By default it is not defined (idle thread calls sleep).
  */
-//#define JTAG_DISABLE_SLEEP
+#define JTAG_DISABLE_SLEEP
 
 /// Minimum stack size (MUST be divisible by 4)
 const unsigned int STACK_MIN=256;
diff --git a/miosix/kernel/kernel.cpp b/miosix/kernel/kernel.cpp
index 38efdd539c37198d2aeb136c4ca9c12d609c2656..965d7980e5865b4b73532b39c855a2056a7ed14f 100644
--- a/miosix/kernel/kernel.cpp
+++ b/miosix/kernel/kernel.cpp
@@ -616,6 +616,7 @@ bool Thread::IRQreportFault(const miosix_private::FaultData& fault)
         return false;
     cur->proc->fault=fault;
     const_cast<Thread*>(cur)->flags.IRQsetUserspace(false);
+    ::ctxsave=cur->ctxsave;
     return true;
 }