From dd0fe054d7353bc86880d08c7d9d541f47ee8b12 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Mon, 16 Dec 2019 00:48:39 +0100
Subject: [PATCH] zynq::flash: add working erase(), add barely working
 program()

---
 src/zynq/flash/mod.rs | 55 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 55 insertions(+)

diff --git a/src/zynq/flash/mod.rs b/src/zynq/flash/mod.rs
index 64f3877..85a80cc 100644
--- a/src/zynq/flash/mod.rs
+++ b/src/zynq/flash/mod.rs
@@ -1,5 +1,6 @@
 //! Quad-SPI Flash Controller
 
+use crate::{print, println};
 use core::marker::PhantomData;
 use crate::regs::{RegisterR, RegisterW, RegisterRW};
 use super::slcr;
@@ -26,6 +27,12 @@ const INST_READ: u8 = 0x03;
 const INST_WRDI: u8 = 0x04;
 /// Instruction: Write Enable
 const INST_WREN: u8 = 0x06;
+/// Instruction: Program page
+const INST_PP: u8 = 02;
+/// Instruction: Sector Erase
+const INST_SE: u8 = 0xD8;
+/// Instruction: Erase 4K Block
+const INST_BE_4K: u8 = 0x20;
 
 #[derive(Clone)]
 pub enum SpiWord {
@@ -411,6 +418,54 @@ impl Flash<Manual> {
             .bytes_transfer().skip(6).take(len)
     }
 
+    pub fn erase(&mut self, offset: u32) {
+        let args = Some(((INST_BE_4K as u32) << 24) | (offset as u32));
+        self.transfer(args.into_iter(), 4);
+
+        let sr1 = self.wait_while_sr1_zeroed();
+
+        if sr1.e_err() {
+            println!("E_ERR");
+        } else if sr1.p_err() {
+            println!("P_ERR");
+        } else if sr1.wip() {
+            print!("Erase in progress");
+            while self.read_reg::<SR1>().wip() {
+                print!(".");
+            }
+            println!("");
+        } else {
+            println!("erased? sr1={:02X}", sr1.inner);
+        }
+    }
+
+    pub fn program<I: Iterator<Item=u32>>(&mut self, offset: u32, data: I) {
+        {
+            let len = 4 + 4 * data.size_hint().0;
+            let args = Some(SpiWord::W32(((INST_PP as u32) << 24) | (offset as u32)))
+                .into_iter()
+                .chain(data.map(SpiWord::W32));
+            self.transfer(args, len);
+        }
+
+        // let sr1 = self.wait_while_sr1_zeroed();
+        let sr1 = self.read_reg::<SR1>();
+
+        if sr1.e_err() {
+            println!("E_ERR");
+        } else if sr1.p_err() {
+            println!("P_ERR");
+        } else if sr1.wip() {
+            println!("Program in progress");
+            while self.read_reg::<SR1>().wip() {
+                print!(".");
+            }
+            println!("");
+        } else {
+            println!("programmed? sr1={:02X}", sr1.inner);
+        }
+    }
+
     pub fn write_enabled<F: Fn(&mut Self) -> R, R>(&mut self, f: F) -> R {
         // Write Enable
         let args = Some(INST_WREN);