diff --git a/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h b/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
index 6487b3e..91ab510 100755
--- a/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
+++ b/source/firmware/arch/stm32f4xx/board/stm32f4-discovery/include/stm32f4-discovery.h
@@ -193,6 +193,7 @@ static const struct stm32f4_gpio t4c4_gpio = {
 static struct stm32f4_pwm str32f4_pwm_4 = {
 		.pwm_gpio = &t4c4_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
 		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_4,
@@ -228,6 +229,7 @@ static const struct stm32f4_gpio stm32f4_pwm_t4c3_gpio = {
 static struct stm32f4_pwm str32f4_pwm_3 = {
 		.pwm_gpio = &stm32f4_pwm_t4c3_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
 		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_3,
@@ -263,6 +265,7 @@ static const struct stm32f4_gpio stm32f4_pwm_t4c2_gpio = {
 static struct stm32f4_pwm str32f4_pwm_2 = {
 		.pwm_gpio = &stm32f4_pwm_t4c2_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
 		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_2,
@@ -298,6 +301,7 @@ static const struct stm32f4_gpio stm32f4_pwm_t4c1_gpio = {
 static struct stm32f4_pwm str32f4_pwm_1 = {
 		.pwm_gpio = &stm32f4_pwm_t4c1_gpio,
 		.timer_handle = &tim4_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t4_output_compare_cfg,
 		.master_cfg = &t4_master_cfg,
 		.channel = TIM_CHANNEL_1,
@@ -361,6 +365,7 @@ static const struct stm32f4_gpio t5c2_gpio = {
 static struct stm32f4_pwm str32f4_pwm5_c2 = {
 		.pwm_gpio = &t5c2_gpio,
 		.timer_handle = &tim5_handle,
+		.timer_src_frequency_MHz = 84,
 		.output_compare_cfg = &t5_output_compare_cfg,
 		.master_cfg = &t5_master_cfg,
 		.channel = TIM_CHANNEL_2,
@@ -380,6 +385,69 @@ const struct driver pwm5_c2 = {
 		&pwm5_ch2,
 };
 
+// PWM2 CHANNEL 4
+/* apb1 clock = 84MHz */
+/* period_reg = src_clk / presc / cnt_clk  */
+/*       1679 = 84MHZ   / 1000  / 50Hz - 1 */
+static TIM_HandleTypeDef t2_handle = {
+		.Instance = TIM2,
+		.Init.Prescaler = 1000,
+		.Init.CounterMode = TIM_COUNTERMODE_UP,
+		.Init.Period = 0xffff,
+		.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1,
+		.Init.RepetitionCounter = 0,
+};
+
+static TIM_IC_InitTypeDef t2_input_capture_init = {
+		.ICPrescaler = TIM_ICPSC_DIV1,
+		.ICFilter = 0,
+		.ICPolarity = TIM_ICPOLARITY_BOTHEDGE,
+		.ICSelection = TIM_ICSELECTION_INDIRECTTI,
+};
+
+static TIM_SlaveConfigTypeDef t2_slave_config = {
+		.SlaveMode = TIM_SLAVEMODE_RESET,
+		.InputTrigger = TIM_TS_TI1FP1,
+		.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE,
+		.TriggerFilter = 0,
+};
+
+static GPIO_InitTypeDef port_cfg_b10 = {
+		.Pin = GPIO_PIN_10,
+		.Mode = GPIO_MODE_AF_OD,
+		.Speed = GPIO_SPEED_FREQ_HIGH,
+		.Pull = GPIO_NOPULL,
+		.Alternate = GPIO_AF1_TIM2,
+};
+
+static const struct stm32f4_gpio b10_gpio = {
+		.port = GPIOB,
+		.pin = &port_cfg_b10,
+};
+
+static struct stm32f4_pwm stm32f4_pwm2_c4 = {
+		.pwm_gpio = &b10_gpio,
+		.timer_handle = &t2_handle,
+		.timer_src_frequency_MHz = 84,
+		.input_capture_init = &t2_input_capture_init,
+		.slave_config = &t2_slave_config,
+		.channel = TIM_CHANNEL_4,
+};
+
+static const struct pwm pwm2_ch4 = {
+		.arch_dep_device = &stm32f4_pwm2_c4,
+		.fp = &stm32f4_pwm_fp,
+};
+
+#ifdef TEST_APP
+static const struct driver pwm2_c4 = {
+#else
+const struct driver pwm2_c4 = {
+#endif
+		DRIVER_TYPE_PWM,
+		&pwm2_ch4,
+};
+
 // UART 1
 static const GPIO_InitTypeDef port_cfg_uart1 = {
 		.Pin = GPIO_PIN_6 | GPIO_PIN_7,
diff --git a/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h b/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h
index cff1216..d15e30e 100644
--- a/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h
+++ b/source/firmware/arch/stm32f4xx/driver/include/stm32f4_pwm.h
@@ -15,7 +15,10 @@ struct stm32f4_pwm {
 	TIM_HandleTypeDef *timer_handle;
 	TIM_OC_InitTypeDef *output_compare_cfg;
 	TIM_MasterConfigTypeDef *master_cfg;
+	TIM_IC_InitTypeDef *input_capture_init;
+	TIM_SlaveConfigTypeDef *slave_config;
 	uint32_t channel;
+	uint32_t timer_src_frequency_MHz;
 };
 #pragma pack(pop)
 
@@ -23,11 +26,15 @@ struct stm32f4_pwm {
 int stm32f4_pwm_open(const void *pwm);
 int stm32f4_pwm_close(const void *pwm);
 int stm32f4_pwm_set_duty_cycle(const void *pwm, unsigned int duty_cycle_percent);
+int stm32f4_pwm_get_period_ns(const void *pwm);
+int stm32f4_pwm_get_pulse_width_ns(const void *pwm);
 
 static const struct pwm_fp stm32f4_pwm_fp = {
 		.open = stm32f4_pwm_open,
 		.close = stm32f4_pwm_close,
 		.set_duty_cycle = stm32f4_pwm_set_duty_cycle,
+		.get_period = stm32f4_pwm_get_period_ns,
+		.get_pulse_width = stm32f4_pwm_get_pulse_width_ns,
 };
 
 #endif /* SOURCE_FIRMWARE_ARCH_STM32F4XX_DRIVER_INCLUDE_STM32F4_PWM_H_ */
diff --git a/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c b/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c
index e51dd99..e16de95 100644
--- a/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c
+++ b/source/firmware/arch/stm32f4xx/driver/stm32f4_pwm.c
@@ -9,42 +9,158 @@
 
 #include "stm32f4xx.h"
 
+#include "isr.h"
+
 #include "gpio.h"
 #include "stm32f4_gpio.h"
 
 #include "pwm.h"
 #include "stm32f4_pwm.h"
 
+struct stm32f4_pwm_channel_object {
+	struct stm32f4_pwm *pwm;
+	uint32_t period;
+	uint32_t period_start;
+	uint32_t pulse;
+	uint32_t pulse_start;
+};
+
+struct stm32f4_pwm_timer_object {
+	struct stm32f4_pwm_channel_object channel_1;
+	struct stm32f4_pwm_channel_object channel_2;
+	struct stm32f4_pwm_channel_object channel_3;
+	struct stm32f4_pwm_channel_object channel_4;
+};
+
+struct stm32f4_pwm_object {
+	struct stm32f4_pwm_timer_object timer_2;
+	struct stm32f4_pwm_timer_object timer_4;
+	struct stm32f4_pwm_timer_object timer_5;
+};
+
+static volatile struct stm32f4_pwm_object pwm_object = {
+		.timer_2.channel_1.pwm = NULL,
+		.timer_2.channel_1.period = 0,
+		.timer_2.channel_1.period_start = 0,
+		.timer_2.channel_1.pulse = 0,
+		.timer_2.channel_1.pulse_start = 0,
+		.timer_2.channel_2.pwm = NULL,
+		.timer_2.channel_2.period = 0,
+		.timer_2.channel_2.period_start = 0,
+		.timer_2.channel_2.pulse = 0,
+		.timer_2.channel_2.pulse_start = 0,
+		.timer_2.channel_3.pwm = NULL,
+		.timer_2.channel_3.period = 0,
+		.timer_2.channel_3.period_start = 0,
+		.timer_2.channel_3.pulse = 0,
+		.timer_2.channel_3.pulse_start = 0,
+		.timer_2.channel_4.pwm = NULL,
+		.timer_2.channel_4.period = 0,
+		.timer_2.channel_4.period_start = 0,
+		.timer_2.channel_4.pulse = 0,
+		.timer_2.channel_4.pulse_start = 0,
+		.timer_4.channel_1.pwm = NULL,
+		.timer_4.channel_1.period = 0,
+		.timer_4.channel_1.period_start = 0,
+		.timer_4.channel_1.pulse = 0,
+		.timer_4.channel_1.pulse_start = 0,
+		.timer_4.channel_2.pwm = NULL,
+		.timer_4.channel_2.period = 0,
+		.timer_4.channel_2.period_start = 0,
+		.timer_4.channel_2.pulse = 0,
+		.timer_4.channel_2.pulse_start = 0,
+		.timer_4.channel_3.pwm = NULL,
+		.timer_4.channel_3.period = 0,
+		.timer_4.channel_3.period_start = 0,
+		.timer_4.channel_3.pulse = 0,
+		.timer_4.channel_3.pulse_start = 0,
+		.timer_4.channel_4.pwm = NULL,
+		.timer_4.channel_4.period = 0,
+		.timer_4.channel_4.period_start = 0,
+		.timer_4.channel_4.pulse = 0,
+		.timer_4.channel_4.pulse_start = 0,
+		.timer_5.channel_1.pwm = NULL,
+		.timer_5.channel_1.period = 0,
+		.timer_5.channel_1.period_start = 0,
+		.timer_5.channel_1.pulse = 0,
+		.timer_5.channel_1.pulse_start = 0,
+		.timer_5.channel_2.pwm = NULL,
+		.timer_5.channel_2.period = 0,
+		.timer_5.channel_2.period_start = 0,
+		.timer_5.channel_2.pulse = 0,
+		.timer_5.channel_2.pulse_start = 0,
+		.timer_5.channel_3.pwm = NULL,
+		.timer_5.channel_3.period = 0,
+		.timer_5.channel_3.period_start = 0,
+		.timer_5.channel_3.pulse = 0,
+		.timer_5.channel_3.pulse_start = 0,
+		.timer_5.channel_4.pwm = NULL,
+		.timer_5.channel_4.period = 0,
+		.timer_5.channel_4.period_start = 0,
+		.timer_5.channel_4.pulse = 0,
+		.timer_5.channel_4.pulse_start = 0,
+};
+
 int stm32f4_pwm_open(const void *pwm)
 {
 	if(NULL == pwm)
 		return -1;
 	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
+	IRQn_Type irq_type = TIM2_IRQn;
 	stm32f4_gpio_open(this->pwm_gpio);
+	if(this->timer_handle->Instance == TIM2) {
+		__HAL_RCC_TIM2_CLK_ENABLE();
+		irq_type = TIM2_IRQn;
+		if(this->channel == TIM_CHANNEL_1)
+			pwm_object.timer_2.channel_1.pwm = this;
+		else if(this->channel == TIM_CHANNEL_2)
+			pwm_object.timer_2.channel_2.pwm = this;
+		else if(this->channel == TIM_CHANNEL_3)
+			pwm_object.timer_2.channel_3.pwm = this;
+		else if(this->channel == TIM_CHANNEL_4)
+			pwm_object.timer_2.channel_4.pwm = this;
+	}
 	if(this->timer_handle->Instance == TIM4) {
 		__HAL_RCC_TIM4_CLK_ENABLE();
+		irq_type = TIM4_IRQn;
+		if(this->channel == TIM_CHANNEL_1)
+			pwm_object.timer_4.channel_1.pwm = this;
+		else if(this->channel == TIM_CHANNEL_2)
+			pwm_object.timer_4.channel_2.pwm = this;
+		else if(this->channel == TIM_CHANNEL_3)
+			pwm_object.timer_4.channel_3.pwm = this;
+		else if(this->channel == TIM_CHANNEL_4)
+			pwm_object.timer_4.channel_4.pwm = this;
 	}
 	if(this->timer_handle->Instance == TIM5) {
 		__HAL_RCC_TIM5_CLK_ENABLE();
+		irq_type = TIM5_IRQn;
+		if(this->channel == TIM_CHANNEL_1)
+			pwm_object.timer_5.channel_1.pwm = this;
+		else if(this->channel == TIM_CHANNEL_2)
+			pwm_object.timer_5.channel_2.pwm = this;
+		else if(this->channel == TIM_CHANNEL_3)
+			pwm_object.timer_5.channel_3.pwm = this;
+		else if(this->channel == TIM_CHANNEL_4)
+			pwm_object.timer_5.channel_4.pwm = this;
 	}
-	HAL_TIM_PWM_Init(this->timer_handle);
-	HAL_TIM_PWM_ConfigChannel(this->timer_handle, this->output_compare_cfg, this->channel);
-
-	HAL_TIMEx_MasterConfigSynchronization(this->timer_handle, this->master_cfg);
-
-	TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
-	sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
-	sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
-	sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
-	sBreakDeadTimeConfig.DeadTime = 0;
-	sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
-	sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
-	sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
-	HAL_TIMEx_ConfigBreakDeadTime(this->timer_handle, &sBreakDeadTimeConfig);
-
-
 	HAL_TIM_Base_Start(this->timer_handle);
-	HAL_TIM_PWM_Start(this->timer_handle, this->channel);
+	if((NULL != this->output_compare_cfg) && (NULL != this->master_cfg)) {
+		/* pwm output */
+		HAL_TIM_PWM_Init(this->timer_handle);
+		HAL_TIM_PWM_ConfigChannel(this->timer_handle, this->output_compare_cfg, this->channel);
+		HAL_TIMEx_MasterConfigSynchronization(this->timer_handle, this->master_cfg);
+		HAL_TIM_PWM_Start(this->timer_handle, this->channel);
+	}
+	else if((NULL != this->input_capture_init) && (NULL != this->slave_config)) {
+		/* pwm input */
+		HAL_NVIC_SetPriority(irq_type, 5, 1);
+		HAL_TIM_IC_Init(this->timer_handle);
+		HAL_TIM_IC_ConfigChannel(this->timer_handle, this->input_capture_init, this->channel);
+		HAL_TIM_SlaveConfigSynchronization(this->timer_handle, this->slave_config);
+		HAL_TIM_IC_Start_IT(this->timer_handle, this->channel);
+		HAL_NVIC_EnableIRQ(irq_type);
+	}
 	return 0;
 }
 
@@ -55,7 +171,12 @@ int stm32f4_pwm_close(const void *pwm)
 	stm32f4_pwm_set_duty_cycle(pwm, 0);
 	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
 	HAL_TIM_Base_Stop(this->timer_handle);
-	HAL_TIM_PWM_Stop(this->timer_handle, this->channel);
+	if((NULL != this->output_compare_cfg) && (NULL != this->master_cfg)) {
+		HAL_TIM_PWM_Stop(this->timer_handle, this->channel);
+	}
+	else if((NULL != this->input_capture_init) && (NULL != this->slave_config)) {
+		HAL_TIM_IC_Stop_IT(this->timer_handle, this->channel);
+	}
 
 	return 0;
 }
@@ -70,3 +191,259 @@ int stm32f4_pwm_set_duty_cycle(const void *pwm, unsigned int duty_cycle_percent)
 
 	return 0;
 }
+
+int stm32f4_pwm_get_period_ns(const void *pwm)
+{
+	if(NULL == pwm)
+		return -1;
+
+	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
+	int ret = 0;
+
+	if(this->timer_handle->Instance == TIM2) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_2.channel_1.period;
+			return ret * pwm_object.timer_2.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_2.channel_2.period;
+			return ret * pwm_object.timer_2.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_2.channel_3.period;
+			return ret * pwm_object.timer_2.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_2.channel_4.period;
+			return ret * pwm_object.timer_2.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM4) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_4.channel_1.period;
+			return ret * pwm_object.timer_4.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_4.channel_2.period;
+			return ret * pwm_object.timer_4.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_4.channel_3.period;
+			return ret * pwm_object.timer_4.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_4.channel_4.period;
+			return ret * pwm_object.timer_4.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM5) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_5.channel_1.period;
+			return ret * pwm_object.timer_5.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_5.channel_2.period;
+			return ret * pwm_object.timer_5.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_5.channel_3.period;
+			return ret * pwm_object.timer_5.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_5.channel_4.period;
+			return ret * pwm_object.timer_5.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+
+	return 0;
+}
+
+int stm32f4_pwm_get_pulse_width_ns(const void *pwm)
+{
+	if(NULL == pwm)
+		return -1;
+
+	struct stm32f4_pwm *this = (struct stm32f4_pwm *)pwm;
+	int ret = 0;
+
+	if(this->timer_handle->Instance == TIM2) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_2.channel_1.pulse;
+			return ret * pwm_object.timer_2.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_2.channel_2.pulse;
+			return ret * pwm_object.timer_2.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_2.channel_3.pulse;
+			return ret * pwm_object.timer_2.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_2.channel_4.pulse;
+			return ret * pwm_object.timer_2.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_2.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM4) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_4.channel_1.pulse;
+			return ret * pwm_object.timer_4.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_4.channel_2.pulse;
+			return ret * pwm_object.timer_4.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_4.channel_3.pulse;
+			return ret * pwm_object.timer_4.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_4.channel_4.pulse;
+			return ret * pwm_object.timer_4.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_4.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+	else if(this->timer_handle->Instance == TIM5) {
+		if(this->channel == TIM_CHANNEL_1) {
+			ret = (int)pwm_object.timer_5.channel_1.pulse;
+			return ret * pwm_object.timer_5.channel_1.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_1.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_2) {
+			ret = (int)pwm_object.timer_5.channel_2.pulse;
+			return ret * pwm_object.timer_5.channel_2.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_2.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_3) {
+			ret = (int)pwm_object.timer_5.channel_3.pulse;
+			return ret * pwm_object.timer_5.channel_3.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_3.pwm->timer_src_frequency_MHz;
+		}
+		else if(this->channel == TIM_CHANNEL_4) {
+			ret = (int)pwm_object.timer_5.channel_4.pulse;
+			return ret * pwm_object.timer_5.channel_4.pwm->timer_handle->Init.Prescaler / pwm_object.timer_5.channel_4.pwm->timer_src_frequency_MHz;
+		}
+	}
+
+	return 0;
+}
+
+static void handle_irq_tim2_cc1(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 1 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC1) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC1) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC1);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR1_CC1S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR1;
+				if(pwm_object.timer_2.channel_1.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_1.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_1.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_1.period_start)
+							pwm_object.timer_2.channel_1.period = read - pwm_object.timer_2.channel_1.period_start;
+						pwm_object.timer_2.channel_1.period_start = read;
+						pwm_object.timer_2.channel_1.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_1.pulse_start)
+							pwm_object.timer_2.channel_1.pulse = read - pwm_object.timer_2.channel_1.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+static void handle_irq_tim2_cc2(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 2 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC2) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC2) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC2);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR1_CC2S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR2;
+				if(pwm_object.timer_2.channel_2.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_2.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_2.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_2.period_start)
+							pwm_object.timer_2.channel_2.period = read - pwm_object.timer_2.channel_2.period_start;
+						pwm_object.timer_2.channel_2.period_start = read;
+						pwm_object.timer_2.channel_2.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_2.pulse_start)
+							pwm_object.timer_2.channel_2.pulse = read - pwm_object.timer_2.channel_2.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+static void handle_irq_tim2_cc3(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 3 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC3) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC3) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC3);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR2_CC3S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR3;
+				if(pwm_object.timer_2.channel_3.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_3.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_3.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_3.period_start)
+							pwm_object.timer_2.channel_3.period = read - pwm_object.timer_2.channel_3.period_start;
+						pwm_object.timer_2.channel_3.period_start = read;
+						pwm_object.timer_2.channel_3.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_3.pulse_start)
+							pwm_object.timer_2.channel_3.pulse = read - pwm_object.timer_2.channel_3.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+static void handle_irq_tim2_cc4(TIM_HandleTypeDef *timer_handle)
+{
+	/* Capture compare 4 event */
+	if(__HAL_TIM_GET_FLAG(timer_handle, TIM_FLAG_CC4) != RESET) {
+		if(__HAL_TIM_GET_IT_SOURCE(timer_handle, TIM_IT_CC4) != RESET) {
+			__HAL_TIM_CLEAR_IT(timer_handle, TIM_IT_CC4);
+			/* Input capture event */
+			if((timer_handle->Instance->CCMR2 & TIM_CCMR2_CC4S) != 0x00U) {
+				uint32_t read = timer_handle->Instance->CCR4;
+				if(pwm_object.timer_2.channel_4.pwm->input_capture_init->ICPolarity == TIM_ICPOLARITY_BOTHEDGE) {
+					GPIO_TypeDef* port = pwm_object.timer_2.channel_4.pwm->pwm_gpio->port;
+					uint16_t pin = pwm_object.timer_2.channel_4.pwm->pwm_gpio->pin->Pin;
+					if(GPIO_PIN_SET == HAL_GPIO_ReadPin(port, pin)) {
+						if(read > pwm_object.timer_2.channel_4.period_start)
+							pwm_object.timer_2.channel_4.period = read - pwm_object.timer_2.channel_4.period_start;
+						pwm_object.timer_2.channel_4.period_start = read;
+						pwm_object.timer_2.channel_4.pulse_start = read;
+					}
+					else {
+						if(read > pwm_object.timer_2.channel_4.pulse_start)
+							pwm_object.timer_2.channel_4.pulse = read - pwm_object.timer_2.channel_4.pulse_start;
+					}
+				}
+			}
+		}
+	}
+}
+
+void TIM2_IRQHandler(void)
+{
+	enter_isr();
+
+	handle_irq_tim2_cc1(pwm_object.timer_2.channel_1.pwm->timer_handle);
+	handle_irq_tim2_cc2(pwm_object.timer_2.channel_2.pwm->timer_handle);
+	handle_irq_tim2_cc3(pwm_object.timer_2.channel_3.pwm->timer_handle);
+	handle_irq_tim2_cc4(pwm_object.timer_2.channel_4.pwm->timer_handle);
+
+	exit_isr();
+}
diff --git a/source/firmware/kernel/driver/driver.c b/source/firmware/kernel/driver/driver.c
index aa36474..32d052f 100644
--- a/source/firmware/kernel/driver/driver.c
+++ b/source/firmware/kernel/driver/driver.c
@@ -157,7 +157,13 @@ int drv_ioctl(const struct driver *driver, unsigned int cmd, const void *data)
 	case DRIVER_TYPE_PWM:
 		if(cmd == IOCTL_PWM_SET_DUTY_CYCLE) {
 			unsigned int *duty = (unsigned int *)data;
-			pwm_set_duty_cycle((const struct pwm *)(driver->device_driver), *duty);
+			return pwm_set_duty_cycle((const struct pwm *)(driver->device_driver), *duty);
+		}
+		else if(cmd == IOCTL_PWM_GET_PERIOD_NS) {
+			return pwm_get_period_ns((const struct pwm *)(driver->device_driver));
+		}
+		else if(cmd == IOCTL_PWM_GET_PULSE_WIDTH_NS) {
+			return pwm_get_pulse_width_ns((const struct pwm *)(driver->device_driver));
 		}
 		break;
 	case DRIVER_TYPE_RTC:
