Add support for remote reset commands.

[fw/stlink] / src / gdb-server.c

diff --git a/src/gdb-server.c b/src/gdb-server.c
index 67ab0a5802911fef821df4f22add6f2acbb72fbf..b48f7d6d8256af38f9676c34aaf6f51968402ff6 100644 (file)
--- a/src/gdb-server.c
+++ b/src/gdb-server.c
@@ -223,19 +223,31 @@ static int flash_populate(stm32_addr_t addr, uint8_t* data, unsigned length) {
 static int flash_go(struct stlink* sl) {
        int error = -1;
 
+       // Some kinds of clock settings do not allow writing to flash.
+       stlink_reset(sl);
+
        for(struct flash_block* fb = flash_root; fb; fb = fb->next) {
                #ifdef DEBUG
                printf("flash_do: block %08x -> %04x\n", fb->addr, fb->length);
                #endif
 
-               stlink_erase_flash_page(sl, fb->addr);
+               unsigned length = fb->length;
+               for(stm32_addr_t page = fb->addr; page < fb->addr + fb->length; page += 0x400) {
+                       #ifdef DEBUG
+                       printf("flash_do: page %08x\n", page);
+                       #endif
+
+                       stlink_erase_flash_page(sl, page);
 
-               if(!stlink_write_flash(sl, fb->addr, fb->data, fb->length) < 0) {
-                       fprintf(stderr, "Flash writing failed.\n");
-                       goto error;
+                       if(stlink_write_flash(sl, page, fb->data + (page - fb->addr),
+                                       length > 0x400 ? 0x400 : length) < 0)
+                               goto error;
                }
+
        }
 
+       stlink_reset(sl);
+
        error = 0;
 
 error:
@@ -291,6 +303,12 @@ int serve(struct stlink* sl, int port) {
 
        printf("GDB connected.\n");
 
+       /*
+        * To allow resetting the chip from GDB it is required to
+        * emulate attaching and detaching to target.
+        */
+       unsigned int attached = 1;
+
        while(1) {
                char* packet;
 
@@ -377,16 +395,10 @@ int serve(struct stlink* sl, int port) {
                }
 
                case 'v': {
-                       char *separator = strstr(packet, ":"), *params = "";
-                       if(separator == NULL) {
-                               separator = packet + strlen(packet);
-                       } else {
-                               params = separator + 1;
-                       }
+                       char *params = NULL;
+                       char *cmdName = strtok_r(packet, ":;", &params);
 
-                       unsigned cmdNameLength = (separator - &packet[1]);
-                       char* cmdName = calloc(cmdNameLength + 1, 1);
-                       strncpy(cmdName, &packet[1], cmdNameLength);
+                       cmdName++; // vCommand -> Command
 
                        if(!strcmp(cmdName, "FlashErase")) {
                                char *s_addr, *s_length;
@@ -451,23 +463,21 @@ int serve(struct stlink* sl, int port) {
                                } else {
                                        reply = strdup("OK");
                                }
+                       } else if(!strcmp(cmdName, "Kill")) {
+                               attached = 0;
 
-                               stlink_reset(sl);
+                               reply = strdup("OK");
                        }
 
                        if(reply == NULL)
                                reply = strdup("");
 
-                       free(cmdName);
-
                        break;
                }
 
                case 'c':
                        stlink_run(sl);
 
-                       printf("Core running, waiting for interrupt (either in chip or GDB).\n");
-
                        while(1) {
                                int status = gdb_check_for_interrupt(client);
                                if(status < 0) {
@@ -485,7 +495,7 @@ int serve(struct stlink* sl, int port) {
                                        break;
                                }
 
-                               usleep(200000);
+                               usleep(100000);
                        }
 
                        reply = strdup("S05"); // TRAP
@@ -498,7 +508,12 @@ int serve(struct stlink* sl, int port) {
                        break;
 
                case '?':
-                       reply = strdup("S05"); // TRAP
+                       if(attached) {
+                               reply = strdup("S05"); // TRAP
+                       } else {
+                               /* Stub shall reply OK if not attached. */
+                               reply = strdup("OK");
+                       }
                        break;
 
                case 'g':
@@ -636,12 +651,28 @@ int serve(struct stlink* sl, int port) {
                        break;
                }
 
-               case 'k': {
-                       // After this function will be entered afterwards, the
-                       // chip will be reset anyway. So this is a no-op.
+               case '!': {
+                       /*
+                        * Enter extended mode which allows restarting.
+                        * We do support that always.
+                        */
+
+                       reply = strdup("OK");
+
+                       break;
+               }
+
+               case 'R': {
+                       /* Reset the core. */
+
+                       stlink_reset(sl);
+                       init_code_breakpoints(sl);
 
-                       close(client);
-                       return 0;
+                       attached = 1;
+
+                       reply = strdup("OK");
+
+                       break;
                }
 
                default: