diff --git a/src/dap.rs b/src/dap.rs index 74c661d..5bffa2c 100644 --- a/src/dap.rs +++ b/src/dap.rs @@ -1,4 +1,8 @@ -use crate::{jtag, swd, swj, swo, usb}; +use crate::{ + jtag::{self, TransferInfo, TransferResult}, + swd::{self, APnDP, RnW}, + swj, swo, usb, +}; mod command; mod request; @@ -19,14 +23,25 @@ pub trait DapLeds { fn react_to_host_status(&mut self, host_status: HostStatus); } +/// The SWD interface configuration. +pub struct TransferConfig { + /// The number of idle cycles to wait after a transfer. + pub idle_cycles: u8, + /// The number of retries after a `Wait` response. + pub wait_retries: usize, + /// The number of retries if read value does not match. + pub match_retries: usize, + /// The match set by a write transfer. + pub match_mask: u32, +} + /// DAP handler. pub struct Dap<'a, DEPS, LEDS, WAIT, JTAG, SWD, SWO> { state: State, swo: Option, swo_streaming: bool, - swd_wait_retries: usize, - match_retries: usize, version_string: &'a str, + transfer_config: TransferConfig, // mode: Option, leds: LEDS, wait: WAIT, @@ -56,9 +71,13 @@ where state: State::new(dependencies), swo, swo_streaming: false, - swd_wait_retries: 5, - match_retries: 8, version_string, + transfer_config: TransferConfig { + idle_cycles: 1, + wait_retries: 5, + match_retries: 8, + match_mask: 0, + }, // mode: None, leds, wait, @@ -280,13 +299,21 @@ where let idx = req.next_u8(); let word = req.next_u32(); match (SWD::AVAILABLE, JTAG::AVAILABLE, &mut self.state) { - (_, true, State::Jtag(_jtag)) => { - // TODO: Implement one day. - let _ = idx; - resp.write_err(); + (_, true, State::Jtag(jtag)) => { + let config = jtag.config(); + if !config.select_index(idx) { + resp.write_err(); + return; + } + + jtag.shift_ir(jtag::JTAG_IR_ABORT); + jtag.write_abort(word); + + resp.write_ok(); } (true, _, State::Swd(swd)) => { - match swd.write_dp(self.swd_wait_retries, swd::DPRegister::DPIDR, word) { + let wait_retries = self.transfer_config.wait_retries; + match swd.write_dp(wait_retries, swd::DPRegister::DPIDR, word) { Ok(_) => resp.write_ok(), Err(_) => resp.write_err(), } @@ -363,6 +390,15 @@ where } fn process_swd_configure(&mut self, mut req: Request, resp: &mut ResponseWriter) { + let _config = match &mut self.state { + State::Swd(swd) => swd.config(), + State::None { deps, .. } => deps.swd_config(), + _ => { + resp.write_err(); + return; + } + }; + // TODO: Do we want to support other configs? let config = req.next_u8(); let clk_period = config & 0b011; @@ -396,7 +432,7 @@ where // Other integers are normal. n => n as usize, }; - let nbytes = (nbits + 7) / 8; + let nbytes = nbits.div_ceil(8); let output = (sequence_info & 0x80) == 0; if output { @@ -550,160 +586,439 @@ where fn process_jtag_sequence(&mut self, req: Request, resp: &mut ResponseWriter) { self.state.to_jtag(); - match &mut self.state { - State::Jtag(jtag) => { - // Run requested JTAG sequences. Cannot fail. - let size = jtag.sequences(req.rest(), resp.remaining()); - resp.skip(size as _); - - resp.write_ok(); + let jtag = match &mut self.state { + State::Jtag(jtag) => jtag, + _ => { + resp.write_err(); + return; } - _ => resp.write_err(), }; + + // Always succeeds + resp.write_ok(); + + jtag.sequences(req, resp); } - fn process_jtag_configure(&self, _req: Request, _resp: &mut ResponseWriter) { - // TODO: Implement one day (needs proper JTAG support) + fn process_jtag_configure(&mut self, mut req: Request, resp: &mut ResponseWriter) { + let config = match &mut self.state { + State::Jtag(jtag) => jtag.config(), + State::None { deps, .. } => deps.jtag_config(), + _ => { + resp.write_err(); + return; + } + }; + + let count = req.next_u8(); + if !config.update_device_count(count) { + resp.write_err(); + return; + } + + let mut bits = 0; + for n in 0..count as usize { + let length = req.next_u8(); + config.scan_chain[n].ir_length = length; + config.scan_chain[n].ir_before = bits; + bits += length as u16; + } + for n in 0..count as usize { + bits -= config.scan_chain[n].ir_length as u16; + config.scan_chain[n].ir_after = bits; + debug!( + "JTAG TAP #{}: before: {}, length: {}, after: {}", + n, + config.scan_chain[n].ir_before, + config.scan_chain[n].ir_length, + config.scan_chain[n].ir_after + ); + } + + resp.write_ok(); } - fn process_jtag_idcode(&self, _req: Request, _resp: &mut ResponseWriter) { - // TODO: Implement one day (needs proper JTAG support) + fn process_jtag_idcode(&mut self, mut req: Request, resp: &mut ResponseWriter) { + self.state.to_jtag(); + + let jtag = match &mut self.state { + State::Jtag(jtag) => jtag, + _ => { + resp.write_err(); + return; + } + }; + + if !jtag.config().select_index(req.next_u8()) { + resp.write_err(); + return; + } + + jtag.shift_ir(jtag::JTAG_IR_IDCODE); + let data = jtag.shift_dr(0); + + resp.write_ok(); + resp.write_u32(data); } fn process_transfer_configure(&mut self, mut req: Request, resp: &mut ResponseWriter) { - // We don't support variable idle cycles - // TODO: Should we? - let _idle_cycles = req.next_u8(); + // TODO: We don't support variable idle cycles for SWD + self.transfer_config.idle_cycles = req.next_u8(); // Send number of wait retries through to SWD - self.swd_wait_retries = req.next_u16() as usize; + self.transfer_config.wait_retries = req.next_u16() as usize; // Store number of match retries - self.match_retries = req.next_u16() as usize; + self.transfer_config.match_retries = req.next_u16() as usize; resp.write_ok(); } - fn process_transfer(&mut self, mut req: Request, resp: &mut ResponseWriter) { + fn process_transfer(&mut self, req: Request, resp: &mut ResponseWriter) { self.state.to_last_mode(); + let config = &mut self.transfer_config; + match &mut self.state { + State::Jtag(jtag) => Self::process_transfer_jtag(config, jtag, req, resp), + State::Swd(swd) => Self::process_transfer_swd(config, swd, req, resp), + _ => return, + } + } + + fn process_transfer_swd( + transfer_config: &mut TransferConfig, + swd: &mut SWD, + mut req: Request, + resp: &mut ResponseWriter, + ) { let _idx = req.next_u8(); let ntransfers = req.next_u8(); - let mut match_mask = 0xFFFF_FFFFu32; - match &mut self.state { - State::Jtag(_jtag) => { - // TODO: Implement one day. + // Skip two bytes in resp to reserve space for final status, + // which we update while processing. + resp.write_u16(0); + + let wait_retries = transfer_config.wait_retries; + let match_retries = transfer_config.match_retries; + + for transfer_idx in 0..ntransfers { + // Store how many transfers we execute in the response + resp.write_u8_at(1, transfer_idx + 1); + + // Parse the next transfer request + let transfer_req = req.next_u8(); + let apndp = swd::APnDP::try_from(transfer_req & (1 << 0)).unwrap(); + let rnw = swd::RnW::try_from((transfer_req & (1 << 1)) >> 1).unwrap(); + let a = swd::DPRegister::try_from((transfer_req & (3 << 2)) >> 2).unwrap(); + let vmatch = (transfer_req & (1 << 4)) != 0; + let mmask = (transfer_req & (1 << 5)) != 0; + let _ts = (transfer_req & (1 << 7)) != 0; + + if rnw == swd::RnW::R { + // Issue register read + let mut read_value = if apndp == swd::APnDP::AP { + // Reads from AP are posted, so we issue the + // read and subsequently read RDBUFF for the data. + // This requires an additional transfer so we'd + // ideally keep track of posted reads and just + // keep issuing new AP reads, but our reads are + // sufficiently fast that for now this is simpler. + let rdbuff = swd::DPRegister::RDBUFF; + if swd.read_ap(wait_retries, a).check(resp.mut_at(2)).is_none() { + break; + } + match swd.read_dp(wait_retries, rdbuff).check(resp.mut_at(2)) { + Some(v) => v, + None => break, + } + } else { + // Reads from DP are not posted, so directly read the register. + match swd.read_dp(wait_retries, a).check(resp.mut_at(2)) { + Some(v) => v, + None => break, + } + }; + + // Handle value match requests by retrying if needed. + // Since we're re-reading the same register the posting + // is less important and we can just use the returned value. + if vmatch { + let target_value = req.next_u32(); + let mut match_tries = 0; + while (read_value & transfer_config.match_mask) != target_value { + match_tries += 1; + if match_tries > match_retries { + break; + } + + read_value = match swd + .read(wait_retries, apndp.into(), a) + .check(resp.mut_at(2)) + { + Some(v) => v, + None => break, + } + } + + // If we didn't read the correct value, set the value mismatch + // flag in the response and quit early. + if (read_value & transfer_config.match_mask) != target_value { + resp.write_u8_at(1, resp.read_u8_at(1) | (1 << 4)); + break; + } + } else { + // Save read register value + resp.write_u32(read_value); + } + } else { + // Write transfer processing + + // Writes with match_mask set just update the match mask + if mmask { + transfer_config.match_mask = req.next_u32(); + continue; + } + + // Otherwise issue register write + let write_value = req.next_u32(); + if swd + .write(wait_retries, apndp, a, write_value) + .check(resp.mut_at(2)) + .is_none() + { + break; + } } - State::Swd(swd) => { - // Skip two bytes in resp to reserve space for final status, - // which we update while processing. - resp.write_u16(0); - - for transfer_idx in 0..ntransfers { - // Store how many transfers we execute in the response - resp.write_u8_at(1, transfer_idx + 1); - - // Parse the next transfer request - let transfer_req = req.next_u8(); - let apndp = swd::APnDP::try_from(transfer_req & (1 << 0)).unwrap(); - let rnw = swd::RnW::try_from((transfer_req & (1 << 1)) >> 1).unwrap(); - let a = swd::DPRegister::try_from((transfer_req & (3 << 2)) >> 2).unwrap(); - let vmatch = (transfer_req & (1 << 4)) != 0; - let mmask = (transfer_req & (1 << 5)) != 0; - let _ts = (transfer_req & (1 << 7)) != 0; - - if rnw == swd::RnW::R { - // Issue register read - let mut read_value = if apndp == swd::APnDP::AP { - // Reads from AP are posted, so we issue the - // read and subsequently read RDBUFF for the data. - // This requires an additional transfer so we'd - // ideally keep track of posted reads and just - // keep issuing new AP reads, but our reads are - // sufficiently fast that for now this is simpler. - let rdbuff = swd::DPRegister::RDBUFF; - if swd - .read_ap(self.swd_wait_retries, a) - .check(resp.mut_at(2)) - .is_none() - { - break; - } - match swd - .read_dp(self.swd_wait_retries, rdbuff) - .check(resp.mut_at(2)) - { - Some(v) => v, - None => break, - } - } else { - // Reads from DP are not posted, so directly read the register. - match swd.read_dp(self.swd_wait_retries, a).check(resp.mut_at(2)) { - Some(v) => v, - None => break, - } - }; - - // Handle value match requests by retrying if needed. - // Since we're re-reading the same register the posting - // is less important and we can just use the returned value. - if vmatch { - let target_value = req.next_u32(); - let mut match_tries = 0; - while (read_value & match_mask) != target_value { - match_tries += 1; - if match_tries > self.match_retries { - break; - } - - read_value = match swd - .read(self.swd_wait_retries, apndp.into(), a) - .check(resp.mut_at(2)) - { - Some(v) => v, - None => break, - } - } - - // If we didn't read the correct value, set the value mismatch - // flag in the response and quit early. - if (read_value & match_mask) != target_value { - resp.write_u8_at(1, resp.read_u8_at(1) | (1 << 4)); - break; - } - } else { - // Save read register value - resp.write_u32(read_value); + } + } + + fn process_transfer_jtag( + transfer_config: &mut TransferConfig, + jtag: &mut JTAG, + mut req: Request, + resp: &mut ResponseWriter, + ) { + let idx = req.next_u8(); + let transfer_count = req.next_u8(); + let mut ntransfers = transfer_count; + + resp.skip(2); + + if !jtag.config().select_index(idx) { + // goto end + resp.write_u8_at(1, 0); + resp.write_u8_at(2, 0); + return; + } + + let mut post_read = false; + let mut ir = 0; + let mut response_value = TransferResult::Fault; + let mut response_count = 0; + + while ntransfers > 0 { + debug!("JTAG transfer {}/{}", response_count + 1, transfer_count); + let request_value = jtag::TransferInfo::from(req.next_u8()); + ntransfers -= 1; + let request_ir = if request_value.ap_ndp == APnDP::AP { + jtag::JTAG_IR_APACC + } else { + jtag::JTAG_IR_DPACC + }; + if request_value.r_nw == RnW::R { + // Read register + if post_read { + // Read was posted before + if ir == request_ir && !request_value.match_value { + // Read previous data and post next read + + response_value = + transfer_with_retry(jtag, request_value, transfer_config, 0); + } else { + // Select JTAG chain + if ir != jtag::JTAG_IR_DPACC { + ir = jtag::JTAG_IR_DPACC; + jtag.shift_ir(ir); } + + // Read previous data + response_value = + transfer_with_retry(jtag, TransferInfo::RDBUFF, transfer_config, 0); + post_read = false; + } + + if let TransferResult::Ok(data) = response_value { + // Store previous data + resp.write_u32(data); } else { - // Write transfer processing + break; + } - // Writes with match_mask set just update the match mask - if mmask { - match_mask = req.next_u32(); - continue; + if post_read && request_value.timestamp { + resp.write_u32(0); // TODO real timestamp + } + } + + if request_value.match_value { + // Read with value match + let match_value = req.next_u32(); + let mut match_retry = transfer_config.match_retries; + + // Select JTAG chain + if ir != request_ir { + ir = request_ir; + jtag.shift_ir(ir); + } + // Post DP/AP read + response_value = transfer_with_retry(jtag, request_value, transfer_config, 0); + if !matches!(response_value, TransferResult::Ok(_)) { + break; + } + loop { + response_value = + transfer_with_retry(jtag, request_value, transfer_config, 0); + match response_value { + TransferResult::Ok(data) + if (data & transfer_config.match_mask) != match_value => {} + _ => break, } - // Otherwise issue register write - let write_value = req.next_u32(); - if swd - .write(self.swd_wait_retries, apndp, a, write_value) - .check(resp.mut_at(2)) - .is_none() - { + if match_retry == 0 { break; } + match_retry -= 1; + } + + if let TransferResult::Ok(data) = response_value { + if (data & transfer_config.match_mask) != match_value { + response_value = TransferResult::Mismatch; + break; + } + } else { + break; + } + } else { + // Normal read + if !post_read { + // Select JTAG chain + if ir != request_ir { + ir = request_ir; + jtag.shift_ir(ir); + } + // Post DP/AP read + response_value = + transfer_with_retry(jtag, request_value, transfer_config, 0); + if !matches!(response_value, TransferResult::Ok(_)) { + break; + } + + if request_value.timestamp { + resp.write_u32(0); // TODO real timestamp + } + post_read = true; + } + } + } else { + // Write register + if post_read { + // Select JTAG chain + if ir != jtag::JTAG_IR_DPACC { + ir = jtag::JTAG_IR_DPACC; + jtag.shift_ir(ir); + } + // Read previous data + response_value = + transfer_with_retry(jtag, TransferInfo::RDBUFF, transfer_config, 0); + + if let TransferResult::Ok(data) = response_value { + // Store previous data + resp.write_u32(data); + post_read = false; + } else { + break; + } + } + // Load data + let data = req.next_u32(); + if request_value.match_value { + // Write match mask + transfer_config.match_mask = data; + response_value = TransferResult::Ok(0); + } else { + // Select JTAG chain + if ir != request_ir { + ir = request_ir; + jtag.shift_ir(ir); + } + + response_value = + transfer_with_retry(jtag, request_value, transfer_config, data); + if !matches!(response_value, TransferResult::Ok(_)) { + break; + } + + if request_value.timestamp { + resp.write_u32(0); // TODO real timestamp } } } - _ => return, + response_count += 1; + } + + while ntransfers > 0 { + ntransfers -= 1; + // Process canceled requests + let request_value = TransferInfo::from(req.next_u8()); + if request_value.r_nw == RnW::R { + // Read register + if request_value.match_value { + // Read with value match + req.next_u32(); + } + } else { + // Write register + req.next_u32(); + } + } + + if matches!(response_value, TransferResult::Ok(_)) { + // Select JTAG chain + if ir != jtag::JTAG_IR_DPACC { + jtag.shift_ir(jtag::JTAG_IR_DPACC); + } + + // Check last write, or read previous read's result. + response_value = transfer_with_retry(jtag, TransferInfo::RDBUFF, transfer_config, 0); + + if post_read { + // Read previous data + if let TransferResult::Ok(data) = response_value { + resp.write_u32(data); + } + } } + + resp.write_u8_at(1, response_count); + resp.write_u8_at(2, response_value.status()); } - fn process_transfer_block(&mut self, mut req: Request, resp: &mut ResponseWriter) { + fn process_transfer_block(&mut self, req: Request, resp: &mut ResponseWriter) { self.state.to_last_mode(); + let config = &mut self.transfer_config; + match &mut self.state { + State::Jtag(jtag) => Self::process_transfer_block_jtag(config, jtag, req, resp), + State::Swd(swd) => Self::process_transfer_block_swd(config, swd, req, resp), + _ => return, + } + } + + fn process_transfer_block_swd( + transfer_config: &mut TransferConfig, + swd: &mut SWD, + mut req: Request, + resp: &mut ResponseWriter, + ) { let _idx = req.next_u8(); let ntransfers = req.next_u16(); let transfer_req = req.next_u8(); @@ -711,81 +1026,164 @@ where let rnw = swd::RnW::try_from((transfer_req & (1 << 1)) >> 1).unwrap(); let a = swd::DPRegister::try_from((transfer_req & (3 << 2)) >> 2).unwrap(); - match &mut self.state { - State::Jtag(_jtag) => { - // TODO: Implement one day. - } - State::Swd(swd) => { - // Skip three bytes in resp to reserve space for final status, - // which we update while processing. - resp.write_u16(0); - resp.write_u8(0); - - // Keep track of how many transfers we executed, - // so if there is an error the host knows where - // it happened. - let mut transfers = 0; - - // If reading an AP register, post first read early. - if rnw == swd::RnW::R - && apndp == swd::APnDP::AP - && swd - .read_ap(self.swd_wait_retries, a) - .check(resp.mut_at(3)) - .is_none() - { - // Quit early on error - resp.write_u16_at(1, 1); - return; - } + let wait_retries = transfer_config.wait_retries; + + // Skip three bytes in resp to reserve space for final status, + // which we update while processing. + resp.skip(3); - for transfer_idx in 0..ntransfers { - transfers = transfer_idx; - if rnw == swd::RnW::R { - // Handle repeated reads - let read_value = if apndp == swd::APnDP::AP { - // For AP reads, the first read was posted, so on the final - // read we need to read RDBUFF instead of the AP register. - if transfer_idx < ntransfers - 1 { - match swd.read_ap(self.swd_wait_retries, a).check(resp.mut_at(3)) { - Some(v) => v, - None => break, - } - } else { - let rdbuff = swd::DPRegister::RDBUFF.into(); - match swd - .read_dp(self.swd_wait_retries, rdbuff) - .check(resp.mut_at(3)) - { - Some(v) => v, - None => break, - } - } - } else { - // For DP reads, no special care required - match swd.read_dp(self.swd_wait_retries, a).check(resp.mut_at(3)) { - Some(v) => v, - None => break, - } - }; - - // Save read register value to response - resp.write_u32(read_value); + // Keep track of how many transfers we executed, + // so if there is an error the host knows where + // it happened. + let mut transfers = 0; + + // If reading an AP register, post first read early. + if rnw == swd::RnW::R + && apndp == swd::APnDP::AP + && swd.read_ap(wait_retries, a).check(resp.mut_at(3)).is_none() + { + // Quit early on error + resp.write_u16_at(1, 1); + return; + } + + for transfer_idx in 0..ntransfers { + transfers = transfer_idx; + if rnw == swd::RnW::R { + // Handle repeated reads + let read_value = if apndp == swd::APnDP::AP { + // For AP reads, the first read was posted, so on the final + // read we need to read RDBUFF instead of the AP register. + if transfer_idx < ntransfers - 1 { + match swd.read_ap(wait_retries, a).check(resp.mut_at(3)) { + Some(v) => v, + None => break, + } } else { - // Handle repeated register writes - let write_value = req.next_u32(); - let result = swd.write(self.swd_wait_retries, apndp, a, write_value); - if result.check(resp.mut_at(3)).is_none() { - break; + let rdbuff = swd::DPRegister::RDBUFF.into(); + match swd.read_dp(wait_retries, rdbuff).check(resp.mut_at(3)) { + Some(v) => v, + None => break, + } + } + } else { + // For DP reads, no special care required + match swd.read_dp(wait_retries, a).check(resp.mut_at(3)) { + Some(v) => v, + None => break, + } + }; + + // Save read register value to response + resp.write_u32(read_value); + } else { + // Handle repeated register writes + let write_value = req.next_u32(); + let result = swd.write(wait_retries, apndp, a, write_value); + if result.check(resp.mut_at(3)).is_none() { + break; + } + } + } + + // Write number of transfers to response + resp.write_u16_at(1, transfers + 1); + } + + fn process_transfer_block_jtag( + transfer_config: &mut TransferConfig, + jtag: &mut JTAG, + mut req: Request, + resp: &mut ResponseWriter, + ) { + let idx = req.next_u8(); + let request_count = req.next_u16(); + let mut nrequests = request_count; + let mut request_value = TransferInfo::from(req.next_u8()); + + let mut response_count = 0; + resp.skip(3); + + // Device index (JTAP TAP) + if !jtag.config().select_index(idx) { + // goto end + resp.write_u16_at(1, response_count); + resp.write_u8_at(3, 0); + return; + } + + if nrequests == 0 { + // goto end + resp.write_u16_at(1, response_count); + resp.write_u8_at(3, 0); + return; + } + + // Select JTAG chain + let ir = if request_value.ap_ndp == APnDP::AP { + jtag::JTAG_IR_APACC + } else { + jtag::JTAG_IR_DPACC + }; + jtag.shift_ir(ir); + + let mut response_value = jtag::TransferResult::Fault; + if request_value.r_nw == RnW::R { + // Post read + debug!("Posting read for {} transfers", request_count); + response_value = transfer_with_retry(jtag, request_value, transfer_config, 0); + if matches!(response_value, TransferResult::Ok(_)) { + // Read register block + while nrequests > 0 { + debug!("Reading {}/{}", response_count + 1, request_count); + nrequests -= 1; + // Read DP/AP register + if nrequests == 0 { + // Last read + if ir != jtag::JTAG_IR_DPACC { + jtag.shift_ir(jtag::JTAG_IR_DPACC); } + request_value = TransferInfo::RDBUFF; } + response_value = transfer_with_retry(jtag, request_value, transfer_config, 0); + if let TransferResult::Ok(data) = response_value { + // Store data + resp.write_u32(data); + response_count += 1; + } else { + // goto end + break; + } + } + } + } else { + // Write register block + while nrequests > 0 { + debug!("Writing {}/{}", response_count + 1, request_count); + nrequests -= 1; + // Load data + let data = req.next_u32(); + // Write DP/AP register + response_value = transfer_with_retry(jtag, request_value, transfer_config, data); + if !matches!(response_value, TransferResult::Ok(_)) { + // goto end + break; } + response_count += 1; - // Write number of transfers to response - resp.write_u16_at(1, transfers + 1); + if request_count == 0 { + // Check last write + if ir != jtag::JTAG_IR_DPACC { + jtag.shift_ir(jtag::JTAG_IR_DPACC); + } + response_value = + transfer_with_retry(jtag, TransferInfo::RDBUFF, transfer_config, 0); + } } - _ => return, } + + resp.write_u16_at(1, response_count); + resp.write_u8_at(3, response_value.status()); } fn process_transfer_abort(&mut self) { @@ -833,6 +1231,33 @@ impl CheckResult for swd::Result { } } +fn transfer_with_retry( + jtag: &mut impl jtag::Jtag, + info: jtag::TransferInfo, + transfer_config: &TransferConfig, + data: u32, +) -> jtag::TransferResult { + let mut response_value; + let mut retry = transfer_config.wait_retries; + + if info.r_nw == RnW::W { + debug!("Transfer: {:?} ({:x})", info, data); + } else { + debug!("Transfer: {:?}", info); + } + + loop { + // Read register until retry counter expires or the read returns !Wait + response_value = jtag.transfer(info.r_nw, info.a2a3 as u8, transfer_config, data); + if response_value != jtag::TransferResult::Wait || retry == 0 { + debug!("Transfer result: {:x}", response_value); + break; + } + retry -= 1; + } + response_value +} + #[cfg(test)] mod test { use super::*; diff --git a/src/dap/request.rs b/src/dap/request.rs index 93f2b4c..5c3c6a1 100644 --- a/src/dap/request.rs +++ b/src/dap/request.rs @@ -1,7 +1,10 @@ use super::Command; pub struct Request<'a> { + /// The CMSIS-DAP command. pub command: Command, + + /// The request payload. pub data: &'a [u8], } @@ -15,28 +18,33 @@ impl<'a> Request<'a> { Some(Request { command, data }) } + /// Consumes the next byte and returns it as a `u8` value. pub fn next_u8(&mut self) -> u8 { let value = self.data[0]; self.data = &self.data[1..]; value } + /// Consumes the next two bytes and returns them as a `u16` value. pub fn next_u16(&mut self) -> u16 { let value = u16::from_le_bytes(self.data[0..2].try_into().unwrap()); self.data = &self.data[2..]; value } + /// Consumes the next four bytes and returns them as a `u32` value. pub fn next_u32(&mut self) -> u32 { let value = u32::from_le_bytes(self.data[0..4].try_into().unwrap()); self.data = &self.data[4..]; value } + /// Consumes the first `count` bytes of the data. pub fn consume(&mut self, count: usize) { self.data = &self.data[count..]; } + /// Returns the remaining data. pub fn rest(self) -> &'a [u8] { &self.data } diff --git a/src/jtag.rs b/src/jtag.rs index 7dc032e..157407e 100644 --- a/src/jtag.rs +++ b/src/jtag.rs @@ -1,12 +1,428 @@ +use crate::{ + dap::{self, TransferConfig}, + swd::{APnDP, DPRegister, RnW}, +}; + +/// Describes a JTAG sequence request. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct SequenceInfo { + /// The number of bits to shift in/out. + pub n_bits: u8, + + /// If the TDO data should be captured. + pub capture: bool, + + /// The TMS level to output. + pub tms: bool, +} + +impl From for SequenceInfo { + fn from(byte: u8) -> Self { + const JTAG_SEQUENCE_TCK: u8 = 0x3F; + const JTAG_SEQUENCE_TMS: u8 = 0x40; + const JTAG_SEQUENCE_TDO: u8 = 0x80; + + let n_bits = match byte & JTAG_SEQUENCE_TCK { + // CMSIS-DAP says 0 means 64 bits + 0 => 64, + // Other integers are normal. + n => n, + }; + + Self { + n_bits: n_bits as u8, + capture: (byte & JTAG_SEQUENCE_TDO) != 0, + tms: (byte & JTAG_SEQUENCE_TMS) != 0, + } + } +} + +/// Describes a single transfer in a JTAG transfer request. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub(crate) struct TransferInfo { + /// Whether the transfer is to an AP or DP. + pub ap_ndp: APnDP, + pub r_nw: RnW, + pub a2a3: DPRegister, + pub match_value: bool, + pub match_mask: bool, + pub timestamp: bool, +} + +impl TransferInfo { + pub(crate) const RDBUFF: Self = Self { + r_nw: RnW::R, + a2a3: DPRegister::RDBUFF, + ap_ndp: APnDP::DP, + match_value: false, + match_mask: false, + timestamp: false, + }; +} + +impl From for TransferInfo { + fn from(byte: u8) -> Self { + const DAP_TRANSFER_APNDP: u8 = 1 << 0; + const DAP_TRANSFER_RNW: u8 = 1 << 1; + const DAP_TRANSFER_A2: u8 = 1 << 2; + const DAP_TRANSFER_A3: u8 = 1 << 3; + const DAP_TRANSFER_MATCH_VALUE: u8 = 1 << 4; + const DAP_TRANSFER_MATCH_MASK: u8 = 1 << 5; + const DAP_TRANSFER_TIMESTAMP: u8 = 1 << 7; + + Self { + ap_ndp: if byte & DAP_TRANSFER_APNDP != 0 { + APnDP::AP + } else { + APnDP::DP + }, + r_nw: if byte & DAP_TRANSFER_RNW != 0 { + RnW::R + } else { + RnW::W + }, + a2a3: DPRegister::try_from((byte & (DAP_TRANSFER_A2 | DAP_TRANSFER_A3)) >> 2).unwrap(), + match_value: byte & DAP_TRANSFER_MATCH_VALUE != 0, + match_mask: byte & DAP_TRANSFER_MATCH_MASK != 0, + timestamp: byte & DAP_TRANSFER_TIMESTAMP != 0, + } + } +} + +/// Describes the position of a TAP in the JTAG chain. +#[derive(Debug, Default, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct TapConfig { + /// The number of bits in the IR register. + pub ir_length: u8, + /// The number of bypass bits before the IR register. + pub ir_before: u16, + /// The number of bypass bits after the IR register. + pub ir_after: u16, +} + +impl TapConfig { + /// Empty value for array initialization + pub const INIT: Self = Self { + ir_length: 0, + ir_before: 0, + ir_after: 0, + }; +} + +/// JTAG interface configuraiton. +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct Config { + /// The number of devices on the JTAG chain. + pub device_count: u8, + /// The position of the selected device. + pub index: u8, + /// TAPs on the scan chain. + pub scan_chain: &'static mut [TapConfig], +} + +impl Config { + /// Creates a new, empty JTAG interface configuration. + /// + /// The probe will be able to connect to JTAG chains with up to + /// `chain_buffer.len()` TAPs. + pub fn new(chain_buffer: &'static mut [TapConfig]) -> Self { + Self { + device_count: 0, + index: 0, + scan_chain: chain_buffer, + } + } + + /// Returns information about the currently selected TAP. + pub fn current_tap(&self) -> TapConfig { + self.scan_chain[self.index as usize] + } + + pub(crate) fn update_device_count(&mut self, count: u8) -> bool { + if count as usize >= self.scan_chain.len() { + return false; + } + self.device_count = count; + true + } +} + +impl Config { + /// Selects the device at the given index. + /// + /// If the index is out of bounds, it returns `false` and does not change the + /// selected device. + pub fn select_index(&mut self, index: u8) -> bool { + if index >= self.device_count { + warn!("Invalid JTAG TAP index: {}", index); + return false; + } + if index != self.index { + info!("Selecting JTAG TAP #{}", index); + self.index = index; + } + true + } +} + +const IDLE_TO_SHIFT_DR: &[bool] = &[true, false, false]; +const IDLE_TO_SHIFT_IR: &[bool] = &[true, true, false, false]; +const SHIFT_TO_IDLE: &[bool] = &[true, true, false]; +const EXIT1_TO_IDLE: &[bool] = &[true, false]; + +pub(crate) const JTAG_IR_ABORT: u32 = 0x08; +pub(crate) const JTAG_IR_DPACC: u32 = 0x0A; +pub(crate) const JTAG_IR_APACC: u32 = 0x0B; +pub(crate) const JTAG_IR_IDCODE: u32 = 0x0E; + +// TODO: currently this only supports JTAG DP V0. +const DAP_TRANSFER_OK_FAULT: u32 = 0x02; + +/// JTAG interface. pub trait Jtag: From { /// If JTAG is available or not. const AVAILABLE: bool; + /// Returns a mutable reference to the JTAG interface configuration. + fn config(&mut self) -> &mut Config; + /// Handle a JTAG sequence request. - fn sequences(&mut self, data: &[u8], rxbuf: &mut [u8]) -> u32; + /// + /// The implementor is responsible for parsing the request. The first byte contains + /// the number of sequences to process. Each sequence is described by a byte, which + /// contains the number of bits to shift in/out, whether to capture TDO, and the TMS + /// level to output. This info byte may be parsed by [`SequenceInfo`]'s `From` + /// implementation. + fn sequences(&mut self, mut req: dap::Request, resp: &mut dap::ResponseWriter) { + // Run requested JTAG sequences. Cannot fail. + let sequence_count = req.next_u8(); + + for _ in 0..sequence_count { + let sequence_info = SequenceInfo::from(req.next_u8()); + let n_bytes = sequence_info.n_bits.div_ceil(8) as usize; + self.sequence(sequence_info, &req.data[..n_bytes], resp.remaining()); + + req.consume(n_bytes); + if sequence_info.capture { + resp.skip(n_bytes); + } + } + } + + /// Handle a single JTAG sequence. + /// + /// This function handles the individual JTAG sequences that are broken up by + /// the `sequences` function. It is called for each sequence in the request. + /// + /// For better performance, you may choose to provide an empty implementation + /// of this function and handle the sequences by overriding `sequences` instead. + fn sequence(&mut self, info: SequenceInfo, tdi: &[u8], rxbuf: &mut [u8]); + + /// Send out a sequence of TMS bits, while keeping TDI unchanged. + fn tms_sequence(&mut self, tms: &[bool]); /// Set the maximum clock frequency, return `true` if it is valid. fn set_clock(&mut self, max_frequency: u32) -> bool; - // TODO: What is missing for the 2 other JTAG commands + /// Shift out the instruction register (IR). + /// + /// This function starts from Test/Idle and ends in Test/Idle, after shifting out idle bits. + fn shift_ir(&mut self, ir: u32) { + self.tms_sequence(IDLE_TO_SHIFT_IR); + + let tap = self.config().current_tap(); + let ir_length = tap.ir_length; + let bypass_bits_before = tap.ir_before; + let bypass_bits_after = tap.ir_after; + + shift_repeated_tdi(self, 0xFF, bypass_bits_before, false); + + shift_register_data(self, ir, ir_length, bypass_bits_after == 0); + bypass_after_data(self, bypass_bits_after); + + self.tms_sequence(EXIT1_TO_IDLE); + } + + /// Shift out the data register (DR) and return the captured bits. + /// + /// This function starts from Test/Idle and ends in Test/Idle, after shifting out idle bits. + fn shift_dr(&mut self, dr: u32) -> u32 { + self.tms_sequence(IDLE_TO_SHIFT_DR); + + let device_index = self.config().index as usize; + let device_count = self.config().device_count as usize; + let bypass_bits_before = device_index as u16; + let bypass_bits_after = device_count as u16 - bypass_bits_before - 1; + + shift_repeated_tdi(self, 0xFF, bypass_bits_before, false); + + let dr = shift_register_data(self, dr, 32, bypass_bits_after == 0); + bypass_after_data(self, bypass_bits_after); + + self.tms_sequence(EXIT1_TO_IDLE); + + dr + } + + /// Shift out the data register (DR) for an ABORT command. + fn write_abort(&mut self, data: u32) { + transfer(self, RnW::W, 0, 8, data, false); + } + + /// Execute a JTAG DAP transfer. + /// + /// This function executes the data part of a DPACC or APACC scan, starting from Test/Idle + /// and ending in Test/Idle, after shifting out idle bits. + fn transfer( + &mut self, + r_nw: RnW, + a2a3: u8, + transfer_config: &TransferConfig, + data: u32, + ) -> TransferResult { + transfer(self, r_nw, a2a3, transfer_config.idle_cycles, data, true) + } +} + +fn transfer( + jtag: &mut impl Jtag, + r_nw: RnW, + a2a3: u8, + idle_cycles: u8, + data: u32, + check_ack: bool, +) -> TransferResult { + jtag.tms_sequence(IDLE_TO_SHIFT_DR); + + let device_index = jtag.config().index as usize; + let device_count = jtag.config().device_count as usize; + let bypass_bits_before = device_index as u16; + let bypass_bits_after = device_count as u16 - bypass_bits_before - 1; + + shift_repeated_tdi(jtag, 0xFF, bypass_bits_before, false); + + // Shift out the register address and read/write bits, read back ack. + let ack = shift_register_data(jtag, (a2a3 << 1) as u32 | (r_nw as u32), 3, false); + + // Based on ack, shift out data or stop the transfer. + let result = if ack == DAP_TRANSFER_OK_FAULT || !check_ack { + let captured_dr = shift_register_data(jtag, data, 32, bypass_bits_after == 0); + bypass_after_data(jtag, bypass_bits_after); + + jtag.tms_sequence(EXIT1_TO_IDLE); + + TransferResult::Ok(captured_dr) + } else { + jtag.tms_sequence(SHIFT_TO_IDLE); + TransferResult::Wait + }; + + shift_repeated_tdi(jtag, 0xFF, idle_cycles as u16, false); + + result +} + +/// Shift out data, while assuming to be in either Shift-DR or Shift-IR state. +/// +/// If `exit_shift` is true, it will exit the shift state (into Exit1-DR or Exit1-IR). +/// +/// The function will return the captured TDO data. +fn shift_register_data( + jtag: &mut impl Jtag, + mut data: u32, + clock_cycles: u8, + exit_shift: bool, +) -> u32 { + // All bits except last with TMS = 0 + let mut captured_dr = 0; + let mut clocks = clock_cycles; + while clocks > 1 { + let bits = (clocks - 1).min(8); + + let captured_byte = shift_tdi(jtag, data as u8, bits, false); + captured_dr >>= bits; + captured_dr |= (captured_byte as u32) << (clock_cycles - bits); + + data >>= bits; + clocks -= bits; + } + + // Last bit (with TMS = 1 if exit_shift) + let captured_byte = shift_tdi(jtag, data as u8, 1, exit_shift); + captured_dr >>= 1; + captured_dr |= (captured_byte as u32) << (clock_cycles - 1); + + captured_dr +} + +/// Shift out `clocks` bits of TDI data, with TMS set to the given value. +fn shift_repeated_tdi(jtag: &mut impl Jtag, tdi: u8, mut clocks: u16, tms: bool) { + while clocks > 0 { + let n = clocks.min(8); + clocks -= n; + + shift_tdi(jtag, tdi, n as u8, tms); + } +} + +/// Shift out `clocks` (at most 8) bits of TDI data, with TMS set to the given value. +/// +/// Returns the captured TDO data. +fn shift_tdi(jtag: &mut impl Jtag, tdi: u8, clocks: u8, tms: bool) -> u8 { + let mut tdo = 0; + jtag.sequence( + SequenceInfo { + n_bits: clocks as u8, + capture: true, + tms, + }, + &[tdi], + core::slice::from_mut(&mut tdo), + ); + tdo +} + +/// Send the bypass bits after data bits. +/// +/// Bypass bits are used to skip over TAPs in the JTAG chain. The TDI value is 1, while TMS is +/// driven to stay in Shift-DR or Shift-IR until the last bit, where TMS is driven to 1 to exit +/// the shift state. +fn bypass_after_data(jtag: &mut impl Jtag, bypass_after: u16) { + if bypass_after > 0 { + if bypass_after > 1 { + // Send the bypass bits after the DR. + shift_repeated_tdi(jtag, 0xFF, bypass_after.saturating_sub(1), false); + } + + // Send the last bypass bit with TMS = 1 + shift_repeated_tdi(jtag, 0xFF, 1, true); + } +} + +/// The result of a single DAP JTAG transfer. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub enum TransferResult { + /// The transfer was successful and this variant contains the captured data. + Ok(u32), + /// The device returned a wait response. + Wait, + /// The device returned a fault response. + Fault, + /// The device returned data that does not match what the transfer expects. + Mismatch, // TODO shouldn't be part of the public API. +} + +impl TransferResult { + pub(crate) fn status(&self) -> u8 { + match self { + TransferResult::Ok(_) => 0x1, + TransferResult::Wait => 0x2, + TransferResult::Fault => 0x8, + TransferResult::Mismatch => 0x10, + } + } } diff --git a/src/mock_device.rs b/src/mock_device.rs index ce380c1..cd8a3e3 100644 --- a/src/mock_device.rs +++ b/src/mock_device.rs @@ -9,13 +9,16 @@ pub trait SwdJtagDevice { fn process_swj_sequence(&mut self, data: &[u8], nbits: usize); // swd + fn swd_config(&mut self) -> &mut swd::Config; fn read_inner(&mut self, apndp: swd::APnDP, a: swd::DPRegister) -> swd::Result; fn write_inner(&mut self, apndp: swd::APnDP, a: swd::DPRegister, data: u32) -> swd::Result<()>; fn read_sequence(&mut self, num_bits: usize, data: &mut [u8]) -> swd::Result<()>; fn write_sequence(&mut self, num_bits: usize, data: &[u8]) -> swd::Result<()>; // jtag - fn sequences(&mut self, data: &[u8], rxbuf: &mut [u8]) -> u32; + fn jtag_config(&mut self) -> &mut jtag::Config; + fn sequence(&mut self, info: jtag::SequenceInfo, tdi: &[u8], resp: &mut [u8]); + fn tms_sequence(&mut self, tms: &[bool]); // swd/jtag fn set_clock(&mut self, max_frequency: u32) -> bool; @@ -37,6 +40,14 @@ impl swj::Dependencies for MockSwdJtagDevice { fn process_swj_sequence(&mut self, data: &[u8], nbits: usize) { SwdJtagDevice::process_swj_sequence(self, data, nbits) } + + fn jtag_config(&mut self) -> &mut jtag::Config { + SwdJtagDevice::jtag_config(self) + } + + fn swd_config(&mut self) -> &mut swd::Config { + SwdJtagDevice::swd_config(self) + } } impl swd::Swd for MockSwdJtagDevice { @@ -61,6 +72,10 @@ impl swd::Swd for MockSwdJtagDevice { fn write_sequence(&mut self, num_bits: usize, data: &[u8]) -> swd::Result<()> { SwdJtagDevice::write_sequence(self, num_bits, data) } + + fn config(&mut self) -> &mut swd::Config { + Self::swd_config(self) + } } impl jtag::Jtag for MockSwdJtagDevice { @@ -70,7 +85,15 @@ impl jtag::Jtag for MockSwdJtagDevice { SwdJtagDevice::set_clock(self, max_frequency) } - fn sequences(&mut self, data: &[u8], rxbuf: &mut [u8]) -> u32 { - SwdJtagDevice::sequences(self, data, rxbuf) + fn sequence(&mut self, info: jtag::SequenceInfo, tdi: &[u8], resp: &mut [u8]) { + SwdJtagDevice::sequence(self, info, tdi, resp) + } + + fn config(&mut self) -> &mut jtag::Config { + SwdJtagDevice::jtag_config(self) + } + + fn tms_sequence(&mut self, tms: &[bool]) { + SwdJtagDevice::tms_sequence(self, tms) } } diff --git a/src/swd.rs b/src/swd.rs index 3f1fa79..602c7da 100644 --- a/src/swd.rs +++ b/src/swd.rs @@ -100,11 +100,23 @@ pub enum DataPhase { AlwaysDataPhase = 1, } +/// The SWD interface configuration. +pub struct Config {} + +impl Default for Config { + fn default() -> Self { + Self {} + } +} + /// Definition of SWD communication. pub trait Swd: From { /// If SWD is available or not. const AVAILABLE: bool; + /// Returns a mutable reference to the SWD interface configuration. + fn config(&mut self) -> &mut Config; + /// Helper method over `read_inner` to retry during `AckWait`. fn read(&mut self, wait_retries: usize, apndp: APnDP, a: DPRegister) -> Result { for _ in 0..wait_retries { diff --git a/src/swj.rs b/src/swj.rs index c76bd96..0c0a937 100644 --- a/src/swj.rs +++ b/src/swj.rs @@ -1,5 +1,7 @@ use bitflags::bitflags; +use crate::{jtag, swd}; + bitflags! { /// Pin definitions in the SWJ_Pins command pub struct Pins: u8 { @@ -35,4 +37,10 @@ pub trait Dependencies: From + From { /// Set pins in high impedance mode fn high_impedance_mode(&mut self); + + /// Returns the SWD interface configuration. + fn swd_config(&mut self) -> &mut swd::Config; + + /// Returns the JTAG interface configuration. + fn jtag_config(&mut self) -> &mut jtag::Config; }