DRTIO port - satman firmware #136
Merged
sb10q
merged 6 commits from mwojcik/artiq-zynq:drtio_satman
into master
1 year ago
@ -1,21 +1,6 @@ |
||||
use std::env; |
||||
use std::fs::File; |
||||
use std::io::Write; |
||||
use std::path::PathBuf; |
||||
|
||||
extern crate build_zynq; |
||||
|
||||
fn main() { |
||||
// Put the linker script somewhere the linker can find it
|
||||
let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap()); |
||||
File::create(out.join("link.x")) |
||||
.unwrap() |
||||
.write_all(include_bytes!("link.x")) |
||||
.unwrap(); |
||||
println!("cargo:rustc-link-search={}", out.display()); |
||||
|
||||
// Only re-run the build script when link.x is changed,
|
||||
// instead of when any part of the source code changes.
|
||||
println!("cargo:rerun-if-changed=link.x"); |
||||
build_zynq::add_linker_script(); |
||||
build_zynq::cfg(); |
||||
} |
||||
|
@ -0,0 +1,28 @@ |
||||
[package] |
||||
authors = ["M-Labs"] |
||||
name = "satman" |
||||
version = "0.0.0" |
||||
build = "build.rs" |
||||
|
||||
[features] |
||||
target_zc706 = ["libboard_zynq/target_zc706", "libsupport_zynq/target_zc706", "libconfig/target_zc706", "libboard_artiq/target_zc706"] |
||||
target_kasli_soc = ["libboard_zynq/target_kasli_soc", "libsupport_zynq/target_kasli_soc", "libconfig/target_kasli_soc", "libboard_artiq/target_kasli_soc"] |
||||
default = ["target_zc706", ] |
||||
|
||||
[build-dependencies] |
||||
build_zynq = { path = "../libbuild_zynq" } |
||||
|
||||
[dependencies] |
||||
log = { version = "0.4", default-features = false } |
||||
embedded-hal = "0.2" |
||||
|
||||
libboard_zynq = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git", features = ["ipv6"]} |
||||
libsupport_zynq = { default-features = false, features = ["alloc_core"], git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" } |
||||
libcortex_a9 = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" } |
||||
libasync = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" } |
||||
libregister = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git" } |
||||
libconfig = { git = "https://git.m-labs.hk/M-Labs/zynq-rs.git", features = ["ipv6"] } |
||||
|
||||
libboard_artiq = { path = "../libboard_artiq" } |
||||
unwind = { path = "../libunwind" } |
||||
libc = { path = "../libc" } |
@ -0,0 +1,6 @@ |
||||
extern crate build_zynq; |
||||
|
||||
fn main() { |
||||
build_zynq::add_linker_script(); |
||||
build_zynq::cfg(); |
||||
} |
@ -0,0 +1,626 @@ |
||||
#![no_std] |
||||
#![no_main] |
||||
#![feature(never_type, panic_info_message, asm, naked_functions)] |
||||
#![feature(alloc_error_handler)] |
||||
|
||||
#[macro_use] |
||||
extern crate log; |
||||
|
||||
extern crate embedded_hal; |
||||
|
||||
extern crate libboard_zynq; |
||||
extern crate libboard_artiq; |
||||
extern crate libsupport_zynq; |
||||
extern crate libcortex_a9; |
||||
extern crate libregister; |
||||
|
||||
extern crate unwind; |
||||
|
||||
extern crate alloc; |
||||
|
||||
use libboard_zynq::{i2c::I2c, timer::GlobalTimer, time::Milliseconds, print, println, mpcore, gic, stdio}; |
||||
use libsupport_zynq::ram; |
||||
#[cfg(has_si5324)] |
||||
use libboard_artiq::si5324; |
||||
use libboard_artiq::{pl::csr, drtio_routing, drtioaux, logger, identifier_read, init_gateware}; |
||||
use libcortex_a9::{spin_lock_yield, interrupt_handler, regs::{MPIDR, SP}, notify_spin_lock, asm, l2c::enable_l2_cache}; |
||||
use libregister::{RegisterW, RegisterR}; |
||||
|
||||
use embedded_hal::blocking::delay::DelayUs; |
||||
use core::sync::atomic::{AtomicBool, Ordering}; |
||||
|
||||
mod repeater; |
||||
|
||||
fn drtiosat_reset(reset: bool) { |
||||
unsafe { |
||||
csr::drtiosat::reset_write(if reset { 1 } else { 0 }); |
||||
} |
||||
} |
||||
|
||||
fn drtiosat_reset_phy(reset: bool) { |
||||
unsafe { |
||||
csr::drtiosat::reset_phy_write(if reset { 1 } else { 0 }); |
||||
} |
||||
} |
||||
|
||||
fn drtiosat_link_rx_up() -> bool { |
||||
unsafe { |
||||
csr::drtiosat::rx_up_read() == 1 |
||||
} |
||||
} |
||||
|
||||
fn drtiosat_tsc_loaded() -> bool { |
||||
unsafe { |
||||
let tsc_loaded = csr::drtiosat::tsc_loaded_read() == 1; |
||||
if tsc_loaded { |
||||
csr::drtiosat::tsc_loaded_write(1); |
||||
} |
||||
tsc_loaded |
||||
} |
||||
} |
||||
|
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
macro_rules! forward { |
||||
($routing_table:expr, $destination:expr, $rank:expr, $repeaters:expr, $packet:expr, $timer:expr) => {{ |
||||
let hop = $routing_table.0[$destination as usize][$rank as usize]; |
||||
if hop != 0 { |
||||
let repno = (hop - 1) as usize; |
||||
if repno < $repeaters.len() { |
||||
return $repeaters[repno].aux_forward($packet, $timer); |
||||
} else { |
||||
return Err(drtioaux::Error::RoutingError); |
||||
} |
||||
} |
||||
}} |
||||
} |
||||
|
||||
#[cfg(not(has_drtio_routing))] |
||||
macro_rules! forward { |
||||
($routing_table:expr, $destination:expr, $rank:expr, $repeaters:expr, $packet:expr, $timer:expr) => {} |
||||
} |
||||
|
||||
fn process_aux_packet(_repeaters: &mut [repeater::Repeater], |
||||
_routing_table: &mut drtio_routing::RoutingTable, _rank: &mut u8, |
||||
packet: drtioaux::Packet, timer: &mut GlobalTimer, i2c: &mut I2c) -> Result<(), drtioaux::Error> { |
||||
// In the code below, *_chan_sel_write takes an u8 if there are fewer than 256 channels,
|
||||
// and u16 otherwise; hence the `as _` conversion.
|
||||
match packet { |
||||
drtioaux::Packet::EchoRequest => |
||||
drtioaux::send(0, &drtioaux::Packet::EchoReply), |
||||
drtioaux::Packet::ResetRequest => { |
||||
info!("resetting RTIO"); |
||||
drtiosat_reset(true); |
||||
timer.delay_us(100); |
||||
drtiosat_reset(false); |
||||
for rep in _repeaters.iter() { |
||||
if let Err(e) = rep.rtio_reset(timer) { |
||||
error!("failed to issue RTIO reset ({:?})", e); |
||||
} |
||||
} |
||||
drtioaux::send(0, &drtioaux::Packet::ResetAck) |
||||
}, |
||||
|
||||
drtioaux::Packet::DestinationStatusRequest { destination: _destination } => { |
||||
#[cfg(has_drtio_routing)] |
||||
let hop = _routing_table.0[_destination as usize][*_rank as usize]; |
||||
#[cfg(not(has_drtio_routing))] |
||||
let hop = 0; |
||||
|
||||
if hop == 0 { |
||||
let errors; |
||||
unsafe { |
||||
errors = csr::drtiosat::rtio_error_read(); |
||||
} |
||||
if errors & 1 != 0 { |
||||
let channel; |
||||
unsafe { |
||||
channel = csr::drtiosat::sequence_error_channel_read(); |
||||
csr::drtiosat::rtio_error_write(1); |
||||
} |
||||
drtioaux::send(0, |
||||
&drtioaux::Packet::DestinationSequenceErrorReply { channel })?; |
||||
} else if errors & 2 != 0 { |
||||
let channel; |
||||
unsafe { |
||||
channel = csr::drtiosat::collision_channel_read(); |
||||
csr::drtiosat::rtio_error_write(2); |
||||
} |
||||
drtioaux::send(0, |
||||
&drtioaux::Packet::DestinationCollisionReply { channel })?; |
||||
} else if errors & 4 != 0 { |
||||
let channel; |
||||
unsafe { |
||||
channel = csr::drtiosat::busy_channel_read(); |
||||
csr::drtiosat::rtio_error_write(4); |
||||
} |
||||
drtioaux::send(0, |
||||
&drtioaux::Packet::DestinationBusyReply { channel })?; |
||||
} |
||||
else { |
||||
drtioaux::send(0, &drtioaux::Packet::DestinationOkReply)?; |
||||
} |
||||
} |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
{ |
||||
if hop != 0 { |
||||
let hop = hop as usize; |
||||
if hop <= csr::DRTIOREP.len() { |
||||
let repno = hop - 1; |
||||
match _repeaters[repno].aux_forward(&drtioaux::Packet::DestinationStatusRequest { |
||||
destination: _destination |
||||
}, timer) { |
||||
Ok(()) => (), |
||||
Err(drtioaux::Error::LinkDown) => drtioaux::send(0, &drtioaux::Packet::DestinationDownReply)?, |
||||
Err(e) => { |
||||
drtioaux::send(0, &drtioaux::Packet::DestinationDownReply)?; |
||||
error!("aux error when handling destination status request: {:?}", e); |
||||
}, |
||||
} |
||||
} else { |
||||
drtioaux::send(0, &drtioaux::Packet::DestinationDownReply)?; |
||||
} |
||||
} |
||||
} |
||||
|
||||
Ok(()) |
||||
} |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
drtioaux::Packet::RoutingSetPath { destination, hops } => { |
||||
_routing_table.0[destination as usize] = hops; |
||||
for rep in _repeaters.iter() { |
||||
if let Err(e) = rep.set_path(destination, &hops, timer) { |
||||
error!("failed to set path ({:?})", e); |
||||
} |
||||
} |
||||
drtioaux::send(0, &drtioaux::Packet::RoutingAck) |
||||
} |
||||
#[cfg(has_drtio_routing)] |
||||
drtioaux::Packet::RoutingSetRank { rank } => { |
||||
*_rank = rank; |
||||
drtio_routing::interconnect_enable_all(_routing_table, rank); |
||||
|
||||
let rep_rank = rank + 1; |
||||
for rep in _repeaters.iter() { |
||||
if let Err(e) = rep.set_rank(rep_rank, timer) { |
||||
error!("failed to set rank ({:?})", e); |
||||
} |
||||
} |
||||
|
||||
info!("rank: {}", rank); |
||||
info!("routing table: {}", _routing_table); |
||||
|
||||
drtioaux::send(0, &drtioaux::Packet::RoutingAck) |
||||
} |
||||
|
||||
#[cfg(not(has_drtio_routing))] |
||||
drtioaux::Packet::RoutingSetPath { destination: _, hops: _ } => { |
||||
drtioaux::send(0, &drtioaux::Packet::RoutingAck) |
||||
} |
||||
#[cfg(not(has_drtio_routing))] |
||||
drtioaux::Packet::RoutingSetRank { rank: _ } => { |
||||
drtioaux::send(0, &drtioaux::Packet::RoutingAck) |
||||
} |
||||
|
||||
drtioaux::Packet::MonitorRequest { destination: _destination, channel: _channel, probe: _probe } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
let value; |
||||
#[cfg(has_rtio_moninj)] |
||||
unsafe { |
||||
csr::rtio_moninj::mon_chan_sel_write(channel as _); |
||||
csr::rtio_moninj::mon_probe_sel_write(probe); |
||||
csr::rtio_moninj::mon_value_update_write(1); |
||||
value = csr::rtio_moninj::mon_value_read(); |
||||
} |
||||
#[cfg(not(has_rtio_moninj))] |
||||
{ |
||||
value = 0; |
||||
} |
||||
let reply = drtioaux::Packet::MonitorReply { value: value as u32 }; |
||||
drtioaux::send(0, &reply) |
||||
}, |
||||
drtioaux::Packet::InjectionRequest { destination: _destination, channel: _channel,
|
||||
overrd: _overrd, value: _value } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
#[cfg(has_rtio_moninj)] |
||||
unsafe { |
||||
csr::rtio_moninj::inj_chan_sel_write(channel as _); |
||||
csr::rtio_moninj::inj_override_sel_write(overrd); |
||||
csr::rtio_moninj::inj_value_write(value); |
||||
} |
||||
Ok(()) |
||||
}, |
||||
drtioaux::Packet::InjectionStatusRequest { destination: _destination,
|
||||
channel: _channel, overrd: _overrd } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
let value; |
||||
#[cfg(has_rtio_moninj)] |
||||
unsafe { |
||||
csr::rtio_moninj::inj_chan_sel_write(channel as _); |
||||
csr::rtio_moninj::inj_override_sel_write(overrd); |
||||
value = csr::rtio_moninj::inj_value_read(); |
||||
} |
||||
#[cfg(not(has_rtio_moninj))] |
||||
{ |
||||
value = 0; |
||||
} |
||||
drtioaux::send(0, &drtioaux::Packet::InjectionStatusReply { value: value }) |
||||
}, |
||||
|
||||
drtioaux::Packet::I2cStartRequest { destination: _destination, busno: _busno } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
let succeeded = i2c.start().is_ok(); |
||||
drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded }) |
||||
} |
||||
drtioaux::Packet::I2cRestartRequest { destination: _destination, busno: _busno } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
let succeeded = i2c.restart().is_ok(); |
||||
drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded }) |
||||
} |
||||
drtioaux::Packet::I2cStopRequest { destination: _destination, busno: _busno } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
let succeeded = i2c.stop().is_ok(); |
||||
drtioaux::send(0, &drtioaux::Packet::I2cBasicReply { succeeded: succeeded }) |
||||
} |
||||
drtioaux::Packet::I2cWriteRequest { destination: _destination, busno: _busno, data } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
match i2c.write(data) { |
||||
Ok(ack) => drtioaux::send(0, |
||||
&drtioaux::Packet::I2cWriteReply { succeeded: true, ack: ack }), |
||||
Err(_) => drtioaux::send(0, |
||||
&drtioaux::Packet::I2cWriteReply { succeeded: false, ack: false }) |
||||
} |
||||
} |
||||
drtioaux::Packet::I2cReadRequest { destination: _destination, busno: _busno, ack } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
match i2c.read(ack) { |
||||
Ok(data) => drtioaux::send(0, |
||||
&drtioaux::Packet::I2cReadReply { succeeded: true, data: data }), |
||||
Err(_) => drtioaux::send(0, |
||||
&drtioaux::Packet::I2cReadReply { succeeded: false, data: 0xff }) |
||||
} |
||||
} |
||||
|
||||
drtioaux::Packet::SpiSetConfigRequest { destination: _destination, busno: _busno,
|
||||
flags: _flags, length: _length, div: _div, cs: _cs } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
// todo: reimplement when/if SPI is available
|
||||
//let succeeded = spi::set_config(busno, flags, length, div, cs).is_ok();
|
||||
drtioaux::send(0, |
||||
&drtioaux::Packet::SpiBasicReply { succeeded: false }) |
||||
}, |
||||
drtioaux::Packet::SpiWriteRequest { destination: _destination, busno: _busno, data: _data } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
// todo: reimplement when/if SPI is available
|
||||
//let succeeded = spi::write(busno, data).is_ok();
|
||||
drtioaux::send(0, |
||||
&drtioaux::Packet::SpiBasicReply { succeeded: false }) |
||||
} |
||||
drtioaux::Packet::SpiReadRequest { destination: _destination, busno: _busno } => { |
||||
forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); |
||||
// todo: reimplement when/if SPI is available
|
||||
// match spi::read(busno) {
|
||||
// Ok(data) => drtioaux::send(0,
|
||||
// &drtioaux::Packet::SpiReadReply { succeeded: true, data: data }),
|
||||
// Err(_) => drtioaux::send(0,
|
||||
// &drtioaux::Packet::SpiReadReply { succeeded: false, data: 0 })
|
||||
// }
|
||||
drtioaux::send(0, |
||||
&drtioaux::Packet::SpiReadReply { succeeded: false, data: 0 }) |
||||
} |
||||
|
||||
_ => { |
||||
|
||||
warn!("received unexpected aux packet"); |
||||
Ok(()) |
||||
} |
||||
} |
||||
} |
||||
|
||||
fn process_aux_packets(repeaters: &mut [repeater::Repeater], |
||||
routing_table: &mut drtio_routing::RoutingTable, rank: &mut u8,
|
||||
timer: &mut GlobalTimer, i2c: &mut I2c) { |
||||
let result = |
||||
drtioaux::recv(0).and_then(|packet| { |
||||
if let Some(packet) = packet { |
||||
process_aux_packet(repeaters, routing_table, rank, packet, timer, i2c) |
||||
} else { |
||||
Ok(()) |
||||
} |
||||
}); |
||||
match result { |
||||
Ok(()) => (), |
||||
Err(e) => warn!("aux packet error ({:?})", e) |
||||
} |
||||
} |
||||
|
||||
fn drtiosat_process_errors() { |
||||
let errors; |
||||
unsafe { |
||||
errors = csr::drtiosat::protocol_error_read(); |
||||
} |
||||
if errors & 1 != 0 { |
||||
error!("received packet of an unknown type"); |
||||
} |
||||
if errors & 2 != 0 { |
||||
error!("received truncated packet"); |
||||
} |
||||
if errors & 4 != 0 { |
||||
let destination; |
||||
unsafe { |
||||
destination = csr::drtiosat::buffer_space_timeout_dest_read(); |
||||
} |
||||
error!("timeout attempting to get buffer space from CRI, destination=0x{:02x}", destination) |
||||
} |
||||
if errors & 8 != 0 { |
||||
let channel; |
||||
let timestamp_event; |
||||
let timestamp_counter; |
||||
unsafe { |
||||
channel = csr::drtiosat::underflow_channel_read(); |
||||
timestamp_event = csr::drtiosat::underflow_timestamp_event_read() as i64; |
||||
timestamp_counter = csr::drtiosat::underflow_timestamp_counter_read() as i64; |
||||
} |
||||
error!("write underflow, channel={}, timestamp={}, counter={}, slack={}", |
||||
channel, timestamp_event, timestamp_counter, timestamp_event-timestamp_counter); |
||||
} |
||||
if errors & 16 != 0 { |
||||
error!("write overflow"); |
||||
} |
||||
unsafe { |
||||
csr::drtiosat::protocol_error_write(errors); |
||||
} |
||||
} |
||||
|
||||
|
||||
#[cfg(has_rtio_crg)] |
||||
fn init_rtio_crg(timer: GlobalTimer) { |
||||
unsafe { |
||||
csr::rtio_crg::pll_reset_write(0); |
||||
} |
||||
timer.delay_us(150); |
||||
let locked = unsafe { csr::rtio_crg::pll_locked_read() != 0 }; |
||||
if !locked { |
||||
error!("RTIO clock failed"); |
||||
} |
||||
} |
||||
|
||||
#[cfg(not(has_rtio_crg))] |
||||
fn init_rtio_crg(_timer: GlobalTimer) { } |
||||
|
||||
fn hardware_tick(ts: &mut u64, timer: &mut GlobalTimer) { |
||||
let now = timer.get_time(); |
||||
let mut ts_ms = Milliseconds(*ts); |
||||
if now > ts_ms { |
||||
ts_ms = now + Milliseconds(200); |
||||
*ts = ts_ms.0; |
||||
} |
||||
} |
||||
|
||||
#[cfg(has_si5324)] |
||||
const SI5324_SETTINGS: si5324::FrequencySettings |
||||
= si5324::FrequencySettings { |
||||
n1_hs : 5, |
||||
nc1_ls : 8, |
||||
n2_hs : 7, |
||||
n2_ls : 360, |
||||
n31 : 63, |
||||
n32 : 63, |
||||
bwsel : 4, |
||||
crystal_ref: true |
||||
}; |
||||
|
||||
static mut LOG_BUFFER: [u8; 1<<17] = [0; 1<<17]; |
||||
|
||||
#[no_mangle] |
||||
pub extern fn main_core0() -> i32 { |
||||
enable_l2_cache(0x8); |
||||
|
||||
let mut timer = GlobalTimer::start(); |
||||
|
||||
let buffer_logger = unsafe { |
||||
logger::BufferLogger::new(&mut LOG_BUFFER[..]) |
||||
}; |
||||
buffer_logger.set_uart_log_level(log::LevelFilter::Info); |
||||
buffer_logger.register(); |
||||
log::set_max_level(log::LevelFilter::Info); |
||||
|
||||
init_gateware(); |
||||
|
||||
info!("ARTIQ satellite manager starting..."); |
||||
info!("gateware ident {}", identifier_read(&mut [0; 64])); |
||||
|
||||
ram::init_alloc_core0(); |
||||
|
||||
let mut i2c = I2c::i2c0(); |
||||
i2c.init().expect("I2C initialization failed"); |
||||
|
||||
#[cfg(has_si5324)] |
||||
si5324::setup(&mut i2c, &SI5324_SETTINGS, si5324::Input::Ckin1, &mut timer).expect("cannot initialize Si5324"); |
||||
|
||||
unsafe { |
||||
csr::drtio_transceiver::stable_clkin_write(1); |
||||
} |
||||
timer.delay_us(1500); // wait for CPLL/QPLL lock
|
||||
|
||||
unsafe { |
||||
csr::drtio_transceiver::txenable_write(0xffffffffu32 as _); |
||||
} |
||||
init_rtio_crg(timer); |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()]; |
||||
#[cfg(not(has_drtio_routing))] |
||||
let mut repeaters = [repeater::Repeater::default(); 0]; |
||||
for i in 0..repeaters.len() { |
||||
repeaters[i] = repeater::Repeater::new(i as u8); |
||||
}
|
||||
let mut routing_table = drtio_routing::RoutingTable::default_empty(); |
||||
let mut rank = 1; |
||||
|
||||
let mut hardware_tick_ts = 0; |
||||
|
||||
loop { |
||||
while !drtiosat_link_rx_up() { |
||||
drtiosat_process_errors(); |
||||
#[allow(unused_mut)] |
||||
for mut rep in repeaters.iter_mut() { |
||||
rep.service(&routing_table, rank, &mut timer); |
||||
} |
||||
hardware_tick(&mut hardware_tick_ts, &mut timer); |
||||
} |
||||
|
||||
info!("uplink is up, switching to recovered clock"); |
||||
#[cfg(has_siphaser)] |
||||
{ |
||||
si5324::siphaser::select_recovered_clock(&mut i2c, true, &mut timer).expect("failed to switch clocks"); |
||||
si5324::siphaser::calibrate_skew(&mut timer).expect("failed to calibrate skew"); |
||||
} |
||||
|
||||
drtioaux::reset(0); |
||||
drtiosat_reset(false); |
||||
drtiosat_reset_phy(false); |
||||
|
||||
while drtiosat_link_rx_up() { |
||||
drtiosat_process_errors(); |
||||
process_aux_packets(&mut repeaters, &mut routing_table, &mut rank, &mut timer, &mut i2c); |
||||
#[allow(unused_mut)] |
||||
for mut rep in repeaters.iter_mut() { |
||||
rep.service(&routing_table, rank, &mut timer); |
||||
} |
||||
hardware_tick(&mut hardware_tick_ts, &mut timer); |
||||
if drtiosat_tsc_loaded() { |
||||
info!("TSC loaded from uplink"); |
||||
for rep in repeaters.iter() { |
||||
if let Err(e) = rep.sync_tsc(&mut timer) { |
||||
error!("failed to sync TSC ({:?})", e); |
||||
} |
||||
} |
||||
if let Err(e) = drtioaux::send(0, &drtioaux::Packet::TSCAck) { |
||||
error!("aux packet error: {:?}", e); |
||||
} |
||||
} |
||||
} |
||||
|
||||
drtiosat_reset_phy(true); |
||||
drtiosat_reset(true); |
||||
drtiosat_tsc_loaded(); |
||||
info!("uplink is down, switching to local oscillator clock"); |
||||
#[cfg(has_siphaser)] |
||||
si5324::siphaser::select_recovered_clock(&mut i2c, false, &mut timer).expect("failed to switch clocks"); |
||||
} |
||||
} |
||||
|
||||
extern "C" { |
||||
static mut __stack1_start: u32; |
||||
} |
||||
|
||||
interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, { |
||||
if MPIDR.read().cpu_id() == 1{ |
||||
let mpcore = mpcore::RegisterBlock::mpcore(); |
||||
let mut gic = gic::InterruptController::gic(mpcore); |
||||
let id = gic.get_interrupt_id(); |
||||
if id.0 == 0 { |
||||
gic.end_interrupt(id); |
||||
asm::exit_irq(); |
||||
SP.write(&mut __stack1_start as *mut _ as u32); |
||||
asm::enable_irq(); |
||||
CORE1_RESTART.store(false, Ordering::Relaxed); |
||||
notify_spin_lock(); |
||||
main_core1(); |
||||
} |
||||
stdio::drop_uart(); |
||||
} |
||||
loop {} |
||||
}); |
||||
|
||||
static mut PANICKED: [bool; 2] = [false; 2]; |
||||
|
||||
static CORE1_RESTART: AtomicBool = AtomicBool::new(false); |
||||
|
||||
pub fn restart_core1() { |
||||
let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore()); |
||||
CORE1_RESTART.store(true, Ordering::Relaxed); |
||||
interrupt_controller.send_sgi(gic::InterruptId(0), gic::CPUCore::Core1.into()); |
||||
while CORE1_RESTART.load(Ordering::Relaxed) { |
||||
spin_lock_yield(); |
||||
} |
||||
} |
||||
|
||||
#[no_mangle] |
||||
pub fn main_core1() { |
||||
let mut interrupt_controller = gic::InterruptController::gic(mpcore::RegisterBlock::mpcore()); |
||||
interrupt_controller.enable_interrupts(); |
||||
|
||||
loop {} |
||||
} |
||||
|
||||
#[no_mangle] |
||||
pub extern fn exception(_vect: u32, _regs: *const u32, pc: u32, ea: u32) { |
||||
|
||||
fn hexdump(addr: u32) { |
||||
let addr = (addr - addr % 4) as *const u32; |
||||
let mut ptr = addr; |
||||
println!("@ {:08p}", ptr); |
||||
for _ in 0..4 { |
||||
print!("+{:04x}: ", ptr as usize - addr as usize); |
||||
print!("{:08x} ", unsafe { *ptr }); ptr = ptr.wrapping_offset(1); |
||||
print!("{:08x} ", unsafe { *ptr }); ptr = ptr.wrapping_offset(1); |
||||
print!("{:08x} ", unsafe { *ptr }); ptr = ptr.wrapping_offset(1); |
||||
print!("{:08x}\n", unsafe { *ptr }); ptr = ptr.wrapping_offset(1); |
||||
} |
||||
} |
||||
|
||||
hexdump(pc); |
||||
hexdump(ea); |
||||
panic!("exception at PC 0x{:x}, EA 0x{:x}", pc, ea) |
||||
} |
||||
|
||||
#[no_mangle] // https://github.com/rust-lang/rust/issues/{38281,51647}
|
||||
#[panic_handler] |
||||
pub fn panic_fmt(info: &core::panic::PanicInfo) -> ! { |
||||
let id = MPIDR.read().cpu_id() as usize; |
||||
print!("Core {} ", id); |
||||
unsafe { |
||||
if PANICKED[id] { |
||||
println!("nested panic!"); |
||||
loop {} |
||||
} |
||||
PANICKED[id] = true; |
||||
} |
||||
print!("panic at "); |
||||
if let Some(location) = info.location() { |
||||
print!("{}:{}:{}", location.file(), location.line(), location.column()); |
||||
} else { |
||||
print!("unknown location"); |
||||
} |
||||
if let Some(message) = info.message() { |
||||
println!(": {}", message); |
||||
} else { |
||||
println!(""); |
||||
} |
||||
|
||||
|
||||
loop {} |
||||
} |
||||
|
||||
// linker symbols
|
||||
extern "C" { |
||||
static __text_start: u32; |
||||
static __text_end: u32; |
||||
static __exidx_start: u32; |
||||
static __exidx_end: u32; |
||||
} |
||||
|
||||
#[no_mangle] |
||||
extern fn dl_unwind_find_exidx(_pc: *const u32, len_ptr: *mut u32) -> *const u32 { |
||||
let length; |
||||
let start: *const u32; |
||||
unsafe { |
||||
length = (&__exidx_end as *const u32).offset_from(&__exidx_start) as u32; |
||||
start = &__exidx_start; |
||||
*len_ptr = length; |
||||
} |
||||
start |
||||
} |
@ -0,0 +1,290 @@ |
||||
use libboard_artiq::{drtioaux, drtio_routing}; |
||||
use libboard_zynq::timer::GlobalTimer; |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
use libboard_artiq::{pl::csr}; |
||||
#[cfg(has_drtio_routing)] |
||||
use libboard_zynq::time::Milliseconds; |
||||
#[cfg(has_drtio_routing)] |
||||
use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs; |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
fn rep_link_rx_up(repno: u8) -> bool { |
||||
let repno = repno as usize; |
||||
unsafe { |
||||
(csr::DRTIOREP[repno].rx_up_read)() == 1 |
||||
} |
||||
} |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
#[derive(Clone, Copy, PartialEq)] |
||||
enum RepeaterState { |
||||
Down, |
||||
SendPing { ping_count: u16 }, |
||||
WaitPingReply { ping_count: u16, timeout: Milliseconds }, |
||||
Up, |
||||
Failed |
||||
} |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
impl Default for RepeaterState { |
||||
fn default() -> RepeaterState { RepeaterState::Down } |
||||
} |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
#[derive(Clone, Copy, Default)] |
||||
pub struct Repeater { |
||||
repno: u8, |
||||
auxno: u8, |
||||
state: RepeaterState |
||||
} |
||||
|
||||
#[cfg(has_drtio_routing)] |
||||
impl Repeater { |
||||
pub fn new(repno: u8) -> Repeater { |
||||
Repeater { |
||||
repno: repno, |
||||
auxno: repno + 1, |
||||
state: RepeaterState::Down |
||||
} |
||||
} |
||||
|
||||
#[allow(dead_code)] |
||||
pub fn is_up(&self) -> bool { |
||||
self.state == RepeaterState::Up |
||||
} |
||||
|
||||
pub fn service(&mut self, routing_table: &drtio_routing::RoutingTable, rank: u8, |
||||
timer: &mut GlobalTimer) { |
||||
self.process_local_errors(); |
||||
|
||||
match self.state { |
||||
RepeaterState::Down => { |
||||
if rep_link_rx_up(self.repno) { |
||||
info!("[REP#{}] link RX became up, pinging", self.repno); |
||||
self.state = RepeaterState::SendPing { ping_count: 0 }; |
||||
} |
||||
} |
||||
RepeaterState::SendPing { ping_count } => { |
||||
if rep_link_rx_up(self.repno) { |
||||
drtioaux::send(self.auxno, &drtioaux::Packet::EchoRequest).unwrap(); |
||||
self.state = RepeaterState::WaitPingReply { |
||||
ping_count: ping_count + 1, |
||||
timeout: timer.get_time() + Milliseconds(100) |
||||
} |
||||
} else { |
||||
error!("[REP#{}] link RX went down during ping", self.repno); |
||||
self.state = RepeaterState::Down; |
||||
} |
||||
} |
||||
RepeaterState::WaitPingReply { ping_count, timeout } => { |
||||
if rep_link_rx_up(self.repno) { |
||||
if let Ok(Some(drtioaux::Packet::EchoReply)) = drtioaux::recv(self.auxno) { |
||||
info!("[REP#{}] remote replied after {} packets", self.repno, ping_count); |
||||
self.state = RepeaterState::Up; |
||||
if let Err(e) = self.sync_tsc(timer) { |
||||
error!("[REP#{}] failed to sync TSC ({:?})", self.repno, e); |
||||
self.state = RepeaterState::Failed; |
||||
return; |
||||
} |
||||
if let Err(e) = self.load_routing_table(routing_table, timer) { |
||||
error!("[REP#{}] failed to load routing table ({:?})", self.repno, e); |
||||
self.state = RepeaterState::Failed; |
||||
return; |
||||
} |
||||
if let Err(e) = self.set_rank(rank + 1, timer) { |
||||
error!("[REP#{}] failed to set rank ({:?})", self.repno, e); |
||||
self.state = RepeaterState::Failed; |
||||
return; |
||||
} |
||||
} else { |
||||
if timer.get_time() > timeout { |
||||
if ping_count > 200 { |
||||
error!("[REP#{}] ping failed", self.repno); |
||||
self.state = RepeaterState::Failed; |
||||
} else { |
||||
self.state = RepeaterState::SendPing { ping_count: ping_count }; |
||||
} |
||||
} |
||||
} |
||||
} else { |
||||
error!("[REP#{}] link RX went down during ping", self.repno); |
||||
self.state = RepeaterState::Down; |
||||
} |
||||
} |
||||
RepeaterState::Up => { |
||||
self.process_unsolicited_aux(); |
||||
if !rep_link_rx_up(self.repno) { |
||||
info!("[REP#{}] link is down", self.repno); |
||||
self.state = RepeaterState::Down; |
||||
} |
||||
} |
||||
RepeaterState::Failed => { |
||||
if !rep_link_rx_up(self.repno) { |
||||
info!("[REP#{}] link is down", self.repno); |
||||
self.state = RepeaterState::Down; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
fn process_unsolicited_aux(&self) { |
||||
match drtioaux::recv(self.auxno) { |
||||
Ok(Some(packet)) => warn!("[REP#{}] unsolicited aux packet: {:?}", self.repno, packet), |
||||
Ok(None) => (), |
||||
Err(_) => warn!("[REP#{}] aux packet error", self.repno) |
||||
} |
||||
} |
||||
|
||||
fn process_local_errors(&self) { |
||||
let repno = self.repno as usize; |
||||
let errors; |
||||
unsafe { |
||||
errors = (csr::DRTIOREP[repno].protocol_error_read)(); |
||||
} |
||||
if errors & 1 != 0 { |
||||
error!("[REP#{}] received packet of an unknown type", repno); |
||||
} |
||||
if errors & 2 != 0 { |
||||
error!("[REP#{}] received truncated packet", repno); |
||||
} |
||||
if errors & 4 != 0 { |
||||
let cmd; |
||||
let chan_sel; |
||||
unsafe { |
||||
cmd = (csr::DRTIOREP[repno].command_missed_cmd_read)(); |
||||
chan_sel = (csr::DRTIOREP[repno].command_missed_chan_sel_read)(); |
||||
} |
||||
error!("[REP#{}] CRI command missed, cmd={}, chan_sel=0x{:06x}", repno, cmd, chan_sel) |
||||
} |
||||
if errors & 8 != 0 { |
||||
let destination; |
||||
unsafe { |
||||
destination = (csr::DRTIOREP[repno].buffer_space_timeout_dest_read)(); |
||||
} |
||||
error!("[REP#{}] timeout attempting to get remote buffer space, destination=0x{:02x}", repno, destination); |
||||
} |
||||
unsafe { |
||||
(csr::DRTIOREP[repno].protocol_error_write)(errors); |
||||
} |
||||
} |
||||
|
||||
fn recv_aux_timeout(&self, timeout: u32, timer: &mut GlobalTimer) -> Result<drtioaux::Packet, drtioaux::Error> { |
||||
let max_time = timer.get_time() + Milliseconds(timeout.into()); |
||||
loop { |
||||
if !rep_link_rx_up(self.repno) { |
||||
return Err(drtioaux::Error::LinkDown); |
||||
} |
||||
if timer.get_time() > max_time { |
||||
return Err(drtioaux::Error::TimedOut); |
||||
} |
||||
match drtioaux::recv(self.auxno) { |
||||
Ok(Some(packet)) => return Ok(packet), |
||||
Ok(None) => (), |
||||
Err(e) => return Err(e) |
||||
} |
||||
} |
||||
} |
||||
|
||||
pub fn aux_forward(&self, request: &drtioaux::Packet, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { |
||||
if self.state != RepeaterState::Up { |
||||
return Err(drtioaux::Error::LinkDown); |
||||
} |
||||
drtioaux::send(self.auxno, request).unwrap(); |
||||
let reply = self.recv_aux_timeout(200, timer)?; |
||||
drtioaux::send(0, &reply).unwrap(); |
||||
Ok(()) |
||||
} |
||||
|
||||
pub fn sync_tsc(&self, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { |
||||
if self.state != RepeaterState::Up { |
||||
return Ok(()); |
||||
} |
||||
|
||||
let repno = self.repno as usize; |
||||
unsafe { |
||||
(csr::DRTIOREP[repno].set_time_write)(1); |
||||
while (csr::DRTIOREP[repno].set_time_read)() == 1 {} |
||||
} |
||||
|
||||
// TSCAck is the only aux packet that is sent spontaneously
|
||||
// by the satellite, in response to a TSC set on the RT link.
|
||||
let reply = self.recv_aux_timeout(10000, timer)?; |
||||
if reply == drtioaux::Packet::TSCAck { |
||||
return Ok(()); |
||||
} else { |
||||
return Err(drtioaux::Error::UnexpectedReply); |
||||
} |
||||
} |
||||
|
||||
pub fn set_path(&self, destination: u8, hops: &[u8; drtio_routing::MAX_HOPS], timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { |
||||
if self.state != RepeaterState::Up { |
||||
return Ok(()); |
||||
} |
||||
|
||||
drtioaux::send(self.auxno, &drtioaux::Packet::RoutingSetPath { |
||||
destination: destination, |
||||
hops: *hops |
||||
}).unwrap(); |
||||
let reply = self.recv_aux_timeout(200, timer)?; |
||||
if reply != drtioaux::Packet::RoutingAck { |
||||
return Err(drtioaux::Error::UnexpectedReply); |
||||
} |
||||
Ok(()) |
||||
} |
||||
|
||||
pub fn load_routing_table(&self, routing_table: &drtio_routing::RoutingTable, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { |
||||
for i in 0..drtio_routing::DEST_COUNT { |
||||
self.set_path(i as u8, &routing_table.0[i], timer)?; |
||||
} |
||||
Ok(()) |
||||
} |
||||
|
||||
pub fn set_rank(&self, rank: u8, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { |
||||
if self.state != RepeaterState::Up { |
||||
return Ok(()); |
||||
} |
||||
drtioaux::send(self.auxno, &drtioaux::Packet::RoutingSetRank { |
||||
rank: rank |
||||
}).unwrap(); |
||||
let reply = self.recv_aux_timeout(200, timer)?; |
||||
if reply != drtioaux::Packet::RoutingAck { |
||||
return Err(drtioaux::Error::UnexpectedReply); |
||||
} |
||||
Ok(()) |
||||
} |
||||
|
||||
pub fn rtio_reset(&self, timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { |
||||
let repno = self.repno as usize; |
||||
unsafe { (csr::DRTIOREP[repno].reset_write)(1); } |
||||
timer.delay_us(100); |
||||
unsafe { (csr::DRTIOREP[repno].reset_write)(0); } |
||||
|
||||
if self.state != RepeaterState::Up { |
||||
return Ok(()); |
||||
} |
||||
|
||||
drtioaux::send(self.auxno, &drtioaux::Packet::ResetRequest).unwrap(); |
||||
let reply = self.recv_aux_timeout(200, timer)?; |
||||
if reply != drtioaux::Packet::ResetAck { |
||||
return Err(drtioaux::Error::UnexpectedReply); |
||||
} |
||||
Ok(()) |
||||
} |
||||
} |
||||
|
||||
#[cfg(not(has_drtio_routing))] |
||||
#[derive(Clone, Copy, Default)] |
||||
pub struct Repeater { |
||||
} |
||||
|
||||
#[cfg(not(has_drtio_routing))] |
||||
impl Repeater { |
||||
pub fn new(_repno: u8) -> Repeater { Repeater::default() } |
||||
|
||||
pub fn service(&self, _routing_table: &drtio_routing::RoutingTable, _rank: u8, _timer: &mut GlobalTimer) { } |
||||
|
||||
pub fn sync_tsc(&self, _timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { Ok(()) } |
||||
|
||||
pub fn rtio_reset(&self, _timer: &mut GlobalTimer) -> Result<(), drtioaux::Error> { Ok(()) } |
||||
} |
Loading…
Reference in new issue
Remove
You can also remove
Packet::Jdac*
fromdrtioaux
.Is Jdac stuff then unsupported on all platforms? I imagine that it could be forwarded to a mainline artiq-based satellite that does have ad9154. Or is mixing satellites not supported?
Not supported.