diff --git a/src/libboard_artiq/src/cxp.rs b/src/libboard_artiq/src/cxp.rs
new file mode 100644
index 0000000..7665d7e
--- /dev/null
+++ b/src/libboard_artiq/src/cxp.rs
@@ -0,0 +1,172 @@
+use embedded_hal::blocking::delay::DelayMs;
+use libboard_zynq::timer::GlobalTimer;
+use log::{info, warn};
+
+use crate::{cxp_comms,
+            cxp_comms::Error,
+            cxp_phys,
+            pl::{csr, csr::CXP}};
+
+const STANDARD: u32 = 0x0000;
+const REVISION: u32 = 0x0004;
+const CONNECTION_RESET: u32 = 0x4000;
+const CONTROL_PACKET_SIZE_MAX: u32 = 0x400C;
+const STREAM_PACKET_SIZE_MAX: u32 = 0x4010;
+const CONNECTION_CFG: u32 = 0x4014;
+const CONNECTION_CFG_DEF: u32 = 0x4018;
+const TESTMODE: u32 = 0x401C;
+const CAPABILITY_REGISTER: u32 = 0x403C;
+const VERSION_SUPPORTED: u32 = 0x4044;
+const VERSION_USED: u32 = 0x4048;
+
+const MASTER_CHANNEL: u8 = 0;
+
+pub fn setup(timer: &mut GlobalTimer) {
+    cxp_phys::setup(timer);
+    if let Err(e) = probe_connections(timer) {
+        warn!("CXP init failure {:?} ", e);
+    };
+}
+
+fn connection_reset(channel: u8) -> Result<(), Error> {
+    // ConnectionReset
+    // without this, camera will still blind red @ 1Hz (i.e. no connection)
+    // TODO: rename cxp_comms to cxp_ctrl
+    cxp_comms::write_u32_no_ack(channel, CONNECTION_RESET, 1, false)
+}
+
+fn scan_channels_status() -> Result<(), Error> {
+    let mut result = Err(Error::LinkDown);
+    for ch in 0..cxp_phys::CXP_CHANNELS {
+        if unsafe { (CXP[ch as usize].downconn_rx_ready_read)() } == 1 {
+            info!("CHANNEL #{} discovered", ch);
+            result = Ok(());
+        }
+    }
+    result
+}
+
+fn discover_channels(timer: &mut GlobalTimer) -> Result<(), Error> {
+    // only the simple topology MASTER:ch0, extension:ch1,2,3 is supported right now
+    // TODO: properly support other topology
+
+    // 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 rate
+    for speed in [cxp_phys::CXP_SPEED::CXP_1, cxp_phys::CXP_SPEED::CXP_3].iter() {
+        // Section 12.1.2 (CXP-001-2021)
+        // reset -> wait 200ms -> scan for active channels
+        connection_reset(MASTER_CHANNEL)?;
+
+        cxp_phys::change_linerate(*speed);
+        timer.delay_ms(200);
+        if scan_channels_status().is_ok() {
+            return Ok(());
+        }
+    }
+    Err(Error::LinkDown)
+}
+
+fn negotiate_cxp_version() -> Result<bool, Error> {
+    let rev = cxp_comms::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 = cxp_comms::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);
+        }
+
+        cxp_comms::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
+    );
+
+    let with_tag = major_rev >= 2;
+    Ok(with_tag)
+}
+
+fn negotiate_pak_max_size(with_tag: bool) -> Result<(), Error> {
+    let reg = cxp_comms::read_u32(MASTER_CHANNEL, CONTROL_PACKET_SIZE_MAX, with_tag)?;
+    info!("Max CTRL PAK size = {:#010X}", reg);
+
+    let reg = cxp_comms::read_u32(MASTER_CHANNEL, STREAM_PACKET_SIZE_MAX, with_tag)?;
+    info!("Max STREAM PAK size = {:#010X}", reg);
+    // TODO: set stream packet size
+    Ok(())
+}
+
+fn check_standard() -> Result<(), Error> {
+    let reg = cxp_comms::read_u32(MASTER_CHANNEL, STANDARD, true)?;
+    info!("CoaXPress special WORD = {:#010X} !!!", reg);
+    Ok(())
+}
+
+fn check_optional_features() -> Result<(), Error> {
+    let reg = cxp_comms::read_u32(MASTER_CHANNEL, CAPABILITY_REGISTER, true)?;
+    info!("Camera optional feature = {:#010X}", reg);
+    Ok(())
+}
+
+fn decode_cxp_speed(code: u32) -> Option<cxp_phys::CXP_SPEED> {
+    match code {
+        0x28 => Some(cxp_phys::CXP_SPEED::CXP_1),
+        0x30 => Some(cxp_phys::CXP_SPEED::CXP_2),
+        0x38 => Some(cxp_phys::CXP_SPEED::CXP_3),
+        0x40 => Some(cxp_phys::CXP_SPEED::CXP_5),
+        0x48 => Some(cxp_phys::CXP_SPEED::CXP_6),
+        0x50 => Some(cxp_phys::CXP_SPEED::CXP_10),
+        0x58 => Some(cxp_phys::CXP_SPEED::CXP_12),
+        _ => None,
+    }
+}
+
+fn set_operation_linerate(with_tag: bool, timer: &mut GlobalTimer) -> Result<(), Error> {
+    let reg = cxp_comms::read_u32(MASTER_CHANNEL, CONNECTION_CFG, with_tag)?;
+    info!("Current connection cfg = {:#010X}", reg);
+
+    let cfg = cxp_comms::read_u32(MASTER_CHANNEL, CONNECTION_CFG_DEF, with_tag)?;
+    info!("Best connection cfg = {:#010X}", cfg);
+
+    if let Some(speed) = decode_cxp_speed(cfg & 0xFF) {
+        cxp_comms::write_u32(MASTER_CHANNEL, CONNECTION_CFG, cfg, with_tag)?;
+
+        cxp_phys::change_linerate(speed);
+        timer.delay_ms(500);
+        scan_channels_status()
+    } else {
+        Err(Error::UnsupportedSpeed)
+    }
+}
+
+fn probe_connections(timer: &mut GlobalTimer) -> Result<(), Error> {
+    discover_channels(timer)?;
+    // TODO: Section 12.1.3 (CXP-001-2021)
+    // TODO: add a topology struct and output master channel??
+    // discover_topology(); // get master_channel here
+
+    let with_tag = negotiate_cxp_version()?;
+    // CXP 2.x ONLY, need TAGS
+    // negotiate_pak_max_size(with_tag)?;
+    set_operation_linerate(with_tag, timer)?;
+
+    // DEBUG: print
+    // check_standard()?;
+    // check_optional_features()?;
+    Ok(())
+}