From d4b435e7c048dd348857d22b30e6da42b21b1a51 Mon Sep 17 00:00:00 2001
From: Peter Zotov <whitequark@whitequark.org>
Date: Tue, 15 Feb 2011 22:35:19 +0300
Subject: [PATCH] Implement FlashErase GDB command.

---
 src/gdb-server.c | 50 ++++++++++++++++++++++++++++++++++++++++++++++++
 src/stlink-hw.c  |  6 +++---
 src/stlink-hw.h  |  3 +++
 3 files changed, 56 insertions(+), 3 deletions(-)

diff --git a/src/gdb-server.c b/src/gdb-server.c
index 53df719..c616215 100644
--- a/src/gdb-server.c
+++ b/src/gdb-server.c
@@ -184,6 +184,56 @@ int serve(struct stlink* sl, int port) {
 			break;
 		}
 
+		case 'v': {
+			char *separator = strstr(packet, ":"), *params = "";
+			if(separator == NULL) {
+				separator = packet + strlen(packet);
+			} else {
+				params = separator + 1;
+			}
+
+			unsigned cmdNameLength = (separator - &packet[1]);
+			char* cmdName = calloc(cmdNameLength + 1, 1);
+			strncpy(cmdName, &packet[1], cmdNameLength);
+
+			if(!strcmp(cmdName, "FlashErase")) {
+				char *s_addr, *s_length;
+				char *tok = params;
+
+				s_addr   = strsep(&tok, ",");
+				s_length = tok;
+
+				unsigned addr = strtoul(s_addr, NULL, 16),
+				       length = strtoul(s_length, NULL, 16);
+
+				#ifdef DEBUG
+				printf("FlashErase: addr:%08x,len:%04x\n",
+					addr, length);
+				#endif
+
+				for(stm32_addr_t cur = addr;
+						cur < addr + length; cur += 0x400) {
+					#ifdef DEBUG
+					printf("do_erase: %08x\n", cur);
+					#endif
+
+					stlink_erase_flash_page(sl, cur);
+				}
+
+				reply = strdup("OK");
+			} else if(!strcmp(cmdName, "FlashWrite")) {
+			} else if(!strcmp(cmdName, "FlashDone")) {
+				reply = strdup("OK");
+			}
+
+			if(reply == NULL)
+				reply = strdup("");
+
+			free(cmdName);
+
+			break;
+		}
+
 		case 'c':
 			stlink_run(sl);
 
diff --git a/src/stlink-hw.c b/src/stlink-hw.c
index 441b692..92cd021 100644
--- a/src/stlink-hw.c
+++ b/src/stlink-hw.c
@@ -1108,7 +1108,7 @@ static int write_flash_mem16
 }
 #endif /* not working */
 
-static int erase_flash_page(struct stlink* sl, stm32_addr_t page)
+int stlink_erase_flash_page(struct stlink* sl, stm32_addr_t page)
 {
   /* page an addr in the page to erase */
 
@@ -1138,7 +1138,7 @@ static int erase_flash_page(struct stlink* sl, stm32_addr_t page)
   return 0;
 }
 
-static int __attribute__((unused)) erase_flash_mass(struct stlink* sl)
+int stlink_erase_flash_mass(struct stlink* sl)
 {
   /* wait for ongoing op to finish */
   wait_flash_busy(sl);
@@ -1421,7 +1421,7 @@ static int stlink_fwrite_flash
   for (off = 0; off < mf.len; off += sl->flash_pgsz)
   {
     /* addr must be an addr inside the page */
-    if (erase_flash_page(sl, addr + off) == -1)
+    if (stlink_erase_flash_page(sl, addr + off) == -1)
     {
       fprintf(stderr, "erase_flash_page(0x%x) == -1\n", addr + off);
       goto on_error;
diff --git a/src/stlink-hw.h b/src/stlink-hw.h
index 9ce085b..b902c17 100644
--- a/src/stlink-hw.h
+++ b/src/stlink-hw.h
@@ -155,4 +155,7 @@ void stlink_write_mem8(struct stlink *sl, uint32_t addr, uint16_t len);
 void stlink_write_mem32(struct stlink *sl, uint32_t addr, uint16_t len);
 void stlink_close(struct stlink *sl);
 
+int stlink_erase_flash_page(struct stlink* sl, stm32_addr_t page);
+int stlink_erase_flash_mass(struct stlink* sl);
+
 #endif
-- 
2.30.2