From 08872f834db4f4954490e614ca80e59fa6f95dbe Mon Sep 17 00:00:00 2001
From: Fabien Le Mentec <texane@gmail.com>
Date: Sun, 25 Nov 2012 09:50:44 -0600
Subject: [PATCH] [ merge ] Andreas Seltenreich <uwi7@rz.uni-karlsruhe.de>
 patches

---
 gdbserver/gdb-server.c | 35 +++++++++++++++++++++++++++++------
 src/stlink-common.c    |  6 +++---
 2 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/gdbserver/gdb-server.c b/gdbserver/gdb-server.c
index 9d27ae4..04490ad 100644
--- a/gdbserver/gdb-server.c
+++ b/gdbserver/gdb-server.c
@@ -1076,20 +1076,43 @@ int serve(stlink_t *sl, int port) {
 			stm32_addr_t start = strtoul(s_start, NULL, 16);
 			unsigned     count = strtoul(s_count, NULL, 16);
 
-			for(unsigned int i = 0; i < count; i ++) {
+			if(start % 4) {
+			  unsigned align_count = 4 - start % 4;
+			  if (align_count > count) align_count = count;
+			  for(unsigned int i = 0; i < align_count; i ++) {
 				char hex[3] = { hexdata[i*2], hexdata[i*2+1], 0 };
 				uint8_t byte = strtoul(hex, NULL, 16);
 				sl->q_buf[i] = byte;
+			  }
+			  stlink_write_mem8(sl, start, align_count);
+			  start += align_count;
+			  count -= align_count;
+			  hexdata += 2*align_count;
 			}
 
-			if((count % 4) == 0 && (start % 4) == 0) {
-				stlink_write_mem32(sl, start, count);
-			} else {
-				stlink_write_mem8(sl, start, count);
+			if(count - count % 4) {
+			  unsigned aligned_count = count - count % 4;
+
+			  for(unsigned int i = 0; i < aligned_count; i ++) {
+			    char hex[3] = { hexdata[i*2], hexdata[i*2+1], 0 };
+			    uint8_t byte = strtoul(hex, NULL, 16);
+			    sl->q_buf[i] = byte;
+			  }
+			  stlink_write_mem32(sl, start, aligned_count);
+			  count -= aligned_count;
+			  start += aligned_count;
+			  hexdata += 2*aligned_count;
 			}
 
+			if(count) {
+			  for(unsigned int i = 0; i < count; i ++) {
+			    char hex[3] = { hexdata[i*2], hexdata[i*2+1], 0 };
+			    uint8_t byte = strtoul(hex, NULL, 16);
+			    sl->q_buf[i] = byte;
+			  }
+			  stlink_write_mem8(sl, start, count);
+			}
 			reply = strdup("OK");
-
 			break;
 		}
 
diff --git a/src/stlink-common.c b/src/stlink-common.c
index 66006f0..033cb52 100644
--- a/src/stlink-common.c
+++ b/src/stlink-common.c
@@ -543,7 +543,7 @@ void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     DLOG("*** stlink_write_mem32 %u bytes to %#x\n", len, addr);
     if (len % 4 != 0) {
         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
-        return;
+        abort();
     }
     sl->backend->write_mem32(sl, addr, len);
 }
@@ -553,7 +553,7 @@ void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
                 len % 4);
-        return;
+        abort();
     }
     sl->backend->read_mem32(sl, addr, len);
 }
@@ -563,7 +563,7 @@ void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
     if (len > 0x40 ) { // !!! never ever: Writing more then 0x40 bytes gives unexpected behaviour
         fprintf(stderr, "Error: Data length > 64: +%d byte.\n",
                 len);
-        return;
+        abort();
     }
     sl->backend->write_mem8(sl, addr, len);
 }
-- 
2.30.2