Started implementing tlps

examples/read_pcileech.rs

@@ -4,7 +4,16 @@ use memflow_core::connector::ConnectorArgs;
 use memflow_pcileech::{create_connector, PcieGen};
 
 fn main() {
-    simple_logger::init_with_level(Level::Debug).unwrap();
+    simple_logger::init_with_level(Level::Trace).unwrap();
     let mut conn = create_connector(&ConnectorArgs::new()).unwrap();
     conn.set_pcie_gen(PcieGen::Gen2).unwrap();
+
+    // TODO: put this + more in a conn print trait ->
+    println!(
+        "pcie device opened with link width {} and pcie gen {}",
+        conn.pcie_link_width(),
+        conn.pcie_gen()
+    );
+
+    conn.test_read().unwrap();
 }

src/fpga.rs

@@ -1,3 +1,7 @@
+// TODO: unpub?
+pub mod tlps;
+use tlps::*;
+
 use crate::ft60x::*;
 
 use core::mem::MaybeUninit;
@@ -159,7 +163,6 @@ impl Device {
         let mut device_id = self
             .read_config::<u16>(0x0008, FPGA_CONFIG_PCIE | FPGA_CONFIG_SPACE_READONLY)
             .unwrap_or_default();
-        info!("device_id={}", device_id);
         if device_id == 0 {
             info!("pci device_id is unset. checking pcie magic.");
 
@@ -176,10 +179,11 @@ impl Device {
                     .unwrap_or_default();
             }
         }
-        info!("device_id={:?}", device_id);
+        let device_id_le = device_id.to_be();
+        info!("device_id={:?}", device_id_le);
 
-        // ctx->wDeviceId = _byteswap_ushort(wbsDeviceId);
-        Ok((fpga_id, device_id))
+        // swap device_id bytes only on LE systems
+        Ok((fpga_id, device_id_le))
     }
 
     pub fn write_inactivity_timer(&mut self) -> Result<()> {
@@ -264,6 +268,127 @@ impl Device {
         Ok(buf)
     }
 
+    // TODO: implement more config dump options
+    fn get_pcie_drp(&mut self) {
+        let read_enable = [0x10, 0x00, 0x10, 0x00, 0x80, 0x02, 0x23, 0x77];
+        let read_address = [0x00, 0x00, 0xff, 0xff, 0x80, 0x1c, 0x23, 0x77];
+        let result_meta = [0x00, 0x00, 0x00, 0x00, 0x80, 0x1c, 0x13, 0x77];
+        let result_data = [0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x13, 0x77];
+
+        // create read request
+        for addr in (0..0x100).step_by(32) {
+            for dw in (0..32).step_by(2) {}
+        }
+
+        // interpret result
+    }
+
+    // TODO: send_tlp_ ...
+    /*pub fn send_tlp32(&mut self, tlp: u32, keep_alive: bool, flush: bool) -> Result<()> {
+        self.send_tlp_raw(tlp.as_bytes(), keep_alive, flush)
+    }
+
+    pub fn send_tlp64(&mut self, tlp: u64, keep_alive: bool, flush: bool) -> Result<()> {
+        self.send_tlp_raw(tlp.as_bytes(), keep_alive, flush)
+    }*/
+
+    // TODO: this is duplicated code (see config_parse_response)
+    pub fn recv_tlps_64(&mut self, bytes: u32 /* maybe u16? */) -> Result<()> {
+        let mut respbuf = vec![0u8; 0x1000]; // TEST
+        self.ft60.read_pipe(&mut respbuf)?;
+
+        let view = respbuf.as_data_view();
+        let mut skip = 0;
+        for i in (0..respbuf.len()).step_by(32) {
+            if i + skip >= respbuf.len() {
+                break;
+            }
+
+            while view.copy::<u32>(i + skip) == 0x55556666 {
+                trace!("ftdi workaround detected, skipping 4 bytes");
+                skip += 4;
+                if i + skip + 32 > respbuf.len() {
+                    return Err(Error::Connector("out of range config read"));
+                }
+            }
+
+            let mut status = view.copy::<u32>(i + skip);
+            if status & 0xf0000000 != 0xe0000000 {
+                trace!("invalid status reply, skipping");
+                continue;
+            }
+
+            trace!("parsing data buffer");
+            for j in 0..7 {
+                if (status & 0x03) == 0 {
+                    println!("pcie tlp received :)");
+                }
+                if (status & 0x07) == 4 {
+                    println!("pcie tlp LAST received :)");
+                }
+            }
+        }
+
+        Ok(())
+    }
+
+    pub fn send_tlps_64(&mut self, tlps: &[TlpReadWrite64], keep_alive: bool) -> Result<()> {
+        let bytes = tlps
+            .iter()
+            .map(|t| t.as_bytes())
+            .collect::<Vec<_>>()
+            .concat();
+
+        let bytes32 =
+            unsafe { std::slice::from_raw_parts(bytes.as_ptr() as *const u32, bytes.len() / 4) };
+
+        self.send_tlps_raw(&bytes32, keep_alive)
+    }
+
+    /// Send a TLP to the fpga
+    fn send_tlps_raw(&mut self, tlps: &[u32], keep_alive: bool) -> Result<()> {
+        if tlps.len() > 4 + 32 {
+            return Err(Error::Connector("tlp buffer is too large"));
+        }
+        /*
+        if(cbTlp && (ctx->txbuf.cb + (cbTlp << 1) + (fFlush ? 8 : 0) >= ctx->perf.MAX_SIZE_TX)) {
+            if(!DeviceFPGA_TxTlp(ctxLC, ctx, NULL, 0, FALSE, TRUE)) { return FALSE; }
+        }
+        */
+
+        // TLP_Print
+
+        // create transmit buffer
+        let mut buf = Vec::new();
+        for tlp in tlps.iter() {
+            buf.push(tlp.to_be());
+            buf.push(0x77000000); // TX TLP
+        }
+
+        // TODO: remove this pop in the algorithm
+        if !tlps.is_empty() {
+            buf.pop();
+            buf.push(0x77040000); // TX TLP VALID LAST
+        }
+
+        if keep_alive {
+            buf.push(0xffeeddcc);
+            buf.push(0x77020000);
+        }
+
+        // currently we just flush out every tlp transmission immediately
+        // and not buffer them internally.
+        let byte_buf: &[u8] = unsafe { std::mem::transmute(buf.as_slice()) };
+
+        // TODO: handle error codes?
+        self.ft60.write_pipe(byte_buf)?;
+        //        if status == 0x20 {
+        // failure
+        //      }
+
+        Ok(())
+    }
+
     #[allow(clippy::uninit_assumed_init)]
     fn read_config<T: Pod>(&mut self, addr: u16, flags: u16) -> Result<T> {
         let mut obj: T = unsafe { MaybeUninit::uninit().assume_init() };

src/fpga/tlps.rs

@@ -0,0 +1,141 @@
+use bitfield::bitfield;
+use dataview::{DataView, Pod};
+
+const TLP_READ_32: u8 = 0x00;
+const TLP_READ_64: u8 = 0x20;
+
+const fn pack_bits4(first: u8, second: u8) -> u8 {
+    (first & ((1 << 4) - 1)) + ((second & ((1 << 4) - 1)) << 4)
+}
+
+const fn split_addr64_high(address: u64) -> u32 {
+    ((address & ((1 << 32) - 1)) >> 32) as u32
+}
+
+const fn split_addr64_low(address: u64) -> u32 {
+    (address & ((1 << 32) - 1)) as u32
+}
+
+bitfield! {
+    pub struct TlpHeader(u32);
+    impl Debug;
+    pub len_tlps, set_len_tlps: 9, 0;
+    pub at, _: 11, 10;
+    pub attr, _: 13, 12;
+    pub ep, _: 14;
+    pub td, _: 15;
+    pub r1, _: 19, 16;
+    pub tc, _: 22, 20;
+    pub r2, _: 23;
+    pub ty, set_ty: 31, 24;
+}
+const _: [(); core::mem::size_of::<TlpHeader>()] = [(); 4];
+
+unsafe impl Pod for TlpHeader {}
+
+impl TlpHeader {
+    pub fn new(ty: u8, len_tlps: u16) -> Self {
+        let mut h = Self { 0: 0 };
+
+        println!("ok0");
+        h.set_ty(ty as u32);
+        println!("ok1");
+        h.set_len_tlps(len_tlps as u32);
+        println!("ok2");
+        h
+    }
+}
+
+#[repr(C)]
+#[derive(Pod)]
+pub struct TlpReadWrite32 {
+    header: TlpHeader,
+    be: u8,
+    tag: u8,
+    requester_id: u16,
+    address: u32,
+}
+const _: [(); core::mem::size_of::<TlpReadWrite32>()] = [(); 0xC];
+
+#[allow(unused)]
+impl TlpReadWrite32 {
+    pub fn new_read(address: u32, len: u16, tag: u8, requester_id: u16) -> Self {
+        Self {
+            header: TlpHeader::new(TLP_READ_32, len / 4),
+            be: pack_bits4(0xF, 0xF),
+            tag,
+            requester_id,
+            address,
+        }
+    }
+
+    pub fn set_be(&mut self, first: u8, second: u8) {
+        self.be = pack_bits4(first, second);
+    }
+
+    pub fn set_tag(&mut self, tag: u8) {
+        self.tag = tag;
+    }
+
+    pub fn set_requester_id(&mut self, id: u16) {
+        self.requester_id = id;
+    }
+
+    pub fn set_address(&mut self, address: u32) {
+        self.address = address;
+    }
+}
+
+#[repr(C)]
+#[derive(Pod)]
+pub struct TlpReadWrite64 {
+    header: TlpHeader,
+    be: u8,
+    tag: u8,
+    requester_id: u16,
+    address_high: u32,
+    address_low: u32,
+}
+const _: [(); core::mem::size_of::<TlpReadWrite64>()] = [(); 0x10];
+
+#[allow(unused)]
+impl TlpReadWrite64 {
+    pub fn new_read(address: u64, len: u16, tag: u8, requester_id: u16) -> Self {
+        Self {
+            header: TlpHeader::new(TLP_READ_64, len / 4),
+            be: pack_bits4(0xF, 0xF),
+            tag,
+            requester_id,
+            address_high: split_addr64_high(address),
+            address_low: split_addr64_low(address),
+        }
+    }
+
+    pub fn set_be(&mut self, first: u8, second: u8) {
+        self.be = pack_bits4(first, second);
+    }
+
+    pub fn set_tag(&mut self, tag: u8) {
+        self.tag = tag;
+    }
+
+    pub fn set_requester_id(&mut self, id: u16) {
+        self.requester_id = id;
+    }
+
+    pub fn set_address(&mut self, address: u64) {
+        self.address_high = split_addr64_high(address);
+        self.address_low = split_addr64_low(address);
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::pack_bits4;
+
+    #[test]
+    fn test_pack_bits4() {
+        assert_eq!(pack_bits4(0xA, 0xB), 0xBA);
+        assert_eq!(pack_bits4(0xAB, 0xCD), 0xDB);
+    }
+}

src/lib.rs

@@ -1,6 +1,9 @@
 mod fpga;
 mod ft60x;
 
+//TODO: move?
+use fpga::tlps::*;
+
 use log::{info, warn};
 
 use fpga::{PhyConfigRd, PhyConfigWr};
@@ -61,6 +64,23 @@ impl PciLeech {
         })
     }
 
+    pub fn pcie_link_width(&self) -> u8 {
+        match self.phy_rd.pl_sel_lnk_width() {
+            0 => 1,
+            1 => 2,
+            2 => 4,
+            3 => 8,
+            _ => 0, // invalid
+        }
+    }
+
+    pub fn pcie_gen(&self) -> u8 {
+        match self.phy_rd.pl_sel_lnk_rate() {
+            false => 1,
+            true => 2,
+        }
+    }
+
     pub fn set_pcie_gen(&mut self, gen: PcieGen) -> Result<()> {
         let gen2 = match gen {
             PcieGen::Gen1 => false,
@@ -108,6 +128,34 @@ impl PciLeech {
 
         Ok(())
     }
+
+    // test read functions
+    pub fn test_read(&mut self) -> Result<()> {
+        // create read request
+        /*
+        // cb
+        // device id
+        // addr
+         */
+
+        // TODO: 32bit target
+        let tag = 0; // 0x80 for ecc?
+        println!("stuff0");
+        let tlp = TlpReadWrite64::new_read(0x1000, 0x1000, tag, self.device_id);
+        // TODO: tag++ for every tlp in one request?
+        println!("stuff1");
+        self.device.send_tlps_64(&[tlp], false)?;
+
+        // TODO: read stuff back synchronously?
+        println!("stuff2");
+        std::thread::sleep(std::time::Duration::from_millis(25));
+
+        // bytes added together from read requests
+        self.device.recv_tlps_64(0x1000)?;
+        println!("stuff3");
+
+        Ok(())
+    }
 }
 
 impl PhysicalMemory for PciLeech {