diff --git a/src/x9d/delays.cpp b/src/x9d/delays.cpp
index 670f0061b..ebb5967fa 100644
--- a/src/x9d/delays.cpp
+++ b/src/x9d/delays.cpp
@@ -37,18 +37,16 @@ void delaysInit(void) {
 	
 	// Timer13
   RCC->APB1ENR |= RCC_APB1ENR_TIM13EN;           // Enable clock
-  //TIM14->ARR = 4999;     // 5mS
   TIM13->PSC = Peri1_frequency / 100000;                // 0.1uS from 30MHz
   TIM13->CCER = 0;
   TIM13->CCMR1 = 0;
   TIM13->EGR = 0;
   TIM13->CR1 = 0x03;
   TIM13->DIER = 0;
-  //NVIC_EnableIRQ(TIM8_TRG_COM_TIM14_IRQn) ;
 }
 	
 	
-void delay_01us(uint32_t nb){
+void delay_01us(uint16_t nb){
 	TIM13->CNT = 0;
 	while(TIM13->CNT < nb);
 }
diff --git a/src/x9d/delays.h b/src/x9d/delays.h
index 97fcc83b5..c4cbd7951 100644
--- a/src/x9d/delays.h
+++ b/src/x9d/delays.h
@@ -35,7 +35,7 @@
 #define __DELAYS_H
 
 void delaysInit(void);
-void delay_01us(uint32_t us);
+void delay_01us(uint16_t nb);
 
 
 #endif //__DELAYS_H
\ No newline at end of file
diff --git a/src/x9d/pwr_driver.cpp b/src/x9d/pwr_driver.cpp
index b4f29c437..15e347fbf 100644
--- a/src/x9d/pwr_driver.cpp
+++ b/src/x9d/pwr_driver.cpp
@@ -35,19 +35,16 @@
 
 void soft_power_off()
 {
-  GPIO_SetBits(GPIOPWR,PIN_MCU_PWR);
   GPIO_ResetBits(GPIOPWR,PIN_MCU_PWR);
 }
 
 uint32_t check_soft_power()
 {
-#if 0
   // TODO doesn't work :(
   if (GPIO_ReadInputDataBit(GPIOPWR, PIN_PWR_STATUS) == Bit_RESET)
-    return e_power_off;
-  else
-#endif
     return e_power_on;
+  else
+    return e_power_off;
 }
 
 void pwrInit()
@@ -57,7 +54,7 @@ void pwrInit()
   RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOPWR, ENABLE);
 
   /* GPIO  Configuration*/
-  GPIO_InitStructure.GPIO_Pin = PIN_MCU_PWR | PIN_PWR_LED;
+  GPIO_InitStructure.GPIO_Pin = PIN_MCU_PWR;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
@@ -66,6 +63,9 @@ void pwrInit()
 
   GPIO_InitStructure.GPIO_Pin = PIN_PWR_STATUS;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
   GPIO_Init(GPIOPWR, &GPIO_InitStructure);
   
   GPIO_InitStructure.GPIO_Pin = PIN_PWR_LED;
@@ -74,4 +74,7 @@ void pwrInit()
   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
   GPIO_Init(GPIOPWRLED, &GPIO_InitStructure);
+  
+  // Soft power ON
+  GPIO_SetBits(GPIOPWR,PIN_MCU_PWR);
 }