diff --git a/source/firmware/kernel/driver/include/pwm.h b/source/firmware/kernel/driver/include/pwm.h
index 6d1d883..c8673ee 100644
--- a/source/firmware/kernel/driver/include/pwm.h
+++ b/source/firmware/kernel/driver/include/pwm.h
@@ -14,22 +14,28 @@ typedef int (*pwm_fp_open_t)(const void*);
 //! \brief Function pointer to the close function.
 typedef int (*pwm_fp_close_t)(const void*);
 
-//! \brief Function pointer to the read function.
 typedef int (*pwm_fp_set_duty_cycle_t)(const void*, unsigned int duty_cycle_percent);
 
+typedef int (*pwm_fp_get_period_ns_t)(const void*);
+typedef int (*pwm_fp_get_pulse_width_ns_t)(const void*);
+
 struct pwm_fp {
 	const pwm_fp_open_t open;
 	const pwm_fp_close_t close;
 	const pwm_fp_set_duty_cycle_t set_duty_cycle;
+	const pwm_fp_get_period_ns_t get_period;
+	const pwm_fp_get_pulse_width_ns_t get_pulse_width;
 };
 
 struct pwm {
 	const void *arch_dep_device;	//!< Architecture depended pwm device (i.e. stm32f10x_pwm_t).
-	const struct pwm_fp *fp;				//!< Function pointer for the pwm driver access.
+	const struct pwm_fp *fp;		//!< Function pointer for the pwm driver access.
 };
 
 int pwm_open(const struct pwm *device);
 int pwm_close(const struct pwm *device);
 int pwm_set_duty_cycle(const struct pwm *device, unsigned int duty_cycle_percent);
