diff --git a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.cpp b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.cpp
index 2460463fede231d7532f406fa2429c4e453fa00a..b16190d8a7814946489c10634ae379795e44e3d3 100644
--- a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.cpp
+++ b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.cpp
@@ -11,17 +11,8 @@
 #include <cstdlib>
 #include <cstdio>
 #include "../../../../debugpin.h"
-using namespace miosix;
-
-GPIOtimer& GPIOtimer::instance(){
-    static GPIOtimer instance;
-    return instance;
-}
 
-GPIOtimer::GPIOtimer(): b(HighResolutionTimerBase::instance()),tc(b.getTimerFrequency()) {
-    setPinMode(false);
-    isInput=true;
-}
+using namespace miosix;
 
 Thread* GPIOtimer::tWaitingGPIO=nullptr;
 
@@ -45,21 +36,11 @@ bool GPIOtimer::absoluteWait(long long tick){
     return false;
 }
 
-
-void GPIOtimer::setPinMode(bool inputMode){
-    if(inputMode){
-        expansion::gpio10::mode(Mode::INPUT);
-        isInput=true;
-    }else{
-        expansion::gpio10::mode(Mode::OUTPUT);
-        isInput=false;
-    }
-}
-
 bool GPIOtimer::getMode(){
     return isInput;
 }
 
+//NOTE: Think about how to set the right ms32chkp related to the captured timestamp
 long long GPIOtimer::getExtEventTimestamp() const{
     return b.IRQgetSetTimeCCV2();
 }
