From d7220e1106acff55f8e32fa9f55be59209350167 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?King=20K=C3=A9vin?= <kingkevin@cuvoodoo.info>
Date: Sat, 15 Apr 2017 13:57:02 +0200
Subject: [PATCH] make flash library more general purpose and optimize if

---
 lib/flash_internal.c | 102 ++++++++++++++++++++++++-------------------
 1 file changed, 56 insertions(+), 46 deletions(-)

diff --git a/lib/flash_internal.c b/lib/flash_internal.c
index cf4dedf..f149cf0 100644
--- a/lib/flash_internal.c
+++ b/lib/flash_internal.c
@@ -34,16 +34,12 @@
 
 bool flash_internal_read(uint32_t address, uint8_t *buffer, size_t size)
 {
-	// verify it's in the storage area
-	if (address<STORAGE_START || (address+size)>STORAGE_END) {
-		return false;
-	}
-	if (buffer==NULL || size==0) {
+	// verify it's in the flash area and do other sanity checks
+	if (address<FLASH_BASE || address>(UINT32_MAX-size) || (address+size)>(FLASH_BASE+DESIG_FLASH_SIZE*1024) || buffer==NULL || size==0) {
 		return false;
 	}
 
-	// copy data byte per byte
-	// a more efficient way would be to copy words, than the remaining bytes
+	// copy data byte per byte (a more efficient way would be to copy words, than the remaining bytes)
 	for (size_t i=0; i<size; i++) {
 		buffer[i] = *((uint8_t*)address+i);
 	}
@@ -53,57 +49,71 @@ bool flash_internal_read(uint32_t address, uint8_t *buffer, size_t size)
 
 bool flash_internal_write(uint32_t address, uint8_t *buffer, size_t size)
 {
-	// verify it's in the storage area
-	if (address<STORAGE_START || (address+size)>STORAGE_END) {
+	// verify it's in the flash area and do other sanity checks
+	if (address<FLASH_BASE || address>(UINT32_MAX-size) || (address+size)>(FLASH_BASE+DESIG_FLASH_SIZE*1024) || buffer==NULL || size==0 || size%2) {
 		return false;
 	}
-	if (buffer==NULL || size==0) {
-		return false;
-	}
-
-	uint8_t page[PAGE_SIZE]; // the complete page to write
 
 	flash_unlock(); // unlock flash to be able to write it
-	// go through memory
-	while (size) {
-		uint32_t page_pre = address%PAGE_SIZE; // the beginning data size in the page
-		address -= page_pre; // go to beginning of the page
-		storage_read(address, &page[0], page_pre); // copy existing data
-		if (size>=PAGE_SIZE-page_pre) { // no need to read tailing page data
-			for (uint16_t i=0; i<PAGE_SIZE-page_pre; i++) { // put buffer in page
-				page[page_pre+i] = buffer[i];
+	while (size) { // write page by page until all data has been written
+		uint32_t page_start = address-(address%PAGE_SIZE); // get start of the current page
+		bool erase = false; // verify if the flash to write is erased of if we need to erase the page
+		for (uint32_t flash=address; flash<(address+size) && flash<(page_start+PAGE_SIZE); flash += 2) { // go through page
+			if (*(uint16_t*)(flash)!=0xffff) { // is flash not erased
+				erase = true; // the erase flash
 			}
-			buffer += PAGE_SIZE-page_pre; // adjust remaining buffer
-			size -= PAGE_SIZE-page_pre; // adjust remaining size
-		} else { // need read tailing page data
-			for (uint16_t i=0; i<size; i++) { // put buffer in page
-				page[page_pre+i] = buffer[i];
+		}
+		if (erase) { // make copy of the page to erase and erase it
+			uint8_t page_data[PAGE_SIZE]; // a copy of the complete page before the erase it
+			uint16_t page_i = 0; // index for page data
+			// copy page before address
+			for (uint32_t flash=page_start; flash<address && flash<(page_start+PAGE_SIZE) && page_i<sizeof(page_data); flash++) {
+				page_data[page_i++] = *(uint8_t*)(flash);
 			}
-			buffer += size; // adjust remaining buffer
-			storage_read(address+page_pre+size, &page[page_pre+size], PAGE_SIZE-page_pre-size); // read tailing page data
-			size = 0; // adjust remaining size
-		}
-
-		// write page
-		flash_erase_page(address); // erase current page
-		if (flash_get_status_flags()!=FLASH_SR_EOP) { // operation went wrong
-			flash_lock(); // lock back flash to protect it
-			return false;
-		}
-		for (uint16_t i=0; i<PAGE_SIZE/2; i++) { // write whole page
-			flash_program_half_word(address+i*2, *((uint16_t*)page+i));
+			// copy data starting at address
+			while (size>0 && page_i<sizeof(page_data)) {
+				page_data[page_i++] = *buffer;
+				buffer++;
+				address++;
+				size--;
+			}
+			// copy data after buffer until end of page
+			while (page_i<sizeof(page_data)) {
+				page_data[page_i] = *(uint8_t*)(page_start+page_i);
+				page_i++;
+			}
+			flash_erase_page(page_start); // erase current page
 			if (flash_get_status_flags()!=FLASH_SR_EOP) { // operation went wrong
 				flash_lock(); // lock back flash to protect it
 				return false;
 			}
-			if (*((uint16_t*)address+i)!=*((uint16_t*)page+i)) { // verify the programmed data is right
-				flash_lock(); // lock back flash to protect it
-				return false;
+			for (uint16_t i=0; i<PAGE_SIZE/2; i++) { // write whole page
+				flash_program_half_word(page_start+i*2, *((uint16_t*)(page_data+i*2)));
+				if (flash_get_status_flags()!=FLASH_SR_EOP) { // operation went wrong
+					flash_lock(); // lock back flash to protect it
+					return false;
+				}
+				if (*((uint16_t*)(page_data+i*2))!=*((uint16_t*)(page_start+i*2))) { // verify the programmed data is right
+					flash_lock(); // lock back flash to protect it
+					return false;
+				}
+			}
+		} else { // simply data until end of page
+			while (size>0 && address<(page_start+PAGE_SIZE)) {
+				flash_program_half_word(address, *((uint16_t*)(buffer)));
+				if (flash_get_status_flags()!=FLASH_SR_EOP) { // operation went wrong
+					flash_lock(); // lock back flash to protect it
+					return false;
+				}
+				if (*((uint16_t*)address)!=*((uint16_t*)buffer)) { // verify the programmed data is right
+					flash_lock(); // lock back flash to protect it
+					return false;
+				}
+				buffer += 2;
+				address += 2;
+				size -= 2;
 			}
 		}
-
-		address += PAGE_SIZE; // go to next page
-
 	}
 	flash_lock(); // lock back flash to protect it