xtensa_atomic_emulation_trap/
lib.rs

1#![doc = include_str!("../README.md")]
2#![no_std]
3
4pub const PLATFORM_REGISTER_LEN: usize = 16;
5
6const SCOMPARE1_SR: u32 = 12;
7
8const WSR_INSTRUCTION: u32 = 0b00010011_000000000000_0000;
9const WSR_INSTRUCTION_MASK: u32 = 0b11111111_000000000000_1111;
10
11const S32C1I_INSTRUCTION: u32 = 0b1110_00000000_0010;
12const S32C1I_INSTRUCTION_MASK: u32 = 0b1111_00000000_1111;
13
14static mut SCOMPARE1: u32 = 0;
15
16#[inline(always)]
17#[link_section = ".rwtext"]
18pub unsafe fn atomic_emulation(pc: u32, save_frame: &mut [u32; PLATFORM_REGISTER_LEN]) -> bool {
19    // deref the addr to find the instruction we trapped on.
20    // if the PC address isn't word aligned, we need to read two words and capture the relevant instruction
21    let insn = if pc % 4 != 0 {
22        let prev_aligned = pc & !0x3;
23        let offset = (pc - prev_aligned) as usize;
24
25        let buffer = (*((prev_aligned + 4) as *const u32) as u64) << 32
26            | (*(prev_aligned as *const u32) as u64); // read two words
27        let buffer_bytes = buffer.to_le_bytes();
28
29        u32::from_le_bytes([
30            buffer_bytes[offset],
31            buffer_bytes[offset + 1],
32            buffer_bytes[offset + 2],
33            0,
34        ])
35    } else {
36        *(pc as *const u32)
37    };
38
39    // first check, is it a WSR instruction? RRR Format
40    if (insn & WSR_INSTRUCTION_MASK) == WSR_INSTRUCTION {
41        let target = (insn >> 4) & 0b1111;
42        let sr = (insn >> 8) & 0b11111111;
43        // is the dest register SCOMPARE1?
44        if sr == SCOMPARE1_SR {
45            // save value in our virtual register
46            SCOMPARE1 = save_frame[target as usize];
47            return true;
48        }
49    }
50
51    // next check, is it the S32C1I instruction? RRI8 Format
52    if (insn & S32C1I_INSTRUCTION_MASK) == S32C1I_INSTRUCTION {
53        // decode the instruction
54        let reg_mask = 0b1111;
55        let target = (insn >> 4) & reg_mask;
56        let source = (insn >> 8) & reg_mask;
57        let offset = (insn >> 16) & 0b11111111;
58
59        // get target value and source value (memory address)
60        let target_value = save_frame[target as usize];
61        let source_value = save_frame[source as usize];
62
63        // get the value from memory
64        let source_address = source_value + ((offset as u32) << 2);
65        let memory_value = *(source_address as *const u32);
66
67        if memory_value == SCOMPARE1 {
68            // update the value in memory
69            *(source_address as *mut u32) = target_value;
70        }
71
72        save_frame[target as usize] = memory_value;
73
74        return true;
75    }
76
77    false
78}
xtensa_atomic_emulation_trap/lib.rs

xtensa_atomic_emulation_trap/
lib.rs