@@ -69,11 +50,14 @@ bool GPIOtimer::absoluteWaitTimeoutOrEvent(long long tick){
     if(tick<b.getCurrentTick()){
 	return true;
     }
-    
-    b.setModeGPIOTimer(true);
-    setPinMode(true);
+    if(!isInput){
+	b.setModeGPIOTimer(true);
+	expansion::gpio10::mode(Mode::INPUT);
+	isInput=true;
+    }
     b.setCCInterrupt2(false);
     b.setCCInterrupt2Tim1(true);
+    
     do {
         tWaitingGPIO=Thread::IRQgetCurrentThread();
         Thread::IRQwait();
@@ -99,8 +83,11 @@ bool GPIOtimer::waitTimeoutOrEvent(long long tick){
  */
 bool GPIOtimer::absoluteWaitTrigger(long long tick){
     FastInterruptDisableLock dLock;
-    b.setModeGPIOTimer(false);    //output timer 
-    setPinMode(false);	    //output pin
+    if(isInput){
+	b.setModeGPIOTimer(false);		//output timer 
+	expansion::gpio10::mode(Mode::OUTPUT);	//output pin
+	isInput=false;
+    }
     if(b.IRQsetNextGPIOInterrupt(tick)==WaitResult::WAKEUP_IN_THE_PAST){
 	return true;
     }
@@ -111,15 +98,22 @@ bool GPIOtimer::waitTrigger(long long tick){
     return absoluteWaitTrigger(b.getCurrentTick()+tick);
 }
 
+/*
+ * This takes about 5us to be executed
+ */
 bool GPIOtimer::absoluteSyncWaitTrigger(long long tick){
-    {	
+    {
+	expansion::gpio1::high();
 	FastInterruptDisableLock dLock;
-	b.setModeGPIOTimer(false);	//output timer 
-	setPinMode(false);		//output pin
+	if(isInput){
+	    b.setModeGPIOTimer(false);			//output timer 
+	    expansion::gpio10::mode(Mode::OUTPUT);	//output pin
+	    isInput=false;
+	}
 	if(b.IRQsetNextGPIOInterrupt(tick)==WaitResult::WAKEUP_IN_THE_PAST){
 	    return true;
 	}
-    
+	expansion::gpio1::low();
 	do {
 	    tWaitingGPIO=Thread::IRQgetCurrentThread();
 	    Thread::IRQwait();
@@ -147,3 +141,12 @@ long long GPIOtimer::ns2tick(long long ns){
 long long GPIOtimer::aux1=0;
 GPIOtimer::~GPIOtimer() {}
 
+GPIOtimer& GPIOtimer::instance(){
+    static GPIOtimer instance;
+    return instance;
+}
+
+GPIOtimer::GPIOtimer(): b(HighResolutionTimerBase::instance()),tc(b.getTimerFrequency()) {
+    expansion::gpio10::mode(Mode::INPUT);
+    isInput=true;
+}
\ No newline at end of file
diff --git a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.h b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.h
index c1e8a1130962b267808803a77f8b98ff6c38ba68..5b2c6575b8e4c8c89fd401db8adce94fa9e9587e 100644
--- a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.h
+++ b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/gpio_timer.h
@@ -66,7 +66,6 @@ namespace miosix {
         
     private:
         GPIOtimer();
-        void setPinMode(bool inputMode);
         HighResolutionTimerBase& b;
         bool isInput; 
         TimeConversion tc;
diff --git a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/high_resolution_timer_base.cpp b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/high_resolution_timer_base.cpp
index 0b865b38ac7f01202a2efdd624339454474f74a3..143bc137b3a2becb6bbe33ae3940b2374b9ead5f 100644
--- a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/high_resolution_timer_base.cpp
+++ b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/high_resolution_timer_base.cpp
@@ -31,6 +31,7 @@ static long long ms32time = 0; //most significant 32 bits of counter
 static long long ms32chkp[3] = {0,0,0}; //most significant 32 bits of check point
 static TimeConversion* tc;
 
+static int fase=0;
 
 static inline unsigned int IRQread32Timer(){
     unsigned int high=TIMER3->CNT;
@@ -56,8 +57,7 @@ static inline long long IRQgetTick(){
 inline void interruptGPIOTimerRoutine(){
     if(TIMER1->CC[2].CTRL & TIMER_CC_CTRL_MODE_OUTPUTCOMPARE){
 	TIMER1->CC[2].CTRL = (TIMER1->CC[2].CTRL & ~_TIMER_CC_CTRL_CMOA_MASK) | TIMER_CC_CTRL_CMOA_CLEAR;
-	//I don't need to enable the interrupt because "output" is separated
-	//10 tick are enough to 
+	//10 tick are enough to execute this line
 	TIMER1->CC[2].CCV = static_cast<unsigned int>(TIMER1->CNT+10);//static_cast<unsigned int>(tick & 0xFFFF);
     }
     //Reactivating the thread that is waiting for the event.
@@ -85,7 +85,7 @@ inline void interruptRadioTimerRoutine(){
     }
 }
 
-static __attribute__((noinline)) void callScheduler(){
+static  void callScheduler(){
     TIMER1->IEN &= ~TIMER_IEN_CC1;
     TIMER3->IEN &= ~TIMER_IEN_CC1;
     TIMER1->IFC = TIMER_IFC_CC1;
@@ -117,32 +117,6 @@ static void setupTimers(){
     // If the most significant 32bit aren't match wait for TIM3 to overflow!
 }
 
-/*
- Return EVENT if the timer triggers in this routine, otherwise the timer is set and returns WAITING
- */
-static int fase=0;
-
-static WaitResult setupTimers2(){
-    TIMER1->IEN |= TIMER_IEN_CC2;
-    fase=0;
-    // We assume that this function is called only when the checkpoint is in future
-    if ((ms32chkp[2] == ms32time && static_cast<unsigned short>(TIMER3->CC[2].CCV+1)==TIMER3->CNT && TIMER1->CNT >= TIMER1->CC[2].CCV )
-	    ||  HighResolutionTimerBase::instance().IRQgetSetTimeCCV2()<=IRQgetTick()){
-	// Enable TIM1 since TIM3 has been already matched
-	TIMER1->CC[2].CTRL = (TIMER1->CC[2].CTRL & ~_TIMER_CC_CTRL_CMOA_MASK) | TIMER_CC_CTRL_CMOA_SET;
-	//This line introduce a delay of 10ticks... it should be revised
-	TIMER1->CC[2].CCV = static_cast<unsigned short>(10+TIMER1->CNT);
-	TIMER1->IFC = TIMER_IFC_CC2;
-	TIMER1->IEN &= ~TIMER_IEN_CC2;
-	// If TIM1 matches by the time it is being enabled, call the routine right away
-	interruptGPIOTimerRoutine();
-	return WaitResult::EVENT;
-    }
-    return WaitResult::WAITING;
-}
-
-
-
 void __attribute__((naked)) TIMER3_IRQHandler()
 {
     saveContext();
@@ -198,6 +172,10 @@ void __attribute__((used)) cstirqhnd2(){
     }
 }
 
+/*
+ * This takes about 2.5us to execute, so the Pin can't stay high for less than this value, 
+ * and we can't set more interrupts in a period of 2.5us+
+ */
 void __attribute__((used)) cstirqhnd1(){
     if ((TIMER1->IEN & TIMER_IEN_CC1) && (TIMER1->IF & TIMER_IF_CC1)){
         TIMER1->IFC = TIMER_IFC_CC1;
@@ -210,9 +188,8 @@ void __attribute__((used)) cstirqhnd1(){
 	if(fase==0){
 	    HighResolutionTimerBase& b=HighResolutionTimerBase::instance();
 	    
-	    //nextInterrupt
-	    //The double cast is REALLY necessary to get the correct value
-	    long long t=ms32chkp[2] | static_cast<unsigned int>(static_cast<unsigned short>(TIMER3->CC[2].CCV+1)<<16) | TIMER1->CC[2].CCV;
+	    //get nextInterrupt
+	    long long t=ms32chkp[2]|TIMER1->CC[2].CCV;
 	    long long diff=t-b.IRQgetCurrentTick();
 	    if(diff<=0xFFFF){
 		TIMER1->CC[2].CTRL = (TIMER1->CC[2].CTRL & ~_TIMER_CC_CTRL_CMOA_MASK) | TIMER_CC_CTRL_CMOA_SET;
@@ -333,55 +310,34 @@ void HighResolutionTimerBase::IRQsetNextInterrupt1(long long tick){
 }
 
 /*
- Return true if the pin is going to raise, otherwise false, the pin remain low because the command arrived too late
- *  */ //Should be called at least 4us before the next interrupt, otherwise it returns with WAKEUP_IN_THE_PAST
+ * Return true if the pin is going to raise, otherwise false, the pin remain low because the command arrived too late
+ * Should be called at least 4us before the next interrupt, otherwise it returns with WAKEUP_IN_THE_PAST
+ */
 WaitResult HighResolutionTimerBase::IRQsetNextGPIOInterrupt(long long tick){
     long long curTick = IRQgetTick(); // This require almost 1us about 50ticks
     long long diff=tick-curTick;
     
+    // 150 are enough to make sure that this routine ends and the timer IEN is enabled. 
+    //NOTE: this is really dependent on compiler, optimization and other stuff
     if(diff>150){
 	fase=0;
 	unsigned short t1=static_cast<unsigned short>((tick & 0xFFFF)-1),
 		    t3=static_cast<unsigned short>(((tick & 0xFFFF0000)>>16)-1);
-	//The first number of tick should be enough to execute instructions until TIMER1->IEN
-	ms32chkp[2] = tick & upperMask;
-	TIMER3->CC[2].CCV = t3;
+	//ms32chkp[2] is going to store even the middle part, because we don't need to use TIMER3
+	ms32chkp[2] = tick & (upperMask | 0xFFFF0000);
 	TIMER1->CC[2].CCV = t1;
 
 	TIMER1->IFC = TIMER_IFC_CC2;
 	TIMER1->IEN |= TIMER_IEN_CC2;
+	//0xFFFF because it's the roundtrip of timer
 	if(diff<=0xFFFF){
 	    TIMER1->CC[2].CTRL = (TIMER1->CC[2].CTRL & ~_TIMER_CC_CTRL_CMOA_MASK) | TIMER_CC_CTRL_CMOA_SET;
-	    fase=1; //if phase=1, this means that the value in TIMER3 isn't read and we can still write t3 as usual
+	    fase=1; //if phase=1, this means that we have to shutdown the pin next time that TIMER1 triggers
 	}
 	return WaitResult::WAITING;
     }else{
 	return WaitResult::WAKEUP_IN_THE_PAST;
     }
-    /*
-    long long curTick = IRQgetTick(); //This require almost 1us
-    if(curTick >= tick){
-	// The interrupt is in the past => call timerInt immediately
-	interruptGPIOTimerRoutine();
-	return WaitResult::WAKEUP_IN_THE_PAST;
-    }else{ 
-	ms32chkp[2] = tick & upperMask;
-	TIMER3->CC[2].CCV = static_cast<unsigned int>((tick & 0xFFFF0000)>>16)-1;
-	TIMER1->IFC = TIMER_IFC_CC2;
-	TIMER1->CC[2].CCV = static_cast<unsigned int>(tick & 0xFFFF);
-	
-	fase=0;
-	TIMER1->IEN |= TIMER_IEN_CC2;
-	if(TIMER1->IF & TIMER_IF_CC2 && tick<=IRQgetTick()){
-	    TIMER1->IEN &= ~TIMER_IEN_CC2;
-	    TIMER1->IFC = TIMER_IFC_CC2;
-	    // If TIM1 matches by the time it is being enabled, call the routine right away
-	    interruptGPIOTimerRoutine();
-	    return WaitResult::EVENT;
-	}
-	return WaitResult::WAITING;
-    }
-    */
 }
 
 // In this function I prepare the timer, but i don't enable the timer.
diff --git a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/radio_timer.h b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/radio_timer.h
index 61ee5c4bbc295de359a6faaecc95484ff6c3fda9..8ffba3858829e5aab4f3dd2c549c5d6b9599472d 100644
--- a/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/radio_timer.h
+++ b/miosix/arch/cortexM3_efm32gg/efm32gg332f1024_wandstem/interfaces-impl/radio_timer.h
@@ -31,7 +31,6 @@ namespace miosix{
             void wait(long long tick);         
             bool absoluteWait(long long tick); 
             
-            
             bool absoluteWaitTrigger(long long tick);
             
             bool waitTimeoutOrEvent(long long tick);