Fix byte count when doing unaligned memory read, fix issue #127

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

diff --git a/gdbserver/gdb-server.c b/gdbserver/gdb-server.c
index ae06952bf83041f633803a7fd01a0b2393c511a4..f92fc05f06aa7c11f437c67ab252561fbccf85bc 100644 (file)
--- a/gdbserver/gdb-server.c
+++ b/gdbserver/gdb-server.c
@@ -650,7 +650,8 @@ int serve(stlink_t *sl, int port) {
                perror("listen");
                return 1;
        }
-
+\r
+start_again:
        stlink_force_debug(sl);
        stlink_reset(sl);
        init_code_breakpoints(sl);
@@ -681,7 +682,7 @@ int serve(stlink_t *sl, int port) {
                int status = gdb_recv_packet(client, &packet);
                if(status < 0) {
                        fprintf(stderr, "cannot recv: %d\n", status);
-                       return 1;
+                       goto start_again;         
                }
 
                #ifdef DEBUG
@@ -784,6 +785,16 @@ int serve(stlink_t *sl, int port) {
 
 #ifdef DEBUG
                                        printf("Rcmd: halt\n");
+#endif
+                } else if (!strncmp(params,"6a7461675f7265736574",20)) { //jtag_reset
+                                       reply = strdup("OK");
+
+                                       stlink_jtag_reset(sl, 1);
+                                       stlink_jtag_reset(sl, 0);
+                                       stlink_force_debug(sl);
+
+#ifdef DEBUG
+                                       printf("Rcmd: jtag_reset\n");
 #endif
                 } else if (!strncmp(params,"7265736574",10)) { //reset
                                        reply = strdup("OK");
@@ -1045,9 +1056,9 @@ int serve(stlink_t *sl, int port) {
                        unsigned     count = strtoul(s_count, NULL, 16);
 
                        unsigned adj_start = start % 4;
+                       unsigned count_rnd = (count + adj_start + 4 - 1) / 4 * 4;
 
-                       stlink_read_mem32(sl, start - adj_start, (count % 4 == 0) ?
-                                               count : count + 4 - (count % 4));
+                       stlink_read_mem32(sl, start - adj_start, count_rnd);
 
                        reply = calloc(count * 2 + 1, 1);
                        for(unsigned int i = 0; i < count; i++) {
@@ -1066,20 +1077,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;
                }
 
@@ -1108,13 +1142,14 @@ int serve(stlink_t *sl, int port) {
                                                wf = WATCHREAD;
                                        } else {
                                                wf = WATCHACCESS;
-                                               if(add_data_watchpoint(sl, wf, addr, len) < 0) {
-                                                       reply = strdup("E00");
-                                               } else {
-                                                       reply = strdup("OK");
-                                                       break;
-                                               }
                                        }
+
+                    if(add_data_watchpoint(sl, wf, addr, len) < 0) {
+                        reply = strdup("E00");
+                    } else {
+                        reply = strdup("OK");
+                        break;
+                    }
                                }
 
                                default:
@@ -1186,7 +1221,9 @@ int serve(stlink_t *sl, int port) {
                        int result = gdb_send_packet(client, reply);
                        if(result != 0) {
                                fprintf(stderr, "cannot send: %d\n", result);
-                               return 1;
+                               free(reply);
+                               free(packet);
+                               goto start_again;
                        }
 
                        free(reply);