diff --git a/src/libboard_artiq/src/cxp_upconn.rs b/src/libboard_artiq/src/cxp_upconn.rs
new file mode 100644
index 0000000..f1710b5
--- /dev/null
+++ b/src/libboard_artiq/src/cxp_upconn.rs
@@ -0,0 +1,180 @@
+use core_io::{Error as IoError, Write};
+use crc::crc32;
+use embedded_hal::prelude::_embedded_hal_blocking_delay_DelayUs;
+use io::Cursor;
+use libboard_zynq::{println, timer::GlobalTimer};
+
+use crate::pl::csr;
+
+#[derive(Debug)]
+pub enum Error {
+    BufferError,
+    LinkDown,
+}
+
+impl From<IoError> for Error {
+    fn from(_: IoError) -> Error {
+        Error::BufferError
+    }
+}
+
+pub fn tx_test(timer: &mut GlobalTimer) {
+    const LEN: usize = 4 * 20;
+    let mut pak_arr: [u8; LEN] = [0; LEN];
+
+    unsafe {
+        csr::cxp::upconn_clk_reset_write(1);
+        // csr::cxp::upconn_bitrate2x_enable_write(1);
+        csr::cxp::upconn_clk_reset_write(0);
+
+        send(&Packet::ControlU32Reg(Command::Read { addr: 0x00 })).expect("Cannot send CoaXpress packet");
+
+        csr::cxp::upconn_tx_enable_write(1);
+
+        timer.delay_us(2);
+        // DEBUG: Trigger packet
+        let linktrig_mode: u8 = 0x01;
+        csr::cxp::upconn_trig_delay_write(0x05);
+        csr::cxp::upconn_linktrigger_write(linktrig_mode);
+        csr::cxp::upconn_trig_stb_write(1); // send trig
+
+        // DEBUG: Trigger ACK packet
+        // csr::cxp::upconn_ack_write(1);
+        timer.delay_us(20);
+
+        csr::cxp::upconn_tx_enable_write(0);
+
+        // Collect data
+        let mut i: usize = 0;
+        while csr::cxp::upconn_upconn_phy_debug_buf_dout_valid_read() == 1 {
+            pak_arr[i] = csr::cxp::upconn_upconn_phy_debug_buf_dout_pak_read();
+            // println!("received {:#04X}", pak_arr[i]);
+            csr::cxp::upconn_upconn_phy_debug_buf_inc_write(1);
+            i += 1;
+            if i == LEN {
+                break;
+            }
+        }
+
+        print_packet(&pak_arr);
+    }
+}
+
+pub enum Command<T> {
+    Read { addr: u32 },
+    Write { addr: u32, data: T },
+    ReadWithTag { addr: u32, tag: u8 },
+    WriteWithTag { addr: u32, data: T, tag: u8 },
+}
+
+pub enum Packet {
+    ControlU32Reg(Command<u32>),
+    ControlU64Reg(Command<u64>),
+}
+
+impl Packet {
+    pub fn write_to<W>(&self, writer: &mut W) -> Result<(), Error>
+    where W: Write {
+        match self {
+            Packet::ControlU32Reg(cmd) => match cmd {
+                Command::Read { addr } => {
+                    writer.write(&[0x02; 4])?;
+                    writer.write(&[0x00, 0x00, 0x00, 0x04])?;
+                    writer.write(&addr.to_be_bytes())?;
+                }
+                Command::Write { addr, data } => {
+                    writer.write(&[0x02; 4])?;
+                    writer.write(&[0x01, 0x00, 0x00, 0x04])?;
+                    writer.write(&addr.to_be_bytes())?;
+                    writer.write(&data.to_be_bytes())?;
+                }
+                Command::ReadWithTag { addr, tag } => {
+                    writer.write(&[0x05; 4])?;
+                    writer.write(&[*tag; 4])?;
+                    writer.write(&[0x00, 0x00, 0x00, 0x04])?;
+                    writer.write(&addr.to_be_bytes())?;
+                }
+                Command::WriteWithTag { addr, data, tag } => {
+                    writer.write(&[0x05; 4])?;
+                    writer.write(&[*tag; 4])?;
+                    writer.write(&[0x01, 0x00, 0x00, 0x04])?;
+                    writer.write(&addr.to_be_bytes())?;
+                    writer.write(&data.to_be_bytes())?;
+                }
+            },
+            Packet::ControlU64Reg(cmd) => match cmd {
+                Command::Read { addr } => {
+                    writer.write(&[0x02; 4])?;
+                    writer.write(&[0x00, 0x00, 0x00, 0x08])?;
+                    writer.write(&addr.to_be_bytes())?;
+                }
+                Command::Write { addr, data } => {
+                    writer.write(&[0x02; 4])?;
+                    writer.write(&[0x01, 0x00, 0x00, 0x08])?;
+                    writer.write(&addr.to_be_bytes())?;
+                    writer.write(&data.to_be_bytes())?;
+                }
+                Command::ReadWithTag { addr, tag } => {
+                    writer.write(&[0x05; 4])?;
+                    writer.write(&[*tag; 4])?;
+                    writer.write(&[0x00, 0x00, 0x00, 0x08])?;
+                    writer.write(&addr.to_be_bytes())?;
+                }
+                Command::WriteWithTag { addr, data, tag } => {
+                    writer.write(&[0x05; 4])?;
+                    writer.write(&[*tag; 4])?;
+                    writer.write(&[0x01, 0x00, 0x00, 0x08])?;
+                    writer.write(&addr.to_be_bytes())?;
+                    writer.write(&data.to_be_bytes())?;
+                }
+            },
+        }
+        Ok(())
+    }
+}
+
+pub fn send(packet: &Packet) -> Result<(), Error> {
+    // DEBUG: remove the comment out section
+    // if unsafe { csr::cxp::upconn_tx_enable_read() } == 0 {
+    //     Err(Error::LinkDown)?
+    // }
+
+    const LEN: usize = 4 * 20;
+    let mut buffer: [u8; LEN] = [0; LEN];
+    let mut writer = Cursor::new(&mut buffer[..]);
+
+    packet.write_to(&mut writer)?;
+
+    // Section 9.2.2.2 (CXP-001-2021)
+    // CoaXpress use the polynomial of IEEE-802.3 (Ethernet) CRC but the checksum calculation is different
+    // Also, the calculation does not include the first 4 bytes of packet_type
+    let checksum = crc32::checksum_ieee(&writer.get_ref()[4..writer.position()]);
+    writer.write(&(!checksum).to_le_bytes())?;
+
+    unsafe {
+        let len = writer.position();
+        csr::cxp::upconn_command_len_write(len as u8);
+        for data in writer.get_ref()[..len].iter() {
+            while csr::cxp::upconn_command_writeable_read() == 0 {}
+            csr::cxp::upconn_command_data_write(*data);
+        }
+    }
+
+    Ok(())
+}
+
+fn print_packet(pak: &[u8]) {
+    println!("pak = [");
+    for i in 0..(pak.len() / 4) {
+        println!(
+            "{:#03}     {:#04X} {:#04X} {:#04X} {:#04X},",
+            i + 1,
+            pak[i * 4],
+            pak[i * 4 + 1],
+            pak[i * 4 + 2],
+            pak[i * 4 + 3]
+        )
+    }
+    println!("]");
+    println!("============================================");
+}
diff --git a/src/libboard_artiq/src/lib.rs b/src/libboard_artiq/src/lib.rs
index 70c4cb6..008c2aa 100644
--- a/src/libboard_artiq/src/lib.rs
+++ b/src/libboard_artiq/src/lib.rs
@@ -42,6 +42,8 @@ pub mod si5324;
 pub mod si549;
 use core::{cmp, str};
 
+#[cfg(has_cxp)]
+pub mod cxp_upconn;
 pub fn identifier_read(buf: &mut [u8]) -> &str {
     unsafe {
         pl::csr::identifier::address_write(0);