Skip to content

Commit 20b9620

Browse files
msplrmsplr
committed
Add fault handler for Cortex-M0/M3/M4
1 parent 3aa435c commit 20b9620

File tree

4 files changed

+292
-0
lines changed

4 files changed

+292
-0
lines changed

‎fault.c

Lines changed: 208 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,208 @@
1+
#include <ch.h>
2+
#include <stdint.h>
3+
4+
#if (__CORTEX_M != 0x00 && __CORTEX_M != 0x03 && __CORTEX_M != 0x04)
5+
#error "__CORTEX_M version not supported"
6+
#endif
7+
8+
#if (__CORTEX_M == 0x03 || __CORTEX_M == 0x04)
9+
// ARMv7-M Architecture Reference Manual, Chapter B1.5.14 Fault behavior, p. 669
10+
// MemManage Status Register, MMFSR
11+
#define MMFSR_IACCVIOL (1<<0) // Instruction access violation
12+
#define MMFSR_DACCVIOL (1<<1) // Data access violation
13+
#define MMFSR_MUNSTKERR (1<<3) // Unstacking error
14+
#define MMFSR_MSTKERR (1<<4) // Stacking error
15+
#define MMFSR_MLSPERR (1<<5) // MemManage Fault during FP lazy state preservation
16+
#define MMFSR_MMARVALID (1<<7) // Memory Manage Address Register address valid flag
17+
// BusFault Status Register, BFSR
18+
#define BFSR_IBUSERR (1<<0) // Instruction bus error flag
19+
#define BFSR_PRECISERR (1<<1) // Precise data bus error
20+
#define BFSR_IMPRECISERR (1<<2) // Imprecise data bus error
21+
#define BFSR_UNSTKERR (1<<3) // Unstacking error
22+
#define BFSR_STKERR (1<<4) // Stacking error
23+
#define BFSR_LSPERR (1<<5) // Bus Fault during FP lazy state preservation
24+
#define BFSR_BFARVALID (1<<7) // Bus Fault Address Register address valid flag
25+
// UsageFault Status Register, UFSR
26+
#define UFSR_UNDEFINSTR (1<<0) // The processor attempt to execute an undefined instruction
27+
#define UFSR_INVSTATE (1<<1) // Invalid combination of EPSR and instruction
28+
#define UFSR_INVPC (1<<2) // Attempt to load EXC_RETURN into pc illegally
29+
#define UFSR_NOCP (1<<3) // Attempt to use a coprocessor instruction
30+
#define UFSR_UNALIGNED (1<<8) // Fault occurs when there is an attempt to make an unaligned memory access
31+
#define UFSR_DIVBYZERO (1<<9) // Fault occurs when SDIV or DIV instruction is used with a divisor of 0
32+
// HardFault Status Register, HFSR
33+
#define HFSR_VECTTBL (1<<1) // Fault occurs because of vector table read on exception processing
34+
#define HFSR_FORCED (1<<30) // Hard Fault activated when a configurable Fault was received and cannot activate
35+
#define HFSR_DEBUGEVT (1<<31) // Fault related to debug
36+
#endif
37+
38+
// software saved stack frame
39+
struct arm_soft_frame {
40+
uint32_t r8;
41+
uint32_t r9;
42+
uint32_t r10;
43+
uint32_t r11;
44+
uint32_t r4;
45+
uint32_t r5;
46+
uint32_t r6;
47+
uint32_t r7;
48+
uint32_t lr_irq; // exception return value
49+
50+
#if (__FPU_PRESENT && __FPU_USED)
51+
uint32_t s16;
52+
uint32_t s17;
53+
uint32_t s18;
54+
uint32_t s19;
55+
uint32_t s20;
56+
uint32_t s21;
57+
uint32_t s22;
58+
uint32_t s23;
59+
uint32_t s24;
60+
uint32_t s25;
61+
uint32_t s26;
62+
uint32_t s27;
63+
uint32_t s28;
64+
uint32_t s29;
65+
uint32_t s30;
66+
uint32_t s31;
67+
#endif
68+
};
69+
70+
// hardware saved stack frame
71+
struct arm_irq_frame {
72+
uint32_t r0;
73+
uint32_t r1;
74+
uint32_t r2;
75+
uint32_t r3;
76+
uint32_t r12;
77+
uint32_t lr; // return address of fault context
78+
uint32_t pc; // fault address
79+
uint32_t psr;
80+
81+
#if (__FPU_PRESENT && __FPU_USED)
82+
uint32_t s0;
83+
uint32_t s1;
84+
uint32_t s2;
85+
uint32_t s3;
86+
uint32_t s4;
87+
uint32_t s5;
88+
uint32_t s6;
89+
uint32_t s7;
90+
uint32_t s8;
91+
uint32_t s9;
92+
uint32_t s10;
93+
uint32_t s11;
94+
uint32_t s12;
95+
uint32_t s13;
96+
uint32_t s14;
97+
uint32_t s15;
98+
uint32_t fpscr;
99+
uint32_t reserved;
100+
#endif
101+
};
102+
103+
// must be implemented by user
104+
extern void fault_printf(const char *fmt, ...);
105+
106+
static const char *fault_name(void)
107+
{
108+
#if (__CORTEX_M == 0x00)
109+
return "HardFault";
110+
#elif (__CORTEX_M == 0x03 || __CORTEX_M == 0x04)
111+
static const char *faults[] = {"HardFault", "MemManageFault", "BusFault", "UsageFault"};
112+
uint32_t isr = __get_IPSR();
113+
if (isr >= 3 && isr <= 6) {
114+
return faults[isr - 3];
115+
} else {
116+
return "unknown";
117+
}
118+
#endif
119+
}
120+
121+
void fault_handler(struct arm_soft_frame *soft_frame, void *msp, void *psp)
122+
{
123+
// stack pointer
124+
void *sp = ((soft_frame->lr_irq & 0x4) == 0) ? msp : psp;
125+
struct arm_irq_frame *irq_frame = (struct arm_irq_frame *) sp;
126+
127+
uint32_t fault_stack = (uint32_t)sp + 0x20;
128+
// see ARMv7-M, Chapter B1.5.8 Exception return behavior, p. 652
129+
if (!(soft_frame->lr_irq & (1<<4))) {
130+
fault_stack += 0x68; // extended frame with FPU registers
131+
}
132+
if (irq_frame->psr & (1<<9)) {
133+
fault_stack += 0x04; // 8-byte auto alignment
134+
}
135+
136+
const char *fault = fault_name();
137+
fault_printf("%s at %08x, stack: %08x, called from %08x\n", fault,
138+
irq_frame->pc, fault_stack, irq_frame->lr);
139+
fault_printf("r0-r7: %08x %08x %08x %08x %08x %08x %08x %08x\n",
140+
irq_frame->r0, irq_frame->r1, irq_frame->r2, irq_frame->r3,
141+
soft_frame->r4, soft_frame->r5, soft_frame->r6, soft_frame->r7);
142+
fault_printf("r8-r12: %08x %08x %08x %08x %08x\n", soft_frame->r8,
143+
soft_frame->r9, soft_frame->r10, soft_frame->r11, irq_frame->r12);
144+
fault_printf("msp: %08x psp: %08x lr: %08x pc: %08x lr (IRQ): %08x\n",
145+
(uint32_t)msp, (uint32_t)psp, irq_frame->lr, irq_frame->pc,
146+
soft_frame->lr_irq);
147+
148+
// FPU state
149+
#if (__FPU_PRESENT && __FPU_USED && ARM_CORTEX_FAULT_FPU_DEBUG)
150+
fault_printf("FPSCR: %08x\n", irq_frame->fpscr);
151+
fault_printf("s0-s7: %08x %08x %08x %08x %08x %08x %08x %08x\n",
152+
irq_frame->s0, irq_frame->s1, irq_frame->s2, irq_frame->s3,
153+
irq_frame->s4, irq_frame->s5, irq_frame->s6, irq_frame->s7);
154+
fault_printf("s8-s15: %08x %08x %08x %08x %08x %08x %08x %08x\n",
155+
irq_frame->s8, irq_frame->s9, irq_frame->s10, irq_frame->s11,
156+
irq_frame->s12, irq_frame->s13, irq_frame->s14, irq_frame->s15);
157+
fault_printf("s16-s23: %08x %08x %08x %08x %08x %08x %08x %08x\n",
158+
soft_frame->s16, soft_frame->s17, soft_frame->s18, soft_frame->s19,
159+
soft_frame->s20, soft_frame->s21, soft_frame->s22, soft_frame->s23);
160+
fault_printf("s24-s31: %08x %08x %08x %08x %08x %08x %08x %08x\n",
161+
soft_frame->s24, soft_frame->s25, soft_frame->s26, soft_frame->s27,
162+
soft_frame->s28, soft_frame->s29, soft_frame->s30, soft_frame->s31);
163+
#endif
164+
165+
// detailed debug info
166+
#if (__CORTEX_M == 0x03 || __CORTEX_M == 0x04)
167+
uint16_t UFSR = (SCB->CFSR >> 16) & 0xffff;
168+
uint8_t BFSR = (SCB->CFSR >> 8) & 0xff;
169+
uint8_t MMFSR = (SCB->CFSR) & 0xff;
170+
171+
fault_printf("UFSR: %04x BFSR: %02x MMFSR: %02x\n", UFSR, BFSR, MMFSR);
172+
173+
if (UFSR & UFSR_UNALIGNED) {
174+
fault_printf("\"Unaligned memory access\"\n");
175+
}
176+
if (UFSR & UFSR_DIVBYZERO) {
177+
fault_printf("\"Division by zero\"\n");
178+
}
179+
if (MMFSR & MMFSR_IACCVIOL) {
180+
fault_printf("\"Instruction access violation\"\n");
181+
}
182+
if (MMFSR & MMFSR_DACCVIOL) {
183+
fault_printf("\"Data access violation\"\n");
184+
}
185+
if (MMFSR & MMFSR_MMARVALID) {
186+
fault_printf("address: 0x%08x\n", SCB->MMFAR);
187+
}
188+
if (BFSR & BFSR_BFARVALID) {
189+
fault_printf("address: 0x%08x\n", SCB->BFAR);
190+
}
191+
#endif
192+
193+
chSysHalt(fault);
194+
}
195+
196+
void fault_init(void)
197+
{
198+
#if (__CORTEX_M == 0x03 || __CORTEX_M == 0x04)
199+
chSysLock();
200+
// enable UsageFault, BusFault, MemManageFault
201+
SCB->SHCSR |= SCB_SHCSR_USGFAULTENA_Msk |
202+
SCB_SHCSR_BUSFAULTENA_Msk |
203+
SCB_SHCSR_MEMFAULTENA_Msk;
204+
// enable fault on division by zero
205+
SCB->CCR |= SCB_CCR_DIV_0_TRP_Msk;
206+
chSysUnlock();
207+
#endif
208+
}

