use super::{reset_catch_clear, reset_catch_set, CortexState, Dfsr, ARM_REGISTER_FILE};
use crate::core::{
Architecture, CoreInformation, CoreInterface, CoreRegister, CoreRegisterAddress,
RegisterDescription, RegisterFile, RegisterKind,
};
use crate::error::Error;
use crate::memory::Memory;
use crate::{CoreStatus, DebugProbeError, HaltReason, MemoryInterface};
use anyhow::{anyhow, Result};
use bitfield::bitfield;
use log::debug;
use std::{
mem::size_of,
time::{Duration, Instant},
};
bitfield! {
#[derive(Copy, Clone)]
pub struct Dhcsr(u32);
impl Debug;
pub s_reset_st, _: 25;
pub s_retire_st, _: 24;
pub s_lockup, _: 19;
pub s_sleep, _: 18;
pub s_halt, _: 17;
pub s_regrdy, _: 16;
pub c_maskings, set_c_maskints: 3;
pub c_step, set_c_step: 2;
pub c_halt, set_c_halt: 1;
pub c_debugen, set_c_debugen: 0;
}
impl Dhcsr {
pub fn enable_write(&mut self) {
self.0 &= !(0xffff << 16);
self.0 |= 0xa05f << 16;
}
}
impl From<u32> for Dhcsr {
fn from(value: u32) -> Self {
Self(value)
}
}
impl From<Dhcsr> for u32 {
fn from(value: Dhcsr) -> Self {
value.0
}
}
impl CoreRegister for Dhcsr {
const ADDRESS: u32 = 0xE000_EDF0;
const NAME: &'static str = "DHCSR";
}
bitfield! {
#[derive(Copy, Clone)]
pub struct Dcrsr(u32);
impl Debug;
pub _, set_regwnr: 16;
pub _, set_regsel: 4,0;
}
impl From<u32> for Dcrsr {
fn from(value: u32) -> Self {
Self(value)
}
}
impl From<Dcrsr> for u32 {
fn from(value: Dcrsr) -> Self {
value.0
}
}
impl CoreRegister for Dcrsr {
const ADDRESS: u32 = 0xE000_EDF4;
const NAME: &'static str = "DCRSR";
}
#[derive(Debug, Copy, Clone)]
pub struct Dcrdr(u32);
impl From<u32> for Dcrdr {
fn from(value: u32) -> Self {
Self(value)
}
}
impl From<Dcrdr> for u32 {
fn from(value: Dcrdr) -> Self {
value.0
}
}
impl CoreRegister for Dcrdr {
const ADDRESS: u32 = 0xE000_EDF8;
const NAME: &'static str = "DCRDR";
}
bitfield! {
#[derive(Copy, Clone)]
pub struct BpCtrl(u32);
impl Debug;
pub num_code, _: 7, 4;
pub key, set_key: 1;
pub _, set_enable: 0;
}
impl From<u32> for BpCtrl {
fn from(value: u32) -> Self {
Self(value)
}
}
impl From<BpCtrl> for u32 {
fn from(value: BpCtrl) -> Self {
value.0
}
}
impl CoreRegister for BpCtrl {
const ADDRESS: u32 = 0xE000_2000;
const NAME: &'static str = "BP_CTRL";
}
bitfield! {
#[derive(Copy, Clone)]
pub struct BpCompx(u32);
impl Debug;
pub _, set_bp_match: 31,30;
pub _, set_comp: 28,2;
pub _, set_enable: 0;
}
impl From<u32> for BpCompx {
fn from(value: u32) -> Self {
Self(value)
}
}
impl From<BpCompx> for u32 {
fn from(value: BpCompx) -> Self {
value.0
}
}
impl CoreRegister for BpCompx {
const ADDRESS: u32 = 0xE000_2008;
const NAME: &'static str = "BP_CTRL0";
}
bitfield! {
#[derive(Copy, Clone)]
pub struct Aircr(u32);
impl Debug;
pub get_vectkeystat, set_vectkey: 31,16;
pub endianness, set_endianness: 15;
pub sysresetreq, set_sysresetreq: 2;
pub vectclractive, set_vectclractive: 1;
}
impl From<u32> for Aircr {
fn from(value: u32) -> Self {
Self(value)
}
}
impl From<Aircr> for u32 {
fn from(value: Aircr) -> Self {
value.0
}
}
impl Aircr {
pub fn vectkey(&mut self) {
self.set_vectkey(0x05FA);
}
pub fn vectkeystat(&self) -> bool {
self.get_vectkeystat() == 0xFA05
}
}
impl CoreRegister for Aircr {
const ADDRESS: u32 = 0xE000_ED0C;
const NAME: &'static str = "AIRCR";
}
bitfield! {
#[derive(Copy, Clone)]
pub struct Demcr(u32);
impl Debug;
pub dwtena, set_dwtena: 24;
pub vc_harderr, set_vc_harderr: 10;
pub vc_corereset, set_vc_corereset: 0;
}
impl From<u32> for Demcr {
fn from(value: u32) -> Self {
Self(value)
}
}
impl From<Demcr> for u32 {
fn from(value: Demcr) -> Self {
value.0
}
}
impl CoreRegister for Demcr {
const ADDRESS: u32 = 0xe000_edfc;
const NAME: &'static str = "DEMCR";
}
pub const MSP: CoreRegisterAddress = CoreRegisterAddress(0b01001);
pub const PSP: CoreRegisterAddress = CoreRegisterAddress(0b01010);
const PC: RegisterDescription = RegisterDescription {
name: "PC",
kind: RegisterKind::PC,
address: CoreRegisterAddress(0b0_1111),
};
const XPSR: RegisterDescription = RegisterDescription {
name: "XPSR",
kind: RegisterKind::General,
address: CoreRegisterAddress(0b1_0000),
};
pub struct M0<'probe> {
memory: Memory<'probe>,
state: &'probe mut CortexState,
}
impl<'probe> M0<'probe> {
pub(crate) fn new(
mut memory: Memory<'probe>,
state: &'probe mut CortexState,
) -> Result<Self, Error> {
if !state.initialized() {
let dhcsr = Dhcsr(memory.read_word_32(Dhcsr::ADDRESS)?);
let core_state = if dhcsr.s_sleep() {
CoreStatus::Sleeping
} else if dhcsr.s_halt() {
log::debug!("Core was halted when connecting");
let dfsr = Dfsr(memory.read_word_32(Dfsr::ADDRESS)?);
let reason = dfsr.halt_reason();
CoreStatus::Halted(reason)
} else {
CoreStatus::Running
};
let dfsr_clear = Dfsr::clear_all();
memory.write_word_32(Dfsr::ADDRESS, dfsr_clear.into())?;
state.current_state = core_state;
state.initialize();
}
Ok(Self { memory, state })
}
fn wait_for_core_register_transfer(&mut self) -> Result<()> {
for _ in 0..100 {
let dhcsr_val = Dhcsr(self.memory.read_word_32(Dhcsr::ADDRESS)?);
if dhcsr_val.s_regrdy() {
return Ok(());
}
}
Err(anyhow!(Error::Probe(DebugProbeError::Timeout)))
}
}
impl<'probe> CoreInterface for M0<'probe> {
fn wait_for_core_halted(&mut self, timeout: Duration) -> Result<(), Error> {
let start = Instant::now();
while start.elapsed() < timeout {
let dhcsr_val = Dhcsr(self.memory.read_word_32(Dhcsr::ADDRESS)?);
if dhcsr_val.s_halt() {
return Ok(());
}
}
Err(Error::Probe(DebugProbeError::Timeout))
}
fn core_halted(&mut self) -> Result<bool, Error> {
let dhcsr_val = Dhcsr(self.memory.read_word_32(Dhcsr::ADDRESS)?);
if dhcsr_val.s_halt() {
Ok(true)
} else {
Ok(false)
}
}
fn read_core_reg(&mut self, addr: CoreRegisterAddress) -> Result<u32, Error> {
let mut dcrsr_val = Dcrsr(0);
dcrsr_val.set_regwnr(false);
dcrsr_val.set_regsel(addr.into());
self.memory
.write_word_32(Dcrsr::ADDRESS, dcrsr_val.into())?;
self.wait_for_core_register_transfer()?;
self.memory.read_word_32(Dcrdr::ADDRESS).map_err(From::from)
}
fn write_core_reg(&mut self, addr: CoreRegisterAddress, value: u32) -> Result<()> {
let result: Result<(), Error> = self
.memory
.write_word_32(Dcrdr::ADDRESS, value)
.map_err(From::from);
result?;
let mut dcrsr_val = Dcrsr(0);
dcrsr_val.set_regwnr(true);
dcrsr_val.set_regsel(addr.into());
self.memory
.write_word_32(Dcrsr::ADDRESS, dcrsr_val.into())?;
self.wait_for_core_register_transfer()
}
fn halt(&mut self, timeout: Duration) -> Result<CoreInformation, Error> {
let mut value = Dhcsr(0);
value.set_c_halt(true);
value.set_c_debugen(true);
value.enable_write();
self.memory.write_word_32(Dhcsr::ADDRESS, value.into())?;
self.wait_for_core_halted(timeout)?;
let pc_value = self.read_core_reg(PC.address)?;
Ok(CoreInformation { pc: pc_value })
}
fn run(&mut self) -> Result<(), Error> {
let mut value = Dhcsr(0);
value.set_c_halt(false);
value.set_c_debugen(true);
value.enable_write();
self.memory.write_word_32(Dhcsr::ADDRESS, value.into())?;
self.memory.flush()
}
fn step(&mut self) -> Result<CoreInformation, Error> {
let mut value = Dhcsr(0);
value.set_c_step(true);
value.set_c_halt(false);
value.set_c_debugen(true);
value.set_c_maskints(true);
value.enable_write();
self.memory.write_word_32(Dhcsr::ADDRESS, value.into())?;
self.wait_for_core_halted(Duration::from_millis(100))?;
let pc_value = self.read_core_reg(PC.address)?;
Ok(CoreInformation { pc: pc_value })
}
fn reset(&mut self) -> Result<(), Error> {
let mut value = Aircr(0);
value.vectkey();
value.set_sysresetreq(true);
self.memory.write_word_32(Aircr::ADDRESS, value.into())?;
Ok(())
}
fn reset_and_halt(&mut self, timeout: Duration) -> Result<CoreInformation, Error> {
reset_catch_set(self)?;
self.reset()?;
self.wait_for_core_halted(timeout)?;
const XPSR_THUMB: u32 = 1 << 24;
let xpsr_value = self.read_core_reg(XPSR.address)?;
if xpsr_value & XPSR_THUMB == 0 {
self.write_core_reg(XPSR.address, xpsr_value | XPSR_THUMB)?;
}
reset_catch_clear(self)?;
let pc_value = self.read_core_reg(PC.address)?;
Ok(CoreInformation { pc: pc_value })
}
fn get_available_breakpoint_units(&mut self) -> Result<u32, Error> {
let result = self.memory.read_word_32(BpCtrl::ADDRESS)?;
let register = BpCtrl::from(result);
Ok(register.num_code())
}
fn enable_breakpoints(&mut self, state: bool) -> Result<(), Error> {
debug!("Enabling breakpoints: {:?}", state);
let mut value = BpCtrl(0);
value.set_key(true);
value.set_enable(state);
self.memory.write_word_32(BpCtrl::ADDRESS, value.into())?;
self.state.hw_breakpoints_enabled = true;
Ok(())
}
fn set_breakpoint(&mut self, bp_register_index: usize, addr: u32) -> Result<(), Error> {
debug!("Setting breakpoint on address 0x{:08x}", addr);
let mut value = BpCompx(0);
value.set_bp_match(0b11);
value.set_comp((addr >> 2) & 0x00FF_FFFF);
value.set_enable(true);
let register_addr = BpCompx::ADDRESS + (bp_register_index * size_of::<u32>()) as u32;
self.memory.write_word_32(register_addr, value.into())?;
Ok(())
}
fn registers(&self) -> &'static RegisterFile {
&ARM_REGISTER_FILE
}
fn clear_breakpoint(&mut self, bp_unit_index: usize) -> Result<(), Error> {
let register_addr = BpCompx::ADDRESS + (bp_unit_index * size_of::<u32>()) as u32;
let mut value = BpCompx::from(0);
value.set_enable(false);
self.memory.write_word_32(register_addr, value.into())?;
Ok(())
}
fn hw_breakpoints_enabled(&self) -> bool {
self.state.hw_breakpoints_enabled
}
fn architecture(&self) -> Architecture {
Architecture::Arm
}
fn status(&mut self) -> Result<crate::core::CoreStatus, Error> {
let dhcsr = Dhcsr(self.memory.read_word_32(Dhcsr::ADDRESS)?);
if dhcsr.s_sleep() {
if self.state.current_state.is_halted() {
log::warn!("Expected core to be halted, but core is running");
}
self.state.current_state = CoreStatus::Sleeping;
return Ok(CoreStatus::Sleeping);
}
if dhcsr.s_halt() {
let dfsr = Dfsr(self.memory.read_word_32(Dfsr::ADDRESS)?);
let reason = dfsr.halt_reason();
self.memory
.write_word_32(Dfsr::ADDRESS, Dfsr::clear_all().into())?;
if self.state.current_state.is_halted() {
if reason == HaltReason::Unknown {
return Ok(self.state.current_state);
}
log::debug!(
"Reason for halt has changed, old reason was {:?}, new reason is {:?}",
&self.state.current_state,
&reason
);
}
self.state.current_state = CoreStatus::Halted(reason);
return Ok(CoreStatus::Halted(reason));
}
if self.state.current_state.is_halted() {
log::warn!("Core is running, but we expected it to be halted");
}
self.state.current_state = CoreStatus::Running;
Ok(CoreStatus::Running)
}
}
impl<'probe> MemoryInterface for M0<'probe> {
fn read_word_32(&mut self, address: u32) -> Result<u32, Error> {
self.memory.read_word_32(address)
}
fn read_word_8(&mut self, address: u32) -> Result<u8, Error> {
self.memory.read_word_8(address)
}
fn read_32(&mut self, address: u32, data: &mut [u32]) -> Result<(), Error> {
self.memory.read_32(address, data)
}
fn read_8(&mut self, address: u32, data: &mut [u8]) -> Result<(), Error> {
self.memory.read_8(address, data)
}
fn write_word_32(&mut self, address: u32, data: u32) -> Result<(), Error> {
self.memory.write_word_32(address, data)
}
fn write_word_8(&mut self, address: u32, data: u8) -> Result<(), Error> {
self.memory.write_word_8(address, data)
}
fn write_32(&mut self, address: u32, data: &[u32]) -> Result<(), Error> {
self.memory.write_32(address, data)
}
fn write_8(&mut self, address: u32, data: &[u8]) -> Result<(), Error> {
self.memory.write_8(address, data)
}
fn flush(&mut self) -> Result<(), Error> {
self.memory.flush()
}
}