Xtensa: write memory

probe-rs/examples/xtensa.rs

@@ -1,3 +1,5 @@
+//! This example demonstrates how to use the implemented parts of the Xtensa interface.
+
 use anyhow::Result;
 use probe_rs::config::ScanChainElement;
 use probe_rs::{Lister, MemoryInterface, Probe};
@@ -15,7 +17,8 @@ fn main() -> Result<()> {
 
     probe.set_speed(100)?;
     probe.select_protocol(probe_rs::WireProtocol::Jtag)?;
-    // scan chain for an esp32s3
+
+    // Scan the chain for an esp32s3.
     probe.set_scan_chain(vec![
         ScanChainElement {
             ir_len: Some(5),
@@ -35,13 +38,34 @@ fn main() -> Result<()> {
 
     iface.halt()?;
 
-    const SYSTEM_BASE_REGISTER: u32 = 0x600C_0000;
-    const SYSTEM_DATE_REGISTER: u32 = SYSTEM_BASE_REGISTER | 0x0FFC;
-    let date = iface.read_word_32(SYSTEM_DATE_REGISTER as u64)?;
+    const TEST_MEMORY_REGION_START: u64 = 0x600F_E000;
+    const TEST_MEMORY_LEN: usize = 100;
 
-    iface.leave_ocd_mode()?;
+    let mut saved_memory = vec![0; TEST_MEMORY_LEN];
+    iface.read(TEST_MEMORY_REGION_START, &mut saved_memory[..])?;
+
+    // Zero the memory
+    iface.write(TEST_MEMORY_REGION_START, &[0; TEST_MEMORY_LEN])?;
+
+    // Write a test word into memory, unaligned
+    iface.write_word_32(TEST_MEMORY_REGION_START + 1, 0xDECAFBAD)?;
+    let coffee_opinion = iface.read_word_32(TEST_MEMORY_REGION_START + 1)?;
 
-    println!("SYSTEM peripheral date: {:08x}", date);
+    // Write a test word into memory, aligned
+    iface.write_word_32(TEST_MEMORY_REGION_START + 8, 0xFEEDC0DE)?;
+    let aligned_word = iface.read_word_32(TEST_MEMORY_REGION_START + 8)?;
+
+    let mut readback = [0; 12];
+    iface.read(TEST_MEMORY_REGION_START, &mut readback[..])?;
+
+    tracing::info!("coffee_opinion: {:08X}", coffee_opinion);
+    tracing::info!("aligned_word: {:08X}", aligned_word);
+    tracing::info!("readback: {:X?}", readback);
+
+    // Restore memory we just overwrote
+    iface.write(TEST_MEMORY_REGION_START, &saved_memory[..])?;
+
+    iface.leave_ocd_mode()?;
 
     Ok(())
 }

probe-rs/src/architecture/xtensa/arch/instruction/format.rs

@@ -1,3 +1,17 @@
+//! Instruction formats implemented as functions.
+//!
+//! Instruction formats are common bytecode formats used to simplify instruction implementation.
+//! They are implemented with an opcode and a set of more-or-less standardised slots where
+//! instructions may define their operands.
+//!
+//! For more information, see the Xtensa ISA documentation.
+
+/// Implements the RSR instruction format.
 pub const fn rsr(opcode: u32, rs: u8, t: u8) -> u32 {
     opcode | (rs as u32) << 8 | (t as u32 & 0x0F) << 4
 }
+
+/// Implements the RRI8 instruction format.
+pub const fn rri8(opcode: u32, at: u8, _as: u8, off: u8) -> u32 {
+    opcode | ((off as u32) << 16) | (_as as u32 & 0x0F) << 8 | (at as u32 & 0x0F) << 4
+}

probe-rs/src/architecture/xtensa/arch/instruction/mod.rs

@@ -8,12 +8,31 @@ pub enum Instruction {
     /// Note: this is an illegal instruction when the processor is not in On-Chip Debug Mode
     Lddr32P(CpuRegister),
 
+    /// Stores a 32-bit word from `DDR` to the address in `src`
+    /// Note: this is an illegal instruction when the processor is not in On-Chip Debug Mode
+    Sddr32P(CpuRegister),
+
+    /// Stores 8 bits from `at` to the address in `as` offset by a constant.
+    ///
+    /// This instruction can not access InstrRAM.
+    S8i(CpuRegister, CpuRegister, u8),
+
     /// Reads `SpecialRegister` into `CpuRegister`
     Rsr(SpecialRegister, CpuRegister),
 
     /// Writes `CpuRegister` into `SpecialRegister`
     Wsr(SpecialRegister, CpuRegister),
 
+    /// Invalidates the I-Cache at the address in `CpuRegister` + offset.
+    ///
+    /// The offset will be divided by 4 and has a maximum value of 1020.
+    Ihi(CpuRegister, u32),
+
+    /// Writes back and Invalidates the D-Cache at the address in `CpuRegister` + offset.
+    ///
+    /// The offset will be divided by 4 and has a maximum value of 1020.
+    Dhwbi(CpuRegister, u32),
+
     /// Returns the Core to the Running state
     Rfdo(u8),
 }
@@ -29,8 +48,18 @@ impl Instruction {
     pub fn encode(self) -> InstructionEncoding {
         let narrow = match self {
             Instruction::Lddr32P(src) => 0x0070E0 | (src.address() as u32 & 0x0F) << 8,
+            Instruction::Sddr32P(src) => 0x0070F0 | (src.address() as u32 & 0x0F) << 8,
             Instruction::Rsr(sr, t) => format::rsr(0x030000, sr.address(), t.address()),
             Instruction::Wsr(sr, t) => format::rsr(0x130000, sr.address(), t.address()),
+            Instruction::S8i(at, as_, offset) => {
+                format::rri8(0x004002, at.address(), as_.address(), offset)
+            }
+            Instruction::Ihi(src, offset) => {
+                format::rri8(0x0070E2, 0, src.address(), (offset / 4) as u8)
+            }
+            Instruction::Dhwbi(src, offset) => {
+                format::rri8(0x007052, 0, src.address(), (offset / 4) as u8)
+            }
             Instruction::Rfdo(_) => 0xF1E000,
         };
 

probe-rs/src/architecture/xtensa/arch/mod.rs

@@ -1,5 +1,7 @@
 #![allow(unused)] // TODO remove
 
+use std::ops::Range;
+
 pub mod instruction;
 
 #[derive(Clone, Copy, PartialEq, Eq, Debug, Hash)]

probe-rs/src/architecture/xtensa/communication_interface.rs

@@ -2,7 +2,6 @@
 
 // TODO: remove
 #![allow(missing_docs)]
-#![allow(unused_variables)]
 
 use std::collections::HashMap;
 
@@ -198,6 +197,14 @@ impl XtensaCommunicationInterface {
         status
     }
 
+    fn write_ddr_and_execute(&mut self, value: u32) -> Result<(), XtensaError> {
+        let status = self.xdm.write_ddr_and_execute(value);
+        if let Err(XtensaError::XdmError(err)) = status {
+            self.debug_execution_error(err)?
+        }
+        status
+    }
+
     fn is_register_saved(&mut self, register: Register) -> bool {
         if register == Register::Special(SpecialRegister::Ddr) {
             // Avoid saving DDR
@@ -265,6 +272,44 @@ impl XtensaCommunicationInterface {
 
         Ok(())
     }
+
+    fn write_memory_unaligned8(&mut self, address: u32, data: &[u8]) -> Result<(), crate::Error> {
+        if data.is_empty() {
+            return Ok(());
+        }
+
+        let offset = address as usize % 4;
+        let aligned_address = address & !0x3;
+
+        // Read the aligned word
+        let mut word = [0; 4];
+        self.read(aligned_address as u64, &mut word)?;
+
+        // Replace the written bytes. This will also panic if the input is crossing a word boundary
+        word[offset..][..data.len()].copy_from_slice(data);
+
+        // Write the word back
+        self.write_cpu_register(CpuRegister::A3, aligned_address)?;
+        self.xdm.write_ddr(u32::from_le_bytes(word))?;
+        self.execute_instruction(Instruction::Sddr32P(CpuRegister::A3))?;
+
+        Ok(())
+    }
+}
+
+unsafe trait DataType: Sized {}
+unsafe impl DataType for u8 {}
+unsafe impl DataType for u32 {}
+unsafe impl DataType for u64 {}
+
+fn as_bytes<T: DataType>(data: &[T]) -> &[u8] {
+    unsafe { std::slice::from_raw_parts(data.as_ptr() as *mut u8, std::mem::size_of_val(data)) }
+}
+
+fn as_bytes_mut<T: DataType>(data: &mut [T]) -> &mut [u8] {
+    unsafe {
+        std::slice::from_raw_parts_mut(data.as_mut_ptr() as *mut u8, std::mem::size_of_val(data))
+    }
 }
 
 impl MemoryInterface for XtensaCommunicationInterface {
@@ -281,15 +326,17 @@ impl MemoryInterface for XtensaCommunicationInterface {
 
         // Let's assume we can just do 32b reads, so let's do some pre-massaging on unaligned reads
         if address % 4 != 0 {
-            let word = if dst.len() <= 4 {
+            let offset = address as usize % 4;
+
+            // Avoid executing another read if we only have to read a single word
+            let word = if offset + dst.len() <= 4 {
                 self.xdm.read_ddr()?
             } else {
                 self.read_ddr_and_execute()?
             };
 
             let word = word.to_le_bytes();
 
-            let offset = address as usize % 4;
             let bytes_to_copy = (4 - offset).min(dst.len());
 
             dst[..bytes_to_copy].copy_from_slice(&word[offset..][..bytes_to_copy]);
@@ -339,58 +386,95 @@ impl MemoryInterface for XtensaCommunicationInterface {
     }
 
     fn read_64(&mut self, address: u64, data: &mut [u64]) -> anyhow::Result<(), crate::Error> {
-        let data_8 = unsafe {
-            std::slice::from_raw_parts_mut(
-                data.as_mut_ptr() as *mut u8,
-                std::mem::size_of_val(data),
-            )
-        };
-        self.read_8(address, data_8)
+        self.read_8(address, as_bytes_mut(data))
     }
 
     fn read_32(&mut self, address: u64, data: &mut [u32]) -> anyhow::Result<(), crate::Error> {
-        let data_8 = unsafe {
-            std::slice::from_raw_parts_mut(
-                data.as_mut_ptr() as *mut u8,
-                std::mem::size_of_val(data),
-            )
-        };
-        self.read_8(address, data_8)
+        self.read_8(address, as_bytes_mut(data))
     }
 
     fn read_8(&mut self, address: u64, data: &mut [u8]) -> anyhow::Result<(), crate::Error> {
         self.read(address, data)
     }
 
+    fn write(&mut self, address: u64, data: &[u8]) -> Result<(), crate::Error> {
+        if data.is_empty() {
+            return Ok(());
+        }
+
+        let address = address as u32;
+
+        let mut addr = address;
+        let mut buffer = data;
+
+        // We store the unaligned head of the data separately
+        if addr % 4 != 0 {
+            let unaligned_bytes = (4 - (addr % 4) as usize).min(buffer.len());
+
+            self.write_memory_unaligned8(addr, &buffer[..unaligned_bytes])?;
+
+            buffer = &buffer[unaligned_bytes..];
+            addr += unaligned_bytes as u32;
+        }
+
+        if buffer.len() > 4 {
+            // Prepare store instruction
+            self.write_cpu_register(CpuRegister::A3, addr as u32)?;
+            self.xdm
+                .write_instruction(Instruction::Sddr32P(CpuRegister::A3))?;
+
+            while buffer.len() > 4 {
+                let mut word = [0; 4];
+                word[..].copy_from_slice(&buffer[..4]);
+                let word = u32::from_le_bytes(word);
+
+                // Write data to DDR and store
+                self.write_ddr_and_execute(word)?;
+
+                buffer = &buffer[4..];
+                addr += 4;
+            }
+        }
+
+        // We store the narrow tail of the data separately
+        if !buffer.is_empty() {
+            self.write_memory_unaligned8(addr, buffer)?;
+        }
+
+        // TODO: implement cache flushing on CPUs that need it.
+
+        Ok(())
+    }
+
     fn write_word_64(&mut self, address: u64, data: u64) -> anyhow::Result<(), crate::Error> {
-        todo!()
+        self.write(address, &data.to_le_bytes())
     }
 
     fn write_word_32(&mut self, address: u64, data: u32) -> anyhow::Result<(), crate::Error> {
-        todo!()
+        self.write(address, &data.to_le_bytes())
     }
 
     fn write_word_8(&mut self, address: u64, data: u8) -> anyhow::Result<(), crate::Error> {
-        todo!()
+        self.write(address, &[data])
     }
 
     fn write_64(&mut self, address: u64, data: &[u64]) -> anyhow::Result<(), crate::Error> {
-        todo!()
+        self.write_8(address, as_bytes(data))
     }
 
     fn write_32(&mut self, address: u64, data: &[u32]) -> anyhow::Result<(), crate::Error> {
-        todo!()
+        self.write_8(address, as_bytes(data))
     }
 
     fn write_8(&mut self, address: u64, data: &[u8]) -> anyhow::Result<(), crate::Error> {
-        todo!()
+        self.write(address, data)
     }
 
     fn supports_8bit_transfers(&self) -> anyhow::Result<bool, crate::Error> {
-        todo!()
+        Ok(true)
     }
 
     fn flush(&mut self) -> anyhow::Result<(), crate::Error> {
-        todo!()
+        Ok(())
     }
 }

probe-rs/src/architecture/xtensa/mod.rs

@@ -9,7 +9,7 @@ mod xdm;
 
 pub mod communication_interface;
 
-/// A interface to operate Xtensa cores.
+/// An interface to operate Xtensa cores.
 pub struct Xtensa<'probe> {
     interface: &'probe mut XtensaCommunicationInterface,
 }