morgan/artiq-zynq
1
0
Fork 0
forked from M-Labs/artiq-zynq
Code Pull Requests Activity

CXP firmware: camera setup init

Browse Source 
cxp FW: add camera discovery
cxp FW: add multi channel discovery
cxp FW: add CXP version negotiate
cxp FW: add CXP operation linerate setter
cxp FW: add ConnectReset, CXP version readout
cxp FW: add connection tester
cxp FW: add master channel & topology checker
cxp FW: set packet max size to 2KiB
cxp FW: add frame buffer cfg & hostconnid
This commit is contained in:
morgan 2024-11-26 16:43:14 +08:00
parent f91ad862bd
commit 387f122b1d
1 changed files with 388 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

388
src/libboard_artiq/src/cxp.rs Normal file
View File

@ -0,0 +1,388 @@
use core::{fmt, result::Result};
use embedded_hal::blocking::delay::DelayMs;
use libboard_zynq::timer::GlobalTimer;
use log::{info, warn};
use crate::{cxp_ctrl::{read_u32, read_u64, reset_tag, send_test_packet, write_bytes_no_ack, write_u32, write_u64},
cxp_phys::{self, CXP_CHANNELS, CXP_SPEED},
cxp_proto::Error as ProtoError,
pl::csr::{cxp_frame_pipeline, CXP}};
// Bootstrap registers address
const STANDARD: u32 = 0x0000;
const REVISION: u32 = 0x0004;
const CONNECTION_RESET: u32 = 0x4000;
const DEVICE_CONNECTION_ID: u32 = 0x4004;
const MASTER_HOST_CONNECTION_ID: u32 = 0x4008;
const CONTROL_PACKET_SIZE_MAX: u32 = 0x400C;
const STREAM_PACKET_SIZE_MAX: u32 = 0x4010;
const CONNECTION_CFG: u32 = 0x4014;
const CONNECTION_CFG_DEFAULT: u32 = 0x4018;
const TESTMODE: u32 = 0x401C;
const TEST_ERROR_COUNT_SELECTOR: u32 = 0x4020;
const TEST_ERROR_COUNT: u32 = 0x4024;
const TEST_PACKET_COUNT_TX: u32 = 0x4028;
const TEST_PACKET_COUNT_RX: u32 = 0x4030;
const VERSION_SUPPORTED: u32 = 0x4044;
const VERSION_USED: u32 = 0x4048;
// Setup const
const MAX_STREAM_PAK_SIZE: u32 = 0x800; // 2 KiB
const TX_TEST_CNT: u8 = 10;
pub const MASTER_CHANNEL: u8 = 0;
const HOST_CONNECTION_ID: u32 = 0xC001C0DE; // TODO: rename this
#[derive(Debug)]
pub enum Error {
CameraNotDetected,
ConnectionLost,
UnstableDownConn,
UnstableUpConn,
UnsupportedSpeed(u32),
UnsupportedTopology,
UnsupportedVersion,
Protocol(ProtoError),
}
impl From<ProtoError> for Error {
fn from(value: ProtoError) -> Error {
Error::Protocol(value)
}
}
impl fmt::Display for Error {
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
match self {
&Error::CameraNotDetected => write!(f, "CameraNotDetected"),
&Error::ConnectionLost => write!(f, "ConnectionLost Some active channels cannot be detected"),
&Error::UnstableDownConn => write!(f, "UnstableDownConn DownConnection test failed"),
&Error::UnstableUpConn => write!(f, "UnstableUpConn UpConnection test failed"),
&Error::UnsupportedSpeed(linerate_code) => write!(
f,
"UnsupportedSpeed {:#X} linerate code is not supported",
linerate_code
),
&Error::UnsupportedTopology => write!(
f,
"UnsupportedTopology CH#1 should be the master channel while CH#2-4 should be connected to extension \
#1-3 respectively"
),
&Error::UnsupportedVersion => write!(
f,
"UnsupportedVersion Cannot find a compatible protocol version between the grabber & camera"
),
&Error::Protocol(ref err) => write!(f, "ProtocolError {}", err),
}
}
}
pub fn setup() -> Result<bool, Error> {
// assume timer was initialized successfully
let mut timer = unsafe { GlobalTimer::get() };
camera_setup(&mut timer)
}
fn scan_active_channels() -> u8 {
let mut active_channels: u8 = 0;
for ch in 0..CXP_CHANNELS {
if unsafe { (CXP[ch as usize].downconn_rx_ready_read)() } == 1 {
info!("ch#{} is up <---------------------------------", ch);
active_channels += 1;
}
}
active_channels
}
fn discover_camera(timer: &mut GlobalTimer) -> Result<(), Error> {
// Section 7.6 (CXP-001-2021)
// 1.25Gbps (CXP_1) and 3.125Gbps (CXP_3) are the discovery rate
// both linerate need to be checked as camera only support ONE of discovery rates
for speed in [CXP_SPEED::CXP_1, CXP_SPEED::CXP_3].iter() {
cxp_phys::change_linerate(*speed);
// Section 12.1.2 (CXP-001-2021)
// send ConnectionReset on all channels -> wait 200ms -> scan for active channels
for ch in 0..CXP_CHANNELS {
write_bytes_no_ack(ch, CONNECTION_RESET, &1_u32.to_be_bytes(), false)?;
}
timer.delay_ms(200);
if scan_active_channels() > 0 {
return Ok(());
}
}
Err(Error::CameraNotDetected)
}
fn check_master_channel() -> Result<(), Error> {
if read_u32(MASTER_CHANNEL, DEVICE_CONNECTION_ID, false)? == 0 {
Ok(())
} else {
warn!(
"Channel #{} is not connected to master channel of the camera",
MASTER_CHANNEL
);
Err(Error::UnsupportedTopology)
}
}
fn check_connection_topology(active_channels: u8) -> Result<(), Error> {
// only the simple topology MASTER:ch0, extension:ch1,2,3 is supported right now
for ch in 0..active_channels {
if read_u32(ch, DEVICE_CONNECTION_ID, false)? != ch as u32 {
warn!("Channel #{} is not connected to the right port of the camera", ch);
return Err(Error::UnsupportedTopology);
};
match ch {
0 => info!("CHANNEL #{} is active - master connection", ch),
_ => info!("CHANNEL #{} is active - extension connection", ch),
}
}
Ok(())
}
fn negotiate_active_channels(timer: &mut GlobalTimer) -> Result<u8, Error> {
let mut available_chs = read_u32(MASTER_CHANNEL, CONNECTION_CFG_DEFAULT, false)? >> 16;
available_chs = available_chs.min(CXP_CHANNELS as u32);
// activate channels on camera but preserve the discovery linerate
let current_cfg = read_u32(MASTER_CHANNEL, CONNECTION_CFG, false)?;
write_u32(
MASTER_CHANNEL,
CONNECTION_CFG,
current_cfg & 0xFFFF | available_chs << 16,
false,
)?;
timer.delay_ms(200);
let active_channels = scan_active_channels();
check_connection_topology(active_channels)?;
if available_chs > active_channels as u32 {
info!(
"Only detected {} channel(s), disabling excess channels on camera",
active_channels
);
write_u32(
MASTER_CHANNEL,
CONNECTION_CFG,
current_cfg & 0xFFFF | ((active_channels as u32) << 16),
false,
)?;
timer.delay_ms(200);
// check no active channels are down after the cfg change
if active_channels != scan_active_channels() {
return Err(Error::ConnectionLost);
}
}
Ok(active_channels)
}
fn set_host_connection_id() -> Result<(), Error> {
// TODO: not mandatory??
write_u32(MASTER_CHANNEL, MASTER_HOST_CONNECTION_ID, HOST_CONNECTION_ID, false)?;
let reg = read_u32(MASTER_CHANNEL, CONNECTION_CFG, false)?;
info!("host connection id set as = {}", reg);
Ok(())
}
fn negotiate_cxp_version() -> Result<bool, Error> {
let rev = read_u32(MASTER_CHANNEL, REVISION, false)?;
let mut major_rev: u32 = rev >> 16;
let mut minor_rev: u32 = rev & 0xFF;
info!("Camera's CXP revision is {}.{}", major_rev, minor_rev);
// Section 12.1.4 (CXP-001-2021)
// For CXP 2.0 and onward, Host need to check the VersionSupported register to determine
// the highest common version that supported by both device & host
if major_rev >= 2 {
let reg = read_u32(MASTER_CHANNEL, VERSION_SUPPORTED, false)?;
if ((reg >> 3) & 1) == 1 {
major_rev = 2;
minor_rev = 1;
} else if ((reg >> 2) & 1) == 1 {
major_rev = 2;
minor_rev = 0;
} else {
return Err(Error::UnsupportedVersion);
}
write_u32(MASTER_CHANNEL, VERSION_USED, major_rev << 16 | minor_rev, false)?;
}
info!(
"Camera supports CXP {}.{}, using CXP {}.{} protcol now",
major_rev, minor_rev, major_rev, minor_rev
);
Ok(major_rev >= 2)
}
fn negotiate_pak_max_size(with_tag: bool) -> Result<(), Error> {
let reg = read_u32(MASTER_CHANNEL, CONTROL_PACKET_SIZE_MAX, with_tag)?;
info!("Max CTRL PAK size = {:#010X}", reg);
let reg = read_u32(MASTER_CHANNEL, STREAM_PACKET_SIZE_MAX, with_tag)?;
info!("Max STREAM PAK size = {:#010X}", reg);
// TODO: set stream packet size
write_u32(MASTER_CHANNEL, STREAM_PACKET_SIZE_MAX, MAX_STREAM_PAK_SIZE, with_tag)?;
let reg = read_u32(MASTER_CHANNEL, STREAM_PACKET_SIZE_MAX, with_tag)?;
info!("Max STREAM PAK size = {:#010X}", reg);
Ok(())
}
fn check_magic_word() -> Result<(), Error> {
let reg = read_u32(MASTER_CHANNEL, STANDARD, true)?;
info!("CoaXPress magic WORD = {:#010X} !!!", reg);
Ok(())
}
fn decode_cxp_speed(linerate_code: u32) -> Option<CXP_SPEED> {
match linerate_code {
0x28 => Some(CXP_SPEED::CXP_1),
0x30 => Some(CXP_SPEED::CXP_2),
0x38 => Some(CXP_SPEED::CXP_3),
0x40 => Some(CXP_SPEED::CXP_5),
0x48 => Some(CXP_SPEED::CXP_6),
0x50 => Some(CXP_SPEED::CXP_10),
0x58 => Some(CXP_SPEED::CXP_12),
_ => None,
}
}
fn set_operation_linerate(active_channels: u8, with_tag: bool, timer: &mut GlobalTimer) -> Result<(), Error> {
let current_cfg = read_u32(MASTER_CHANNEL, CONNECTION_CFG, with_tag)?;
info!("Current connection cfg = {:#010X}", current_cfg);
let recommended_linerate_code = read_u32(MASTER_CHANNEL, CONNECTION_CFG_DEFAULT, with_tag)? & 0xFFFF;
info!("recommended_linerate_code = {:#010X}", recommended_linerate_code);
if let Some(speed) = decode_cxp_speed(recommended_linerate_code) {
// preserve the number of active channels
write_u32(
MASTER_CHANNEL,
CONNECTION_CFG,
current_cfg & 0xFFFF0000 | recommended_linerate_code,
with_tag,
)?;
cxp_phys::change_linerate(speed);
timer.delay_ms(200);
// check no active channels are down after linerate change
if scan_active_channels() == active_channels {
Ok(())
} else {
Err(Error::ConnectionLost)
}
} else {
Err(Error::UnsupportedSpeed(recommended_linerate_code))
}
}
fn test_counter_reset(channel: u8, with_tag: bool) -> Result<(), Error> {
unsafe { (CXP[channel as usize].downconn_bootstrap_test_counts_reset_write)(1) };
write_u32(MASTER_CHANNEL, TEST_ERROR_COUNT_SELECTOR, channel as u32, with_tag)?;
write_u32(MASTER_CHANNEL, TEST_ERROR_COUNT, 0, with_tag)?;
write_u64(MASTER_CHANNEL, TEST_PACKET_COUNT_TX, 0, with_tag)?;
write_u64(MASTER_CHANNEL, TEST_PACKET_COUNT_RX, 0, with_tag)?;
Ok(())
}
fn verify_test_result(channel: u8, with_tag: bool) -> Result<(), Error> {
write_u32(MASTER_CHANNEL, TEST_ERROR_COUNT_SELECTOR, channel as u32, with_tag)?;
// Section 9.9.3 (CXP-001-2021)
// verify grabber -> camera connection test result
if read_u64(MASTER_CHANNEL, TEST_PACKET_COUNT_RX, with_tag)? != TX_TEST_CNT as u64 {
return Err(Error::UnstableUpConn);
};
if read_u32(MASTER_CHANNEL, TEST_ERROR_COUNT, with_tag)? > 0 {
return Err(Error::UnstableUpConn);
};
// Section 9.9.4 (CXP-001-2021)
// verify camera -> grabber connection test result
let camera_test_pak_cnt = read_u64(MASTER_CHANNEL, TEST_PACKET_COUNT_TX, true)?;
unsafe {
if (CXP[channel as usize].downconn_bootstrap_test_packet_counter_read)() != camera_test_pak_cnt as u16 {
info!(
"CHANNEL #{} test packet cnt = {}",
channel,
(CXP[channel as usize].downconn_bootstrap_test_packet_counter_read)()
);
return Err(Error::UnstableDownConn);
};
if (CXP[channel as usize].downconn_bootstrap_test_error_counter_read)() > 0 {
info!(
"CHANNEL #{} test packet error cnt = {}",
channel,
(CXP[channel as usize].downconn_bootstrap_test_error_counter_read)()
);
return Err(Error::UnstableDownConn);
};
};
info!("CHANNEL #{} pass testing", channel);
Ok(())
}
fn test_channels_stability(active_channels: u8, with_tag: bool, timer: &mut GlobalTimer) -> Result<(), Error> {
// let active_channels = active_channels as usize;
for ch in 0..active_channels {
test_counter_reset(ch, with_tag)?;
}
// grabber -> camera connection test
for ch in 0..active_channels {
for _ in 0..TX_TEST_CNT {
send_test_packet(ch)?;
// sending the whole test sequence @ 20.833Mbps will take a minimum of 1.972ms
// and leave some room to send IDLE word
timer.delay_ms(2);
}
}
// camera -> grabber connection test
// enabling the TESTMODE on master channel will send test packets on all channels
// and ctrl packet write overhead is used as a delay
write_u32(MASTER_CHANNEL, TESTMODE, 1, with_tag)?;
write_u32(MASTER_CHANNEL, TESTMODE, 0, with_tag)?;
for ch in 0..active_channels {
verify_test_result(ch, with_tag)?;
}
Ok(())
}
fn camera_setup(timer: &mut GlobalTimer) -> Result<bool, Error> {
reset_tag();
discover_camera(timer)?;
check_master_channel()?;
let active_channels = negotiate_active_channels(timer)?;
set_host_connection_id()?;
let cxp_v2_or_greater = negotiate_cxp_version()?;
negotiate_pak_max_size(cxp_v2_or_greater)?;
set_operation_linerate(active_channels, cxp_v2_or_greater, timer)?;
// DEBUG: print
check_magic_word()?;
test_channels_stability(active_channels, cxp_v2_or_greater, timer)?;
// unsafe{
// // don't set two identical routingid, the router cannot handle it
// // it can handle it after adding the priority decoder
// cxp_frame_pipeline::buffer_1_routingid_write(0x00);
// }
Ok(cxp_v2_or_greater)
}