Write flash correctly.
[fw/stlink] / src / gdb-server.c
1 /* -*- tab-width:8 -*- */
2
3 /*
4  Copyright (C)  2011 Peter Zotov <whitequark@whitequark.org>
5  Use of this source code is governed by a BSD-style
6  license that can be found in the LICENSE file.
7 */
8
9 #include <stdio.h>
10 #include <string.h>
11 #include <stdlib.h>
12 #include <unistd.h>
13 #include <sys/types.h>
14 #include <sys/socket.h>
15 #include <netinet/in.h>
16 #include <arpa/inet.h>
17 #include "gdb-remote.h"
18 #include "stlink-hw.h"
19
20 static const char hex[] = "0123456789abcdef";
21
22 // configured for STM32F100RB
23 static const char* const c_memory_map =
24   "<?xml version=\"1.0\"?>"
25   "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
26   "     \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
27   "<memory-map>"
28   "  <memory type=\"rom\" start=\"0x00000000\" length=\"0x20000\"/>"    // code = sram or flash
29   "  <memory type=\"ram\" start=\"0x20000000\" length=\"0x2000\"/>"     // sram 8k
30   "  <memory type=\"flash\" start=\"0x08000000\" length=\"0x20000\">"   // flash 128k
31   "    <property name=\"blocksize\">0x400</property>"                   // 1k pages
32   "  </memory>"
33   "  <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
34   "  <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
35   "</memory-map>";
36
37 int serve(struct stlink* sl, int port);
38
39 int main(int argc, char** argv) {
40         if(argc != 3) {
41                 fprintf(stderr, "Usage: %s <port> /dev/sgX\n", argv[0]);
42                 return 1;
43         }
44
45         struct stlink *sl = stlink_quirk_open(argv[2], 0);
46         if (sl == NULL)
47                 return 1;
48
49         if(stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE)
50                 stlink_enter_swd_mode(sl);
51
52         stlink_core_id(sl);
53         printf("Debugging ARM core %08x.\n", sl->core_id);
54
55         int port = atoi(argv[1]);
56
57         while(serve(sl, port) == 0);
58
59         stlink_close(sl);
60
61         return 0;
62 }
63
64 #define CODE_BREAK_NUM  6
65
66 #define CODE_BREAK_LOW  0x01
67 #define CODE_BREAK_HIGH 0x02
68
69 struct code_hw_breakpoint {
70         stm32_addr_t addr;
71         int          type;
72 };
73
74 struct code_hw_breakpoint code_breaks[CODE_BREAK_NUM];
75
76 static void init_code_breakpoints(struct stlink* sl) {
77         memset(sl->q_buf, 0, 4);
78         sl->q_buf[0] = 0x03; // KEY | ENABLE
79         stlink_write_mem32(sl, 0xe0002000, 4);
80
81         memset(sl->q_buf, 0, 4);
82         for(int i = 0; i < CODE_BREAK_NUM; i++) {
83                 code_breaks[i].type = 0;
84                 stlink_write_mem32(sl, 0xe0002008 + i * 4, 4);
85         }
86 }
87
88 static int update_code_breakpoint(struct stlink* sl, stm32_addr_t addr, int set) {
89         stm32_addr_t fpb_addr = addr & ~0x3;
90         int type = addr & 0x2 ? CODE_BREAK_HIGH : CODE_BREAK_LOW;
91
92         if(addr & 1) {
93                 fprintf(stderr, "update_code_breakpoint: unaligned address %08x\n", addr);
94                 return -1;
95         }
96
97         int id = -1;
98         for(int i = 0; i < CODE_BREAK_NUM; i++) {
99                 if(fpb_addr == code_breaks[i].addr ||
100                         (set && code_breaks[i].type == 0)) {
101                         id = i;
102                         break;
103                 }
104         }
105
106         if(id == -1) {
107                 if(set) return -1; // Free slot not found
108                 else    return 0;  // Breakpoint is already removed
109         }
110
111         struct code_hw_breakpoint* brk = &code_breaks[id];
112
113         brk->addr = fpb_addr;
114
115         if(set) brk->type |= type;
116         else    brk->type &= ~type;
117
118         memset(sl->q_buf, 0, 4);
119
120         if(brk->type == 0) {
121                 #ifdef DEBUG
122                 printf("clearing hw break %d\n", id);
123                 #endif
124
125                 stlink_write_mem32(sl, 0xe0002008 + id * 4, 4);
126         } else {
127                 sl->q_buf[0] = ( brk->addr        & 0xff) | 1;
128                 sl->q_buf[1] = ((brk->addr >> 8)  & 0xff);
129                 sl->q_buf[2] = ((brk->addr >> 16) & 0xff);
130                 sl->q_buf[3] = ((brk->addr >> 24) & 0xff) | (brk->type << 6);
131
132                 #ifdef DEBUG
133                 printf("setting hw break %d at %08x (%d)\n",
134                         id, brk->addr, brk->type);
135                 printf("reg %02x %02x %02x %02x\n",
136                         sl->q_buf[3], sl->q_buf[2], sl->q_buf[1], sl->q_buf[0]);
137                 #endif
138
139                 stlink_write_mem32(sl, 0xe0002008 + id * 4, 4);
140         }
141
142         return 0;
143 }
144
145 #define FLASH_BASE 0x08000000
146 #define FLASH_PAGE 0x400
147 #define FLASH_PAGE_MASK (~((1 << 10) - 1))
148 #define FLASH_SIZE (FLASH_PAGE * 128)
149
150 struct flash_block {
151         stm32_addr_t addr;
152         unsigned     length;
153         uint8_t*     data;
154
155         struct flash_block* next;
156 };
157
158 static struct flash_block* flash_root;
159
160 static int flash_add_block(stm32_addr_t addr, unsigned length) {
161         if(addr < FLASH_BASE || addr + length > FLASH_BASE + FLASH_SIZE) {
162                 fprintf(stderr, "flash_add_block: incorrect bounds\n");
163                 return -1;
164         }
165
166         if(addr % FLASH_PAGE != 0 || length % FLASH_PAGE != 0) {
167                 fprintf(stderr, "flash_add_block: unaligned block\n");
168                 return -1;
169         }
170
171         struct flash_block* new = malloc(sizeof(struct flash_block));
172         new->next = flash_root;
173
174         new->addr   = addr;
175         new->length = length;
176         new->data   = calloc(length, 1);
177
178         flash_root = new;
179
180         return 0;
181 }
182
183 static int flash_populate(stm32_addr_t addr, uint8_t* data, unsigned length) {
184         int fit_blocks = 0, fit_length = 0;
185
186         for(struct flash_block* fb = flash_root; fb; fb = fb->next) {
187                 /* Block: ------X------Y--------
188                  * Data:            a-----b
189                  *                a--b
190                  *            a-----------b
191                  * Block intersects with data, if:
192                  *  a < Y && b > x
193                  */
194
195                 unsigned X = fb->addr, Y = fb->addr + fb->length;
196                 unsigned a = addr, b = addr + length;
197                 if(a < Y && b > X) {
198                         // from start of the block
199                         unsigned start = (a > X ? a : X) - X;
200                         unsigned end   = (b > Y ? Y : b) - X;
201
202                         memcpy(fb->data + start, data, end - start);
203
204                         fit_blocks++;
205                         fit_length += end - start;
206                 }
207         }
208
209         if(fit_blocks == 0) {
210                 fprintf(stderr, "Unfit data block %08x -> %04x\n", addr, length);
211                 return -1;
212         }
213
214         if(fit_length != length) {
215                 fprintf(stderr, "warning: data block %08x -> %04x truncated to %04x\n",
216                         addr, length, fit_length);
217                 fprintf(stderr, "(this is not an error, just a GDB glitch)\n");
218         }
219
220         return 0;
221 }
222
223 static int flash_go(struct stlink* sl) {
224         int error = -1;
225
226         for(struct flash_block* fb = flash_root; fb; fb = fb->next) {
227                 #ifdef DEBUG
228                 printf("flash_do: block %08x -> %04x\n", fb->addr, fb->length);
229                 #endif
230
231                 stlink_erase_flash_page(sl, fb->addr);
232
233                 if(!stlink_write_flash(sl, fb->addr, fb->data, fb->length) < 0) {
234                         fprintf(stderr, "Flash writing failed.\n");
235                         goto error;
236                 }
237         }
238
239         error = 0;
240
241 error:
242         for(struct flash_block* fb = flash_root, *next; fb; fb = next) {
243                 next = fb->next;
244                 free(fb->data);
245                 free(fb);
246         }
247
248         flash_root = NULL;
249
250         return error;
251 }
252
253 int serve(struct stlink* sl, int port) {
254         int sock = socket(AF_INET, SOCK_STREAM, 0);
255         if(sock < 0) {
256                 perror("socket");
257                 return 1;
258         }
259
260         unsigned int val = 1;
261         setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
262
263         struct sockaddr_in serv_addr = {0};
264         serv_addr.sin_family = AF_INET;
265         serv_addr.sin_addr.s_addr = inet_addr("127.0.0.1");
266         serv_addr.sin_port = htons(port);
267
268         if(bind(sock, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
269                 perror("bind");
270                 return 1;
271         }
272
273         if(listen(sock, 5) < 0) {
274                 perror("listen");
275                 return 1;
276         }
277
278         stlink_force_debug(sl);
279         stlink_reset(sl);
280         init_code_breakpoints(sl);
281
282         printf("Listening at *:%d...\n", port);
283
284         int client = accept(sock, NULL, NULL);
285         if(client < 0) {
286                 perror("accept");
287                 return 1;
288         }
289
290         close(sock);
291
292         printf("GDB connected.\n");
293
294         while(1) {
295                 char* packet;
296
297                 int status = gdb_recv_packet(client, &packet);
298                 if(status < 0) {
299                         fprintf(stderr, "cannot recv: %d\n", status);
300                         return 1;
301                 }
302
303                 #ifdef DEBUG
304                 printf("recv: %s\n", packet);
305                 #endif
306
307                 char* reply = NULL;
308
309                 switch(packet[0]) {
310                 case 'q': {
311                         if(packet[1] == 'P' || packet[1] == 'C' || packet[1] == 'L') {
312                                 reply = strdup("");
313                                 break;
314                         }
315
316                         char *separator = strstr(packet, ":"), *params = "";
317                         if(separator == NULL) {
318                                 separator = packet + strlen(packet);
319                         } else {
320                                 params = separator + 1;
321                         }
322
323                         unsigned queryNameLength = (separator - &packet[1]);
324                         char* queryName = calloc(queryNameLength + 1, 1);
325                         strncpy(queryName, &packet[1], queryNameLength);
326
327                         #ifdef DEBUG
328                         printf("query: %s;%s\n", queryName, params);
329                         #endif
330
331                         if(!strcmp(queryName, "Supported")) {
332                                 reply = strdup("PacketSize=3fff;qXfer:memory-map:read+");
333                         } else if(!strcmp(queryName, "Xfer")) {
334                                 char *type, *op, *annex, *s_addr, *s_length;
335                                 char *tok = params;
336
337                                 type     = strsep(&tok, ":");
338                                 op       = strsep(&tok, ":");
339                                 annex    = strsep(&tok, ":");
340                                 s_addr   = strsep(&tok, ",");
341                                 s_length = tok;
342
343                                 unsigned addr = strtoul(s_addr, NULL, 16),
344                                        length = strtoul(s_length, NULL, 16);
345
346                                 #ifdef DEBUG
347                                 printf("Xfer: type:%s;op:%s;annex:%s;addr:%d;length:%d\n",
348                                         type, op, annex, addr, length);
349                                 #endif
350
351                                 const char* data = NULL;
352
353                                 if(!strcmp(type, "memory-map") && !strcmp(op, "read"))
354                                         data = c_memory_map;
355
356                                 if(data) {
357                                         unsigned data_length = strlen(data);
358                                         if(addr + length > data_length)
359                                                 length = data_length - addr;
360
361                                         if(length == 0) {
362                                                 reply = strdup("l");
363                                         } else {
364                                                 reply = calloc(length + 2, 1);
365                                                 reply[0] = 'm';
366                                                 strncpy(&reply[1], data, length);
367                                         }
368                                 }
369                         }
370
371                         if(reply == NULL)
372                                 reply = strdup("");
373
374                         free(queryName);
375
376                         break;
377                 }
378
379                 case 'v': {
380                         char *separator = strstr(packet, ":"), *params = "";
381                         if(separator == NULL) {
382                                 separator = packet + strlen(packet);
383                         } else {
384                                 params = separator + 1;
385                         }
386
387                         unsigned cmdNameLength = (separator - &packet[1]);
388                         char* cmdName = calloc(cmdNameLength + 1, 1);
389                         strncpy(cmdName, &packet[1], cmdNameLength);
390
391                         if(!strcmp(cmdName, "FlashErase")) {
392                                 char *s_addr, *s_length;
393                                 char *tok = params;
394
395                                 s_addr   = strsep(&tok, ",");
396                                 s_length = tok;
397
398                                 unsigned addr = strtoul(s_addr, NULL, 16),
399                                        length = strtoul(s_length, NULL, 16);
400
401                                 #ifdef DEBUG
402                                 printf("FlashErase: addr:%08x,len:%04x\n",
403                                         addr, length);
404                                 #endif
405
406                                 if(flash_add_block(addr, length) < 0) {
407                                         reply = strdup("E00");
408                                 } else {
409                                         reply = strdup("OK");
410                                 }
411                         } else if(!strcmp(cmdName, "FlashWrite")) {
412                                 char *s_addr, *data;
413                                 char *tok = params;
414
415                                 s_addr = strsep(&tok, ":");
416                                 data   = tok;
417
418                                 unsigned addr = strtoul(s_addr, NULL, 16);
419                                 unsigned data_length = status - (data - packet);
420
421                                 // Length of decoded data cannot be more than
422                                 // encoded, as escapes are removed.
423                                 // Additional byte is reserved for alignment fix.
424                                 uint8_t *decoded = calloc(data_length + 1, 1);
425                                 unsigned dec_index = 0;
426                                 for(int i = 0; i < data_length; i++) {
427                                         if(data[i] == 0x7d) {
428                                                 i++;
429                                                 decoded[dec_index++] = data[i] ^ 0x20;
430                                         } else {
431                                                 decoded[dec_index++] = data[i];
432                                         }
433                                 }
434
435                                 // Fix alignment
436                                 if(dec_index % 2 != 0)
437                                         dec_index++;
438
439                                 #ifdef DEBUG
440                                 printf("binary packet %d -> %d\n", data_length, dec_index);
441                                 #endif
442
443                                 if(flash_populate(addr, decoded, dec_index) < 0) {
444                                         reply = strdup("E00");
445                                 } else {
446                                         reply = strdup("OK");
447                                 }
448                         } else if(!strcmp(cmdName, "FlashDone")) {
449                                 if(flash_go(sl) < 0) {
450                                         reply = strdup("E00");
451                                 } else {
452                                         reply = strdup("OK");
453                                 }
454
455                                 stlink_reset(sl);
456                         }
457
458                         if(reply == NULL)
459                                 reply = strdup("");
460
461                         free(cmdName);
462
463                         break;
464                 }
465
466                 case 'c':
467                         stlink_run(sl);
468
469                         printf("Core running, waiting for interrupt (either in chip or GDB).\n");
470
471                         while(1) {
472                                 int status = gdb_check_for_interrupt(client);
473                                 if(status < 0) {
474                                         fprintf(stderr, "cannot check for int: %d\n", status);
475                                         return 1;
476                                 }
477
478                                 if(status == 1) {
479                                         stlink_force_debug(sl);
480                                         break;
481                                 }
482
483                                 stlink_status(sl);
484                                 if(sl->core_stat == STLINK_CORE_HALTED) {
485                                         break;
486                                 }
487
488                                 usleep(200000);
489                         }
490
491                         reply = strdup("S05"); // TRAP
492                         break;
493
494                 case 's':
495                         stlink_step(sl);
496
497                         reply = strdup("S05"); // TRAP
498                         break;
499
500                 case '?':
501                         reply = strdup("S05"); // TRAP
502                         break;
503
504                 case 'g':
505                         stlink_read_all_regs(sl);
506
507                         reply = calloc(8 * 16 + 1, 1);
508                         for(int i = 0; i < 16; i++)
509                                 sprintf(&reply[i * 8], "%08x", htonl(sl->reg.r[i]));
510
511                         break;
512
513                 case 'p': {
514                         unsigned id = strtoul(&packet[1], NULL, 16), reg = 0xDEADDEAD;
515
516                         if(id < 16) {
517                                 stlink_read_reg(sl, id);
518                                 reg = htonl(sl->reg.r[id]);
519                         } else if(id == 0x19) {
520                                 stlink_read_reg(sl, 16);
521                                 reg = htonl(sl->reg.xpsr);
522                         } else {
523                                 reply = strdup("E00");
524                         }
525
526                         reply = calloc(8 + 1, 1);
527                         sprintf(reply, "%08x", reg);
528
529                         break;
530                 }
531
532                 case 'P': {
533                         char* s_reg = &packet[1];
534                         char* s_value = strstr(&packet[1], "=") + 1;
535
536                         unsigned reg   = strtoul(s_reg,   NULL, 16);
537                         unsigned value = strtoul(s_value, NULL, 16);
538
539                         if(reg < 16) {
540                                 stlink_write_reg(sl, ntohl(value), reg);
541                         } else if(reg == 0x19) {
542                                 stlink_write_reg(sl, ntohl(value), 16);
543                         } else {
544                                 reply = strdup("E00");
545                         }
546
547                         if(!reply) {
548                                 reply = strdup("OK");
549                         }
550
551                         break;
552                 }
553
554                 case 'G':
555                         for(int i = 0; i < 16; i++) {
556                                 char str[9] = {0};
557                                 strncpy(str, &packet[1 + i * 8], 8);
558                                 uint32_t reg = strtoul(str, NULL, 16);
559                                 stlink_write_reg(sl, ntohl(reg), i);
560                         }
561
562                         reply = strdup("OK");
563                         break;
564
565                 case 'm': {
566                         char* s_start = &packet[1];
567                         char* s_count = strstr(&packet[1], ",") + 1;
568
569                         stm32_addr_t start = strtoul(s_start, NULL, 16);
570                         unsigned     count = strtoul(s_count, NULL, 16);
571
572                         unsigned adj_start = start % 4;
573
574                         stlink_read_mem32(sl, start - adj_start, (count % 4 == 0) ?
575                                                 count : count + 4 - (count % 4));
576
577                         reply = calloc(count * 2 + 1, 1);
578                         for(int i = 0; i < count; i++) {
579                                 reply[i * 2 + 0] = hex[sl->q_buf[i + adj_start] >> 4];
580                                 reply[i * 2 + 1] = hex[sl->q_buf[i + adj_start] & 0xf];
581                         }
582
583                         break;
584                 }
585
586                 case 'M': {
587                         char* s_start = &packet[1];
588                         char* s_count = strstr(&packet[1], ",") + 1;
589                         char* hexdata = strstr(packet, ":") + 1;
590
591                         stm32_addr_t start = strtoul(s_start, NULL, 16);
592                         unsigned     count = strtoul(s_count, NULL, 16);
593
594                         for(int i = 0; i < count; i ++) {
595                                 char hex[3] = { hexdata[i*2], hexdata[i*2+1], 0 };
596                                 uint8_t byte = strtoul(hex, NULL, 16);
597                                 sl->q_buf[i] = byte;
598                         }
599
600                         if((count % 4) == 0 && (start % 4) == 0) {
601                                 stlink_write_mem32(sl, start, count);
602                         } else {
603                                 stlink_write_mem8(sl, start, count);
604                         }
605
606                         reply = strdup("OK");
607
608                         break;
609                 }
610
611                 case 'Z': {
612                         if(packet[1] == '1') {
613                                 stm32_addr_t addr = strtoul(&packet[3], NULL, 16);
614                                 if(update_code_breakpoint(sl, addr, 1) < 0) {
615                                         reply = strdup("E00");
616                                 } else {
617                                         reply = strdup("OK");
618                                 }
619                         } else {
620                                 reply = strdup("");
621                         }
622
623                         break;
624                 }
625
626                 case 'z': {
627                         if(packet[1] == '1') {
628                                 stm32_addr_t addr = strtoul(&packet[3], NULL, 16);
629                                 update_code_breakpoint(sl, addr, 0);
630
631                                 reply = strdup("OK");
632                         } else {
633                                 reply = strdup("");
634                         }
635
636                         break;
637                 }
638
639                 case 'k': {
640                         // After this function will be entered afterwards, the
641                         // chip will be reset anyway. So this is a no-op.
642
643                         close(client);
644                         return 0;
645                 }
646
647                 default:
648                         reply = strdup("");
649                 }
650
651                 if(reply) {
652                         #ifdef DEBUG
653                         printf("send: %s\n", reply);
654                         #endif
655
656                         int result = gdb_send_packet(client, reply);
657                         if(result != 0) {
658                                 fprintf(stderr, "cannot send: %d\n", result);
659                                 return 1;
660                         }
661
662                         free(reply);
663                 }
664
665                 free(packet);
666         }
667
668         return 0;
669 }