experiments: implement ps7_init::apply()

experiments/src/main.rs

@@ -42,6 +42,8 @@ const HWADDR: [u8; 6] = [0, 0x23, 0xde, 0xea, 0xbe, 0xef];
 pub fn main_core0() {
     // zynq::clocks::CpuClocks::enable_io(1_250_000_000);
     println!("\nzc706 main");
+    ps7_init::apply();
+    libboard_zynq::stdio::drop_uart();
 
     libboard_zynq::logger::init().unwrap();
     log::set_max_level(log::LevelFilter::Trace);

experiments/src/ps7_init/mod.rs

@@ -22,9 +22,17 @@ pub fn report_differences() {
     }
 }
 
+pub fn apply() {
+    for op in target::INIT_DATA {
+        op.apply();
+    }
+}
+
+#[derive(Clone, Debug)]
 pub enum InitOp {
     MaskWrite(usize, usize, usize),
     MaskPoll(usize, usize),
+    MaskDelay(usize, usize),
 }
 
 impl InitOp {
@@ -32,6 +40,7 @@ impl InitOp {
         match self {
             InitOp::MaskWrite(address, _, _) => *address,
             InitOp::MaskPoll(address, _) => *address,
+            InitOp::MaskDelay(address, _) => *address,
         }
     }
 
@@ -40,17 +49,24 @@ impl InitOp {
     }
 
     fn difference(&self) -> Option<(usize, usize)> {
-        let (mask, expected) = match self {
+        let expected = match self {
             InitOp::MaskWrite(_, mask, expected) =>
-                (*mask, *expected),
+                Some((*mask, *expected)),
             InitOp::MaskPoll(_, mask) =>
-                (*mask, *mask),
+                Some((*mask, *mask)),
+            _ => None,
         };
-        let actual = self.read();
+        match expected {
-        if actual & mask == expected {
+            Some((mask, expected)) => {
-            None
+                let actual = self.read();
-        } else {
+                if actual & mask == expected {
-            Some((actual & mask, expected))
+                    None
+                } else {
+                    Some((actual & mask, expected))
+                }
+            }
+            None =>
+                None
         }
     }
 
@@ -65,4 +81,28 @@ impl InitOp {
             );
         }
     }
+
+    pub fn apply(&self) {
+        let reg = self.address() as *mut usize;
+        println!("apply {:?}", self);
+        match self {
+            InitOp::MaskWrite(_, mask, val) =>
+                unsafe {
+                    *reg = (val & mask) | (*reg & !mask);
+                },
+            InitOp::MaskPoll(_, mask) =>
+                while unsafe { *reg } & mask == 0 {},
+            InitOp::MaskDelay(_, mask) => {
+                let delay = get_number_of_cycles_for_delay(*mask);
+                while unsafe { *reg } < delay {
+                    println!("W");
+                }
+            }
+        }
+    }
+}
+
+fn get_number_of_cycles_for_delay(delay: usize) -> usize {
+    const APU_FREQ: usize = 666666687;
+    APU_FREQ * delay/ (2 * 1000)
 }