From e50fef660766a70c1c9afb963526dfb41d86b4db Mon Sep 17 00:00:00 2001
From: iceman1001 <iceman@iuse.se>
Date: Thu, 3 May 2018 16:10:38 +0200
Subject: [PATCH] fix: 'mem load'  - wrong offset when uploading

---
 armsrc/appmain.c     | 15 +++++++++------
 armsrc/flashmem.c    |  6 +-----
 client/cmdflashmem.c |  4 ++--
 3 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/armsrc/appmain.c b/armsrc/appmain.c
index f64351f5e..24f0f4573 100644
--- a/armsrc/appmain.c
+++ b/armsrc/appmain.c
@@ -1134,6 +1134,8 @@ void UsbPacketReceived(uint8_t *packet, int len) {
 			
 			Dbprintf("FlashMem init idx | %u | len %u ", startidx, len );
 			
+			uint8_t* data = c->d.asBytes;
+			
 			uint32_t tmp = startidx + len;
 			
 			// inside 256b page?
@@ -1145,24 +1147,25 @@ void UsbPacketReceived(uint8_t *packet, int len) {
 					
 						// offset xxxx10,   
 						uint8_t first_len =  (~startidx & 0xFF)+1;
-						
+												
 						// first mem page						
-						res = Flash_WriteData(startidx, c->d.asBytes, first_len);
+						res = Flash_WriteData(startidx, data, first_len);
 						Dbprintf("after 1. offset and larger A | %u | %u | %u == %u", startidx , len,  first_len, res);
 						
-						// second mem page (should be a mod 256)					
-						res = Flash_WriteData(startidx + first_len, c->d.asBytes + first_len, len - first_len);
+						// second mem page (should be a mod 256)
+						res = Flash_WriteData(startidx + first_len, data + first_len, len - first_len);
+						
 						Dbprintf("after 2. offset and larger  B | %u | %u | %u == %u", startidx + first_len, len, len-first_len, res);
 						
 						isok = (res == (len - first_len)) ? 1 : 0;
 						
 					} else {
-						res = Flash_WriteData(startidx, c->d.asBytes, len);
+						res = Flash_WriteData(startidx, data, len);
 						Dbprintf("offset and within | %u | %u | %u", startidx, len, res);
 						isok = (res == len) ? 1 : 0;
 					}					
 			} else {				
-				res = Flash_WriteData(startidx, c->d.asBytes, len);
+				res = Flash_WriteData(startidx, data, len);
 				Dbprintf("writing idx | %u | len %u ", startidx, len );
 				isok = (res == len) ? 1 : 0;
 			}
diff --git a/armsrc/flashmem.c b/armsrc/flashmem.c
index 8eecf6009..1e74f8cd3 100644
--- a/armsrc/flashmem.c
+++ b/armsrc/flashmem.c
@@ -250,11 +250,7 @@ uint16_t Flash_WriteData(uint32_t address, uint8_t *in, uint16_t len) {
 		Dbprintf("Flash_WriteData,  block out-of-range");
 		return 0;
 	}
-	
-	// if 256b,  empty out lower index.
-	if (len == 256)
-		address &= 0xFFFF00;
-	
+
 	if (!FlashInit()) {
 		Dbprintf("Flash_WriteData init fail");
 		return 0;
diff --git a/client/cmdflashmem.c b/client/cmdflashmem.c
index 0f162987a..df17e2b9e 100644
--- a/client/cmdflashmem.c
+++ b/client/cmdflashmem.c
@@ -124,14 +124,14 @@ int CmdFlashMemLoad(const char *Cmd){
 	
 	//Send to device
 	uint32_t bytes_sent = 0;
-	uint32_t bytes_remaining  = bytes_read;
+	uint32_t bytes_remaining = bytes_read;
 
 	while (bytes_remaining > 0){
 		uint32_t bytes_in_packet = MIN(FLASH_MEM_BLOCK_SIZE, bytes_remaining);
 		
 		UsbCommand c = {CMD_WRITE_FLASH_MEM, {start_index + bytes_sent, bytes_in_packet, 0}};
 				
-		memcpy(c.d.asBytes, dump + start_index + bytes_sent, bytes_in_packet);
+		memcpy(c.d.asBytes, dump + bytes_sent, bytes_in_packet);
 		clearCommandBuffer();
 		SendCommand(&c);