diff --git a/src/gpio.c b/src/gpio.c
index 78f8e747460320c2365ceb5f9769c37e74e11b21..d5da9fc35178958c4a94afc41e4eb9099ff2efcc 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -36,7 +36,7 @@
 #define GPI_EXTI1_IRQn              EXTI15_10_IRQn
 #define GPI_EXTI1_IRQHandler        EXTI15_10_IRQHandler
 
-#define GPI_EXTI2_IRQn              EXTI4_IRQn
+#define GPI_EXTI2_IRQn              EXTI3_IRQn
 #define GPI_EXTI2_IRQHandler        EXTI3_IRQHandler
 
 #define GPI_EXTI3_IRQn              EXTI4_IRQn
@@ -48,22 +48,115 @@
 #define GPO_TIMER_IRQHandler        TIM2_IRQHandler
 
 // private variables
+TIM_HandleTypeDef    GPO_TIM_Handle;
+
+// Pushbuttons callbacks
+void (*reset_pb_callback)(void);
+void (*user_pb_callback)(void);
+void (*start_pb_callback)(void);
+void (*mode_pb_callback)(void);
+__IO uint16_t led_tx_period;
+__IO uint16_t led_rx_period;
+__IO uint16_t led_usr1_period;
+__IO uint16_t led_usr2_period;
 
 // IRQ handler functions
 void GPI_EXTI1_IRQHandler(void)
 {
+  if(__HAL_GPIO_EXTI_GET_IT(RESET_PB_PIN) != RESET)
+  {
+    __HAL_GPIO_EXTI_CLEAR_IT(RESET_PB_PIN);
+    if(reset_pb_callback!=0)
+      reset_pb_callback();
+  }
+  if(__HAL_GPIO_EXTI_GET_IT(USER_PB_PIN) != RESET)
+  {
+    __HAL_GPIO_EXTI_CLEAR_IT(USER_PB_PIN);
+    if(user_pb_callback!=0)
+      user_pb_callback();
+  }
 }
 
 void GPI_EXTI2_IRQHandler(void)
 {
+  if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET)
+  {
+    __HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN);
+    if(start_pb_callback!=0)
+      start_pb_callback();
+  }
 }
 
 void GPI_EXTI3_IRQHandler(void)
 {
+  if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET)
+  {
+    __HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN);
+    if(mode_pb_callback!=0)
+      mode_pb_callback();
+  }
 }
 
 void GPO_TIMER_IRQHandler(void)
 {
+  uint32_t capture;
+
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC1) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_CC1) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_IT_CC1);
+      capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_1);
+      __HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_1, (capture + led_tx_period));
+      HAL_GPIO_TogglePin(LED_TX_GPIO_PORT,LED_TX_PIN);
+    }
+  }
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC2) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_CC2) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_IT_CC2);
+      capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_2);
+      __HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_2, (capture + led_rx_period));
+      HAL_GPIO_TogglePin(LED_RX_GPIO_PORT,LED_RX_PIN);
+    }
+  }
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC3) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_CC3) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_IT_CC3);
+      capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_3);
+      __HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_3, (capture + led_usr1_period));
+      HAL_GPIO_TogglePin(LED_USR1_GPIO_PORT,LED_USR1_PIN);
+    }
+  }
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC4) != RESET)
+  {
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_CC4) !=RESET)
+    {
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_IT_CC4);
+      capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_4);
+      __HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_4, (capture + led_usr2_period));
+      HAL_GPIO_TogglePin(LED_USR2_GPIO_PORT,LED_USR2_PIN);
+    }
+  }
+  /* TIM Update event */
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_UPDATE) != RESET)
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_UPDATE) !=RESET)
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_IT_UPDATE);
+  /* TIM Break input event */
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_BREAK) != RESET)
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_BREAK) !=RESET)
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_IT_BREAK);
+  /* TIM Trigger detection event */
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_TRIGGER) != RESET)
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_TRIGGER) !=RESET)
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_IT_TRIGGER);
+  /* TIM commutation event */
+  if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_COM) != RESET)
+    if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM_Handle, TIM_IT_COM) !=RESET)
+      __HAL_TIM_CLEAR_IT(&GPO_TIM_Handle, TIM_FLAG_COM);
 }
 
 // private functions