+int pwm_get_period_ns(const struct pwm *device);
+int pwm_get_pulse_width_ns(const struct pwm *device);
 
 #endif /* SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_PWM_H_ */
diff --git a/source/firmware/kernel/driver/pwm.c b/source/firmware/kernel/driver/pwm.c
index a8e733e..422c741 100644
--- a/source/firmware/kernel/driver/pwm.c
+++ b/source/firmware/kernel/driver/pwm.c
@@ -31,3 +31,19 @@ int pwm_set_duty_cycle(const struct pwm *device, unsigned int duty_cycle_percent
 	pwm_fp_set_duty_cycle_t set = device->fp->set_duty_cycle;
 	return set(device->arch_dep_device, duty_cycle_percent);
 }
+
+int pwm_get_period_ns(const struct pwm *device)
+{
+	if(NULL == device)
+		return -1;
+	pwm_fp_get_period_ns_t get = device->fp->get_period;
+	return get(device->arch_dep_device);
+}
+
+int pwm_get_pulse_width_ns(const struct pwm *device)
+{
+	if(NULL == device)
+		return -1;
+	pwm_fp_get_pulse_width_ns_t get = device->fp->get_pulse_width;
+	return get(device->arch_dep_device);
+}
diff --git a/source/firmware/kernel/interface/driver.h b/source/firmware/kernel/interface/driver.h
index e91349d..6ce5b5d 100644
--- a/source/firmware/kernel/interface/driver.h
+++ b/source/firmware/kernel/interface/driver.h
@@ -8,7 +8,9 @@
 #ifndef SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_DRIVER_H_
 #define SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_DRIVER_H_
 
