use jaylink::{CommunicationSpeed, JayLink};
use std::convert::TryInto;
use std::iter;
use std::sync::Mutex;
use crate::{
architecture::arm::dp::Ctrl,
architecture::arm::{DapError, PortType, Register},
probe::{
DAPAccess, DebugProbe, DebugProbeError, DebugProbeInfo, DebugProbeType, JTAGAccess,
WireProtocol,
},
};
#[derive(Debug)]
pub(crate) struct JLink {
handle: Mutex<JayLink>,
jtag_idle_cycles: u8,
protocol: Option<WireProtocol>,
current_ir_reg: u32,
speed_khz: u32,
}
impl JLink {
fn idle_cycles(&self) -> u8 {
self.jtag_idle_cycles
}
fn select_interface(
&mut self,
protocol: Option<WireProtocol>,
) -> Result<WireProtocol, DebugProbeError> {
let handle = self.handle.get_mut().unwrap();
let capabilities = handle.read_capabilities()?;
if capabilities.contains(jaylink::Capabilities::SELECT_IF) {
if let Some(protocol) = protocol {
let jlink_interface = match protocol {
WireProtocol::Swd => jaylink::Interface::Swd,
WireProtocol::Jtag => jaylink::Interface::Jtag,
};
if handle
.read_available_interfaces()?
.any(|interface| interface == jlink_interface)
{
handle.select_interface(jlink_interface)?;
Ok(protocol)
} else {
Err(DebugProbeError::UnsupportedProtocol(protocol))
}
} else {
let current_protocol = handle.read_current_interface()?;
match current_protocol {
jaylink::Interface::Swd => Ok(WireProtocol::Swd),
jaylink::Interface::Jtag => Ok(WireProtocol::Jtag),
x => unimplemented!("J-Link: Protocol {} is not yet supported.", x),
}
}
} else {
match protocol {
Some(WireProtocol::Jtag) => Ok(WireProtocol::Jtag),
Some(p) => Err(DebugProbeError::UnsupportedProtocol(p)),
None => Ok(WireProtocol::Jtag),
}
}
}
fn read_dr(&mut self, register_bits: usize) -> Result<Vec<u8>, DebugProbeError> {
log::debug!("Read {} bits from DR", register_bits);
let tms_enter_shift = [true, false, false];
let tms_shift_out_value = iter::repeat(false).take(register_bits - 1);
let tms_enter_idle = [true, true, false];
let mut tms = Vec::with_capacity(register_bits + 7);
tms.extend_from_slice(&tms_enter_shift);
tms.extend(tms_shift_out_value);
tms.extend_from_slice(&tms_enter_idle);
let tdi = iter::repeat(false).take(tms.len() + self.idle_cycles() as usize);
tms.extend(iter::repeat(false).take(self.idle_cycles() as usize));
let jlink = self.handle.get_mut().unwrap();
let mut response = jlink.jtag_io(tms, tdi)?;
log::trace!("Response: {:?}", response);
let _remainder = response.split_off(tms_enter_shift.len());
let mut remaining_bits = register_bits;
let mut result = Vec::new();
while remaining_bits >= 8 {
let byte = bits_to_byte(response.split_off(8)) as u8;
result.push(byte);
remaining_bits -= 8;
}
if remaining_bits > 0 {
result.push(bits_to_byte(response.split_off(remaining_bits)) as u8);
}
log::debug!("Read from DR: {:?}", result);
Ok(result)
}
fn write_ir(&mut self, data: &[u8], len: usize) -> Result<(), DebugProbeError> {
log::debug!("Write IR: {:?}, len={}", data, len);
if data.len() * 8 < len {
todo!("Proper error for incorrect length");
}
if len < 1 {
todo!("Proper error for incorrect length");
}
let tms_enter_ir_shift = [true, true, false, false];
let tms_data = iter::repeat(false).take(len - 1);
let tms_enter_idle = [true, true, false];
let mut tms = Vec::with_capacity(tms_enter_ir_shift.len() + len + tms_enter_ir_shift.len());
tms.extend_from_slice(&tms_enter_ir_shift);
tms.extend(tms_data);
tms.extend_from_slice(&tms_enter_idle);
let tdi_enter_ir_shift = [false, false, false, false];
let tdi_enter_idle = [false, false];
let mut tdi = Vec::with_capacity(tdi_enter_ir_shift.len() + tdi_enter_idle.len() + len);
tdi.extend_from_slice(&tdi_enter_ir_shift);
let num_bytes = len / 8;
let num_bits = len - (num_bytes * 8);
for bytes in &data[..num_bytes] {
let mut byte = *bytes;
for _ in 0..8 {
tdi.push(byte & 1 == 1);
byte >>= 1;
}
}
if num_bits > 0 {
let mut remaining_byte = data[num_bytes];
for _ in 0..num_bits {
tdi.push(remaining_byte & 1 == 1);
remaining_byte >>= 1;
}
}
tdi.extend_from_slice(&tms_enter_idle);
log::trace!("tms: {:?}", tms);
log::trace!("tdi: {:?}", tdi);
let jlink = self.handle.get_mut().unwrap();
let response = jlink.jtag_io(tms, tdi)?;
log::trace!("Response: {:?}", response);
assert!(
len < 8,
"Not yet implemented for IR registers larger than 8 bit"
);
self.current_ir_reg = data[0] as u32;
Ok(())
}
fn write_dr(&mut self, data: &[u8], register_bits: usize) -> Result<Vec<u8>, DebugProbeError> {
log::debug!("Write DR: {:?}, len={}", data, register_bits);
let tms_enter_shift = [true, false, false];
let tms_shift_out_value = iter::repeat(false).take(register_bits - 1);
let tms_enter_idle = [true, true, false];
let mut tms = Vec::with_capacity(register_bits + 7);
tms.extend_from_slice(&tms_enter_shift);
tms.extend(tms_shift_out_value);
tms.extend_from_slice(&tms_enter_idle);
let tdi_enter_shift = [false, false, false];
let tdi_enter_idle = [false, false];
let mut tdi =
Vec::with_capacity(tdi_enter_shift.len() + tdi_enter_idle.len() + register_bits);
tdi.extend_from_slice(&tdi_enter_shift);
let num_bytes = register_bits / 8;
let num_bits = register_bits - (num_bytes * 8);
for bytes in &data[..num_bytes] {
let mut byte = *bytes;
for _ in 0..8 {
tdi.push(byte & 1 == 1);
byte >>= 1;
}
}
if num_bits > 0 {
let mut remaining_byte = data[num_bytes];
for _ in 0..num_bits {
tdi.push(remaining_byte & 1 == 1);
remaining_byte >>= 1;
}
}
tdi.extend_from_slice(&tdi_enter_idle);
tms.extend(iter::repeat(false).take(self.idle_cycles() as usize));
tdi.extend(iter::repeat(false).take(self.idle_cycles() as usize));
let jlink = self.handle.get_mut().unwrap();
let mut response = jlink.jtag_io(tms, tdi)?;
log::trace!("Response: {:?}", response);
let _remainder = response.split_off(tms_enter_shift.len());
let mut remaining_bits = register_bits;
let mut result = Vec::new();
while remaining_bits >= 8 {
let byte = bits_to_byte(response.split_off(8)) as u8;
result.push(byte);
remaining_bits -= 8;
}
if remaining_bits > 0 {
result.push(bits_to_byte(response.split_off(remaining_bits)) as u8);
}
log::trace!("result: {:?}", result);
Ok(result)
}
}
impl DebugProbe for JLink {
fn new_from_probe_info(info: &super::DebugProbeInfo) -> Result<Box<Self>, DebugProbeError> {
let mut usb_devices: Vec<_> = jaylink::scan_usb()?
.filter(|usb_info| {
usb_info.vid() == info.vendor_id && usb_info.pid() == info.product_id
})
.collect();
if usb_devices.len() != 1 {
return Err(DebugProbeError::ProbeCouldNotBeCreated);
}
Ok(Box::new(JLink {
handle: Mutex::new(usb_devices.pop().unwrap().open()?),
jtag_idle_cycles: 0,
protocol: None,
current_ir_reg: 1,
speed_khz: 0,
}))
}
fn select_protocol(&mut self, protocol: WireProtocol) -> Result<(), DebugProbeError> {
let actual_protocol = self.select_interface(Some(protocol))?;
if actual_protocol == protocol {
self.protocol = Some(protocol);
Ok(())
} else {
self.protocol = Some(actual_protocol);
Err(DebugProbeError::UnsupportedProtocol(protocol))
}
}
fn get_name(&self) -> &'static str {
"J-Link"
}
fn speed(&self) -> u32 {
self.speed_khz
}
fn set_speed(&mut self, speed_khz: u32) -> Result<u32, DebugProbeError> {
if speed_khz == 0 || speed_khz >= 0xffff {
return Err(DebugProbeError::UnsupportedSpeed(speed_khz));
}
let jlink = self.handle.get_mut().unwrap();
let actual_speed_khz;
if let Ok(speeds) = jlink.read_speeds() {
log::debug!("Supported speeds: {:?}", speeds);
let speed_hz = 1000 * speed_khz;
let div = (speeds.base_freq() + speed_hz - 1) / speed_hz;
log::debug!("Divider: {}", div);
let div = std::cmp::max(div, speeds.min_div() as u32);
actual_speed_khz = ((speeds.base_freq() / div) + 999) / 1000;
assert!(actual_speed_khz <= speed_khz);
} else {
actual_speed_khz = speed_khz;
}
jlink.set_speed(CommunicationSpeed::khz(actual_speed_khz as u16).unwrap())?;
self.speed_khz = actual_speed_khz;
Ok(actual_speed_khz)
}
fn attach(&mut self) -> Result<(), super::DebugProbeError> {
let protocol = self.protocol.unwrap_or(WireProtocol::Jtag);
self.select_protocol(protocol)?;
log::debug!("Attaching with protocol '{:?}'", protocol);
match protocol {
WireProtocol::Jtag => {
let jlink = self.handle.get_mut().unwrap();
log::info!(
"Target voltage: {:2.2} V",
jlink.read_target_voltage()? as f32 / 1000f32
);
log::debug!("Resetting JTAG chain using trst");
jlink.reset_trst()?;
log::debug!("Resetting JTAG chain by setting tms high for 32 bits");
let tms = vec![true, true, true, true, true, false];
let tdi = iter::repeat(false).take(6);
let response: Vec<_> = jlink.jtag_io(tms, tdi)?.collect();
log::debug!("Response to reset: {:?}", response);
let idcode_bytes = self.read_dr(32)?;
let idcode = u32::from_le_bytes((&idcode_bytes[..]).try_into().unwrap());
log::debug!("IDCODE: {:#010x}", idcode);
}
WireProtocol::Swd => {
let jlink = self.handle.get_mut().unwrap();
let jtag_to_swd_sequence = [
false, true, true, true, true, false, false, true, true, true, true, false,
false, true, true, true,
];
let swd_io_sequence =
iter::repeat(true).take(64)
.chain(jtag_to_swd_sequence.iter().copied())
.chain(iter::repeat(true).take(64))
.chain(iter::repeat(false).take(10));
let direction =
iter::repeat(true).take(64)
.chain(iter::repeat(true).take(16))
.chain(iter::repeat(true).take(64))
.chain(iter::repeat(true).take(10));
jlink.swd_io(direction, swd_io_sequence)?;
log::debug!("Sucessfully swapped to SWD.");
}
}
Ok(())
}
fn detach(&mut self) -> Result<(), super::DebugProbeError> {
unimplemented!()
}
fn target_reset(&mut self) -> Result<(), super::DebugProbeError> {
unimplemented!()
}
fn dedicated_memory_interface(&self) -> Option<crate::Memory> {
None
}
fn get_interface_dap(&self) -> Option<&dyn DAPAccess> {
Some(self as _)
}
fn get_interface_dap_mut(&mut self) -> Option<&mut dyn DAPAccess> {
Some(self as _)
}
fn get_interface_jtag(&self) -> Option<&dyn JTAGAccess> {
Some(self as _)
}
fn get_interface_jtag_mut(&mut self) -> Option<&mut dyn JTAGAccess> {
Some(self as _)
}
}
impl JTAGAccess for JLink {
fn read_register(&mut self, address: u32, len: u32) -> Result<Vec<u8>, DebugProbeError> {
let address_bits = address.to_le_bytes();
assert!(
address <= 0x1f,
"JTAG Register addresses are fixed to 5 bits"
);
if self.current_ir_reg != address {
self.write_ir(&address_bits[..1], 5)?;
}
self.read_dr(len as usize)
}
fn write_register(
&mut self,
address: u32,
data: &[u8],
len: u32,
) -> Result<Vec<u8>, DebugProbeError> {
let address_bits = address.to_le_bytes();
assert!(
address <= 0x1f,
"JTAG Register addresses are fixed to 5 bits"
);
if self.current_ir_reg != address {
self.write_ir(&address_bits[..1], 5)?;
}
self.write_dr(data, len as usize)
}
fn set_idle_cycles(&mut self, idle_cycles: u8) {
self.jtag_idle_cycles = idle_cycles;
}
}
impl DAPAccess for JLink {
fn read_register(&mut self, port: PortType, address: u16) -> Result<u32, DebugProbeError> {
let port = match port {
PortType::DebugPort => false,
PortType::AccessPort(_) => true,
};
let a2 = (address >> 2) & 0x01 == 1;
let a3 = (address >> 3) & 0x01 == 1;
let mut swd_io_sequence = vec![
false,
false,
true,
port,
true,
a2,
a3,
port ^ true ^ a2 ^ a3,
false,
true,
false,
false,
false,
];
for _ in 0..32 {
swd_io_sequence.push(false);
}
swd_io_sequence.push(false);
swd_io_sequence.push(false);
let direction = iter::repeat(true)
.take(2)
.chain(iter::repeat(true).take(8))
.chain(iter::repeat(false).take(3))
.chain(iter::repeat(false).take(32))
.chain(iter::repeat(false).take(1))
.chain(iter::repeat(false).take(1));
let mut retries = 0;
while retries < 5 {
let mut result_sequence = self
.handle
.get_mut()
.unwrap()
.swd_io(direction.clone(), swd_io_sequence.iter().copied())?;
result_sequence.split_off(2);
result_sequence.split_off(8);
let ack = result_sequence.split_off(3).collect::<Vec<_>>();
if ack[1] {
retries += 1;
log::debug!("DAP line busy, retries remaining {}.", 5 - retries);
continue;
}
if ack[2] {
let response =
DAPAccess::read_register(self, PortType::DebugPort, Ctrl::ADDRESS as u16)?;
let ctrl = Ctrl::from(response);
log::error!(
"Reading DAP register failed. Ctrl/Stat register value is: {:#?}",
ctrl
);
return Err(DapError::FaultResponse.into());
}
if port {
return DAPAccess::read_register(self, PortType::DebugPort, 0x0C);
} else {
let register_val = result_sequence.split_off(32);
let value = bits_to_byte(register_val);
return if let Some(parity) = result_sequence.next() {
if (value.count_ones() % 2 == 1) == parity {
log::trace!("DAP read {}.", value);
Ok(value)
} else {
log::error!("DAP read fault.");
Err(DebugProbeError::Unknown)
}
} else {
log::error!("DAP read fault.");
Err(DebugProbeError::Unknown)
};
}
}
log::error!("DAP read timeout.");
Err(DebugProbeError::Timeout)
}
fn write_register(
&mut self,
port: PortType,
address: u16,
mut value: u32,
) -> Result<(), DebugProbeError> {
let port = match port {
PortType::DebugPort => false,
PortType::AccessPort(_) => true,
};
let a2 = (address >> 2) & 0x01 == 1;
let a3 = (address >> 3) & 0x01 == 1;
let mut swd_io_sequence = vec![
false,
false,
true,
port,
false,
a2,
a3,
port ^ false ^ a2 ^ a3,
false,
true,
false,
false,
false,
false,
false,
];
let mut parity = false;
for _ in 0..32 {
let bit = value & 1 == 1;
swd_io_sequence.push(bit);
parity ^= bit;
value >>= 1;
}
swd_io_sequence.push(parity);
let direction = iter::repeat(true)
.take(2)
.chain(iter::repeat(true).take(8))
.chain(iter::repeat(false).take(3))
.chain(iter::repeat(false).take(2))
.chain(iter::repeat(true).take(32))
.chain(iter::repeat(true).take(1));
let mut retries = 0;
while retries < 5 {
let mut result_sequence = self
.handle
.get_mut()
.unwrap()
.swd_io(direction.clone(), swd_io_sequence.iter().copied())?;
result_sequence.split_off(2);
result_sequence.split_off(8);
let ack = result_sequence.by_ref().take(3).collect::<Vec<_>>();
if ack[1] {
retries += 1;
log::debug!("DAP line busy, retries remaining {}.", 5 - retries);
continue;
}
if ack[2] {
let response =
DAPAccess::read_register(self, PortType::DebugPort, Ctrl::ADDRESS as u16)?;
let ctrl = Ctrl::from(response);
log::error!(
"Writing DAP register failed. Ctrl/Stat register value is: {:#?}",
ctrl
);
return Err(DebugProbeError::Unknown);
}
log::trace!("DAP wrote {}.", value);
return Ok(());
}
log::error!("DAP write timeout.");
Err(DebugProbeError::Timeout)
}
}
fn bits_to_byte(bits: impl IntoIterator<Item = bool>) -> u32 {
let mut bit_val = 0u32;
for (index, bit) in bits.into_iter().take(32).enumerate() {
if bit {
bit_val |= 1 << index;
}
}
bit_val
}
pub(crate) fn list_jlink_devices() -> Result<impl Iterator<Item = DebugProbeInfo>, DebugProbeError>
{
Ok(jaylink::scan_usb()?.map(|device_info| {
DebugProbeInfo::new(
format!(
"J-Link (VID: {:#06x}, PID: {:#06x})",
device_info.vid(),
device_info.pid()
),
device_info.vid(),
device_info.pid(),
None,
DebugProbeType::JLink,
)
}))
}
impl From<jaylink::Error> for DebugProbeError {
fn from(e: jaylink::Error) -> DebugProbeError {
DebugProbeError::ProbeSpecific(Box::new(e))
}
}