diff --git a/chconf.h b/chconf.h
index 8167cc6ef..ec46d0ebf 100644
--- a/chconf.h
+++ b/chconf.h
@@ -486,7 +486,7 @@
 }
 
 #ifndef __ASSEMBLER__
-void main_stop_motor_and_reset(void);
+void main_fault_handler(void);
 #endif
 
 /**
@@ -495,7 +495,7 @@ void main_stop_motor_and_reset(void);
  *          the system is halted.
  */
 #define CH_CFG_SYSTEM_HALT_HOOK(reason) {                                   \
-  main_stop_motor_and_reset();                                              \
+  main_system_halt(reason);                                                 \
 }
 
 /** @} */
diff --git a/irq_handlers.c b/irq_handlers.c
index be3ec445e..fb537983d 100644
--- a/irq_handlers.c
+++ b/irq_handlers.c
@@ -75,21 +75,21 @@ CH_IRQ_HANDLER(PVD_IRQHandler) {
 }
 
 CH_IRQ_HANDLER(NMI_Handler) {
-	main_stop_motor_and_reset();
+	main_fault_handler();
 }
 
 CH_IRQ_HANDLER(HardFault_Handler) {
-	main_stop_motor_and_reset();
+	main_fault_handler();
 }
 
 CH_IRQ_HANDLER(MemManage_Handler) {
-	main_stop_motor_and_reset();
+	main_fault_handler();
 }
 
 CH_IRQ_HANDLER(BusFault_Handler) {
-	main_stop_motor_and_reset();
+	main_fault_handler();
 }
 
 CH_IRQ_HANDLER(UsageFault_Handler) {
-	main_stop_motor_and_reset();
+	main_fault_handler();
 }
diff --git a/main.c b/main.c
index a02d4c53c..87a408477 100644
--- a/main.c
+++ b/main.c
@@ -91,6 +91,8 @@
 foc_profile g_foc_profile;
 #endif
 
+__attribute__((section(".noinit"))) CrashInfo crash_info;
+
 // Private variables
 static THD_WORKING_AREA(periodic_thread_wa, 256);
 static THD_WORKING_AREA(led_thread_wa, 256);
