From 2e928788699cca76db883103c726c8834bdc0e66 Mon Sep 17 00:00:00 2001
From: OBattler <oubattler@gmail.com>
Date: Tue, 20 Oct 2020 19:33:10 +0200
Subject: [PATCH] The 29*0x0 Flash now correctly determines the page address by
 the address of the last byte loaded, and only writes the bytes in the buffer
 that marked as dirty.

---
 src/mem/sst_flash.c | 25 ++++++++++++++++++-------
 1 file changed, 18 insertions(+), 7 deletions(-)

diff --git a/src/mem/sst_flash.c b/src/mem/sst_flash.c
index 0a6cd1b12..37d514a3b 100644
--- a/src/mem/sst_flash.c
+++ b/src/mem/sst_flash.c
@@ -40,9 +40,11 @@ typedef struct sst_t
 			dirty;
 
     uint32_t		size, mask,
-			page_mask, page_base;
+			page_mask, page_base,
+			last_addr;
         
-    uint8_t		page_buffer[128];
+    uint8_t		page_buffer[128],
+			page_dirty[128];
     uint8_t		*array;
 
     mem_mapping_t	mapping[8], mapping_h[8];
@@ -127,6 +129,7 @@ sst_new_command(sst_t *dev, uint32_t addr, uint8_t val)
 		if (!dev->is_39) {
 			dev->sdp = 1;
 			memset(dev->page_buffer, 0xff, 128);
+			memset(dev->page_dirty, 0x00, 128);
 			dev->page_bytes = 0;
 		}
 		dev->command_state = 6;
@@ -148,8 +151,13 @@ static void
 sst_page_write(void *priv)
 {
     sst_t *dev = (sst_t *) priv;
+    int i;
 
-    memcpy(&(dev->array[dev->page_base]), dev->page_buffer, 128);
+    dev->page_base = dev->last_addr & dev->page_mask;
+    for (i = 0; i < 128; i++) {
+	if (dev->page_dirty[i])
+		dev->array[dev->page_base + i] = dev->page_buffer[i];
+    }
     dev->dirty = 1;
     dev->page_bytes = 0;
     dev->command_state = 0;
@@ -174,11 +182,13 @@ static void
 sst_buf_write(sst_t *dev, uint32_t addr, uint8_t val)
 {
     dev->page_buffer[addr & 0x0000007f] = val;
+    dev->page_dirty[addr & 0x0000007f] = 1;
     timer_disable(&dev->page_write_timer);
     dev->page_bytes++;
-    if (dev->page_bytes >= 128)
+    dev->last_addr = addr;
+    if (dev->page_bytes >= 128) {
 	sst_page_write(dev);
-    else
+    } else
 	timer_on_auto(&dev->page_write_timer, 210.0);
 }
 
@@ -201,7 +211,9 @@ sst_write(uint32_t addr, uint8_t val, void *p)
 		else {
 			if (!dev->is_39 && !dev->sdp && (dev->command_state == 0)) {
 				/* 29 series, software data protection off, start loading the page. */
-				dev->page_base = addr & dev->page_mask;		/* First byte, A7 onwards of its address are the page mask. */
+				memset(dev->page_buffer, 0xff, 128);
+				memset(dev->page_dirty, 0x00, 128);
+				dev->page_bytes = 0;
 				dev->command_state = 7;
 				sst_buf_write(dev, addr, val);
 			}
@@ -231,7 +243,6 @@ sst_write(uint32_t addr, uint8_t val, void *p)
 			dev->command_state = 0;
 			dev->dirty = 1;
 		} else {
-			dev->page_base = addr & dev->page_mask;		/* First byte, A7 onwards of its address are the page mask. */
 			dev->command_state++;
 			sst_buf_write(dev, addr, val);
 		}