Add fault handlers to ease ARM-v7m (Cortex M3/M4(F)/M7 debugging

5 files changed, 406 insertions, 0 deletions

diff --git a/os/common/ports/ARMCMx/compilers/GCC/utils/fault_handlers_v7m.c b/os/common/ports/ARMCMx/compilers/GCC/utils/fault_handlers_v7m.c
new file mode 100644
index 0000000..37b7fc7
--- /dev/null
+++ b/os/common/ports/ARMCMx/compilers/GCC/utils/fault_handlers_v7m.c
@@ -0,0 +1,215 @@
+/*
+    ChibiOS - Copyright (C) 2019 Diego Ismirlian (dismirlian(at)google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#include "fault_handlers.h"
+#include <hal.h>
+#include <string.h>
+
+#ifndef FAULT_NO_PRINT
+#include <chprintf.h>
+#include <memstreams.h>
+
+#define fault_printf(f, ...) 										\
+	chprintf((BaseSequentialStream *)(&ms), 	\
+			f "\n", ##__VA_ARGS__)
+
+static MemoryStream ms;
+#else
+#define fault_printf(f, ...) do {} while(0)
+#endif
+
+static struct fault_info fault_info;
+
+static void _mem_fault(void) {
+	fault_printf("== Mem faults follow ==");
+
+	if (SCB->CFSR & SCB_CFSR_MSTKERR_Msk) {
+		fault_printf("Stacking error");
+		fault_info.decoded_fault_registers.memfault.stacking_error = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_MUNSTKERR_Msk) {
+		fault_printf("Unstacking error");
+		fault_info.decoded_fault_registers.memfault.unstacking_error = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_DACCVIOL_Msk) {
+		fault_printf("Data Access Violation");
+		fault_info.decoded_fault_registers.memfault.data_access_violation = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_MMARVALID_Msk) {
+		fault_printf("Address: 0x%08x", (uint32_t)SCB->MMFAR);
+		fault_info.decoded_fault_registers.memfault.data_access_violation_address = (uint32_t)SCB->MMFAR;
+	} else {
+		fault_printf("Address: unknown");
+		fault_info.decoded_fault_registers.memfault.data_access_violation_address = 0xffffffff;
+	}
+	if (SCB->CFSR & SCB_CFSR_IACCVIOL_Msk) {
+		fault_printf("Instruction Access Violation");
+		fault_info.decoded_fault_registers.memfault.instruction_access_violation = true;
+	}
+}
+
+static void _bus_fault(void) {
+	fault_printf("== Bus faults follow ==");
+
+	if (SCB->CFSR & SCB_CFSR_STKERR_Msk) {
+		fault_printf("Stacking error");
+		fault_info.decoded_fault_registers.busfault.stacking_error = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_UNSTKERR_Msk) {
+		fault_printf("Unstacking error");
+		fault_info.decoded_fault_registers.busfault.unstacking_error = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_PRECISERR_Msk) {
+		fault_printf("Precise data bus error");
+		fault_info.decoded_fault_registers.busfault.precise_data_bus_error = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_BFARVALID_Msk) {
+		fault_printf("Address: 0x%08x", (uint32_t)SCB->BFAR);
+		fault_info.decoded_fault_registers.busfault.precise_data_bus_error_address = (uint32_t)SCB->BFAR;
+	} else {
+		fault_printf("Address: unknown");
+		fault_info.decoded_fault_registers.busfault.precise_data_bus_error_address = 0xffffffff;
+	}
+	if (SCB->CFSR & SCB_CFSR_IMPRECISERR_Msk) {
+		fault_printf("Imprecise data bus error");
+		fault_info.decoded_fault_registers.busfault.imprecise_data_bus_error = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_IBUSERR_Msk) {
+		fault_printf("Instruction bus error");
+		fault_info.decoded_fault_registers.busfault.instruction_bus_error = true;
+	}
+}
+
+static void _usage_fault(void) {
+	fault_printf("== Usage faults follow ==");
+
+	if (SCB->CFSR & SCB_CFSR_DIVBYZERO_Msk) {
+		fault_printf("Division by zero");
+		fault_info.decoded_fault_registers.usagefault.division_by_zero = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_UNALIGNED_Msk) {
+		fault_printf("Unaligned memory access");
+		fault_info.decoded_fault_registers.usagefault.unaligned_memory_access = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_NOCP_Msk) {
+		fault_printf("No coprocessor instructions");
+		fault_info.decoded_fault_registers.usagefault.no_coprocessor_instructions = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_INVPC_Msk) {
+		fault_printf("Invalid load of PC");
+		fault_info.decoded_fault_registers.usagefault.invalid_load_of_pc = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_INVSTATE_Msk) {
+		fault_printf("Invalid state");
+		fault_info.decoded_fault_registers.usagefault.invalid_state = true;
+	}
+	if (SCB->CFSR & SCB_CFSR_UNDEFINSTR_Msk) {
+		fault_printf("Undefined instruction");
+		fault_info.decoded_fault_registers.usagefault.undefined_instruction = true;
+	}
+}
+
+static void _init_fault_info(void) {
+#ifndef FAULT_NO_PRINT
+	msObjectInit(&ms,
+			(uint8_t *)fault_info.decoded_info_string,
+			sizeof(fault_info.decoded_info_string) - 1, 0);
+#endif
+}
+
+static void _save_fault_info(void) {
+	memset(&fault_info.decoded_fault_registers, 0, sizeof(fault_info.decoded_fault_registers));
+#ifndef FAULT_NO_PRINT
+	memset(&fault_info.decoded_info_string, 0, sizeof(fault_info.decoded_info_string));
+#endif
+
+	if (ch.rlist.current) {
+		fault_printf("Thread: 0x%08x, %s",
+				ch.rlist.current, ch.rlist.current->name);
+
+		fault_info.decoded_fault_registers.general.current_thread_address = (uint32_t)ch.rlist.current;
+		fault_info.decoded_fault_registers.general.current_thread_name = ch.rlist.current->name;
+	} else {
+		fault_printf("Thread: unknown");
+	}
+
+	if (SCB->HFSR & SCB_HFSR_VECTTBL_Msk) {
+		fault_printf("Bus fault on vector table read");
+		fault_info.decoded_fault_registers.general.bus_fault_on_ivt_read = true;
+	}
+
+	if (SCB->HFSR & SCB_HFSR_FORCED_Msk) {
+		fault_info.decoded_fault_registers.general.escalation = true;
+		_mem_fault();
+		_bus_fault();
+		_usage_fault();
+	}
+	if (!(SCB->HFSR &
+			(SCB_HFSR_VECTTBL_Msk | SCB_HFSR_FORCED_Msk))) {
+		fault_printf("No fault info");
+	}
+}
+
+#if defined(FAULT_INFO_HOOK)
+void FAULT_INFO_HOOK(const struct fault_info *info);
+#endif
+
+void _hardfault_info(void) {
+	_init_fault_info();
+	fault_printf("HardFault Handler");
+	_save_fault_info();
+
+#if defined(FAULT_INFO_HOOK)
+	FAULT_INFO_HOOK(&fault_info);
+#endif
+}
+
+void _hardfault_epilogue(void) __attribute__((used, naked));
+void _hardfault_epilogue(void) {
+
+	/* This is part of the HardFault handler
+	 *
+	 * You may inspect fault_info.decoded_fault_registers and
+	 * fault_info.decoded_info_string to get a description of the fault that
+	 * occurred.
+	 *
+	 * Also, the debugger should show an artificial call stack that led to the
+	 * fault. This stack is reconstructed in assembly code, until GDB includes
+	 * a way of automatically unwind an exception stack.
+	 *
+	 */
+	__asm volatile(
+			"bkpt 	#0					\n"
+			"b 		_hardfault_exit		\n"
+	);
+}
+
+void _unhandled_exception(void) {
+	/* This is an unhandled exception
+	 *
+	 * Once the breakpoint is hit, the debugger should show the ISR number
+	 * in the vector_number variable. Don't trust the debugger's stack unwind;
+	 * the _unhandled_exception ISR is shared among all undefined vectors.
+	 */
+
+	volatile uint32_t vector_number = SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk;
+	(void)vector_number;
+
+	__asm volatile("bkpt #0");
+
+	/* we are here if there is no debugger attached */
+	chSysHalt("unhandled exception");
+}
diff --git a/os/common/ports/ARMCMx/compilers/GCC/utils/fault_handlers_v7m.mk b/os/common/ports/ARMCMx/compilers/GCC/utils/fault_handlers_v7m.mk
new file mode 100644
index 0000000..8e04203
--- /dev/null
+++ b/os/common/ports/ARMCMx/compilers/GCC/utils/fault_handlers_v7m.mk
@@ -0,0 +1,4 @@
+ALLINC		+= 	$(CHIBIOS_CONTRIB)/os/various/fault_handlers		\
+				$(CHIBIOS_CONTRIB)/os/common/ports/ARMCMx/compilers/GCC/utils/
+ALLCSRC 	+= 	$(CHIBIOS_CONTRIB)/os/common/ports/ARMCMx/compilers/GCC/utils/fault_handlers_v7m.c
+ALLXASMSRC 	+= 	$(CHIBIOS_CONTRIB)/os/common/ports/ARMCMx/compilers/GCC/utils/hardfault_handler_v7m.S
diff --git a/os/common/ports/ARMCMx/compilers/GCC/utils/hardfault_handler_v7m.S b/os/common/ports/ARMCMx/compilers/GCC/utils/hardfault_handler_v7m.S
new file mode 100644
index 0000000..9b4b96f
--- /dev/null
+++ b/os/common/ports/ARMCMx/compilers/GCC/utils/hardfault_handler_v7m.S
@@ -0,0 +1,113 @@
+/*
+    ChibiOS - Copyright (C) 2019 Diego Ismirlian (dismirlian(at)google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+                .syntax unified
+                .cpu    cortex-m4
+
+                .section .data._fault_stack
+                .align 3
+_fault_stack:
+                .skip 256
+_fault_stack_end:
+
+                .thumb
+
+                .section .text.HardFault_Handler
+                .thumb_func
+                .globl  HardFault_Handler
+HardFault_Handler:
+                /* disable further interrupts */
+                cpsid i
+
+                /* preserve the ISR sp for later */
+                mov     r1, sp
+
+                /* set the sp to a separate stack, in case of sp corruption */
+                ldr     sp, .L1
+
+                /* preserve the ISR lr and sp for later */
+                push    {r1, lr}
+
+                /* print info */
+                bl       _hardfault_info
+
+                /* restore the sp and the lr */
+                pop     {r1, lr}
+                mov     sp, r1
+
+                /* Try to rebuild the stack for the debugger.
+                 * The idea is that the debugger will unwind the stack, and
+                 * show a call to the HardFault_Handler from the offending
+                 * instruction */
+
+                /* check which stack was in use */
+                tst     lr, #4          //check bit 2 of EXC_RETURN
+                ite     eq
+                mrseq   r0, msp
+                mrsne   r0, psp         //r0 points to the stack that was in use
+
+                /* try to rebuild the stack for the debugger */
+                mov     sp, r0          //sp points to the end of the IRQ stack
+                /* check if FPU registers were stacked */
+                tst     lr, #16         //check bit 4 of EXC_RETURN
+                ite     eq
+                addeq   sp, #104        //jump over the IRQ+FPU stack
+                addne   sp, #32         //jump over the IRQ stack
+
+                /* compensate padding */
+                ldr     r1, [sp, #28]   //r1 = stacked xPSR
+                tst     r1, #512        //check bit 9 of the stacked xPSR
+                ite     eq
+                addeq   sp, #0          //add 0 to sp if there was no padding
+                addne   sp, #4          //add 4 to sp if there was padding
+                /* here, sp finally points to the stack before the IRQ was triggered */
+
+                /* set lr to the stacked PC address, so the debugger can show where the
+                   fault was produced (may not be accurate, depending on the fault) */
+                ldr     lr, [r0, #24]
+
+                /* restore used registers */
+                ldr     r0, [r0, #0]
+                ldr     r1, [r0, #4]
+
+                b       _hardfault_epilogue
+
+                .align  2
+.L1:            .word _fault_stack_end
+
+
+                .section .text._hardfault_exit
+                .thumb_func
+                .globl  _hardfault_exit
+_hardfault_exit:
+                /* we are here if there is no debugger attached */
+
+                /* restore the sp to the separate stack */
+                ldr     sp, .L3
+
+                /* call chSysHalt */
+                ldr     r0, =.L2
+                bl      chSysHalt
+
+                b       .
+
+                .align  2
+.L3:            .word _fault_stack_end
+
+                .align  2
+.L2:            .asciz  "hard fault"
+
+                .align  2
diff --git a/os/common/ports/ARMCMx/compilers/GCC/utils/port_fault_handlers.h b/os/common/ports/ARMCMx/compilers/GCC/utils/port_fault_handlers.h
new file mode 100644
index 0000000..ca98459
--- /dev/null
+++ b/os/common/ports/ARMCMx/compilers/GCC/utils/port_fault_handlers.h
@@ -0,0 +1,52 @@
+/*
+    ChibiOS - Copyright (C) 2019 Diego Ismirlian (dismirlian(at)google's mail)
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+        http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+*/
+
+#ifndef FAULT_HANDLERS_v7m_H_
+#define FAULT_HANDLERS_v7m_H_
+
+struct decoded_fault_registers {
+	struct general {
+		bool bus_fault_on_ivt_read;
+		bool escalation;
+		uint32_t current_thread_address;
+		const char *current_thread_name;
+	} general;
+	struct memfault {
+		bool stacking_error;
+		bool unstacking_error;
+		bool data_access_violation;
+		uint32_t data_access_violation_address;
+		bool instruction_access_violation;
+	} memfault;
+	struct busfault {
+		bool stacking_error;
+		bool unstacking_error;
+		bool precise_data_bus_error;
+		uint32_t precise_data_bus_error_address;
+		bool imprecise_data_bus_error;
+		bool instruction_bus_error;
+	} busfault;
+	struct usagefault {
+		bool division_by_zero;
+		bool unaligned_memory_access;
+		bool no_coprocessor_instructions;
+		bool invalid_load_of_pc;
+		bool invalid_state;
+		bool undefined_instruction;
+	} usagefault;
+};
+
+#endif /* FAULT_HANDLERS_v7m_H_ */
diff --git a/os/various/fault_handlers/fault_handlers.h b/os/various/fault_handlers/fault_handlers.h
new file mode 100644
index 0000000..1fb210b
--- /dev/null
+++ b/os/various/fault_handlers/fault_handlers.h
@@ -0,0 +1,22 @@
+#ifndef FAULT_HANDLERS_H_
+#define FAULT_HANDLERS_H_
+
+#include <ch.h>
+#include "port_fault_handlers.h"
+
+/*
+ * Notes:
+ *
+ * 1) #define FAULT_NO_PRINT to remove chprintf, etc
+ * 2) #define FAULT_INFO_HOOK(fault_info) to receive a struct fault_info when
+ *    a fault is produced.
+ */
+
+struct fault_info {
+	struct decoded_fault_registers decoded_fault_registers;
+#ifndef FAULT_NO_PRINT
+	char decoded_info_string[300];
+#endif
+};
+
+#endif /* FAULT_HANDLERS_H_ */