-#define IOCTL_PWM_SET_DUTY_CYCLE	0
+#define IOCTL_PWM_SET_DUTY_CYCLE		0
+#define IOCTL_PWM_GET_PERIOD_NS			1
+#define IOCTL_PWM_GET_PULSE_WIDTH_NS	2
 
 enum driver_type {
 	DRIVER_TYPE_ADC,
diff --git a/source/test/pwm/main.c b/source/test/pwm/main.c
index 225fba0..9823541 100644
--- a/source/test/pwm/main.c
+++ b/source/test/pwm/main.c
@@ -18,84 +18,20 @@
 #include "list.h"
 #include "shell.h"
 
-static TIM_HandleTypeDef timer_handle = {
-		.Instance = TIM2,
-		.Init.Prescaler = 1000,
-		.Init.CounterMode = TIM_COUNTERMODE_UP,
-		.Init.Period = 0xffff,
-		.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1,
-		.Init.RepetitionCounter = 0,
-};
-
-static TIM_IC_InitTypeDef input_capture_init = {
-		.ICPrescaler = TIM_ICPSC_DIV1,
-		.ICFilter = 0,
-//		.ICPolarity = TIM_ICPOLARITY_FALLING,
-		.ICPolarity = TIM_ICPOLARITY_BOTHEDGE,
-		.ICSelection = TIM_ICSELECTION_INDIRECTTI,
-};
-
-static TIM_SlaveConfigTypeDef   slave_config = {
-		.SlaveMode = TIM_SLAVEMODE_RESET,
-		.InputTrigger = TIM_TS_TI1FP1,
-		.TriggerPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE,
-		.TriggerFilter = 0,
-};
-
-static GPIO_InitTypeDef port_cfg_b10 = {
-		.Pin = GPIO_PIN_10,
-		.Mode = GPIO_MODE_AF_OD,
-		.Speed = GPIO_SPEED_FREQ_HIGH,
-		.Pull = GPIO_NOPULL,
-		.Alternate = GPIO_AF1_TIM2,
-};
-
-static const struct stm32f4_gpio b10_gpio = {
-		.port = GPIOB,
-		.pin = &port_cfg_b10,
-};
-
-static const struct gpio __gpio_b10 = {
-		(void*)&b10_gpio,
-		&gpio_fp
-};
-
-static const struct driver gpio_b10 = {
-		DRIVER_TYPE_GPIO,
-		&__gpio_b10,
-};
-
-static int period = 0, period_start = 0, pulse = 0, pulse_start = 0;
-
-void config_input(void)
-{
-	__HAL_RCC_TIM2_CLK_ENABLE();
-	drv_open(&gpio_b10);
-	HAL_NVIC_SetPriority(TIM2_IRQn, 0, 1);
-
-	/* cfg timer */
-	HAL_TIM_IC_Init(&timer_handle);
-	HAL_TIM_IC_ConfigChannel(&timer_handle, &input_capture_init, TIM_CHANNEL_4);
-	HAL_TIM_SlaveConfigSynchronization(&timer_handle, &slave_config);
-	HAL_TIM_IC_Start_IT(&timer_handle, TIM_CHANNEL_4);
-
-	HAL_NVIC_EnableIRQ(TIM2_IRQn);
-}
-
 int main(void)
 {
 	char print_buffer[80];
+
 	drv_open(&pwm_1);
 	drv_open(&pwm_2);
 	drv_open(&pwm_3);
 	drv_open(&pwm_4);
 
 	drv_open(&pwm5_c2);
+	drv_open(&pwm2_c4);
 
 	drv_open(&uart_1);
 
-	config_input();
-
 	while(1) {
 		for(unsigned int duty = 0; duty < 100; duty++) {
 			drv_ioctl(&pwm_1, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
@@ -104,12 +40,14 @@ int main(void)
 			drv_ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			sleep_ms(100);
 
-			float dist = 84000.0 / pulse;
-			dist = 343.0 / dist;
-			dist /= 2.0;
-			dist *= 100; // m -> cm
-			int d = (int)dist;
-			sprintf(print_buffer, "distance: %d cm\r\n", d);
+			int pulse = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
+			int period = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PERIOD_NS, NULL);
+			int frq = 1000000 / period;
+			sprintf(print_buffer, "Frequency: %dHz\r\n", frq);
+			drv_write(&uart_1, print_buffer, strlen(print_buffer));
+
+			int dist = 343 * pulse / 1000 / 2;
+			sprintf(print_buffer, "distance: %d mm\r\n", dist);
 			drv_write(&uart_1, print_buffer, strlen(print_buffer));
 		}
 		for(unsigned int duty = 98; duty > 0; duty--) {
@@ -119,45 +57,17 @@ int main(void)
 			drv_ioctl(&pwm_4, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
 			sleep_ms(100);
 
-			float dist = 84000.0 / pulse;
-			dist = 343.0 / dist;
-			dist /= 2.0;
-			dist *= 100; // m -> cm
-			int d = (int)dist;
-			sprintf(print_buffer, "distance: %d cm\r\n", d);
+			int pulse = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
+			int period = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PERIOD_NS, NULL);
+			int frq = 1000000 / period;
+			sprintf(print_buffer, "Frequency: %dHz\r\n", frq);
+			drv_write(&uart_1, print_buffer, strlen(print_buffer));
+
+			int dist = 343 * pulse / 1000 / 2;
+			sprintf(print_buffer, "distance: %d mm\r\n", dist);
 			drv_write(&uart_1, print_buffer, strlen(print_buffer));
 		}
 	}
 
 	return 0;
 }
-
-#include "isr.h"
-void TIM2_IRQHandler(void)
-{
-	enter_isr();
-	TIM_HandleTypeDef *htim = &timer_handle;
-
-	/* Capture compare 4 event */
-	if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC4) != RESET) {
-		if(__HAL_TIM_GET_IT_SOURCE(htim, TIM_IT_CC4) !=RESET) {
-			__HAL_TIM_CLEAR_IT(htim, TIM_IT_CC4);
-			/* Input capture event */
-			if((htim->Instance->CCMR2 & TIM_CCMR2_CC4S) != 0x00U) {
-				uint32_t ch4 = htim->Instance->CCR4;
-				if(GPIO_PIN_SET == HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_10)) {
-					if(ch4 > period_start)
-						period = ch4 - period_start;
-					period_start = ch4;
-					pulse_start = ch4;
-				}
-				else {
-					if(ch4 > pulse_start)
-						pulse = ch4 - pulse_start;
-				}
-			}
-		}
-	}
-
-	exit_isr();
-}