diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c
index 7d72ae7..c37d2b4 100644
--- a/Core/Src/stm32h7xx_it.c
+++ b/Core/Src/stm32h7xx_it.c
@@ -111,10 +111,16 @@ void NMI_Handler(void)
 /**
   * @brief This function handles Hard fault interrupt.
   */
-void HardFault_Handler(void)
+__attribute__((naked)) void HardFault_Handler(void)
 {
   /* USER CODE BEGIN HardFault_IRQn 0 */
-
+  __asm volatile (
+	"TST lr, #4 \n"
+	"ITE EQ \n"
+	"MRSEQ r0, MSP \n"
+	"MRSNE r0, PSP \n"
+	"B hard_fault_handler_c \n"
+  );
   /* USER CODE END HardFault_IRQn 0 */
   while (1)
   {
@@ -126,10 +132,16 @@ void HardFault_Handler(void)
 /**
   * @brief This function handles Memory management fault.
   */
-void MemManage_Handler(void)
+__attribute__((naked)) void MemManage_Handler(void)
 {
   /* USER CODE BEGIN MemoryManagement_IRQn 0 */
-
+  __asm volatile (
+	"TST lr, #4 \n"
+	"ITE EQ \n"
+	"MRSEQ r0, MSP \n"
+	"MRSNE r0, PSP \n"
+	"B hard_fault_handler_c \n"
+  );
   /* USER CODE END MemoryManagement_IRQn 0 */
   while (1)
   {
@@ -141,10 +153,16 @@ void MemManage_Handler(void)
 /**
   * @brief This function handles Pre-fetch fault, memory access fault.
   */
-void BusFault_Handler(void)
+__attribute__((naked)) void BusFault_Handler(void)
 {
   /* USER CODE BEGIN BusFault_IRQn 0 */
-
+  __asm volatile (
+	"TST lr, #4 \n"
+	"ITE EQ \n"
+	"MRSEQ r0, MSP \n"
+	"MRSNE r0, PSP \n"
+	"B hard_fault_handler_c \n"
+  );
   /* USER CODE END BusFault_IRQn 0 */
   while (1)
   {
@@ -156,10 +174,16 @@ void BusFault_Handler(void)
 /**
   * @brief This function handles Undefined instruction or illegal state.
   */
-void UsageFault_Handler(void)
+__attribute__((naked)) void UsageFault_Handler(void)
 {
   /* USER CODE BEGIN UsageFault_IRQn 0 */
-  
+  __asm volatile (
+	"TST lr, #4 \n"
+	"ITE EQ \n"
+	"MRSEQ r0, MSP \n"
+	"MRSNE r0, PSP \n"
+	"B hard_fault_handler_c \n"
+  );
   /* USER CODE END UsageFault_IRQn 0 */
   while (1)
   {