diff --git a/firmware/hw_layer/ports/stm32/stm32f7/mpu_util.cpp b/firmware/hw_layer/ports/stm32/stm32f7/mpu_util.cpp
index 97aef38a2f..547814bec3 100644
--- a/firmware/hw_layer/ports/stm32/stm32f7/mpu_util.cpp
+++ b/firmware/hw_layer/ports/stm32/stm32f7/mpu_util.cpp
@@ -177,10 +177,16 @@ void sys_dual_bank(void) {
 
 void stm32_stop() {
 	SysTick->CTRL = 0;
-	__disable_irq();
-	RCC->AHB1RSTR = RCC_AHB1RSTR_GPIOERST;
+	SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
+	enginePins.errorLedPin.setValue(0);
+	enginePins.runningLedPin.setValue(0);
+	enginePins.communicationLedPin.setValue(0);
+	enginePins.warningLedPin.setValue(0);
 
-	// configure mode bits
+
+	palEnableLineEvent(PAL_LINE(GPIOA, 0), PAL_EVENT_MODE_RISING_EDGE);
+
+	PWR->CSR1 |= PWR_CSR1_WUIF;
 	PWR->CR1 &= ~PWR_CR1_PDDS;	// cleared PDDS means stop mode (not standby) 
 	PWR->CR1 |= PWR_CR1_FPDS;	// turn off flash in stop mode
 	PWR->CR1 |= PWR_CR1_UDEN;	// regulator underdrive in stop mode
@@ -188,9 +194,8 @@ void stm32_stop() {
 	PWR->CR1 |= PWR_CR1_LPDS;	// regulator in low power mode
 
 	// enable Deepsleep mode
-	SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
-
-	__WFE();
+	__disable_irq();
+	__WFI();
 
 	// Lastly, reboot
 	NVIC_SystemReset();
@@ -198,18 +203,11 @@ void stm32_stop() {
 
 void stm32_standby() {
 	SysTick->CTRL = 0;
-	__disable_irq();
-	RCC->AHB1RSTR = RCC_AHB1RSTR_GPIOERST;
-
-	// configure mode bits
-	PWR->CR1 |= PWR_CR1_PDDS;		// PDDS = use standby mode (not stop mode)
-
-	// enable Deepsleep mode
 	SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;
-
-	// Wait for event - this should never return as it kills the chip until a reset
-	__WFE();
-
-	// Lastly, reboot
-	NVIC_SystemReset();
+	PWR->CR1 |= PWR_CR1_PDDS;		// PDDS = use standby mode (not stop mode)
+	PWR->CSR2 |= PWR_CSR2_EWUP1; //EWUP1: Enable Wakeup pin for PA0
+	PWR->CR2 |= PWR_CR2_CWUPF1; //Clear Wakeup Pin flag for PA0
+	PWR->CR1 |= PWR_CR1_CSBF;  //Clear standby flag
+	__disable_irq();
+	__WFI();
 }