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

//! Instrumentation Trace Macrocell
//!
//! **NOTE** This module is only available on ARMv7-M and newer

use core::{fmt, mem, ptr, slice};

use aligned::Aligned;

use peripheral::itm::Stim;

// NOTE assumes that `bytes` is 32-bit aligned
unsafe fn write_words(stim: &mut Stim, bytes: &[u32]) {
    let mut p = bytes.as_ptr();
    for _ in 0..bytes.len() {
        while !stim.is_fifo_ready() {}
        stim.write_u32(ptr::read(p));
        p = p.offset(1);
    }
}

struct Port<'p>(&'p mut Stim);

impl<'p> fmt::Write for Port<'p> {
    fn write_str(&mut self, s: &str) -> fmt::Result {
        write_all(self.0, s.as_bytes());
        Ok(())
    }
}

/// Writes a `buffer` to the ITM `port`
pub fn write_all(port: &mut Stim, buffer: &[u8]) {
    unsafe {
        let mut len = buffer.len();
        let mut ptr = buffer.as_ptr();

        if len == 0 {
            return;
        }

        // 0x01 OR 0x03
        if ptr as usize % 2 == 1 {
            while !port.is_fifo_ready() {}
            port.write_u8(*ptr);

            // 0x02 OR 0x04
            ptr = ptr.offset(1);
            len -= 1;
        }

        // 0x02
        if ptr as usize % 4 == 2 {
            if len > 1 {
                // at least 2 bytes
                while !port.is_fifo_ready() {}
                port.write_u16(ptr::read(ptr as *const u16));

                // 0x04
                ptr = ptr.offset(2);
                len -= 2;
            } else {
                if len == 1 {
                    // last byte
                    while !port.is_fifo_ready() {}
                    port.write_u8(*ptr);
                }

                return;
            }
        }

        write_aligned(port, mem::transmute(slice::from_raw_parts(ptr, len)));
    }
}

/// Writes a 4-byte aligned `buffer` to the ITM `port`
///
/// # Examples
///
/// ``` ignore
/// let mut buffer: Aligned<u32, _> = Aligned([0; 14]);
///
/// buffer.copy_from_slice(b"Hello, world!\n");
///
/// itm::write_aligned(&itm.stim[0], &buffer);
///
/// // Or equivalently
/// itm::write_aligned(&itm.stim[0], &Aligned(*b"Hello, world!\n"));
/// ```
pub fn write_aligned(port: &mut Stim, buffer: &Aligned<u32, [u8]>) {
    unsafe {
        let len = buffer.len();

        if len == 0 {
            return;
        }

        let split = len & !0b11;
        write_words(
            port,
            slice::from_raw_parts(buffer.as_ptr() as *const u32, split >> 2),
        );

        // 3 bytes or less left
        let mut left = len & 0b11;
        let mut ptr = buffer.as_ptr().offset(split as isize);

        // at least 2 bytes left
        if left > 1 {
            while !port.is_fifo_ready() {}
            port.write_u16(ptr::read(ptr as *const u16));

            ptr = ptr.offset(2);
            left -= 2;
        }

        // final byte
        if left == 1 {
            while !port.is_fifo_ready() {}
            port.write_u8(*ptr);
        }
    }
}

/// Writes `fmt::Arguments` to the ITM `port`
pub fn write_fmt(port: &mut Stim, args: fmt::Arguments) {
    use core::fmt::Write;

    Port(port).write_fmt(args).ok();
}

/// Writes a string to the ITM `port`
pub fn write_str(port: &mut Stim, string: &str) {
    write_all(port, string.as_bytes())
}