‎fault.h

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
#ifndef FAULT_H
2+
#define FAULT_H
3+
4+
#ifdef __cplusplus
5+
extern "C" {
6+
#endif
7+
8+
void fault_init(void);
9+
10+
#ifdef __cplusplus
11+
}
12+
#endif
13+
14+
#endif /* FAULT_H */

‎fault_v6m.s

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
.syntax unified
2+
.cpu cortex-m0
3+
.fpu softvfp
4+
.thumb
5+
.text
6+
7+
.thumb_func
8+
.globl HardFault_Handler
9+
HardFault_Handler:
10+
mrs r1, msp
11+
mrs r2, psp
12+
13+
// save registers on stack
14+
push {r4-r7, lr}
15+
mov r4, r8
16+
mov r5, r9
17+
mov r6, r10
18+
mov r7, r11
19+
push {r4-r7}
20+
21+
mrs r0, msp
22+
b fault_handler
23+
// never return

‎fault_v7m.s

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
#define TRUE 1
2+
#define FALSE 0
3+
4+
.syntax unified
5+
.cpu cortex-m4
6+
7+
#if (CORTEX_USE_FPU == TRUE)
8+
.fpu fpv4-sp-d16
9+
#else
10+
.fpu softvfp
11+
#endif
12+
13+
.thumb
14+
.text
15+
16+
.thumb_func
17+
.globl HardFault_Handler
18+
HardFault_Handler:
19+
mrs r1, msp
20+
mrs r2, psp
21+
22+
// save registers on stack
23+
#if (CORTEX_USE_FPU == TRUE)
24+
vpush {s16-s31}
25+
#endif
26+
push {r4-r7, lr}
27+
push {r8-r11}
28+
29+
mrs r0, msp
30+
b fault_handler
31+
// never return
32+
33+
34+
.thumb_func
35+
.globl MemManage_Handler
36+
MemManage_Handler:
37+
b HardFault_Handler
38+
39+
.thumb_func
40+
.globl BusFault_Handler
41+
BusFault_Handler:
42+
b HardFault_Handler
43+
44+
.thumb_func
45+
.globl UsageFault_Handler
46+
UsageFault_Handler:
47+
b HardFault_Handler

0 commit comments

Comments
 (0)