From 89dfb00b04bff7fd1f8656744ef8d74bd79a9a6a Mon Sep 17 00:00:00 2001
From: Alex Voinea <voinea.dragos.alexandru@gmail.com>
Date: Sun, 8 Dec 2019 08:43:43 +0200
Subject: [PATCH] Fix optiboot erase function after address 0xFFFF

---
 Firmware/optiboot_w25x20cl.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Firmware/optiboot_w25x20cl.cpp b/Firmware/optiboot_w25x20cl.cpp
index e84bf639..584c32fe 100644
--- a/Firmware/optiboot_w25x20cl.cpp
+++ b/Firmware/optiboot_w25x20cl.cpp
@@ -258,11 +258,11 @@ void optiboot_w25x20cl_enter()
         uint32_t addr = (((uint32_t)rampz) << 16) | address;
         // During a single bootloader run, only erase a 64kB block once.
         // An 8bit bitmask 'pages_erased' covers 512kB of FLASH memory.
-        if (address == 0 && (pages_erased & (1 << addr)) == 0) {
+        if ((address == 0) && (pages_erased & (1 << (addr >> 16))) == 0) {
           w25x20cl_wait_busy();
           w25x20cl_enable_wr();
           w25x20cl_block64_erase(addr);
-          pages_erased |= (1 << addr);
+          pages_erased |= (1 << (addr >> 16));
         }
         w25x20cl_wait_busy();
         w25x20cl_enable_wr();