diff --git a/CM7/Core/Inc/exti.h b/CM7/Core/Inc/exti.h
new file mode 100644
index 0000000..3187b6b
--- /dev/null
+++ b/CM7/Core/Inc/exti.h
@@ -0,0 +1,14 @@
+#ifndef EXTI_H
+#define EXTI_H
+
+#include <stdint.h>
+
+typedef void (*exti_callback_t)(void *);
+
+/**
+ * @brief Registers callback to run when external interrupt occurs on specified pin
+ * 
+ */
+void exti_register_callback(uint16_t pin_mask, exti_callback_t callback, void *param);
+
+#endif /* EXTI_H */
\ No newline at end of file
diff --git a/CM7/Core/Inc/stm32h7xx_it.h b/CM7/Core/Inc/stm32h7xx_it.h
index ed65d7b..593d9a8 100644
--- a/CM7/Core/Inc/stm32h7xx_it.h
+++ b/CM7/Core/Inc/stm32h7xx_it.h
@@ -53,8 +53,10 @@ void BusFault_Handler(void);
 void UsageFault_Handler(void);
 void DebugMon_Handler(void);
 void SysTick_Handler(void);
+void EXTI2_IRQHandler(void);
 void DMA1_Stream0_IRQHandler(void);
 void DMA1_Stream2_IRQHandler(void);
+void EXTI15_10_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 
 /* USER CODE END EFP */
diff --git a/CM7/Core/Src/exti.c b/CM7/Core/Src/exti.c
new file mode 100644
index 0000000..3795a4f
--- /dev/null
+++ b/CM7/Core/Src/exti.c
@@ -0,0 +1,38 @@
+#include "exti.h"
+
+#include <stdint.h>
+#include <stddef.h>
+
+#define NUM_EXTI_PINS 16
+
+struct callback_data {
+    exti_callback_t cb;
+    void *param;
+};
+
+static struct callback_data callbacks[NUM_EXTI_PINS] = {0};
+
+// Overload weak EXTI callback function
+void HAL_GPIO_EXTI_Callback(uint16_t pin_mask)
+{
+    // loop through callbacks and call them if the bit is set
+    for (int i = 0; i < NUM_EXTI_PINS; i++) {
+        if (pin_mask & (1 << i) && callbacks[i].cb != NULL) {
+            callbacks[i].cb(callbacks[i].param);
+        }
+    }
+}
+
+void exti_register_callback(uint16_t pin_mask, exti_callback_t callback, void *param)
+{
+    if (pin_mask == 0)
+        return;
+
+    // get index of set bit in pin_mask
+    // stolen from https://stackoverflow.com/questions/14767308/how-to-compute-log-base-2-using-bitwise-operators
+    int i = 0;
+    while (pin_mask >>= 1) i++;
+
+    callbacks[i].cb = callback;
+    callbacks[i].param = param;
+}
\ No newline at end of file
diff --git a/CM7/Core/Src/stm32h7xx_hal_msp.c b/CM7/Core/Src/stm32h7xx_hal_msp.c
index a3cad01..52accbd 100644
--- a/CM7/Core/Src/stm32h7xx_hal_msp.c
+++ b/CM7/Core/Src/stm32h7xx_hal_msp.c
@@ -698,9 +698,8 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* htim_encoder)
     /**TIM2 GPIO Configuration
     PA0     ------> TIM2_CH1
     PA1     ------> TIM2_CH2
-    PA2     ------> TIM2_CH3
     */
-    GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2;
+    GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
@@ -723,9 +722,8 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* htim_encoder)
     /**TIM4 GPIO Configuration
     PD12     ------> TIM4_CH1
     PD13     ------> TIM4_CH2
-    PD14     ------> TIM4_CH3
     */
-    GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_14;
+    GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_13;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
@@ -879,9 +877,8 @@ void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* htim_encoder)
     /**TIM2 GPIO Configuration
     PA0     ------> TIM2_CH1
     PA1     ------> TIM2_CH2
-    PA2     ------> TIM2_CH3
     */
-    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2);
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_0|GPIO_PIN_1);
 
   /* USER CODE BEGIN TIM2_MspDeInit 1 */
 
@@ -898,9 +895,8 @@ void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* htim_encoder)
     /**TIM4 GPIO Configuration
     PD12     ------> TIM4_CH1
     PD13     ------> TIM4_CH2
-    PD14     ------> TIM4_CH3
     */
-    HAL_GPIO_DeInit(GPIOD, GPIO_PIN_12|GPIO_PIN_13|GPIO_PIN_14);
+    HAL_GPIO_DeInit(GPIOD, GPIO_PIN_12|GPIO_PIN_13);
 
   /* USER CODE BEGIN TIM4_MspDeInit 1 */
 
diff --git a/CM7/Core/Src/stm32h7xx_it.c b/CM7/Core/Src/stm32h7xx_it.c
index 15636eb..da9d301 100644
--- a/CM7/Core/Src/stm32h7xx_it.c
+++ b/CM7/Core/Src/stm32h7xx_it.c
@@ -183,6 +183,20 @@ void SysTick_Handler(void)
 /* please refer to the startup file (startup_stm32h7xx.s).                    */
 /******************************************************************************/
 
+/**
+  * @brief This function handles EXTI line2 interrupt.
+  */
+void EXTI2_IRQHandler(void)
+{
+  /* USER CODE BEGIN EXTI2_IRQn 0 */
+
+  /* USER CODE END EXTI2_IRQn 0 */
+  HAL_GPIO_EXTI_IRQHandler(ENC1_Z_Pin);
+  /* USER CODE BEGIN EXTI2_IRQn 1 */
+
+  /* USER CODE END EXTI2_IRQn 1 */
+}
+
 /**
   * @brief This function handles DMA1 stream0 global interrupt.
   */
@@ -211,6 +225,20 @@ void DMA1_Stream2_IRQHandler(void)
   /* USER CODE END DMA1_Stream2_IRQn 1 */
 }
 
+/**
+  * @brief This function handles EXTI line[15:10] interrupts.
+  */
+void EXTI15_10_IRQHandler(void)
+{
+  /* USER CODE BEGIN EXTI15_10_IRQn 0 */
+
+  /* USER CODE END EXTI15_10_IRQn 0 */
+  HAL_GPIO_EXTI_IRQHandler(ENC2_Z_Pin);
+  /* USER CODE BEGIN EXTI15_10_IRQn 1 */
+
+  /* USER CODE END EXTI15_10_IRQn 1 */
+}
+
 /* USER CODE BEGIN 1 */
 
 /* USER CODE END 1 */