From f364f46c60753e45df0a1abe3d1f01a53976e8f9 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sat, 16 Nov 2024 15:44:17 -0800
Subject: [PATCH] Fix unaligned (start address) writes.

---
 minichlink/microgdbstub.h       |  2 +-
 minichlink/minichlink.c         |  3 +--
 minichlink/pgm-esp32s2-ch32xx.c | 18 ++++++++++++------
 3 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/minichlink/microgdbstub.h b/minichlink/microgdbstub.h
index b06fa253..224c7ab2 100644
--- a/minichlink/microgdbstub.h
+++ b/minichlink/microgdbstub.h
@@ -814,7 +814,7 @@ void MicroGDBStubSendReply( const void * data, int len, int docs )
 	
 	if( listenMode == 2 )
 	{
-		//printf( ">>>>%s<<<<(%d)\n", data );
+		//printf( ">>>>%s<<<<\n", data );
 		send( serverSocket, data, len, MSG_NOSIGNAL );
 	}
 }
diff --git a/minichlink/minichlink.c b/minichlink/minichlink.c
index 2cb80e40..5cb697d4 100644
--- a/minichlink/minichlink.c
+++ b/minichlink/minichlink.c
@@ -1439,13 +1439,13 @@ int DefaultWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t blob
 				for( i = 0; i < sectorsize/64; i++ )
 				{
 					int r = MCF.BlockWrite64( dev, base + i*64, blob + rsofar+i*64 );
-					rsofar += 64;
 					if( r )
 					{
 						fprintf( stderr, "Error writing block at memory %08x (error = %d)\n", base, r );
 						return r;
 					}
 				}
+				rsofar += sectorsize;
 			}
 			else 					// Block Write not avaialble
 			{
@@ -1547,7 +1547,6 @@ int DefaultWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t blob
 						MCF.WriteWord( dev, j*4+base, *(uint32_t*)(tempblock + j * 4) );
 
 						// On the v2xx, v3xx, you also need to make sure FLASH->STATR & 2 is not set.  This is only an issue when running locally.
-						rsofar += 4;
 					}
 
 					if( iss->target_chip_type == CHIP_CH32V20x || iss->target_chip_type == CHIP_CH32V30x )
diff --git a/minichlink/pgm-esp32s2-ch32xx.c b/minichlink/pgm-esp32s2-ch32xx.c
index 2c454a31..54f2a6ac 100644
--- a/minichlink/pgm-esp32s2-ch32xx.c
+++ b/minichlink/pgm-esp32s2-ch32xx.c
@@ -132,10 +132,10 @@ int ESPReadAllCPURegisters( void * dev, uint32_t * regret )
 }
 
 int DefaultReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_size, uint8_t * blob );
-int ESPReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_size, uint8_t * blob )
+int ESPReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_size_in, uint8_t * blob )
 {
+	int read_size = read_size_in;
 	struct ESP32ProgrammerStruct * eps = (struct ESP32ProgrammerStruct *)dev;
-
 	uint32_t address_to_read_from_2 = address_to_read_from;
 	uint8_t * blob_2 = blob;
 	int r = 0;
@@ -157,6 +157,11 @@ int ESPReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_
 		read_size -= nrb2r;
 	}
 
+	if( read_size <= 0 )
+	{
+		return 0;
+	}
+
 	int words = read_size / 4;
 
 	ESPFlushLLCommands( dev );
@@ -172,7 +177,7 @@ int ESPReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_
 			read_size -= 4;
 			words_this_group++;
 		}
-		if( ( SRemain( eps ) < 7 ) || w == words )
+		if( ( SRemain( eps ) < 8 ) || ( words_this_group * 5 > eps->replysize - 4 ) || w == words )
 		{
 			ESPFlushLLCommands( dev );
 			uint8_t * resp = eps->reply + 1;
@@ -184,16 +189,17 @@ int ESPReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_
 				resp += 5;
 				blob_2 += 4;
 			}
+			words_this_group = 0;
 		}
 		w++;
 	}
 
-	if( read_size )
+	if( read_size > 0 )
 	{
 		r = DefaultReadBinaryBlob( dev, address_to_read_from_2, read_size, blob_2 );
 		if( r )	return r;
 	}
-	
+
 	return 0;
 }
 
@@ -213,7 +219,7 @@ int ESPFlushLLCommands( void * dev )
 	int r;
 
 	eps->commandbuffer[0] = 0xad; // Key report ID
-	eps->commandbuffer[eps->commandplace] = 0xff;
+	memset( eps->commandbuffer + eps->commandplace, 0xff, eps->commandbuffersize - eps->commandplace - 1 );
 
 #if 0
 	int i;