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

#![cfg_attr(feature = "std", allow(unreachable_code, unused_variables))]

use core::{
    fmt::{self, Write},
    slice,
};

const ADDRESS_BASE: usize = 0xE000_0000;

/// ITM stimulus port handler.
#[derive(Clone, Copy)]
pub struct Port {
    address: usize,
}

pub trait Integer: Copy {
    fn write(self, address: usize);
}

impl Port {
    /// Creates a new ITM stimulus port handler.
    ///
    /// # Panics
    ///
    /// If `port` is out of bounds.
    pub fn new(address: usize) -> Self {
        assert!(address < 0x20);
        Self {
            address: ADDRESS_BASE + (address << 2),
        }
    }

    /// Writes `bytes` to the stimulus port.
    pub fn write_bytes(self, bytes: &[u8]) {
        fn write_slice<T: Integer>(port: Port, slice: &[T]) {
            for item in slice {
                port.write(*item);
            }
        }
        let mut end = bytes.len();
        if end < 4 {
            return write_slice(self, bytes);
        }
        let mut start = bytes.as_ptr() as usize;
        let mut rem = start & 0b11;
        end += start;
        if rem != 0 {
            rem = 0b100 - rem;
            write_slice(self, unsafe {
                slice::from_raw_parts(start as *const u8, rem)
            });
            start += rem;
        }
        rem = end & 0b11;
        end -= rem;
        write_slice(self, unsafe {
            slice::from_raw_parts(start as *const u32, end - start >> 2)
        });
        write_slice(self, unsafe {
            slice::from_raw_parts(end as *const u8, rem)
        });
    }

    /// Writes `value` of type `u8`, `u16` or `u32` to the stimulus port.
    ///
    /// This method could be chained.
    pub fn write<T: Integer>(self, value: T) -> Self {
        value.write(self.address);
        self
    }
}

impl Write for Port {
    fn write_str(&mut self, string: &str) -> fmt::Result {
        self.write_bytes(string.as_bytes());
        Ok(())
    }
}

impl Integer for u8 {
    fn write(self, address: usize) {
        #[cfg(feature = "std")]
        return unimplemented!();
        unsafe {
            asm!("
            0:
                ldrexb r0, [$1]
                cmp r0, #0
                itt ne
                strexbne r0, $0, [$1]
                cmpne r0, #1
                beq 0b
            "   :
                : "r"(self), "r"(address as *mut Self)
                : "r0", "cc"
                : "volatile"
            );
        }
    }
}

impl Integer for u16 {
    fn write(self, address: usize) {
        #[cfg(feature = "std")]
        return unimplemented!();
        unsafe {
            asm!("
            0:
                ldrexh r0, [$1]
                cmp r0, #0
                itt ne
                strexhne r0, $0, [$1]
                cmpne r0, #1
                beq 0b
            "   :
                : "r"(self), "r"(address as *mut Self)
                : "r0", "cc"
                : "volatile"
            );
        }
    }
}

impl Integer for u32 {
    fn write(self, address: usize) {
        #[cfg(feature = "std")]
        return unimplemented!();
        unsafe {
            asm!("
            0:
                ldrex r0, [$1]
                cmp r0, #0
                itt ne
                strexne r0, $0, [$1]
                cmpne r0, #1
                beq 0b
            "   :
                : "r"(self), "r"(address as *mut Self)
                : "r0", "cc"
                : "volatile"
            );
        }
    }
}