From 8ffde0bfc2d12a5ee939053791c8f2ca36ffe2cd Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Sun, 3 Mar 2019 17:07:32 +0000
Subject: [PATCH] - fixed data corruption after flash

git-svn-id: http://moon:8086/svn/projects/HendiControl@54 fda53097-d464-4ada-af97-ba876c37ca34
---
 .../Bootloader/Bootloader/Bootloader.cproj    |  2 +-
 Control/firmware/Bootloader/Bootloader/main.c | 60 ++++++++++++-------
 2 files changed, 40 insertions(+), 22 deletions(-)

diff --git a/Control/firmware/Bootloader/Bootloader/Bootloader.cproj b/Control/firmware/Bootloader/Bootloader/Bootloader.cproj
index 6b65d49..acdd47e 100644
--- a/Control/firmware/Bootloader/Bootloader/Bootloader.cproj
+++ b/Control/firmware/Bootloader/Bootloader/Bootloader.cproj
@@ -4,7 +4,7 @@
     <SchemaVersion>2.0</SchemaVersion>
     <ProjectVersion>7.0</ProjectVersion>
     <ToolchainName>com.Atmel.AVRGCC8.C</ToolchainName>
-    <ProjectGuid>{c77f8c5d-b585-42ee-bf03-003a6350cd7a}</ProjectGuid>
+    <ProjectGuid>{dce6c7e3-ee26-4d79-826b-08594b9ad897}</ProjectGuid>
     <avrdevice>ATmega328P</avrdevice>
     <avrdeviceseries>none</avrdeviceseries>
     <OutputType>Executable</OutputType>
diff --git a/Control/firmware/Bootloader/Bootloader/main.c b/Control/firmware/Bootloader/Bootloader/main.c
index 6a08626..c2261ae 100644
--- a/Control/firmware/Bootloader/Bootloader/main.c
+++ b/Control/firmware/Bootloader/Bootloader/main.c
@@ -49,6 +49,14 @@ typedef enum _eState
 void write_page(uint16_t page_addr, uint8_t *buf);
 void program_page (uint32_t page, uint8_t *buf);
 
+void init_page(uint8_t *pPageBuffer, size_t size)
+{
+	for (int i=0; i < size; i++)
+	{
+		pPageBuffer[i] = 0xFF;
+	}
+}
+
 int main(void)
 {
 	uint8_t page_buffer[SPM_PAGESIZE];
@@ -113,38 +121,48 @@ int main(void)
 						break;
 						case srec_data:
 						{
-							uint16_t next_page_addr = srec.addr & ~SPM_PAGESIZE_MASK;
-							int withinPage = (page_addr == next_page_addr);
-							if (!withinPage)
+							uint16_t new_page_addr = srec.addr & ~SPM_PAGESIZE_MASK;
+							if (new_page_addr != page_addr)
 							{
-								if (!page_isEmpty)
-								{
-									write_page(page_addr, page_buffer);
-									page_isEmpty = 1;
-								}
+								eeprom_busy_wait ();
+								boot_spm_busy_wait ();
+								boot_page_erase (new_page_addr);
 							}
+							page_addr = new_page_addr;
 
 							if (page_isEmpty)
 							{
 								// Begin new page
 								page_isEmpty = 0;
-								page_addr = srec.addr & ~SPM_PAGESIZE_MASK;
-								for (int i=0; i < SPM_PAGESIZE; i++)
-								{
-									page_buffer[i] = 0xFF;
-								}
-								eeprom_busy_wait ();
-								boot_spm_busy_wait ();
-								boot_page_erase (page_addr);
-								
-								uart_putc('.');
+								init_page(page_buffer, SPM_PAGESIZE);
 							}
 							int start = srec.addr & SPM_PAGESIZE_MASK;
-							for (int i=0; i < srec.size; i++)
+							int i = 0;
+							int to_write = srec.size;							
+							if ((start + to_write) > SPM_PAGESIZE)
 							{
-								page_buffer[start + i] = srec.data[i];
+								to_write = SPM_PAGESIZE - start;
+							}
+							for (; i < to_write; i++)
+							{
+								page_buffer[start++] = srec.data[i];
+							}
+							// Page buffer is complete -> write to flash
+							if ((start + i) >= SPM_PAGESIZE)
+							{
+								write_page(page_addr, page_buffer);
+								page_isEmpty = 1;
+								init_page(page_buffer, SPM_PAGESIZE);
+							}
+							// Copy remaining data to start of page buffer
+							start=0;
+							for (; i < srec.size; i++)
+							{
+								page_isEmpty = 0;
+								page_buffer[start++] = srec.data[i];
 							}
 							image_size += srec.size;
+							uart_putc('.');
 						}
 						break;
 						case srec_rec:
@@ -155,8 +173,8 @@ int main(void)
 						{
 							write_page(page_addr, page_buffer);
 							page_isEmpty = 1;
-							uart_puts("\nImage size: 0x"); print_word(image_size); uart_puts("\n");
 						}
+						uart_puts("\nImage size: 0x"); print_word(image_size); uart_puts("\n");
 						/* Reenable RWW-section again. We need this if we want to jump back */
 						/* to the application after bootloading. */
 						boot_rww_enable ();