From 387f122b1dc5e7254703b12071d355c0a6a121fa Mon Sep 17 00:00:00 2001 From: morgan Date: Tue, 26 Nov 2024 16:43:14 +0800 Subject: [PATCH] CXP firmware: camera setup init 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 --- src/libboard_artiq/src/cxp.rs | 388 ++++++++++++++++++++++++++++++++++ 1 file changed, 388 insertions(+) create mode 100644 src/libboard_artiq/src/cxp.rs diff --git a/src/libboard_artiq/src/cxp.rs b/src/libboard_artiq/src/cxp.rs new file mode 100644 index 0000000..936525b --- /dev/null +++ b/src/libboard_artiq/src/cxp.rs @@ -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 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 { + // 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 { + 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 { + 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 { + 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 { + 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) +}