1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151

//! Capture functions for the cortex-m platform

use stackdump_core::register_data::RegisterData;
use stackdump_core::{memory_region::ArrayMemoryRegion, register_data::ArrayRegisterData};

/// Capture the core registers and the stack
#[cfg(not(has_fpu))]
pub fn capture<const SIZE: usize>(
    stack: &mut ArrayMemoryRegion<SIZE>,
    core_registers: &mut ArrayRegisterData<16, u32>,
    _cs: &bare_metal::CriticalSection,
) {
    capture_core_registers(core_registers);
    capture_stack(
        core_registers
            .register(stackdump_core::gimli::Arm::SP)
            .unwrap(),
        stack,
    );
}

/// Capture the core & fpu registers and the stack
#[cfg(has_fpu)]
pub fn capture<const SIZE: usize>(
    stack: &mut ArrayMemoryRegion<SIZE>,
    core_registers: &mut ArrayRegisterData<16, u32>,
    fpu_registers: &mut ArrayRegisterData<32, u32>,
    _cs: &bare_metal::CriticalSection,
) {
    capture_core_registers(core_registers);
    capture_fpu_registers(fpu_registers);
    capture_stack(
        core_registers
            .register(stackdump_core::gimli::Arm::SP)
            .unwrap(),
        stack,
    );
}

fn capture_core_registers(buffer: &mut ArrayRegisterData<16, u32>) {
    #[cfg(cortex_m)]
    use core::arch::asm;

    // This array is going to hold the register data
    let mut register_array = arrayvec::ArrayVec::new();

    unsafe {
        // We've got 16 registers, so make space for that
        register_array.set_len(16);

        #[cfg(cortex_m)]
        asm!(
            "str r0, [{0}, #0]",
            "str r1, [{0}, #4]",
            "str r2, [{0}, #8]",
            "str r3, [{0}, #12]",
            "str r4, [{0}, #16]",
            "str r5, [{0}, #20]",
            "str r6, [{0}, #24]",
            "str r7, [{0}, #28]",
            "str r8, [{0}, #32]",
            "str r9, [{0}, #36]",
            "str r10, [{0}, #40]",
            "str r11, [{0}, #44]",
            "str r12, [{0}, #48]",
            "str sp, [{0}, #52]",
            "str lr, [{0}, #56]",
            "mov {tmp}, pc", // We can't use the str instruction with the PC register directly, so store it in tmp
            "str {tmp}, [{0}, #60]",
            in(reg) register_array.as_mut_ptr(), // Every register is going to be written to an offset of this pointer
            tmp = out(reg) _, // We need a temporary register
        );
    }

    *buffer = ArrayRegisterData::new(stackdump_core::gimli::Arm::R0, register_array);
}

#[cfg(has_fpu)]
fn capture_fpu_registers(buffer: &mut ArrayRegisterData<32, u32>) {
    #[cfg(cortex_m)]
    use core::arch::asm;

    // This array is going to hold the register data
    let mut register_array = arrayvec::ArrayVec::new();

    unsafe {
        // We've got 32 registers, so make space for that
        register_array.set_len(32);

        #[cfg(cortex_m)]
        asm!(
            "vstr s0, [{0}, #0]",
            "vstr s1, [{0}, #4]",
            "vstr s2, [{0}, #8]",
            "vstr s3, [{0}, #12]",
            "vstr s4, [{0}, #16]",
            "vstr s5, [{0}, #20]",
            "vstr s6, [{0}, #24]",
            "vstr s7, [{0}, #28]",
            "vstr s8, [{0}, #32]",
            "vstr s9, [{0}, #36]",
            "vstr s10, [{0}, #40]",
            "vstr s11, [{0}, #44]",
            "vstr s12, [{0}, #48]",
            "vstr s13, [{0}, #52]",
            "vstr s14, [{0}, #56]",
            "vstr s15, [{0}, #60]",
            "vstr s16, [{0}, #64]",
            "vstr s17, [{0}, #68]",
            "vstr s18, [{0}, #72]",
            "vstr s19, [{0}, #76]",
            "vstr s20, [{0}, #80]",
            "vstr s21, [{0}, #84]",
            "vstr s22, [{0}, #88]",
            "vstr s23, [{0}, #92]",
            "vstr s24, [{0}, #96]",
            "vstr s25, [{0}, #100]",
            "vstr s26, [{0}, #104]",
            "vstr s27, [{0}, #108]",
            "vstr s28, [{0}, #112]",
            "vstr s29, [{0}, #116]",
            "vstr s30, [{0}, #120]",
            "vstr s31, [{0}, #124]",
            in(reg) register_array.as_mut_ptr(), // Every register is going to be written to an offset of this pointer
        );
    }

    *buffer = ArrayRegisterData::new(stackdump_core::gimli::Arm::S0, register_array)
}

/// Capture the stack from the current given stack pointer until the start of the stack into the given stack memory region.
/// The captured stack will be the smallest of the sizes of the current stack size or the memory region size.
///
/// If the memory region is too small, it will contain the top stack space and miss the bottom stack space.
/// This is done because the top of the stack is often more interesting than the bottom.
fn capture_stack<const SIZE: usize>(stack_pointer: u32, stack: &mut ArrayMemoryRegion<SIZE>) {
    extern "C" {
        static mut _stack_start: core::ffi::c_void;
    }

    /// Get the start address of the stack. The stack grows to lower addresses,
    /// so this should be the highest stack address you can get.
    fn stack_start() -> u32 {
        unsafe { &_stack_start as *const _ as u32 }
    }

    let stack_size = stack_start().saturating_sub(stack_pointer).min(SIZE as u32);
    unsafe {
        stack.copy_from_memory(stack_pointer as *const u8, stack_size as usize);
    }
}