@@ -71,28 +164,264 @@ void GPO_TIMER_IRQHandler(void)
 // public functions
 void gpio_init(void)
 {
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TIM_ClockConfigTypeDef sClockSourceConfig;
+  TIM_MasterConfigTypeDef sMasterConfig;
+
+  /* initialize the callback function pointers */
+  reset_pb_callback=0;
+  user_pb_callback=0;
+  start_pb_callback=0;
+  mode_pb_callback=0;
+  led_tx_period=0;
+  led_rx_period=0;
+  led_usr1_period=0;
+  led_usr2_period=0;
+
+  /* enable clocks */
+  ENABLE_LED_TX_GPIO_CLK;
+  ENABLE_LED_RX_GPIO_CLK;
+  ENABLE_LED_USR1_GPIO_CLK;
+  ENABLE_LED_USR2_GPIO_CLK;
+  ENABLE_START_PB_GPIO_CLK;
+  ENABLE_MODE_PB_GPIO_CLK;
+  ENABLE_RESET_PB_GPIO_CLK;
+  ENABLE_USER_PB_GPIO_CLK;
+
+  /* initlaize GPIO pins */
+  GPIO_InitStructure.Pin = LED_TX_PIN;
+  GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+  GPIO_InitStructure.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(LED_TX_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = LED_RX_PIN;
+  HAL_GPIO_Init(LED_RX_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = LED_USR1_PIN;
+  HAL_GPIO_Init(LED_USR1_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = LED_USR2_PIN;
+  HAL_GPIO_Init(LED_USR2_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = START_PB_PIN;
+  GPIO_InitStructure.Mode = GPIO_MODE_IT_FALLING;
+  GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+  HAL_GPIO_Init(START_PB_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = MODE_PB_PIN;
+  HAL_GPIO_Init(MODE_PB_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = RESET_PB_PIN;
+  HAL_GPIO_Init(RESET_PB_GPIO_PORT, &GPIO_InitStructure);
+
+  GPIO_InitStructure.Pin = USER_PB_PIN;
+  HAL_GPIO_Init(USER_PB_GPIO_PORT, &GPIO_InitStructure);
+
+  /* enable external interrupts */
+  // clear any pending interrupts
+  if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET)
+    __HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN);
+  if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET)
+    __HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN);
+  if(__HAL_GPIO_EXTI_GET_IT(RESET_PB_PIN) != RESET)
+    __HAL_GPIO_EXTI_CLEAR_IT(RESET_PB_PIN);
+  if(__HAL_GPIO_EXTI_GET_IT(USER_PB_PIN) != RESET)
+    __HAL_GPIO_EXTI_CLEAR_IT(USER_PB_PIN);
+  HAL_NVIC_SetPriority(GPI_EXTI1_IRQn, 3, 0);
+  HAL_NVIC_EnableIRQ(GPI_EXTI1_IRQn);
+
+  HAL_NVIC_SetPriority(GPI_EXTI2_IRQn, 3, 0);
+  HAL_NVIC_EnableIRQ(GPI_EXTI2_IRQn);
+
+  HAL_NVIC_SetPriority(GPI_EXTI3_IRQn, 3, 0);
+  HAL_NVIC_EnableIRQ(GPI_EXTI3_IRQn);
+
+  /* LED's timer configuration */
+  ENABLE_GPO_TIMER_CLK;
+
+  GPO_TIM_Handle.Instance=GPO_TIMER;
+  GPO_TIM_Handle.Init.Period = 0xFFFF;
+  GPO_TIM_Handle.Init.Prescaler = 42000;
+  GPO_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV2;
+  GPO_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+  GPO_TIM_Handle.Init.RepetitionCounter=0;
+  HAL_TIM_Base_Init(&GPO_TIM_Handle);
+  // initialize the timer interrupts
+  HAL_NVIC_SetPriority(GPO_TIMER_IRQn, 3, 1);
+  HAL_NVIC_EnableIRQ(GPO_TIMER_IRQn);
+  /* use the internal clock */
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  HAL_TIM_ConfigClockSource(&GPO_TIM_Handle, &sClockSourceConfig);
+  HAL_TIM_OC_Init(&GPO_TIM_Handle);
+  /* disable master/slave mode */
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  HAL_TIMEx_MasterConfigSynchronization(&GPO_TIM_Handle, &sMasterConfig);
 }
 
 void gpio_set_led(led_t led_id)
 {
+  switch(led_id)
+  {
+    case TXD_LED:
+      HAL_GPIO_WritePin(LED_TX_GPIO_PORT,LED_TX_PIN,GPIO_PIN_SET);
+      break;
+    case RXD_LED:
+      HAL_GPIO_WritePin(LED_RX_GPIO_PORT,LED_RX_PIN,GPIO_PIN_SET);
+      break;
+    case USER1_LED:
+      HAL_GPIO_WritePin(LED_USR1_GPIO_PORT,LED_USR1_PIN,GPIO_PIN_SET);
+      break;
+    case USER2_LED:
+      HAL_GPIO_WritePin(LED_USR2_GPIO_PORT,LED_USR2_PIN,GPIO_PIN_SET);
+      break;
+  }
 }
 
 void gpio_clear_led(led_t led_id)
 {
+  switch(led_id)
+  {
+    case TXD_LED:
+      HAL_GPIO_WritePin(LED_TX_GPIO_PORT,LED_TX_PIN,GPIO_PIN_RESET);
+      break;
+    case RXD_LED:
+      HAL_GPIO_WritePin(LED_RX_GPIO_PORT,LED_RX_PIN,GPIO_PIN_RESET);
+      break;
+    case USER1_LED:
+      HAL_GPIO_WritePin(LED_USR1_GPIO_PORT,LED_USR1_PIN,GPIO_PIN_RESET);
+      break;
+    case USER2_LED:
+      HAL_GPIO_WritePin(LED_USR2_GPIO_PORT,LED_USR2_PIN,GPIO_PIN_RESET);
+      break;
+  }
 }
 
 void gpio_toggle_led(led_t led_id)
 {
+  switch(led_id)
+  {
+    case TXD_LED:
+      HAL_GPIO_TogglePin(LED_TX_GPIO_PORT,LED_TX_PIN);
+      break;
+    case RXD_LED:
+      HAL_GPIO_TogglePin(LED_RX_GPIO_PORT,LED_RX_PIN);
+      break;
+    case USER1_LED:
+      HAL_GPIO_TogglePin(LED_USR1_GPIO_PORT,LED_USR1_PIN);
+      break;
+    case USER2_LED:
+      HAL_GPIO_TogglePin(LED_USR2_GPIO_PORT,LED_USR2_PIN);
+      break;
+  }
 }
 
 void gpio_blink_led(led_t led_id, int16_t period_ms)
 {
+  TIM_OC_InitTypeDef TIM_OCInitStructure;
+
+  TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
+  TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+  TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+
+  switch(led_id)
+  {
+    case TXD_LED:
+      if(period_ms>1)
+      {
+        led_tx_period=period_ms;
+        TIM_OCInitStructure.Pulse = led_tx_period;
+        HAL_TIM_OC_ConfigChannel(&GPO_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
+        HAL_TIM_OC_Start_IT(&GPO_TIM_Handle, TIM_CHANNEL_1);
+      }
+      else
+        HAL_TIM_OC_Stop_IT(&GPO_TIM_Handle, TIM_CHANNEL_1);
+      break;
+    case RXD_LED:
+      if(period_ms>1)
+      {
+        led_rx_period=period_ms;
+        TIM_OCInitStructure.Pulse = led_rx_period;
+        HAL_TIM_OC_ConfigChannel(&GPO_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_2);
+        HAL_TIM_OC_Start_IT(&GPO_TIM_Handle, TIM_CHANNEL_2);
+      }
+      else
+        HAL_TIM_OC_Stop_IT(&GPO_TIM_Handle, TIM_CHANNEL_2);
+      break;
+    case USER1_LED:
+      if(period_ms>1)
+      {
+        led_usr1_period=period_ms;
+        TIM_OCInitStructure.Pulse = led_usr1_period;
+        HAL_TIM_OC_ConfigChannel(&GPO_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_3);
+        HAL_TIM_OC_Start_IT(&GPO_TIM_Handle, TIM_CHANNEL_3);
+      }
+      else
+        HAL_TIM_OC_Stop_IT(&GPO_TIM_Handle, TIM_CHANNEL_3);
+      break;
+    case USER2_LED:
+      if(period_ms>1)
+      {
+        led_usr2_period=period_ms;
+        TIM_OCInitStructure.Pulse = led_usr2_period;
+        HAL_TIM_OC_ConfigChannel(&GPO_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_4);
+        HAL_TIM_OC_Start_IT(&GPO_TIM_Handle, TIM_CHANNEL_4);
+      }
+      else
+        HAL_TIM_OC_Stop_IT(&GPO_TIM_Handle, TIM_CHANNEL_4);
+      break;
+  }
 }
 
 uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id)
 {
+  switch(pb_id)
+  {
+    case START_PB:
+      if(HAL_GPIO_ReadPin(START_PB_GPIO_PORT,START_PB_PIN)==GPIO_PIN_SET)
+        return 0x01;
+      else
+        return 0x00;
+      break;
+    case MODE_PB:
+      if(HAL_GPIO_ReadPin(MODE_PB_GPIO_PORT,MODE_PB_PIN)==GPIO_PIN_SET)
+        return 0x01;
+      else
+        return 0x00;
+      break;
+    case RESET_PB:
+      if(HAL_GPIO_ReadPin(RESET_PB_GPIO_PORT,RESET_PB_PIN)==GPIO_PIN_SET)
+        return 0x01;
+      else
+        return 0x00;
+      break;
+    case USER_PB:
+      if(HAL_GPIO_ReadPin(USER_PB_GPIO_PORT,USER_PB_PIN)==GPIO_PIN_SET)
+        return 0x01;
+      else
+        return 0x00;
+      break;
+  }
+
+  return 0x00;
 }
 
 void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void))
 {
+  switch(pb_id)
+  {
+    case START_PB:
+      start_pb_callback=callback;
+      break;
+    case MODE_PB:
+      mode_pb_callback=callback;
+      break;
+    case RESET_PB:
+      reset_pb_callback=callback;
+      break;
+    case USER_PB:
+      user_pb_callback=callback;
+      break;
+  }
 }