diff --git a/src/satman/Cargo.toml b/src/satman/Cargo.toml new file mode 100644 index 0000000..6c44e5a --- /dev/null +++ b/src/satman/Cargo.toml @@ -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" } \ No newline at end of file diff --git a/src/satman/build.rs b/src/satman/build.rs new file mode 100644 index 0000000..04ead8d --- /dev/null +++ b/src/satman/build.rs @@ -0,0 +1,21 @@ +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::cfg(); +} diff --git a/src/satman/link.x b/src/satman/link.x new file mode 100644 index 0000000..1f148a8 --- /dev/null +++ b/src/satman/link.x @@ -0,0 +1,86 @@ +ENTRY(Reset); + +MEMORY +{ + SDRAM : ORIGIN = 0x00100000, LENGTH = 0x1FF00000 +} + +SECTIONS +{ + __text_start = .; + .text : + { + KEEP(*(.text.exceptions)); + *(.text.boot); + *(.text .text.*); + } > SDRAM + __text_end = .; + + __exidx_start = .; + .ARM.exidx : + { + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + } > SDRAM + __exidx_end = .; + + .ARM.extab : + { + * (.ARM.extab*) + } > SDRAM + + .rodata : ALIGN(4) + { + *(.rodata .rodata.*); + } > SDRAM + + .data : ALIGN(4) + { + *(.data .data.*); + } > SDRAM + + .bss (NOLOAD) : ALIGN(4) + { + __bss_start = .; + *(.bss .bss.*); + . = ALIGN(4); + __bss_end = .; + } > SDRAM + + .heap (NOLOAD) : ALIGN(8) + { + __heap0_start = .; + . += 0x8000000; + __heap0_end = .; + __heap1_start = .; + . += 0x8000000; + __heap1_end = .; + } > SDRAM + + .stack1 (NOLOAD) : ALIGN(8) + { + __stack1_end = .; + . += 0x1000000; + __stack1_start = .; + } > SDRAM + + .stack0 (NOLOAD) : ALIGN(8) + { + __stack0_end = .; + . += 0x20000; + __stack0_start = .; + } > SDRAM + + .irq_stack1 (NOLOAD) : ALIGN(8) + { + __irq_stack1_end = .; + . += 0x100; + __irq_stack1_start = .; + } > SDRAM + + .irq_stack0 (NOLOAD) : ALIGN(8) + { + __irq_stack0_end = .; + . += 0x100; + __irq_stack0_start = .; + } > SDRAM +} diff --git a/src/satman/satman.ld b/src/satman/satman.ld new file mode 100644 index 0000000..69cc737 --- /dev/null +++ b/src/satman/satman.ld @@ -0,0 +1,55 @@ +INCLUDE generated/output_format.ld +INCLUDE generated/regions.ld +ENTRY(_reset_handler) + +SECTIONS +{ + .vectors : + { + *(.vectors) + } > main_ram + + .text : + { + *(.text .text.*) + } > main_ram + + /* https://sourceware.org/bugzilla/show_bug.cgi?id=20475 */ + .got : + { + PROVIDE(_GLOBAL_OFFSET_TABLE_ = .); + *(.got) + } > main_ram + + .got.plt : + { + *(.got.plt) + } > main_ram + + .rodata : + { + _frodata = .; + *(.rodata .rodata.*) + _erodata = .; + } > main_ram + + .data : + { + *(.data .data.*) + } > main_ram + + .bss ALIGN(4) : + { + _fbss = .; + *(.bss .bss.*) + . = ALIGN(4); + _ebss = .; + } > main_ram + + .stack : + { + _estack = .; + . += 0x10000; + _fstack = . - 4; + } > main_ram +} diff --git a/src/satman/src/jdac_common.rs b/src/satman/src/jdac_common.rs new file mode 100644 index 0000000..f47cc03 --- /dev/null +++ b/src/satman/src/jdac_common.rs @@ -0,0 +1,37 @@ +pub const INIT: u8 = 0x00; +pub const PRINT_STATUS: u8 = 0x01; +pub const PRBS: u8 = 0x02; +pub const STPL: u8 = 0x03; + +pub const SYSREF_DELAY_DAC: u8 = 0x10; +pub const SYSREF_SLIP: u8 = 0x11; +pub const SYNC: u8 = 0x12; + +pub const DDMTD_SYSREF_RAW: u8 = 0x20; +pub const DDMTD_SYSREF: u8 = 0x21; + + +fn average_2phases(a: i32, b: i32, modulo: i32) -> i32 { + let diff = ((a - b + modulo/2 + modulo) % modulo) - modulo/2; + return (modulo + b + diff/2) % modulo; +} + +pub fn average_phases(phases: &[i32], modulo: i32) -> i32 { + if phases.len() == 1 { + panic!("input array length must be a power of 2"); + } else if phases.len() == 2 { + average_2phases(phases[0], phases[1], modulo) + } else { + let cut = phases.len()/2; + average_2phases( + average_phases(&phases[..cut], modulo), + average_phases(&phases[cut..], modulo), + modulo) + } +} + +pub const RAW_DDMTD_N_SHIFT: i32 = 6; +pub const RAW_DDMTD_N: i32 = 1 << RAW_DDMTD_N_SHIFT; +pub const DDMTD_DITHER_BITS: i32 = 1; +pub const DDMTD_N_SHIFT: i32 = RAW_DDMTD_N_SHIFT + DDMTD_DITHER_BITS; +pub const DDMTD_N: i32 = 1 << DDMTD_N_SHIFT; \ No newline at end of file diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs new file mode 100644 index 0000000..0f57bf4 --- /dev/null +++ b/src/satman/src/main.rs @@ -0,0 +1,634 @@ +#![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 }) + } + + drtioaux::Packet::JdacBasicRequest { destination: _destination, dacno: _dacno, + reqno: _reqno, param: _param } => { + forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); + let (succeeded, retval) = (false, 0); + drtioaux::send(0, + &drtioaux::Packet::JdacBasicReply { succeeded: succeeded, retval: retval }) + } + + _ => { + 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 +} diff --git a/src/satman/src/repeater.rs b/src/satman/src/repeater.rs new file mode 100644 index 0000000..ed12365 --- /dev/null +++ b/src/satman/src/repeater.rs @@ -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 { + 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(()) } +}