sftool_lib/
sifli_debug.rs

1use crate::SifliTool;
2use probe_rs::architecture::arm::armv8m::Dcrdr;
3use probe_rs::{MemoryMappedRegister, RegisterId, memory_mapped_bitfield_register};
4use std::cmp::{max, min};
5use std::fmt;
6use std::io::{BufReader, BufWriter, Read, Write};
7use std::time::{Duration, Instant};
8use probe_rs::architecture::arm::ArmError;
9
10const START_WORD: [u8; 2] = [0x7E, 0x79];
11const DEFUALT_RECV_TIMEOUT: Duration = Duration::from_secs(3);
12const DEFUALT_UART_BAUD: u32 = 1000000;
13
14#[derive(Debug)]
15pub(crate) enum SifliUartCommand<'a> {
16    Enter,
17    Exit,
18    MEMRead { addr: u32, len: u16 },
19    MEMWrite { addr: u32, data: &'a [u32] },
20}
21
22#[derive(Debug)]
23pub(crate) enum SifliUartResponse {
24    Enter,
25    Exit,
26    MEMRead { data: Vec<u8> },
27    MEMWrite,
28}
29
30impl fmt::Display for SifliUartCommand<'_> {
31    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
32        match self {
33            SifliUartCommand::Enter => write!(f, "Enter"),
34            SifliUartCommand::Exit => write!(f, "Exit"),
35            SifliUartCommand::MEMRead { addr, len } => {
36                write!(f, "MEMRead {{ addr: {:#X}, len: {:#X} }}", addr, len)
37            }
38            SifliUartCommand::MEMWrite { addr, data } => {
39                write!(f, "MEMWrite {{ addr: {:#X}, data: [", addr)?;
40                for (i, d) in data.iter().enumerate() {
41                    if i > 0 {
42                        write!(f, ", ")?;
43                    }
44                    write!(f, "{:#X}", d)?;
45                }
46                write!(f, "] }}")
47            }
48        }
49    }
50}
51
52impl fmt::Display for SifliUartResponse {
53    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
54        match self {
55            SifliUartResponse::Enter => write!(f, "Enter"),
56            SifliUartResponse::Exit => write!(f, "Exit"),
57            SifliUartResponse::MEMRead { data } => {
58                write!(f, "MEMRead {{ data: [")?;
59                for (i, byte) in data.iter().enumerate() {
60                    if i > 0 {
61                        write!(f, ", ")?;
62                    }
63                    write!(f, "{:#04X}", byte)?;
64                }
65                write!(f, "] }}")
66            }
67            SifliUartResponse::MEMWrite => write!(f, "MEMWrite"),
68        }
69    }
70}
71
72memory_mapped_bitfield_register! {
73    pub struct Dcrsr(u32);
74    0xE000_EDF4, "DCRSR",
75    impl From;
76    pub _, set_regwnr: 16;
77    // If the processor does not implement the FP extension the REGSEL field is bits `[4:0]`, and bits `[6:5]` are Reserved, SBZ.
78    pub _, set_regsel: 6,0;
79}
80
81memory_mapped_bitfield_register! {
82    pub struct Dhcsr(u32);
83    0xE000_EDF0, "DHCSR",
84    impl From;
85    pub s_reset_st, _: 25;
86    pub s_retire_st, _: 24;
87    pub s_lockup, _: 19;
88    pub s_sleep, _: 18;
89    pub s_halt, _: 17;
90    pub s_regrdy, _: 16;
91    pub c_maskints, set_c_maskints: 3;
92    pub c_step, set_c_step: 2;
93    pub c_halt, set_c_halt: 1;
94    pub c_debugen, set_c_debugen: 0;
95}
96
97impl Dhcsr {
98    /// This function sets the bit to enable writes to this register.
99    ///
100    /// C1.6.3 Debug Halting Control and Status Register, DHCSR:
101    /// Debug key:
102    /// Software must write 0xA05F to this field to enable write accesses to bits
103    /// `[15:0]`, otherwise the processor ignores the write access.
104    pub fn enable_write(&mut self) {
105        self.0 &= !(0xffff << 16);
106        self.0 |= 0xa05f << 16;
107    }
108}
109
110impl SifliTool {
111    fn create_header(len: u16) -> Vec<u8> {
112        let mut header = vec![];
113        header.extend_from_slice(&START_WORD);
114        header.extend_from_slice(&len.to_le_bytes());
115        header.push(0x10);
116        header.push(0x00);
117        header
118    }
119
120    fn send(
121        writer: &mut BufWriter<Box<dyn Write + Send>>,
122        command: &SifliUartCommand,
123    ) -> Result<(), std::io::Error> {
124        let mut send_data = vec![];
125        match command {
126            SifliUartCommand::Enter => {
127                let temp = [0x41, 0x54, 0x53, 0x46, 0x33, 0x32, 0x05, 0x21];
128                send_data.extend_from_slice(&temp);
129            }
130            SifliUartCommand::Exit => {
131                let temp = [0x41, 0x54, 0x53, 0x46, 0x33, 0x32, 0x18, 0x21];
132                send_data.extend_from_slice(&temp);
133            }
134            SifliUartCommand::MEMRead { addr, len } => {
135                send_data.push(0x40);
136                send_data.push(0x72);
137                send_data.extend_from_slice(&addr.to_le_bytes());
138                send_data.extend_from_slice(&len.to_le_bytes());
139            }
140            SifliUartCommand::MEMWrite { addr, data } => {
141                send_data.push(0x40);
142                send_data.push(0x77);
143                send_data.extend_from_slice(&addr.to_le_bytes());
144                send_data.extend_from_slice(&(data.len() as u16).to_le_bytes());
145                for d in data.iter() {
146                    send_data.extend_from_slice(&d.to_le_bytes());
147                }
148            }
149        }
150
151        let header = Self::create_header(send_data.len() as u16);
152        writer.write_all(&header)?;
153        writer.write_all(&send_data)?;
154        writer.flush()?;
155
156        Ok(())
157    }
158
159    fn recv(
160        reader: &mut BufReader<Box<dyn Read + Send>>,
161    ) -> Result<SifliUartResponse, std::io::Error> {
162        let start_time = Instant::now();
163        let mut buffer = vec![];
164        let mut recv_data = vec![];
165
166        loop {
167            if start_time.elapsed() >= DEFUALT_RECV_TIMEOUT {
168                return Err(std::io::Error::new(std::io::ErrorKind::TimedOut, "Timeout"));
169            }
170
171            let mut byte = [0; 1];
172            if reader.read_exact(&mut byte).is_err() {
173                continue;
174            }
175
176            if (byte[0] == START_WORD[0]) || (buffer.len() == 1 && byte[0] == START_WORD[1]) {
177                buffer.push(byte[0]);
178            } else {
179                buffer.clear();
180            }
181            tracing::info!("Recv buffer: {:?}", buffer);
182
183            if buffer.ends_with(&START_WORD) {
184                let err = Err(std::io::Error::new(
185                    std::io::ErrorKind::InvalidData,
186                    "Invalid frame start",
187                ));
188                recv_data.clear();
189                // Header Length
190                let mut temp = [0; 2];
191                if reader.read_exact(&mut temp).is_err() {
192                    return err;
193                }
194                let size = u16::from_le_bytes(temp);
195                tracing::info!("Recv size: {}", size);
196
197                // Header channel and crc
198                if reader.read_exact(&mut temp).is_err() {
199                    return err;
200                }
201
202                while recv_data.len() < size as usize {
203                    if reader.read_exact(&mut byte).is_err() {
204                        return err;
205                    }
206                    recv_data.push(byte[0]);
207                    tracing::info!("Recv data: {:?}", recv_data);
208                }
209                break;
210            } else if buffer.len() == 2 {
211                buffer.clear();
212            }
213        }
214
215        if recv_data[recv_data.len() - 1] != 0x06 {
216            return Err(std::io::Error::new(
217                std::io::ErrorKind::InvalidData,
218                "Invalid frame end",
219            ));
220        }
221
222        match recv_data[0] {
223            0xD1 => Ok(SifliUartResponse::Enter),
224            0xD0 => Ok(SifliUartResponse::Exit),
225            0xD2 => {
226                let data = recv_data[1..recv_data.len() - 1].to_vec();
227                Ok(SifliUartResponse::MEMRead { data })
228            }
229            0xD3 => Ok(SifliUartResponse::MEMWrite),
230            _ => Err(std::io::Error::new(
231                std::io::ErrorKind::InvalidData,
232                "Invalid response",
233            )),
234        }
235    }
236
237    pub(crate) fn debug_command(
238        &mut self,
239        command: SifliUartCommand,
240    ) -> Result<SifliUartResponse, std::io::Error> {
241        tracing::info!("Command: {}", command);
242        let writer: Box<dyn Write + Send> = self.port.try_clone()?;
243        let mut buf_writer = BufWriter::new(writer);
244
245        let reader: Box<dyn Read + Send> = self.port.try_clone()?;
246        let mut buf_reader = BufReader::new(reader);
247
248        let ret = Self::send(&mut buf_writer, &command);
249        if let Err(e) = ret {
250            tracing::error!("Command send error: {:?}", e);
251            return Err(e);
252        }
253
254        match command {
255            SifliUartCommand::Exit => Ok(SifliUartResponse::Exit),
256            _ => Self::recv(&mut buf_reader),
257        }
258    }
259
260    pub(crate) fn debug_read_word32(&mut self, addr: u32) -> Result<u32, std::io::Error> {
261        let command = SifliUartCommand::MEMRead { addr, len: 1 };
262        match self.debug_command(command) {
263            Ok(SifliUartResponse::MEMRead { data }) => {
264                if data.len() != 4 {
265                    return Err(std::io::Error::new(
266                        std::io::ErrorKind::InvalidData,
267                        "Invalid response length",
268                    ));
269                }
270                let value = u32::from_le_bytes(data.try_into().unwrap());
271                Ok(value)
272            }
273            Ok(_) => Err(std::io::Error::new(
274                std::io::ErrorKind::InvalidData,
275                "Invalid response",
276            )),
277            Err(e) => Err(e),
278        }
279    }
280
281    pub(crate) fn debug_write_word32(&mut self, addr: u32, data: u32) -> Result<(), std::io::Error> {
282        let command = SifliUartCommand::MEMWrite {
283            addr,
284            data: &[data],
285        };
286        match self.debug_command(command) {
287            Ok(SifliUartResponse::MEMWrite) => Ok(()),
288            Ok(_) => Err(std::io::Error::new(
289                std::io::ErrorKind::InvalidData,
290                "Invalid response",
291            )),
292            Err(e) => Err(e),
293        }
294    }
295
296    pub(crate) fn debug_write_memory(&mut self, address: u32, data: &[u8]) -> Result<(), std::io::Error> {
297        if data.is_empty() {
298            return Ok(());
299        }
300
301        let address = if (address & 0xff000000) == 0x12000000 {
302            (address & 0x00ffffff) | 0x62000000
303        } else {
304            address
305        };
306
307        let addr_usize = address as usize;
308        // Calculate the start address and end address after alignment
309        let start_aligned = addr_usize - (addr_usize % 4);
310        let end_aligned = (addr_usize + data.len()).div_ceil(4) * 4;
311        let total_bytes = end_aligned - start_aligned;
312        let total_words = total_bytes / 4;
313
314        let mut buffer = vec![0u8; total_bytes];
315
316        for i in 0..total_words {
317            let block_addr = start_aligned + i * 4;
318            let block_end = block_addr + 4;
319
320            // Determine if the current 4-byte block is ‘completely overwritten’ by the new data written to it
321            // If the block is completely in the new data area, then copy the new data directly
322            if block_addr >= addr_usize && block_end <= addr_usize + data.len() {
323                let offset_in_data = block_addr - addr_usize;
324                buffer[i * 4..i * 4 + 4].copy_from_slice(&data[offset_in_data..offset_in_data + 4]);
325            } else {
326                // For the rest of the cases (header or tail incomplete overwrite):
327                // Call MEMRead first to read out the original 4-byte block.
328                let resp = self.debug_command(SifliUartCommand::MEMRead {
329                    addr: block_addr as u32,
330                    len: 1,
331                })?;
332                let mut block: [u8; 4] = match resp {
333                    SifliUartResponse::MEMRead { data: d } if d.len() == 4 => {
334                        [d[0], d[1], d[2], d[3]]
335                    }
336                    _ => {
337                        return Err(std::io::Error::new(
338                            std::io::ErrorKind::InvalidData,
339                            "Invalid response length",
340                        ));
341                    }
342                };
343                // Calculate the overlap of the block with the new data area
344                let overlap_start = max(block_addr, addr_usize);
345                let overlap_end = min(block_end, addr_usize + data.len());
346                if overlap_start < overlap_end {
347                    let in_block_offset = overlap_start - block_addr;
348                    let in_data_offset = overlap_start - addr_usize;
349                    let overlap_len = overlap_end - overlap_start;
350                    block[in_block_offset..in_block_offset + overlap_len]
351                        .copy_from_slice(&data[in_data_offset..in_data_offset + overlap_len]);
352                }
353                buffer[i * 4..i * 4 + 4].copy_from_slice(&block);
354            }
355        }
356
357        let words: Vec<u32> = buffer
358            .chunks_exact(4)
359            .map(|chunk| u32::from_le_bytes(chunk.try_into().expect("chunk length is 4")))
360            .collect();
361
362        // Write the entire alignment area at once
363        self.debug_command(SifliUartCommand::MEMWrite {
364            addr: start_aligned as u32,
365            data: &words,
366        })?;
367
368        Ok(())
369    }
370    
371    fn wait_for_core_register_transfer(
372        &mut self,
373        timeout: Duration
374    ) -> Result<(), std::io::Error> {
375        // now we have to poll the dhcsr register, until the dhcsr.s_regrdy bit is set
376        // (see C1-292, cortex m0 arm)
377        let start = Instant::now();
378
379        while start.elapsed() < timeout {
380            let dhcsr_val = Dhcsr(self.debug_read_word32(Dhcsr::get_mmio_address() as u32)?);
381
382            if dhcsr_val.s_regrdy() {
383                return Ok(());
384            }
385        }
386        Err(std::io::Error::new(std::io::ErrorKind::TimedOut, "Timeout"))
387    }
388
389    pub(crate) fn debug_write_core_reg(
390        &mut self,
391        addr: RegisterId,
392        value: u32,
393    ) -> Result<(), std::io::Error> {
394        self.debug_write_word32(Dcrdr::get_mmio_address() as u32, value)?;
395
396        let mut dcrsr_val = Dcrsr(0);
397        dcrsr_val.set_regwnr(true); // Perform a write.
398        dcrsr_val.set_regsel(addr.into()); // The address of the register to write.
399
400        self.debug_write_word32(Dcrsr::get_mmio_address() as u32, dcrsr_val.into())?;
401
402        // self.wait_for_core_register_transfer(Duration::from_millis(100))?;
403        std::thread::sleep(Duration::from_millis(10));
404        Ok(())
405    }
406
407    fn debug_read_core_reg(&mut self, addr: RegisterId) -> Result<u32, std::io::Error> {
408        // Write the DCRSR value to select the register we want to read.
409        let mut dcrsr_val = Dcrsr(0);
410        dcrsr_val.set_regwnr(false); // Perform a read.
411        dcrsr_val.set_regsel(addr.into()); // The address of the register to read.
412
413        self.debug_write_word32(Dcrsr::get_mmio_address() as u32, dcrsr_val.into())?;
414
415        self.wait_for_core_register_transfer(Duration::from_millis(100))?;
416
417        let value = self.debug_read_word32(Dcrdr::get_mmio_address() as u32)?;
418
419        Ok(value)
420    }
421    
422    fn debug_step(&mut self)-> Result<(), std::io::Error> {
423        // 这里我们忽略了很多必要的检查，请参考probe-rs源码
424        let mut value = Dhcsr(0);
425        // Leave halted state.
426        // Step one instruction.
427        value.set_c_step(true);
428        value.set_c_halt(false);
429        value.set_c_debugen(true);
430        value.set_c_maskints(true);
431        value.enable_write();
432
433        self.debug_write_word32(Dhcsr::get_mmio_address() as u32, value.into())?;
434
435        std::thread::sleep(Duration::from_millis(10));
436        Ok(())
437    }
438    
439    pub(crate) fn debug_run(&mut self) -> Result<(), std::io::Error> {
440        self.debug_step()?;
441        let mut value = Dhcsr(0);
442        value.set_c_halt(false);
443        value.set_c_debugen(true);
444        value.enable_write();
445
446        self.debug_write_word32(Dhcsr::get_mmio_address() as u32, value.into())?;
447        
448        std::thread::sleep(Duration::from_millis(10));
449        Ok(())
450    }
451    
452    pub fn debug_halt(&mut self) -> Result<(), std::io::Error> {
453        let mut value = Dhcsr(0);
454        value.set_c_halt(true);
455        value.set_c_debugen(true);
456        value.enable_write();
457
458        self.debug_write_word32(Dhcsr::get_mmio_address() as u32, value.into())?;
459        std::thread::sleep(Duration::from_millis(10));
460        Ok(())
461    }
462}
sftool_lib/sifli_debug.rs

sftool_lib/
sifli_debug.rs
