Check if a cosmo flash sector is erased before erasing

The Winbond flash chip on Cosmo has a unique flash erase algorithm
that significantly penalizes erasing already erased sectors.
Check for this case before running a sector erase command.

Before
real	3m31.962s
user	0m4.325s
sys	0m3.188s

After
real	2m0.817s
user	0m4.380s
sys	0m3.305s

Author: labbott

drv/cosmo-hf/src/main.rs

@@ -270,6 +270,9 @@ impl FlashDriver {
 
     /// Erases the 64KiB flash sector containing the given address
     fn flash_sector_erase(&mut self, addr: FlashAddr) {
+        if self.flash_is_sector_erased(addr) {
+            return;
+        }
         self.flash_write_enable();
         self.drv.data_bytes.set_count(0);
         self.drv.addr.set_addr(addr.0);
@@ -281,6 +284,34 @@ impl FlashDriver {
         self.wait_flash_busy(Trace::SectorEraseBusy);
     }
 
+    /// Returns true if the full 64KB sector is erased
+    /// (all bits are set to `1`)
+    fn flash_is_sector_erased(&mut self, addr: FlashAddr) -> bool {
+        let cnt = SECTOR_SIZE_BYTES / (PAGE_SIZE_BYTES as u32);
+        for i in 0..cnt {
+            let addr = addr.0 + i * (PAGE_SIZE_BYTES as u32);
+            self.clear_fifos();
+            self.drv.data_bytes.set_count(PAGE_SIZE_BYTES as u16);
+            self.drv.addr.set_addr(addr);
+            self.drv.dummy_cycles.set_count(8);
+            self.drv.instr.set_opcode(instr::FAST_READ_QUAD_OUTPUT_4B);
+            let mut erased = true;
+            // We need to complete the read of the full PAGE_SIZE
+            // even if we find a non-erased byte
+            for _ in 0..PAGE_SIZE_BYTES.div_ceil(4) {
+                self.wait_fpga_rx();
+                let v = self.drv.rx_fifo_rdata.fifo_data();
+                if v != u32::MAX {
+                    erased = false;
+                }
+            }
+            if !erased {
+                return false;
+            }
+        }
+        true
+    }
+
     /// Reads data from the given address into a `BufWriter`
     ///
     /// This function will only return an error if it fails to read from a