From 5268839467a0f220c9ffc5cbc2fdad53e87265ab Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Sat, 14 Dec 2019 01:56:49 +0100
Subject: [PATCH] zynq::flash: add write_enabled()

---
 src/zynq/flash/mod.rs  | 23 +++++++++++++++++++++++
 src/zynq/flash/regs.rs |  8 +++++++-
 2 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/src/zynq/flash/mod.rs b/src/zynq/flash/mod.rs
index 950cebd..6bea2d4 100644
--- a/src/zynq/flash/mod.rs
+++ b/src/zynq/flash/mod.rs
@@ -16,6 +16,11 @@ const SINGLE_CAPACITY: u32 = 16 * 1024 * 1024;
 
 /// Instruction: Read Identification
 const INST_RDID: u8 = 0x9F;
+const INST_READ: u8 = 0x03;
+/// Instruction: Write Disable
+const INST_WRDI: u8 = 0x04;
+/// Instruction: Write Enable
+const INST_WREN: u8 = 0x06;
 
 #[derive(Clone)]
 pub enum SpiWord {
@@ -370,6 +375,24 @@ impl Flash<Manual> {
             .bytes_transfer().skip(6).take(len)
     }
 
+    pub fn write_enabled<F: Fn(&mut Self) -> R, R>(&mut self, f: F) -> R {
+        // Write Enable
+        let args = Some(INST_WREN);
+        self.transfer(args.into_iter(), 1);
+        self.regs.gpio.modify(|_, w| w.wp_n(true));
+        while !self.read_reg::<SR1>().wel() {}
+
+        let result = f(self);
+
+        // Write Disable
+        let args = Some(INST_WRDI);
+        self.transfer(args.into_iter(), 1);
+        self.regs.gpio.modify(|_, w| w.wp_n(false));
+        while self.read_reg::<SR1>().wel() {}
+
+        result
+    }
+
     pub fn transfer<'s: 't, 't, Args, W>(&'s mut self, args: Args, len: usize) -> Transfer<'t, Args, W>
     where
         Args: Iterator<Item = W>,
diff --git a/src/zynq/flash/regs.rs b/src/zynq/flash/regs.rs
index 01a50ea..bc1f0d9 100644
--- a/src/zynq/flash/regs.rs
+++ b/src/zynq/flash/regs.rs
@@ -16,7 +16,7 @@ pub struct RegisterBlock {
     pub slave_idle_count: RW<u32>,
     pub tx_thres: RW<u32>,
     pub rx_thres: RW<u32>,
-    pub gpio: RW<u32>,
+    pub gpio: QspiGpio,
     pub _unused1: RO<u32>,
     pub lpbk_dly_adj: RW<u32>,
     pub _unused2: [RO<u32>; 17],
@@ -108,6 +108,12 @@ register_bit!(intr_dis, tx_fifo_underflow, 6);
 register!(enable, Enable, RW, u32);
 register_bit!(enable, spi_en, 0);
 
+// named to avoid confusion with normal gpio
+register!(qspi_gpio, QspiGpio, RW, u32);
+register_bit!(qspi_gpio,
+              /// Write protect pin (inverted)
+              wp_n, 0);
+
 register!(lqspi_cfg, LqspiCfg, RW, u32);
 register_bits!(lqspi_cfg, inst_code, u8, 0, 7);
 register_bits!(lqspi_cfg, dummy_byte, u8, 8, 10);