@@ -105,7 +107,7 @@ static THD_FUNCTION(flash_integrity_check_thread, arg) {
 
 	for(;;) {
 		if (flash_helper_verify_flash_memory_chunk() == FAULT_CODE_FLASH_CORRUPTION) {
-			NVIC_SystemReset();
+			chSysHalt("Flash corruption detected.");
 		}
 
 		chThdSleepMilliseconds(6);
@@ -251,6 +253,15 @@ uint32_t main_calc_hw_crc(void) {
 }
 
 int main(void) {
+	if (RCC->CSR & RCC_CSR_PORRSTF) {
+		// In case of Power On reset erase the whole struct
+		memset(&crash_info, 0, sizeof(crash_info));
+	}
+	crash_info.reset_flags = RCC->CSR;
+
+	// Clear the reset flags
+	RCC->CSR |= RCC_CSR_RMVF;
+
 	halInit();
 	chSysInit();
 
@@ -367,7 +378,7 @@ int main(void) {
 	}
 }
 
-void main_stop_motor_and_reset(void) {
+static void stop_motor_and_reset(void) {
 	TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive);
 	TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
 	TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
@@ -408,3 +419,48 @@ void main_stop_motor_and_reset(void) {
 
 	NVIC_SystemReset();
 }
+
+void main_system_halt(const char *reason) {
+	crash_info.halt_reason = reason;
+	crash_info.registers_stored = false;
+
+	stop_motor_and_reset();
+}
+
+void main_fault_handler(void) __attribute__((naked));
+void main_fault_handler(void) {
+	// Store the currently-in-use stack pointer to the first function argument
+	// and call fault_handler_c
+	__asm volatile (
+		"TST LR, #4\n"
+		"ITE EQ\n"
+		"MRSEQ R0, MSP\n"
+		"MRSNE R0, PSP\n"
+		"B fault_handler_c\n"
+	);
+}
+
+void fault_handler_c(uint32_t *hardfault_args) {
+	// Store the registers dumped on the stack after a crash
+	crash_info.registers.r0 = hardfault_args[0];
+	crash_info.registers.r1 = hardfault_args[1];
+	crash_info.registers.r2 = hardfault_args[2];
+	crash_info.registers.r3 = hardfault_args[3];
+	crash_info.registers.r12 = hardfault_args[4];
+	crash_info.registers.lr = hardfault_args[5];
+	crash_info.registers.pc = hardfault_args[6];
+	crash_info.registers.psr = hardfault_args[7];
+
+	// Store the crash information registers
+	crash_info.registers.cfsr = SCB->CFSR;
+	crash_info.registers.hfsr = SCB->HFSR;
+	crash_info.registers.mmfar = SCB->MMFAR;
+	crash_info.registers.bfar = SCB->BFAR;
+	crash_info.registers.afsr = SCB->AFSR;
+	crash_info.registers.shcsr = SCB->SHCSR;
+
+	crash_info.registers_stored = true;
+	crash_info.halt_reason = NULL;
+
+	stop_motor_and_reset();
+}
diff --git a/main.h b/main.h
index 0aefb954b..b847bf404 100644
--- a/main.h
+++ b/main.h
@@ -22,6 +22,34 @@
 
 bool main_init_done(void);
 uint32_t main_calc_hw_crc(void);
-void main_stop_motor_and_reset(void);
+void main_system_halt(const char *reason);
+void main_fault_handler(void);
+
+typedef struct {
+	uint32_t r0;
+	uint32_t r1;
+	uint32_t r2;
+	uint32_t r3;
+	uint32_t r12;
+	uint32_t lr;
+	uint32_t pc;
+	uint32_t psr;
+
+	uint32_t cfsr;
+	uint32_t hfsr;
+	uint32_t mmfar;
+	uint32_t bfar;
+	uint32_t afsr;
+	uint32_t shcsr;
+} CrashRegisters;
+
+typedef struct {
+	uint32_t reset_flags;
+	const char *halt_reason;
+	CrashRegisters registers;
+	bool registers_stored;
+} CrashInfo;
+
+extern CrashInfo crash_info;
 
 #endif /* MAIN_H_ */
diff --git a/terminal.c b/terminal.c
index ea04cd2b0..2cc95c6b8 100644
--- a/terminal.c
+++ b/terminal.c
@@ -38,6 +38,7 @@
 #include "mempools.h"
 #include "crc.h"
 #include "firmware_metadata.h"
+#include "main.h"
 
 #include <string.h>
 #include <ctype.h>
@@ -67,6 +68,65 @@ static volatile int fault_vec_write = 0;
 static terminal_callback_struct callbacks[CALLBACK_LEN];
 static int callback_write = 0;
 
+static const char* const rcc_flag_names[] = {
+	"Brownout",		// Bit 25 (BOR): Reset due to voltage drop below threshold
+	"Reset Pin",	// Bit 26 (PIN): Reset via the physical NRST pin (button)
+	"Power On",		// Bit 27 (POR/PDR): Cold start or power cycle
+	"Software",		// Bit 28 (SFT): Reset triggered by software (NVIC_SystemReset)
+	"Ind. WDG",		// Bit 29 (IWDG): Independent watchdog triggered (system hung)
+	"Win. WDG",		// Bit 30 (WWDG): Window watchdog triggered (timing violation)
+	"Low Power"		// Bit 31 (LPWR): Reset occurred during Low Power mode entry/exit
+};
+
+void rcc_csr_to_string(uint32_t csr_val, char *buffer) {
+	char *p = buffer;
+	uint32_t flags = csr_val >> 25;
+
+	for (uint8_t i = 0; i < 7; i++) {
+		if (flags & (1 << i)) {
+			const char *name = rcc_flag_names[i];
+			while (*name) {
+				*p++ = *name++;
+			}
+			*p++ = ',';
+		}
+	}
+
+	if (p > buffer) {
+		p--;
+	}
+	*p = '\0';
+}
+
+static void crash_diag(void) {
+	char rst_flags[65];  // sized to fit all possible reset flag strings plus separators
+	rcc_csr_to_string(crash_info.reset_flags, rst_flags);
+	commands_printf("Reset flags: %s", rst_flags);
+
+	if (crash_info.halt_reason) {
+		commands_printf("OS halted, reason: %s", crash_info.halt_reason);
+	}
+
+	if (crash_info.registers_stored) {
+		commands_printf("System crashed, registers:");
+		commands_printf("r0: 0x%08x", crash_info.registers.r0);
+		commands_printf("r1: 0x%08x", crash_info.registers.r1);
+		commands_printf("r2: 0x%08x", crash_info.registers.r2);
+		commands_printf("r3: 0x%08x", crash_info.registers.r3);
+		commands_printf("r12: 0x%08x", crash_info.registers.r12);
+		commands_printf("lr: 0x%08x", crash_info.registers.lr);
+		commands_printf("pc: 0x%08x", crash_info.registers.pc);
+		commands_printf("psr: 0x%08x\n", crash_info.registers.psr);
+
+		commands_printf("cfsr: 0x%08x", crash_info.registers.cfsr);
+		commands_printf("hfsr: 0x%08x", crash_info.registers.hfsr);
+		commands_printf("mmfar: 0x%08x", crash_info.registers.mmfar);
+		commands_printf("bfar: 0x%08x", crash_info.registers.bfar);
+		commands_printf("afsr: 0x%08x", crash_info.registers.afsr);
+		commands_printf("shcsr: 0x%08x", crash_info.registers.shcsr);
+	}
+}
+
 __attribute__((section(".text2"))) void terminal_process_string(char *str) {
 	// Echo command so user can see what they previously ran
 	commands_printf("-> %s \n", str);
@@ -1155,6 +1215,8 @@ __attribute__((section(".text2"))) void terminal_process_string(char *str) {
 	} else if (strcmp(argv[0], "rebootwdt") == 0) {
 		chSysLock();
 		for (;;) {__NOP();}
+	} else if (strcmp(argv[0], "crash_diag") == 0) {
+		crash_diag();
 	}
 
 	// The help command
@@ -1273,6 +1335,9 @@ __attribute__((section(".text2"))) void terminal_process_string(char *str) {
 		commands_printf("rebootwdt");
 		commands_printf("  Reboot using the watchdog timer.");
 
+		commands_printf("crash_diag");
+		commands_printf("  Print crash/reset diagnostics.");
+
 		for (int i = 0;i < callback_write;i++) {
 			if (callbacks[i].cbf == 0) {
 				continue;