sftool_lib/common/
sifli_debug.rs

1use crate::SifliTool;
2use probe_rs::architecture::arm::armv8m::Dcrdr;
3use probe_rs::{MemoryMappedRegister, memory_mapped_bitfield_register};
4use std::cmp::{max, min};
5use std::fmt;
6use std::io::{BufReader, BufWriter, Read, Write};
7use std::time::{Duration, Instant};
8
9pub const START_WORD: [u8; 2] = [0x7E, 0x79];
10pub const DEFUALT_RECV_TIMEOUT: Duration = Duration::from_secs(3);
11
12#[derive(Debug)]
13pub enum SifliUartCommand<'a> {
14    Enter,
15    Exit,
16    MEMRead { addr: u32, len: u16 },
17    MEMWrite { addr: u32, data: &'a [u32] },
18}
19
20#[derive(Debug)]
21pub enum SifliUartResponse {
22    Enter,
23    Exit,
24    MEMRead { data: Vec<u8> },
25    MEMWrite,
26}
27
28#[derive(Debug)]
29pub enum RecvError {
30    Timeout,
31    InvalidHeaderLength,
32    InvalidHeaderChannel,
33    ReadError(std::io::Error),
34    InvalidResponse(u8),
35}
36
37impl From<RecvError> for std::io::Error {
38    fn from(err: RecvError) -> Self {
39        match err {
40            RecvError::Timeout => {
41                std::io::Error::new(std::io::ErrorKind::TimedOut, "Receive data timeout")
42            }
43            RecvError::InvalidHeaderLength => {
44                std::io::Error::new(std::io::ErrorKind::InvalidData, "Invalid frame length")
45            }
46            RecvError::InvalidHeaderChannel => std::io::Error::new(
47                std::io::ErrorKind::InvalidData,
48                "Invalid frame channel information",
49            ),
50            RecvError::ReadError(e) => {
51                std::io::Error::new(e.kind(), format!("Data read error: {}", e))
52            }
53            RecvError::InvalidResponse(code) => std::io::Error::new(
54                std::io::ErrorKind::InvalidData,
55                format!("Invalid response code: {:#04X}", code),
56            ),
57        }
58    }
59}
60
61impl fmt::Display for SifliUartCommand<'_> {
62    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
63        match self {
64            SifliUartCommand::Enter => write!(f, "Enter"),
65            SifliUartCommand::Exit => write!(f, "Exit"),
66            SifliUartCommand::MEMRead { addr, len } => {
67                write!(f, "MEMRead {{ addr: {:#X}, len: {:#X} }}", addr, len)
68            }
69            SifliUartCommand::MEMWrite { addr, data } => {
70                write!(f, "MEMWrite {{ addr: {:#X}, data: [", addr)?;
71                for (i, d) in data.iter().enumerate() {
72                    if i > 0 {
73                        write!(f, ", ")?;
74                    }
75                    write!(f, "{:#X}", d)?;
76                }
77                write!(f, "] }}")
78            }
79        }
80    }
81}
82
83impl fmt::Display for SifliUartResponse {
84    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
85        match self {
86            SifliUartResponse::Enter => write!(f, "Enter"),
87            SifliUartResponse::Exit => write!(f, "Exit"),
88            SifliUartResponse::MEMRead { data } => {
89                write!(f, "MEMRead {{ data: [")?;
90                for (i, byte) in data.iter().enumerate() {
91                    if i > 0 {
92                        write!(f, ", ")?;
93                    }
94                    write!(f, "{:#04X}", byte)?;
95                }
96                write!(f, "] }}")
97            }
98            SifliUartResponse::MEMWrite => write!(f, "MEMWrite"),
99        }
100    }
101}
102
103memory_mapped_bitfield_register! {
104    pub struct Dcrsr(u32);
105    0xE000_EDF4, "DCRSR",
106    impl From;
107    pub _, set_regwnr: 16;
108    // If the processor does not implement the FP extension the REGSEL field is bits `[4:0]`, and bits `[6:5]` are Reserved, SBZ.
109    pub _, set_regsel: 6,0;
110}
111
112memory_mapped_bitfield_register! {
113    pub struct Dhcsr(u32);
114    0xE000_EDF0, "DHCSR",
115    impl From;
116    pub s_reset_st, _: 25;
117    pub s_retire_st, _: 24;
118    pub s_lockup, _: 19;
119    pub s_sleep, _: 18;
120    pub s_halt, _: 17;
121    pub s_regrdy, _: 16;
122    pub c_maskints, set_c_maskints: 3;
123    pub c_step, set_c_step: 2;
124    pub c_halt, set_c_halt: 1;
125    pub c_debugen, set_c_debugen: 0;
126}
127
128impl Dhcsr {
129    /// This function sets the bit to enable writes to this register.
130    ///
131    /// C1.6.3 Debug Halting Control and Status Register, DHCSR:
132    /// Debug key:
133    /// Software must write 0xA05F to this field to enable write accesses to bits
134    /// `[15:0]`, otherwise the processor ignores the write access.
135    pub fn enable_write(&mut self) {
136        self.0 &= !(0xffff << 16);
137        self.0 |= 0xa05f << 16;
138    }
139}
140
141pub trait SifliDebug {
142    fn debug_command(
143        &mut self,
144        command: SifliUartCommand,
145    ) -> Result<SifliUartResponse, std::io::Error>;
146    fn debug_write_word32(&mut self, addr: u32, data: u32) -> Result<(), std::io::Error>;
147    fn debug_read_word32(&mut self, addr: u32) -> Result<u32, std::io::Error>;
148    fn debug_write_core_reg(&mut self, reg: u16, data: u32) -> Result<(), std::io::Error>;
149    fn debug_write_memory(&mut self, addr: u32, data: &[u8]) -> Result<(), std::io::Error>;
150    fn debug_run(&mut self) -> Result<(), std::io::Error>;
151    fn debug_halt(&mut self) -> Result<(), std::io::Error>;
152    fn debug_step(&mut self) -> Result<(), std::io::Error>;
153}
154
155// Trait defining chip-specific frame formatting behavior
156pub trait ChipFrameFormat {
157    /// Create chip-specific header with appropriate endianness and fields
158    fn create_header(len: u16) -> Vec<u8>;
159
160    /// Parse received frame header and return payload size
161    fn parse_frame_header(reader: &mut BufReader<Box<dyn Read + Send>>)
162    -> Result<usize, RecvError>;
163
164    /// Encode command data with chip-specific endianness
165    fn encode_command_data(command: &SifliUartCommand) -> Vec<u8>;
166
167    /// Decode response data with chip-specific endianness
168    fn decode_response_data(data: &[u8]) -> u32;
169
170    /// Apply chip-specific address mapping (default: no mapping)
171    fn map_address(addr: u32) -> u32 {
172        addr
173    }
174}
175
176// Common implementation for communication
177pub fn send_command<F: ChipFrameFormat>(
178    writer: &mut BufWriter<Box<dyn Write + Send>>,
179    command: &SifliUartCommand,
180) -> Result<(), std::io::Error> {
181    let send_data = F::encode_command_data(command);
182    let header = F::create_header(send_data.len() as u16);
183
184    writer.write_all(&header)?;
185    writer.write_all(&send_data)?;
186    writer.flush()?;
187    Ok(())
188}
189
190pub fn recv_response<F: ChipFrameFormat>(
191    reader: &mut BufReader<Box<dyn Read + Send>>,
192) -> Result<SifliUartResponse, std::io::Error> {
193    let start_time = Instant::now();
194    let mut temp: Vec<u8> = vec![];
195
196    // 步骤1: 找到帧起始标记 (START_WORD)
197    tracing::debug!("Waiting for frame start marker...");
198    let mut buffer = vec![];
199
200    loop {
201        if start_time.elapsed() >= DEFUALT_RECV_TIMEOUT {
202            tracing::warn!(
203                "Receive timeout: {} seconds",
204                DEFUALT_RECV_TIMEOUT.as_secs()
205            );
206            return Err(RecvError::Timeout.into());
207        }
208
209        let mut byte = [0; 1];
210        match reader.read_exact(&mut byte) {
211            Ok(_) => {
212                // 处理帧检测逻辑
213                if byte[0] == START_WORD[0] || (buffer.len() == 1 && byte[0] == START_WORD[1]) {
214                    buffer.push(byte[0]);
215
216                    // 检查是否找到完整的START_WORD
217                    if buffer.ends_with(&START_WORD) {
218                        tracing::debug!("Frame start marker found: {:02X?}", START_WORD);
219                        break;
220                    }
221                } else {
222                    // 重置缓冲区
223                    buffer.clear();
224                }
225
226                // 缓冲区超过2个字节但没有匹配START_WORD，清除旧数据
227                if buffer.len() > 2 {
228                    buffer.clear();
229                }
230            }
231            Err(e) if e.kind() == std::io::ErrorKind::WouldBlock => {
232                // 对于非阻塞IO，继续尝试
233                continue;
234            }
235            Err(e) => {
236                tracing::error!("Error reading frame start marker: {}", e);
237                continue; // 继续尝试读取下一个字节
238            }
239        }
240    }
241
242    temp.extend_from_slice(&buffer);
243
244    // 步骤2: 使用芯片特定的帧头解析
245    let payload_size = F::parse_frame_header(reader)?;
246    tracing::debug!("Received packet length: {} bytes", payload_size);
247
248    // 步骤3: 读取有效载荷数据
249    tracing::debug!("Reading payload data ({} bytes)...", payload_size);
250    let mut recv_data = vec![];
251
252    while recv_data.len() < payload_size {
253        let mut byte = [0; 1];
254        match reader.read_exact(&mut byte) {
255            Ok(_) => {
256                recv_data.push(byte[0]);
257            }
258            Err(e) => {
259                tracing::error!("Failed to read payload data: {}", e);
260                return Err(RecvError::ReadError(e).into());
261            }
262        }
263    }
264
265    temp.extend_from_slice(&recv_data);
266
267    // 步骤4: 解析响应数据
268    if recv_data.is_empty() {
269        tracing::error!("Received empty payload data");
270        return Err(RecvError::InvalidResponse(0).into());
271    }
272
273    let response_code = recv_data[0];
274    match response_code {
275        0xD1 => {
276            tracing::info!("Received Enter command response");
277            Ok(SifliUartResponse::Enter)
278        }
279        0xD0 => {
280            tracing::info!("Received Exit command response");
281            Ok(SifliUartResponse::Exit)
282        }
283        0xD2 => {
284            // 提取数据部分，跳过响应代码和最后的校验字节
285            let data = if recv_data.len() > 1 {
286                recv_data[1..recv_data.len() - 1].to_vec()
287            } else {
288                Vec::new()
289            };
290            tracing::info!(
291                "Received memory read response, data length: {} bytes",
292                data.len()
293            );
294            Ok(SifliUartResponse::MEMRead { data })
295        }
296        0xD3 => {
297            tracing::info!("Received memory write response");
298            Ok(SifliUartResponse::MEMWrite)
299        }
300        _ => {
301            tracing::error!("Received unknown response code: {:#04X}", response_code);
302            Err(RecvError::InvalidResponse(response_code).into())
303        }
304    }
305}
306
307// Common helper functions that implement shared debug operations
308pub mod common_debug {
309    use super::*;
310
311    /// Common implementation for debug_command
312    pub fn debug_command_impl<T: SifliTool, F: ChipFrameFormat>(
313        tool: &mut T,
314        command: SifliUartCommand,
315    ) -> Result<SifliUartResponse, std::io::Error> {
316        tracing::info!("Command: {}", command);
317        let writer: Box<dyn Write + Send> = tool.port().try_clone()?;
318        let mut buf_writer = BufWriter::new(writer);
319
320        let reader: Box<dyn Read + Send> = tool.port().try_clone()?;
321        let mut buf_reader = BufReader::new(reader);
322
323        let ret = send_command::<F>(&mut buf_writer, &command);
324        if let Err(e) = ret {
325            tracing::error!("Command send error: {:?}", e);
326            return Err(e);
327        }
328
329        match command {
330            SifliUartCommand::Exit => Ok(SifliUartResponse::Exit),
331            _ => recv_response::<F>(&mut buf_reader),
332        }
333    }
334
335    /// Common implementation for debug_read_word32
336    pub fn debug_read_word32_impl<T: SifliTool, F: ChipFrameFormat>(
337        tool: &mut T,
338        addr: u32,
339    ) -> Result<u32, std::io::Error> {
340        let mapped_addr = F::map_address(addr);
341        let command = SifliUartCommand::MEMRead {
342            addr: mapped_addr,
343            len: 1,
344        };
345
346        // Call debug_command_impl directly instead of using the trait method
347        match debug_command_impl::<T, F>(tool, command) {
348            Ok(SifliUartResponse::MEMRead { data }) => {
349                if data.len() != 4 {
350                    return Err(std::io::Error::new(
351                        std::io::ErrorKind::InvalidData,
352                        "Invalid response length",
353                    ));
354                }
355                let value = F::decode_response_data(&data);
356                Ok(value)
357            }
358            Ok(_) => Err(std::io::Error::new(
359                std::io::ErrorKind::InvalidData,
360                "Invalid response",
361            )),
362            Err(e) => Err(e),
363        }
364    }
365
366    /// Common implementation for debug_write_word32
367    pub fn debug_write_word32_impl<T: SifliTool, F: ChipFrameFormat>(
368        tool: &mut T,
369        addr: u32,
370        data: u32,
371    ) -> Result<(), std::io::Error> {
372        let mapped_addr = F::map_address(addr);
373        let command = SifliUartCommand::MEMWrite {
374            addr: mapped_addr,
375            data: &[data],
376        };
377        match debug_command_impl::<T, F>(tool, command) {
378            Ok(SifliUartResponse::MEMWrite) => Ok(()),
379            Ok(_) => Err(std::io::Error::new(
380                std::io::ErrorKind::InvalidData,
381                "Invalid response",
382            )),
383            Err(e) => Err(e),
384        }
385    }
386
387    /// Common implementation for debug_write_memory with chip-specific mapping
388    pub fn debug_write_memory_impl<T: SifliTool, F: ChipFrameFormat>(
389        tool: &mut T,
390        address: u32,
391        data: &[u8],
392    ) -> Result<(), std::io::Error> {
393        if data.is_empty() {
394            return Ok(());
395        }
396
397        // Apply chip-specific address mapping first
398        let mut mapped_address = F::map_address(address);
399
400        // Then apply the existing mapping logic (common to all chips)
401        mapped_address = if (mapped_address & 0xff000000) == 0x12000000 {
402            (mapped_address & 0x00ffffff) | 0x62000000
403        } else {
404            mapped_address
405        };
406
407        let addr_usize = mapped_address as usize;
408        // Calculate the start address and end address after alignment
409        let start_aligned = addr_usize - (addr_usize % 4);
410        let end_aligned = (addr_usize + data.len()).div_ceil(4) * 4;
411        let total_bytes = end_aligned - start_aligned;
412        let total_words = total_bytes / 4;
413
414        let mut buffer = vec![0u8; total_bytes];
415
416        for i in 0..total_words {
417            let block_addr = start_aligned + i * 4;
418            let block_end = block_addr + 4;
419
420            // Determine if the current 4-byte block is 'completely overwritten' by the new data written to it
421            // If the block is completely in the new data area, then copy the new data directly
422            if block_addr >= addr_usize && block_end <= addr_usize + data.len() {
423                let offset_in_data = block_addr - addr_usize;
424                buffer[i * 4..i * 4 + 4].copy_from_slice(&data[offset_in_data..offset_in_data + 4]);
425            } else {
426                // For the rest of the cases (header or tail incomplete overwrite):
427                // Call MEMRead first to read out the original 4-byte block.
428                let resp = debug_command_impl::<T, F>(
429                    tool,
430                    SifliUartCommand::MEMRead {
431                        addr: block_addr as u32,
432                        len: 1,
433                    },
434                )?;
435                let mut block: [u8; 4] = match resp {
436                    SifliUartResponse::MEMRead { data: d } if d.len() == 4 => {
437                        // Apply chip-specific decoding for proper endianness
438                        let value = F::decode_response_data(&d);
439                        value.to_le_bytes()
440                    }
441                    _ => {
442                        return Err(std::io::Error::new(
443                            std::io::ErrorKind::InvalidData,
444                            "Invalid response length",
445                        ));
446                    }
447                };
448                // Calculate the overlap of the block with the new data area
449                let overlap_start = max(block_addr, addr_usize);
450                let overlap_end = min(block_end, addr_usize + data.len());
451                if overlap_start < overlap_end {
452                    let in_block_offset = overlap_start - block_addr;
453                    let in_data_offset = overlap_start - addr_usize;
454                    let overlap_len = overlap_end - overlap_start;
455                    block[in_block_offset..in_block_offset + overlap_len]
456                        .copy_from_slice(&data[in_data_offset..in_data_offset + overlap_len]);
457                }
458                buffer[i * 4..i * 4 + 4].copy_from_slice(&block);
459            }
460        }
461
462        let words: Vec<u32> = buffer
463            .chunks_exact(4)
464            .map(|chunk| u32::from_le_bytes(chunk.try_into().expect("chunk length is 4")))
465            .collect();
466
467        // Write the entire alignment area at once
468        debug_command_impl::<T, F>(
469            tool,
470            SifliUartCommand::MEMWrite {
471                addr: start_aligned as u32,
472                data: &words,
473            },
474        )?;
475
476        Ok(())
477    }
478
479    /// Common implementation for debug_write_core_reg
480    pub fn debug_write_core_reg_impl<T: SifliTool, F: ChipFrameFormat>(
481        tool: &mut T,
482        addr: u16,
483        value: u32,
484    ) -> Result<(), std::io::Error> {
485        debug_write_word32_impl::<T, F>(tool, Dcrdr::get_mmio_address() as u32, value)?;
486
487        let mut dcrsr_val = Dcrsr(0);
488        dcrsr_val.set_regwnr(true); // Perform a write.
489        dcrsr_val.set_regsel(addr.into()); // The address of the register to write.
490
491        debug_write_word32_impl::<T, F>(tool, Dcrsr::get_mmio_address() as u32, dcrsr_val.into())?;
492
493        // self.wait_for_core_register_transfer(Duration::from_millis(100))?;
494        std::thread::sleep(Duration::from_millis(10));
495        Ok(())
496    }
497
498    /// Common implementation for debug_step
499    pub fn debug_step_impl<T: SifliTool, F: ChipFrameFormat>(
500        tool: &mut T,
501    ) -> Result<(), std::io::Error> {
502        // 这里我们忽略了很多必要的检查，请参考probe-rs源码
503        let mut value = Dhcsr(0);
504        // Leave halted state.
505        // Step one instruction.
506        value.set_c_step(true);
507        value.set_c_halt(false);
508        value.set_c_debugen(true);
509        value.set_c_maskints(true);
510        value.enable_write();
511
512        debug_write_word32_impl::<T, F>(tool, Dhcsr::get_mmio_address() as u32, value.into())?;
513
514        std::thread::sleep(Duration::from_millis(10));
515        Ok(())
516    }
517
518    /// Common implementation for debug_run
519    pub fn debug_run_impl<T: SifliTool, F: ChipFrameFormat>(
520        tool: &mut T,
521    ) -> Result<(), std::io::Error> {
522        debug_step_impl::<T, F>(tool)?;
523        std::thread::sleep(Duration::from_millis(100));
524        let mut value = Dhcsr(0);
525        value.set_c_halt(false);
526        value.set_c_debugen(true);
527        value.enable_write();
528
529        debug_write_word32_impl::<T, F>(tool, Dhcsr::get_mmio_address() as u32, value.into())?;
530
531        std::thread::sleep(Duration::from_millis(100));
532        Ok(())
533    }
534
535    /// Common implementation for debug_halt
536    pub fn debug_halt_impl<T: SifliTool, F: ChipFrameFormat>(
537        tool: &mut T,
538    ) -> Result<(), std::io::Error> {
539        let mut value = Dhcsr(0);
540        value.set_c_halt(true);
541        value.set_c_debugen(true);
542        value.enable_write();
543
544        debug_write_word32_impl::<T, F>(tool, Dhcsr::get_mmio_address() as u32, value.into())?;
545        std::thread::sleep(Duration::from_millis(10));
546        Ok(())
547    }
548}
sftool_lib/common/sifli_debug.rs

sftool_lib/common/
sifli_debug.rs
