1 /*
2 * Copyright (c) 2011 Peter Zotov <whitequark@whitequark.org>
3 * Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
4 */
5
6 #include <ctype.h>
7 #include <getopt.h>
8 #include <signal.h>
9 #include <stdio.h>
10 #include <string.h>
11 #include <stdlib.h>
12 #include <sys/types.h>
13
14 #if defined(_MSC_VER)
15 #include <stdbool.h>
16 #define __attribute__(x)
17 #endif
18
19 #if defined(_WIN32)
20 #include <win32_socket.h>
21 #else
22 #include <unistd.h>
23 #include <sys/socket.h>
24 #include <netinet/in.h>
25 #include <arpa/inet.h>
26 #endif
27
28 #include <stlink.h>
29 #include <helper.h>
30 #include <logging.h>
31 #include "gdb-remote.h"
32 #include "gdb-server.h"
33 #include "semihosting.h"
34
35 #define FLASH_BASE 0x08000000
36
37 // Semihosting doesn't have a short option, we define a value to identify it
38 #define SEMIHOSTING_OPTION 128
39 #define SERIAL_OPTION 127
40
41 // always update the FLASH_PAGE before each use, by calling stlink_calculate_pagesize
42 #define FLASH_PAGE (sl->flash_pgsz)
43
44 static stlink_t *connected_stlink = NULL;
45
46 #if defined(_WIN32)
47 #define close_socket win32_close_socket
48 #define IS_SOCK_VALID(__sock) ((__sock) != INVALID_SOCKET)
49 #else
50 #define close_socket close
51 #define SOCKET int
52 #define IS_SOCK_VALID(__sock) ((__sock) > 0)
53 #endif
54
55 static const char hex[] = "0123456789abcdef";
56
57 typedef struct _st_state_t {
58 // things from command line, bleh
59 int logging_level;
60 int listen_port;
61 int persistent;
62 enum connect_type connect_mode;
63 int freq;
64 char serialnumber[STLINK_SERIAL_BUFFER_SIZE];
65 bool semihosting;
66 const char* current_memory_map;
67 } st_state_t;
68
69
70 int serve(stlink_t *sl, st_state_t *st);
71 char* make_memory_map(stlink_t *sl);
72 static void init_cache(stlink_t *sl);
73
_cleanup()74 static void _cleanup() {
75 if (connected_stlink) {
76 // Switch back to mass storage mode before closing
77 stlink_run(connected_stlink, RUN_NORMAL);
78 stlink_exit_debug_mode(connected_stlink);
79 stlink_close(connected_stlink);
80 }
81 }
82
cleanup(int signum)83 static void cleanup(int signum) {
84 printf("Receive signal %i. Exiting...\n", signum);
85 _cleanup();
86 exit(1);
87 (void)signum;
88 }
89
90 #if defined(_WIN32)
CtrlHandler(DWORD fdwCtrlType)91 BOOL WINAPI CtrlHandler(DWORD fdwCtrlType) {
92 printf("Receive signal %i. Exiting...\r\n", (int)fdwCtrlType);
93 _cleanup();
94 return FALSE;
95 }
96 #endif
97
parse_options(int argc,char ** argv,st_state_t * st)98 int parse_options(int argc, char** argv, st_state_t *st) {
99 static struct option long_options[] = {
100 {"help", no_argument, NULL, 'h'},
101 {"verbose", optional_argument, NULL, 'v'},
102 {"listen_port", required_argument, NULL, 'p'},
103 {"multi", optional_argument, NULL, 'm'},
104 {"no-reset", optional_argument, NULL, 'n'},
105 {"hot-plug", optional_argument, NULL, 'n'},
106 {"connect-under-reset", optional_argument, NULL, 'u'},
107 {"freq", optional_argument, NULL, 'F'},
108 {"version", no_argument, NULL, 'V'},
109 {"semihosting", no_argument, NULL, SEMIHOSTING_OPTION},
110 {"serial", required_argument, NULL, SERIAL_OPTION},
111 {0, 0, 0, 0},
112 };
113 const char * help_str = "%s - usage:\n\n"
114 " -h, --help\t\tPrint this help\n"
115 " -V, --version\t\tPrint the version\n"
116 " -vXX, --verbose=XX\tSpecify a specific verbosity level (0..99)\n"
117 " -v, --verbose\t\tSpecify generally verbose logging\n"
118 " -p 4242, --listen_port=1234\n"
119 "\t\t\tSet the gdb server listen port. "
120 "(default port: " STRINGIFY(DEFAULT_GDB_LISTEN_PORT) ")\n"
121 " -m, --multi\n"
122 "\t\t\tSet gdb server to extended mode.\n"
123 "\t\t\tst-util will continue listening for connections after disconnect.\n"
124 " -n, --no-reset, --hot-plug\n"
125 "\t\t\tDo not reset board on connection.\n"
126 " -u, --connect-under-reset\n"
127 "\t\t\tConnect to the board before executing any instructions.\n"
128 " -F 1800K, --freq=1M\n"
129 "\t\t\tSet the frequency of the SWD/JTAG interface.\n"
130 " --semihosting\n"
131 "\t\t\tEnable semihosting support.\n"
132 " --serial <serial>\n"
133 "\t\t\tUse a specific serial number.\n"
134 "\n"
135 "The STLINK device to use can be specified in the environment\n"
136 "variable STLINK_DEVICE on the format <USB_BUS>:<USB_ADDR>.\n"
137 "\n"
138 ;
139
140
141 int option_index = 0;
142 int c;
143 int q;
144
145 while ((c = getopt_long(argc, argv, "hv::p:mn", long_options, &option_index)) != -1)
146 switch (c) {
147 case 0:
148 break;
149 case 'h':
150 printf(help_str, argv[0]);
151 exit(EXIT_SUCCESS);
152 break;
153 case 'v':
154 if (optarg) {
155 st->logging_level = atoi(optarg);
156 } else {
157 st->logging_level = DEBUG_LOGGING_LEVEL;
158 }
159
160 break;
161 case 'p':
162 sscanf(optarg, "%i", &q);
163 if (q < 0) {
164 fprintf(stderr, "Can't use a negative port to listen on: %d\n", q);
165 exit(EXIT_FAILURE);
166 }
167
168 st->listen_port = q;
169 break;
170
171 case 'm':
172 st->persistent = true;
173 break;
174 case 'n':
175 st->connect_mode = CONNECT_HOT_PLUG;
176 break;
177 case 'u':
178 st->connect_mode = CONNECT_UNDER_RESET;
179 break;
180 case 'F':
181 st->freq = arg_parse_freq(optarg);
182 if (st->freq < 0) {
183 fprintf(stderr, "Can't parse a frequency: %s\n", optarg);
184 exit(EXIT_FAILURE);
185 }
186 break;
187 case 'V':
188 printf("v%s\n", STLINK_VERSION);
189 exit(EXIT_SUCCESS);
190 case SEMIHOSTING_OPTION:
191 st->semihosting = true;
192 break;
193 case SERIAL_OPTION:
194 printf("use serial %s\n", optarg);
195 memcpy(st->serialnumber, optarg, STLINK_SERIAL_BUFFER_SIZE);
196 break;
197 }
198
199
200 if (optind < argc) {
201 printf("non-option ARGV-elements: ");
202
203 while (optind < argc) { printf("%s ", argv[optind++]); }
204
205 printf("\n");
206 }
207
208 return(0);
209 }
210
main(int argc,char ** argv)211 int main(int argc, char** argv) {
212 stlink_t *sl = NULL;
213 st_state_t state;
214 memset(&state, 0, sizeof(state));
215
216 // set defaults ...
217 state.logging_level = DEFAULT_LOGGING_LEVEL;
218 state.listen_port = DEFAULT_GDB_LISTEN_PORT;
219 state.connect_mode = CONNECT_NORMAL; // by default, reset board
220 parse_options(argc, argv, &state);
221
222 printf("st-util\n");
223
224 sl = stlink_open_usb(state.logging_level, state.connect_mode, state.serialnumber, state.freq);
225 if (sl == NULL) { return(1); }
226
227 if (sl->chip_id == STLINK_CHIPID_UNKNOWN) {
228 ELOG("Unsupported Target (Chip ID is %#010x, Core ID is %#010x).\n", sl->chip_id, sl->core_id);
229 return(1);
230 }
231
232 sl->verbose = 0;
233 connected_stlink = sl;
234
235 #if defined(_WIN32)
236 SetConsoleCtrlHandler((PHANDLER_ROUTINE) CtrlHandler, TRUE);
237 #else
238 signal(SIGINT, &cleanup);
239 signal(SIGTERM, &cleanup);
240 signal(SIGSEGV, &cleanup);
241 #endif
242
243 DLOG("Chip ID is %#010x, Core ID is %#08x.\n", sl->chip_id, sl->core_id);
244
245 #if defined(_WIN32)
246 WSADATA wsadata;
247
248 if (WSAStartup(MAKEWORD(2, 2), &wsadata) != 0) { goto winsock_error; }
249 #endif
250
251 do { // don't go beserk if serve() returns with error
252 if (serve(sl, &state)) { usleep (1 * 1000); }
253
254 sl = connected_stlink; // in case serve() changed the connection
255 stlink_run(sl, RUN_NORMAL); // continue
256 } while (state.persistent);
257
258 #if defined(_WIN32)
259 winsock_error:
260 WSACleanup();
261 #endif
262
263 // switch back to mass storage mode before closing
264 stlink_exit_debug_mode(sl);
265 stlink_close(sl);
266
267 return(0);
268 }
269
270 static const char* const target_description =
271 "<?xml version=\"1.0\"?>"
272 "<!DOCTYPE target SYSTEM \"gdb-target.dtd\">"
273 "<target version=\"1.0\">"
274 " <architecture>arm</architecture>"
275 " <feature name=\"org.gnu.gdb.arm.m-profile\">"
276 " <reg name=\"r0\" bitsize=\"32\"/>"
277 " <reg name=\"r1\" bitsize=\"32\"/>"
278 " <reg name=\"r2\" bitsize=\"32\"/>"
279 " <reg name=\"r3\" bitsize=\"32\"/>"
280 " <reg name=\"r4\" bitsize=\"32\"/>"
281 " <reg name=\"r5\" bitsize=\"32\"/>"
282 " <reg name=\"r6\" bitsize=\"32\"/>"
283 " <reg name=\"r7\" bitsize=\"32\"/>"
284 " <reg name=\"r8\" bitsize=\"32\"/>"
285 " <reg name=\"r9\" bitsize=\"32\"/>"
286 " <reg name=\"r10\" bitsize=\"32\"/>"
287 " <reg name=\"r11\" bitsize=\"32\"/>"
288 " <reg name=\"r12\" bitsize=\"32\"/>"
289 " <reg name=\"sp\" bitsize=\"32\" type=\"data_ptr\"/>"
290 " <reg name=\"lr\" bitsize=\"32\"/>"
291 " <reg name=\"pc\" bitsize=\"32\" type=\"code_ptr\"/>"
292 " <reg name=\"xpsr\" bitsize=\"32\" regnum=\"25\"/>"
293 " <reg name=\"msp\" bitsize=\"32\" regnum=\"26\" type=\"data_ptr\" group=\"general\" />"
294 " <reg name=\"psp\" bitsize=\"32\" regnum=\"27\" type=\"data_ptr\" group=\"general\" />"
295 " <reg name=\"control\" bitsize=\"8\" regnum=\"28\" type=\"int\" group=\"general\" />"
296 " <reg name=\"faultmask\" bitsize=\"8\" regnum=\"29\" type=\"int\" group=\"general\" />"
297 " <reg name=\"basepri\" bitsize=\"8\" regnum=\"30\" type=\"int\" group=\"general\" />"
298 " <reg name=\"primask\" bitsize=\"8\" regnum=\"31\" type=\"int\" group=\"general\" />"
299 " <reg name=\"s0\" bitsize=\"32\" regnum=\"32\" type=\"float\" group=\"float\" />"
300 " <reg name=\"s1\" bitsize=\"32\" type=\"float\" group=\"float\" />"
301 " <reg name=\"s2\" bitsize=\"32\" type=\"float\" group=\"float\" />"
302 " <reg name=\"s3\" bitsize=\"32\" type=\"float\" group=\"float\" />"
303 " <reg name=\"s4\" bitsize=\"32\" type=\"float\" group=\"float\" />"
304 " <reg name=\"s5\" bitsize=\"32\" type=\"float\" group=\"float\" />"
305 " <reg name=\"s6\" bitsize=\"32\" type=\"float\" group=\"float\" />"
306 " <reg name=\"s7\" bitsize=\"32\" type=\"float\" group=\"float\" />"
307 " <reg name=\"s8\" bitsize=\"32\" type=\"float\" group=\"float\" />"
308 " <reg name=\"s9\" bitsize=\"32\" type=\"float\" group=\"float\" />"
309 " <reg name=\"s10\" bitsize=\"32\" type=\"float\" group=\"float\" />"
310 " <reg name=\"s11\" bitsize=\"32\" type=\"float\" group=\"float\" />"
311 " <reg name=\"s12\" bitsize=\"32\" type=\"float\" group=\"float\" />"
312 " <reg name=\"s13\" bitsize=\"32\" type=\"float\" group=\"float\" />"
313 " <reg name=\"s14\" bitsize=\"32\" type=\"float\" group=\"float\" />"
314 " <reg name=\"s15\" bitsize=\"32\" type=\"float\" group=\"float\" />"
315 " <reg name=\"s16\" bitsize=\"32\" type=\"float\" group=\"float\" />"
316 " <reg name=\"s17\" bitsize=\"32\" type=\"float\" group=\"float\" />"
317 " <reg name=\"s18\" bitsize=\"32\" type=\"float\" group=\"float\" />"
318 " <reg name=\"s19\" bitsize=\"32\" type=\"float\" group=\"float\" />"
319 " <reg name=\"s20\" bitsize=\"32\" type=\"float\" group=\"float\" />"
320 " <reg name=\"s21\" bitsize=\"32\" type=\"float\" group=\"float\" />"
321 " <reg name=\"s22\" bitsize=\"32\" type=\"float\" group=\"float\" />"
322 " <reg name=\"s23\" bitsize=\"32\" type=\"float\" group=\"float\" />"
323 " <reg name=\"s24\" bitsize=\"32\" type=\"float\" group=\"float\" />"
324 " <reg name=\"s25\" bitsize=\"32\" type=\"float\" group=\"float\" />"
325 " <reg name=\"s26\" bitsize=\"32\" type=\"float\" group=\"float\" />"
326 " <reg name=\"s27\" bitsize=\"32\" type=\"float\" group=\"float\" />"
327 " <reg name=\"s28\" bitsize=\"32\" type=\"float\" group=\"float\" />"
328 " <reg name=\"s29\" bitsize=\"32\" type=\"float\" group=\"float\" />"
329 " <reg name=\"s30\" bitsize=\"32\" type=\"float\" group=\"float\" />"
330 " <reg name=\"s31\" bitsize=\"32\" type=\"float\" group=\"float\" />"
331 " <reg name=\"fpscr\" bitsize=\"32\" type=\"int\" group=\"float\" />"
332 " </feature>"
333 "</target>";
334
335 static const char* const memory_map_template_F4 =
336 "<?xml version=\"1.0\"?>"
337 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
338 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
339 "<memory-map>"
340 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x100000\"/>" // code = sram, bootrom or flash; flash is bigger
341 " <memory type=\"ram\" start=\"0x10000000\" length=\"0x10000\"/>" // ccm ram
342 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x20000\"/>" // sram
343 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x10000\">" // Sectors 0..3
344 " <property name=\"blocksize\">0x4000</property>" // 16kB
345 " </memory>"
346 " <memory type=\"flash\" start=\"0x08010000\" length=\"0x10000\">" // Sector 4
347 " <property name=\"blocksize\">0x10000</property>" // 64kB
348 " </memory>"
349 " <memory type=\"flash\" start=\"0x08020000\" length=\"0xE0000\">" // Sectors 5..11
350 " <property name=\"blocksize\">0x20000</property>" // 128kB
351 " </memory>"
352 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
353 " <memory type=\"ram\" start=\"0x60000000\" length=\"0x7fffffff\"/>" // AHB3 Peripherals
354 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
355 " <memory type=\"rom\" start=\"0x1fff0000\" length=\"0x7800\"/>" // bootrom
356 " <memory type=\"rom\" start=\"0x1fffc000\" length=\"0x10\"/>" // option byte area
357 "</memory-map>";
358
359 static const char* const memory_map_template_F4_HD =
360 "<?xml version=\"1.0\"?>"
361 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
362 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
363 "<memory-map>"
364 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x100000\"/>" // code = sram, bootrom or flash; flash is bigger
365 " <memory type=\"ram\" start=\"0x10000000\" length=\"0x10000\"/>" // ccm ram
366 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x40000\"/>" // sram
367 " <memory type=\"ram\" start=\"0x60000000\" length=\"0x10000000\"/>" // fmc bank 1 (nor/psram/sram)
368 " <memory type=\"ram\" start=\"0x70000000\" length=\"0x20000000\"/>" // fmc bank 2 & 3 (nand flash)
369 " <memory type=\"ram\" start=\"0x90000000\" length=\"0x10000000\"/>" // fmc bank 4 (pc card)
370 " <memory type=\"ram\" start=\"0xC0000000\" length=\"0x20000000\"/>" // fmc sdram bank 1 & 2
371 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x10000\">" // Sectors 0..3
372 " <property name=\"blocksize\">0x4000</property>" // 16kB
373 " </memory>"
374 " <memory type=\"flash\" start=\"0x08010000\" length=\"0x10000\">" // Sector 4
375 " <property name=\"blocksize\">0x10000</property>" // 64kB
376 " </memory>"
377 " <memory type=\"flash\" start=\"0x08020000\" length=\"0xE0000\">" // Sectors 5..11
378 " <property name=\"blocksize\">0x20000</property>" // 128kB
379 " </memory>"
380 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
381 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
382 " <memory type=\"rom\" start=\"0x1fff0000\" length=\"0x7800\"/>" // bootrom
383 " <memory type=\"rom\" start=\"0x1fffc000\" length=\"0x10\"/>" // option byte area
384 "</memory-map>";
385
386 static const char* const memory_map_template_F2 =
387 "<?xml version=\"1.0\"?>"
388 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
389 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
390 "<memory-map>"
391 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x%x\"/>" // code = sram, bootrom or flash; flash is bigger
392 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x%x\"/>" // sram
393 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x10000\">" // Sectors 0..3
394 " <property name=\"blocksize\">0x4000</property>" // 16kB
395 " </memory>"
396 " <memory type=\"flash\" start=\"0x08010000\" length=\"0x10000\">" // Sector 4
397 " <property name=\"blocksize\">0x10000</property>" // 64kB
398 " </memory>"
399 " <memory type=\"flash\" start=\"0x08020000\" length=\"0x%x\">" // Sectors 5..
400 " <property name=\"blocksize\">0x20000</property>" // 128kB
401 " </memory>"
402 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
403 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
404 " <memory type=\"rom\" start=\"0x%08x\" length=\"0x%x\"/>" // bootrom
405 " <memory type=\"rom\" start=\"0x1fffc000\" length=\"0x10\"/>" // option byte area
406 "</memory-map>";
407
408 static const char* const memory_map_template_L4 =
409 "<?xml version=\"1.0\"?>"
410 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
411 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
412 "<memory-map>"
413 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x%x\"/>" // code = sram, bootrom or flash; flash is bigger
414 " <memory type=\"ram\" start=\"0x10000000\" length=\"0x8000\"/>" // SRAM2 (32kB)
415 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x18000\"/>" // SRAM1 (96kB)
416 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x%x\">"
417 " <property name=\"blocksize\">0x800</property>"
418 " </memory>"
419 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
420 " <memory type=\"ram\" start=\"0x60000000\" length=\"0x7fffffff\"/>" // AHB3 Peripherals
421 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
422 " <memory type=\"rom\" start=\"0x1fff0000\" length=\"0x7000\"/>" // bootrom
423 " <memory type=\"rom\" start=\"0x1fff7800\" length=\"0x10\"/>" // option byte area
424 " <memory type=\"rom\" start=\"0x1ffff800\" length=\"0x10\"/>" // option byte area
425 "</memory-map>";
426
427 static const char* const memory_map_template_L496 =
428 "<?xml version=\"1.0\"?>"
429 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
430 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
431 "<memory-map>"
432 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x%x\"/>" // code = sram, bootrom or flash; flash is bigger
433 " <memory type=\"ram\" start=\"0x10000000\" length=\"0x10000\"/>" // SRAM2 (64kB)
434 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x50000\"/>" // SRAM1 + aliased SRAM2 (256 + 64 = 320kB)
435 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x%x\">"
436 " <property name=\"blocksize\">0x800</property>"
437 " </memory>"
438 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
439 " <memory type=\"ram\" start=\"0x60000000\" length=\"0x7fffffff\"/>" // AHB3 Peripherals
440 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
441 " <memory type=\"rom\" start=\"0x1fff0000\" length=\"0x7000\"/>" // bootrom
442 " <memory type=\"rom\" start=\"0x1fff7800\" length=\"0x10\"/>" // option byte area
443 " <memory type=\"rom\" start=\"0x1ffff800\" length=\"0x10\"/>" // option byte area
444 "</memory-map>";
445
446 static const char* const memory_map_template =
447 "<?xml version=\"1.0\"?>"
448 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
449 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
450 "<memory-map>"
451 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x%x\"/>" // code = sram, bootrom or flash; flash is bigger
452 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x%x\"/>" // sram 8kB
453 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x%x\">"
454 " <property name=\"blocksize\">0x%x</property>"
455 " </memory>"
456 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
457 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
458 " <memory type=\"rom\" start=\"0x%08x\" length=\"0x%x\"/>" // bootrom
459 " <memory type=\"rom\" start=\"0x1ffff800\" length=\"0x10\"/>" // option byte area
460 "</memory-map>";
461
462 static const char* const memory_map_template_F7 =
463 "<?xml version=\"1.0\"?>"
464 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
465 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
466 "<memory-map>"
467 " <memory type=\"ram\" start=\"0x00000000\" length=\"0x4000\"/>" // ITCM ram 16kB
468 " <memory type=\"rom\" start=\"0x00200000\" length=\"0x100000\"/>" // ITCM flash
469 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x%x\"/>" // sram
470 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x20000\">" // Sectors 0..3
471 " <property name=\"blocksize\">0x8000</property>" // 32kB
472 " </memory>"
473 " <memory type=\"flash\" start=\"0x08020000\" length=\"0x20000\">" // Sector 4
474 " <property name=\"blocksize\">0x20000</property>" // 128kB
475 " </memory>"
476 " <memory type=\"flash\" start=\"0x08040000\" length=\"0xC0000\">" // Sectors 5..7
477 " <property name=\"blocksize\">0x40000</property>" // 128kB
478 " </memory>"
479 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
480 " <memory type=\"ram\" start=\"0x60000000\" length=\"0x7fffffff\"/>" // AHB3 Peripherals
481 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
482 " <memory type=\"rom\" start=\"0x00100000\" length=\"0xEDC0\"/>" // bootrom
483 " <memory type=\"rom\" start=\"0x1fff0000\" length=\"0x20\"/>" // option byte area
484 "</memory-map>";
485
486 static const char* const memory_map_template_H7 =
487 "<?xml version=\"1.0\"?>"
488 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
489 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
490 "<memory-map>"
491 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x10000\"/>" // ITCMRAM 64kB
492 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x20000\"/>" // DTCMRAM 128kB
493 " <memory type=\"ram\" start=\"0x24000000\" length=\"0x80000\"/>" // RAM D1 512kB
494 " <memory type=\"ram\" start=\"0x30000000\" length=\"0x48000\"/>" // RAM D2 288kB
495 " <memory type=\"ram\" start=\"0x38000000\" length=\"0x10000\"/>" // RAM D3 64kB
496 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x%x\">"
497 " <property name=\"blocksize\">0x%x</property>"
498 " </memory>"
499 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
500 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
501 " <memory type=\"rom\" start=\"0x1ff00000\" length=\"0x20000\"/>" // bootrom
502 "</memory-map>";
503
504
505 static const char* const memory_map_template_F4_DE =
506 "<?xml version=\"1.0\"?>"
507 "<!DOCTYPE memory-map PUBLIC \"+//IDN gnu.org//DTD GDB Memory Map V1.0//EN\""
508 " \"http://sourceware.org/gdb/gdb-memory-map.dtd\">"
509 "<memory-map>"
510 " <memory type=\"rom\" start=\"0x00000000\" length=\"0x80000\"/>" // code = sram, bootrom or flash; flash is bigger
511 " <memory type=\"ram\" start=\"0x20000000\" length=\"0x18000\"/>" // sram
512 " <memory type=\"flash\" start=\"0x08000000\" length=\"0x10000\">" // Sectors 0..3
513 " <property name=\"blocksize\">0x4000</property>" // 16kB
514 " </memory>"
515 " <memory type=\"flash\" start=\"0x08010000\" length=\"0x10000\">" // Sector 4
516 " <property name=\"blocksize\">0x10000</property>" // 64kB
517 " </memory>"
518 " <memory type=\"flash\" start=\"0x08020000\" length=\"0x60000\">" // Sectors 5..7
519 " <property name=\"blocksize\">0x20000</property>" // 128kB
520 " </memory>"
521 " <memory type=\"ram\" start=\"0x40000000\" length=\"0x1fffffff\"/>" // peripheral regs
522 " <memory type=\"ram\" start=\"0xe0000000\" length=\"0x1fffffff\"/>" // cortex regs
523 " <memory type=\"rom\" start=\"0x1fff0000\" length=\"0x7800\"/>" // bootrom
524 " <memory type=\"rom\" start=\"0x1fff7800\" length=\"0x210\"/>" // otp
525 " <memory type=\"rom\" start=\"0x1fffc000\" length=\"0x10\"/>" // option byte area
526 "</memory-map>";
527
make_memory_map(stlink_t * sl)528 char* make_memory_map(stlink_t *sl) {
529 // this will be freed in serve()
530 const size_t sz = 4096;
531 char* map = malloc(sz);
532 map[0] = '\0';
533
534 if (sl->chip_id == STLINK_CHIPID_STM32_F4 ||
535 sl->chip_id == STLINK_CHIPID_STM32_F446 ||
536 sl->chip_id == STLINK_CHIPID_STM32_F411RE) {
537 strcpy(map, memory_map_template_F4);
538 } else if (sl->chip_id == STLINK_CHIPID_STM32_F4_DE) {
539 strcpy(map, memory_map_template_F4_DE);
540 } else if (sl->core_id == STM32F7_CORE_ID) {
541 snprintf(map, sz, memory_map_template_F7,
542 (unsigned int)sl->sram_size);
543 } else if (sl->chip_id == STLINK_CHIPID_STM32_H74XXX) {
544 snprintf(map, sz, memory_map_template_H7,
545 (unsigned int)sl->flash_size,
546 (unsigned int)sl->flash_pgsz);
547 } else if (sl->chip_id == STLINK_CHIPID_STM32_F4_HD) {
548 strcpy(map, memory_map_template_F4_HD);
549 } else if (sl->chip_id == STLINK_CHIPID_STM32_F2) {
550 snprintf(map, sz, memory_map_template_F2,
551 (unsigned int)sl->flash_size,
552 (unsigned int)sl->sram_size,
553 (unsigned int)sl->flash_size - 0x20000,
554 (unsigned int)sl->sys_base,
555 (unsigned int)sl->sys_size);
556 } else if ((sl->chip_id == STLINK_CHIPID_STM32_L4) ||
557 (sl->chip_id == STLINK_CHIPID_STM32_L43X) ||
558 (sl->chip_id == STLINK_CHIPID_STM32_L46X)) {
559 snprintf(map, sz, memory_map_template_L4,
560 (unsigned int)sl->flash_size,
561 (unsigned int)sl->flash_size);
562 } else if (sl->chip_id == STLINK_CHIPID_STM32_L496X) {
563 snprintf(map, sz, memory_map_template_L496,
564 (unsigned int)sl->flash_size,
565 (unsigned int)sl->flash_size);
566 } else {
567 snprintf(map, sz, memory_map_template,
568 (unsigned int)sl->flash_size,
569 (unsigned int)sl->sram_size,
570 (unsigned int)sl->flash_size,
571 (unsigned int)sl->flash_pgsz,
572 (unsigned int)sl->sys_base,
573 (unsigned int)sl->sys_size);
574 }
575
576 return(map);
577 }
578
579 #define DATA_WATCH_NUM 4
580
581 enum watchfun { WATCHDISABLED = 0, WATCHREAD = 5, WATCHWRITE = 6, WATCHACCESS = 7 };
582
583 struct code_hw_watchpoint {
584 stm32_addr_t addr;
585 uint8_t mask;
586 enum watchfun fun;
587 };
588
589 static struct code_hw_watchpoint data_watches[DATA_WATCH_NUM];
590
init_data_watchpoints(stlink_t * sl)591 static void init_data_watchpoints(stlink_t *sl) {
592 uint32_t data;
593 DLOG("init watchpoints\n");
594
595 // set TRCENA in debug command to turn on DWT unit
596 stlink_read_debug32(sl, STLINK_REG_CM3_DEMCR, &data);
597 data |= STLINK_REG_CM3_DEMCR_TRCENA;
598 stlink_write_debug32(sl, STLINK_REG_CM3_DEMCR, data);
599
600 // make sure all watchpoints are cleared
601 for (int i = 0; i < DATA_WATCH_NUM; i++) {
602 data_watches[i].fun = WATCHDISABLED;
603 stlink_write_debug32(sl, STLINK_REG_CM3_DWT_FUNn(i), 0);
604 }
605 }
606
add_data_watchpoint(stlink_t * sl,enum watchfun wf,stm32_addr_t addr,unsigned int len)607 static int add_data_watchpoint(stlink_t *sl, enum watchfun wf, stm32_addr_t addr, unsigned int len) {
608 int i = 0;
609 uint32_t mask, dummy;
610
611 // computer mask
612 // find a free watchpoint
613 // configure
614
615 mask = -1;
616 i = len;
617
618 while (i) {
619 i >>= 1;
620 mask++;
621 }
622
623 if ((mask != (uint32_t)-1) && (mask < 16)) {
624 for (i = 0; i < DATA_WATCH_NUM; i++)
625 // is this an empty slot ?
626 if (data_watches[i].fun == WATCHDISABLED) {
627 DLOG("insert watchpoint %d addr %x wf %u mask %u len %d\n", i, addr, wf, mask, len);
628
629 data_watches[i].fun = wf;
630 data_watches[i].addr = addr;
631 data_watches[i].mask = mask;
632
633 // insert comparator address
634 stlink_write_debug32(sl, STLINK_REG_CM3_DWT_COMPn(i), addr);
635
636 // insert mask
637 stlink_write_debug32(sl, STLINK_REG_CM3_DWT_MASKn(i), mask);
638
639 // insert function
640 stlink_write_debug32(sl, STLINK_REG_CM3_DWT_FUNn(i), wf);
641
642 // just to make sure the matched bit is clear !
643 stlink_read_debug32(sl, STLINK_REG_CM3_DWT_FUNn(i), &dummy);
644 return(0);
645 }
646 }
647
648 DLOG("failure: add watchpoints addr %x wf %u len %u\n", addr, wf, len);
649 return(-1);
650 }
651
delete_data_watchpoint(stlink_t * sl,stm32_addr_t addr)652 static int delete_data_watchpoint(stlink_t *sl, stm32_addr_t addr) {
653 int i;
654
655 for (i = 0; i < DATA_WATCH_NUM; i++) {
656 if ((data_watches[i].addr == addr) && (data_watches[i].fun != WATCHDISABLED)) {
657 DLOG("delete watchpoint %d addr %x\n", i, addr);
658
659 data_watches[i].fun = WATCHDISABLED;
660 stlink_write_debug32(sl, STLINK_REG_CM3_DWT_FUNn(i), 0);
661
662 return(0);
663 }
664 }
665
666 DLOG("failure: delete watchpoint addr %x\n", addr);
667
668 return(-1);
669 }
670
671 static int code_break_num;
672 static int code_lit_num;
673 static int code_break_rev;
674 #define CODE_BREAK_NUM_MAX 15
675 #define CODE_BREAK_LOW 0x01
676 #define CODE_BREAK_HIGH 0x02
677 #define CODE_BREAK_REMAP 0x04
678 #define CODE_BREAK_REV_V1 0x00
679 #define CODE_BREAK_REV_V2 0x01
680
681 struct code_hw_breakpoint {
682 stm32_addr_t addr;
683 int type;
684 };
685
686 static struct code_hw_breakpoint code_breaks[CODE_BREAK_NUM_MAX];
687
init_code_breakpoints(stlink_t * sl)688 static void init_code_breakpoints(stlink_t *sl) {
689 unsigned int val;
690 memset(sl->q_buf, 0, 4);
691 stlink_write_debug32(sl, STLINK_REG_CM3_FP_CTRL, 0x03 /* KEY | ENABLE */);
692 stlink_read_debug32(sl, STLINK_REG_CM3_FP_CTRL, &val);
693 code_break_num = ((val >> 4) & 0xf);
694 code_lit_num = ((val >> 8) & 0xf);
695 code_break_rev = ((val >> 28) & 0xf);
696
697 ILOG("Found %i hw breakpoint registers\n", code_break_num);
698
699 stlink_read_debug32(sl, STLINK_REG_CM3_CPUID, &val);
700 if (((val>>4) & 0xFFF) == 0xC27) {
701 // Cortex-M7 can have locked to write FP_* registers
702 // IHI0029D, p. 48, Lock Access Register
703 stlink_write_debug32(sl, STLINK_REG_CM7_FP_LAR, STLINK_REG_CM7_FP_LAR_KEY);
704 }
705
706 for (int i = 0; i < code_break_num; i++) {
707 code_breaks[i].type = 0;
708 stlink_write_debug32(sl, STLINK_REG_CM3_FP_COMPn(i), 0);
709 }
710 }
711
has_breakpoint(stm32_addr_t addr)712 static int has_breakpoint(stm32_addr_t addr) {
713 for (int i = 0; i < code_break_num; i++)
714 if (code_breaks[i].addr == addr) { return(1); }
715
716 return(0);
717 }
718
update_code_breakpoint(stlink_t * sl,stm32_addr_t addr,int set)719 static int update_code_breakpoint(stlink_t *sl, stm32_addr_t addr, int set) {
720 uint32_t mask;
721 int type;
722 stm32_addr_t fpb_addr;
723
724 if (addr & 1) {
725 ELOG("update_code_breakpoint: unaligned address %08x\n", addr);
726 return(-1);
727 }
728
729 if (code_break_rev == CODE_BREAK_REV_V1) {
730 type = (addr & 0x2) ? CODE_BREAK_HIGH : CODE_BREAK_LOW;
731 fpb_addr = addr & 0x1FFFFFFC;
732 } else {
733 type = CODE_BREAK_REMAP;
734 fpb_addr = addr;
735 }
736
737 int id = -1;
738 for (int i = 0; i < code_break_num; i++)
739 if (fpb_addr == code_breaks[i].addr || (set && code_breaks[i].type == 0)) {
740 id = i;
741 break;
742 }
743
744 if (id == -1) {
745 if (set)
746 return(-1); // free slot not found
747 else
748 return(0); // breakpoint is already removed
749 }
750
751 struct code_hw_breakpoint* bp = &code_breaks[id];
752 bp->addr = fpb_addr;
753 if (set)
754 bp->type |= type;
755 else
756 bp->type &= ~type;
757
758 // DDI0403E, p. 759, FP_COMPn register description
759 mask = ((bp->type&0x03) << 30) | bp->addr | 1;
760
761 if (bp->type == 0) {
762 DLOG("clearing hw break %d\n", id);
763 stlink_write_debug32(sl, STLINK_REG_CM3_FP_COMPn(id), 0);
764 } else {
765 DLOG("setting hw break %d at %08x (%d)\n", id, bp->addr, bp->type);
766 DLOG("reg %08x \n", mask);
767 stlink_write_debug32(sl, STLINK_REG_CM3_FP_COMPn(id), mask);
768 }
769
770 return(0);
771 }
772
773
774 struct flash_block {
775 stm32_addr_t addr;
776 unsigned length;
777 uint8_t* data;
778
779 struct flash_block* next;
780 };
781
782 static struct flash_block* flash_root;
783
flash_add_block(stm32_addr_t addr,unsigned length,stlink_t * sl)784 static int flash_add_block(stm32_addr_t addr, unsigned length, stlink_t *sl) {
785
786 if (addr < FLASH_BASE || addr + length > FLASH_BASE + sl->flash_size) {
787 ELOG("flash_add_block: incorrect bounds\n");
788 return(-1);
789 }
790
791 stlink_calculate_pagesize(sl, addr);
792
793 if (addr % FLASH_PAGE != 0 || length % FLASH_PAGE != 0) {
794 ELOG("flash_add_block: unaligned block\n");
795 return(-1);
796 }
797
798 struct flash_block* new = malloc(sizeof(struct flash_block));
799 new->next = flash_root;
800 new->addr = addr;
801 new->length = length;
802 new->data = malloc(length);
803 memset(new->data, stlink_get_erased_pattern(sl), length);
804
805 flash_root = new;
806 return(0);
807 }
808
flash_populate(stm32_addr_t addr,uint8_t * data,unsigned length)809 static int flash_populate(stm32_addr_t addr, uint8_t* data, unsigned length) {
810 unsigned int fit_blocks = 0, fit_length = 0;
811
812 for (struct flash_block* fb = flash_root; fb; fb = fb->next) {
813 /*
814 * Block: ------X------Y--------
815 * Data: a-----b
816 * a--b
817 * a-----------b
818 * Block intersects with data, if:
819 * a < Y && b > x
820 */
821
822 unsigned X = fb->addr, Y = fb->addr + fb->length;
823 unsigned a = addr, b = addr + length;
824
825 if (a < Y && b > X) {
826 // from start of the block
827 unsigned start = (a > X ? a : X) - X;
828 unsigned end = (b > Y ? Y : b) - X;
829
830 memcpy(fb->data + start, data, end - start);
831
832 fit_blocks++;
833 fit_length += end - start;
834 }
835 }
836
837 if (fit_blocks == 0) {
838 ELOG("Unfit data block %08x -> %04x\n", addr, length);
839 return(-1);
840 }
841
842 if (fit_length != length) {
843 WLOG("data block %08x -> %04x truncated to %04x\n", addr, length, fit_length);
844 WLOG("(this is not an error, just a GDB glitch)\n");
845 }
846
847 return(0);
848 }
849
flash_go(stlink_t * sl,st_state_t * st)850 static int flash_go(stlink_t *sl, st_state_t *st) {
851 int error = -1;
852 int ret;
853 flash_loader_t fl;
854
855 stlink_target_connect(sl, st->connect_mode);
856 stlink_force_debug(sl);
857
858 for (struct flash_block* fb = flash_root; fb; fb = fb->next) {
859 ILOG("flash_erase: block %08x -> %04x\n", fb->addr, fb->length);
860
861 for (stm32_addr_t page = fb->addr; page < fb->addr + fb->length; page += (uint32_t)FLASH_PAGE) {
862 // update FLASH_PAGE
863 stlink_calculate_pagesize(sl, page);
864
865 ILOG("flash_erase: page %08x\n", page);
866 ret = stlink_erase_flash_page(sl, page);
867 if (ret) { goto error; }
868 }
869 }
870
871 ret = stlink_flashloader_start(sl, &fl);
872 if (ret) { goto error; }
873
874 for (struct flash_block* fb = flash_root; fb; fb = fb->next) {
875 ILOG("flash_do: block %08x -> %04x\n", fb->addr, fb->length);
876
877 for (stm32_addr_t page = fb->addr; page < fb->addr + fb->length; page += (uint32_t)FLASH_PAGE) {
878 unsigned length = fb->length - (page - fb->addr);
879
880 // update FLASH_PAGE
881 stlink_calculate_pagesize(sl, page);
882
883 ILOG("flash_do: page %08x\n", page);
884 unsigned len = (length > FLASH_PAGE) ? (unsigned int)FLASH_PAGE : length;
885 ret = stlink_flashloader_write(sl, &fl, page, fb->data + (page - fb->addr), len);
886 if (ret) { goto error; }
887 }
888 }
889
890 stlink_flashloader_stop(sl, &fl);
891 stlink_reset(sl, RESET_SOFT_AND_HALT);
892 error = 0;
893
894 error:
895
896 for (struct flash_block* fb = flash_root, *next; fb; fb = next) {
897 next = fb->next;
898 free(fb->data);
899 free(fb);
900 }
901
902 flash_root = NULL;
903 return(error);
904 }
905
906 struct cache_level_desc {
907 unsigned int nsets;
908 unsigned int nways;
909 unsigned int log2_nways;
910 unsigned int width;
911 };
912
913 struct cache_desc_t {
914 unsigned used;
915
916 // minimal line size in bytes
917 unsigned int dminline;
918 unsigned int iminline;
919
920 // last level of unification (uniprocessor)
921 unsigned int louu;
922
923 struct cache_level_desc icache[7];
924 struct cache_level_desc dcache[7];
925 };
926
927 static struct cache_desc_t cache_desc;
928
929 // return the smallest R so that V <= (1 << R); not performance critical
ceil_log2(unsigned v)930 static unsigned ceil_log2(unsigned v) {
931 unsigned res;
932
933 for (res = 0; (1U << res) < v; res++);
934
935 return(res);
936 }
937
read_cache_level_desc(stlink_t * sl,struct cache_level_desc * desc)938 static void read_cache_level_desc(stlink_t *sl, struct cache_level_desc *desc) {
939 unsigned int ccsidr;
940 unsigned int log2_nsets;
941
942 stlink_read_debug32(sl, STLINK_REG_CM7_CCSIDR, &ccsidr);
943 desc->nsets = ((ccsidr >> 13) & 0x3fff) + 1;
944 desc->nways = ((ccsidr >> 3) & 0x1ff) + 1;
945 desc->log2_nways = ceil_log2 (desc->nways);
946 log2_nsets = ceil_log2 (desc->nsets);
947 desc->width = 4 + (ccsidr & 7) + log2_nsets;
948 ILOG("%08x LineSize: %u, ways: %u, sets: %u (width: %u)\n",
949 ccsidr, 4 << (ccsidr & 7), desc->nways, desc->nsets, desc->width);
950 }
951
init_cache(stlink_t * sl)952 static void init_cache (stlink_t *sl) {
953 unsigned int clidr;
954 unsigned int ccr;
955 unsigned int ctr;
956 int i;
957
958 // Check have cache
959 stlink_read_debug32(sl, STLINK_REG_CM7_CTR, &ctr);
960 if ((ctr >> 29) != 0x04) {
961 cache_desc.used = 0;
962 return;
963 } else
964 cache_desc.used = 1;
965 cache_desc.dminline = 4 << ((ctr >> 16) & 0x0f);
966 cache_desc.iminline = 4 << (ctr & 0x0f);
967
968 stlink_read_debug32(sl, STLINK_REG_CM7_CLIDR, &clidr);
969 cache_desc.louu = (clidr >> 27) & 7;
970
971 stlink_read_debug32(sl, STLINK_REG_CM7_CCR, &ccr);
972 ILOG("Chip clidr: %08x, I-Cache: %s, D-Cache: %s\n",
973 clidr, ccr & STLINK_REG_CM7_CCR_IC ? "on" : "off", ccr & STLINK_REG_CM7_CCR_DC ? "on" : "off");
974 ILOG(" cache: LoUU: %u, LoC: %u, LoUIS: %u\n",
975 (clidr >> 27) & 7, (clidr >> 24) & 7, (clidr >> 21) & 7);
976 ILOG(" cache: ctr: %08x, DminLine: %u bytes, IminLine: %u bytes\n", ctr,
977 cache_desc.dminline, cache_desc.iminline);
978
979 for (i = 0; i < 7; i++) {
980 unsigned int ct = (clidr >> (3 * i)) & 0x07;
981 cache_desc.dcache[i].width = 0;
982 cache_desc.icache[i].width = 0;
983
984 if (ct == 2 || ct == 3 || ct == 4) { // data
985 stlink_write_debug32(sl, STLINK_REG_CM7_CSSELR, i << 1);
986 ILOG("D-Cache L%d: ", i);
987 read_cache_level_desc(sl, &cache_desc.dcache[i]);
988 }
989
990 if (ct == 1 || ct == 3) { // instruction
991 stlink_write_debug32(sl, STLINK_REG_CM7_CSSELR, (i << 1) | 1);
992 ILOG("I-Cache L%d: ", i);
993 read_cache_level_desc(sl, &cache_desc.icache[i]);
994 }
995 }
996 }
997
cache_flush(stlink_t * sl,unsigned ccr)998 static void cache_flush(stlink_t *sl, unsigned ccr) {
999 int level;
1000
1001 if (ccr & STLINK_REG_CM7_CCR_DC) {
1002 for (level = cache_desc.louu - 1; level >= 0; level--) {
1003 struct cache_level_desc *desc = &cache_desc.dcache[level];
1004 unsigned addr;
1005 unsigned max_addr = 1 << desc->width;
1006 unsigned way_sh = 32 - desc->log2_nways;
1007
1008 // D-cache clean by set-ways.
1009 for (addr = (level << 1); addr < max_addr; addr += cache_desc.dminline) {
1010 unsigned int way;
1011
1012 for (way = 0; way < desc->nways; way++) {
1013 stlink_write_debug32(sl, STLINK_REG_CM7_DCCSW, addr | (way << way_sh));
1014 }
1015 }
1016 }
1017 }
1018
1019 // invalidate all I-cache to oPU
1020 if (ccr & STLINK_REG_CM7_CCR_IC) {
1021 stlink_write_debug32(sl, STLINK_REG_CM7_ICIALLU, 0);
1022 }
1023 }
1024
1025 static int cache_modified;
1026
cache_change(stm32_addr_t start,unsigned count)1027 static void cache_change(stm32_addr_t start, unsigned count) {
1028 if (count == 0) { return; }
1029
1030 (void)start;
1031 cache_modified = 1;
1032 }
1033
cache_sync(stlink_t * sl)1034 static void cache_sync(stlink_t *sl) {
1035 unsigned ccr;
1036
1037 if (!cache_desc.used) { return; }
1038
1039 if (!cache_modified) { return; }
1040
1041 cache_modified = 0;
1042 stlink_read_debug32(sl, STLINK_REG_CM7_CCR, &ccr);
1043 if (ccr & (STLINK_REG_CM7_CCR_IC | STLINK_REG_CM7_CCR_DC)) { cache_flush(sl, ccr); }
1044 }
1045
unhexify(const char * in,char * out,size_t out_count)1046 static size_t unhexify(const char *in, char *out, size_t out_count) {
1047 size_t i;
1048 unsigned int c;
1049
1050 for (i = 0; i < out_count; i++) {
1051 if (sscanf(in + (2 * i), "%02x", &c) != 1) { return(i); }
1052
1053 out[i] = (char)c;
1054 }
1055
1056 return(i);
1057 }
1058
serve(stlink_t * sl,st_state_t * st)1059 int serve(stlink_t *sl, st_state_t *st) {
1060 SOCKET sock = socket(AF_INET, SOCK_STREAM, 0);
1061
1062 if (!IS_SOCK_VALID(sock)) {
1063 perror("socket");
1064 return(1);
1065 }
1066
1067 unsigned int val = 1;
1068 setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (char *)&val, sizeof(val));
1069
1070 struct sockaddr_in serv_addr;
1071 memset(&serv_addr, 0, sizeof(struct sockaddr_in));
1072 serv_addr.sin_family = AF_INET;
1073 serv_addr.sin_addr.s_addr = INADDR_ANY;
1074 serv_addr.sin_port = htons(st->listen_port);
1075
1076 if (bind(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
1077 perror("bind");
1078 close_socket(sock);
1079 return(1);
1080 }
1081
1082 if (listen(sock, 5) < 0) {
1083 perror("listen");
1084 close_socket(sock);
1085 return(1);
1086 }
1087
1088 ILOG("Listening at *:%d...\n", st->listen_port);
1089
1090 SOCKET client = accept(sock, NULL, NULL);
1091
1092 // signal (SIGINT, SIG_DFL);
1093 if (!IS_SOCK_VALID(client)) {
1094 perror("accept");
1095 close_socket(sock);
1096 return(1);
1097 }
1098
1099 close_socket(sock);
1100
1101 uint32_t chip_id = sl->chip_id;
1102
1103 stlink_target_connect(sl, st->connect_mode);
1104 stlink_force_debug(sl);
1105
1106 if (sl->chip_id != chip_id) {
1107 WLOG("Target has changed!\n");
1108 }
1109
1110 init_code_breakpoints(sl);
1111 init_data_watchpoints(sl);
1112
1113 init_cache(sl);
1114
1115 st->current_memory_map = make_memory_map(sl);
1116
1117 ILOG("GDB connected.\n");
1118
1119 /*
1120 * To allow resetting the chip from GDB it is required to emulate attaching
1121 * and detaching to target.
1122 */
1123 unsigned int attached = 1;
1124 // if a critical error is detected, break from the loop
1125 int critical_error = 0;
1126 int ret;
1127
1128 while (1) {
1129 ret = 0;
1130 char* packet;
1131
1132 int status = gdb_recv_packet(client, &packet);
1133
1134 if (status < 0) {
1135 ELOG("cannot recv: %d\n", status);
1136 close_socket(client);
1137 return(1);
1138 }
1139
1140 DLOG("recv: %s\n", packet);
1141
1142 char* reply = NULL;
1143 struct stlink_reg regp;
1144
1145 switch (packet[0]) {
1146 case 'q': {
1147 if (packet[1] == 'P' || packet[1] == 'C' || packet[1] == 'L') {
1148 reply = strdup("");
1149 break;
1150 }
1151
1152 char *separator = strstr(packet, ":"), *params = "";
1153
1154 if (separator == NULL) {
1155 separator = packet + strlen(packet);
1156 } else {
1157 params = separator + 1;
1158 }
1159
1160 unsigned queryNameLength = (unsigned int)(separator - &packet[1]);
1161 char* queryName = calloc(queryNameLength + 1, 1);
1162 strncpy(queryName, &packet[1], queryNameLength);
1163
1164 DLOG("query: %s;%s\n", queryName, params);
1165
1166 if (!strcmp(queryName, "Supported")) {
1167 reply = strdup("PacketSize=3fff;qXfer:memory-map:read+;qXfer:features:read+");
1168 } else if (!strcmp(queryName, "Xfer")) {
1169 char *type, *op, *__s_addr, *s_length;
1170 char *tok = params;
1171 char *annex __attribute__((unused));
1172
1173 type = strsep(&tok, ":");
1174 op = strsep(&tok, ":");
1175 annex = strsep(&tok, ":");
1176 __s_addr = strsep(&tok, ",");
1177 s_length = tok;
1178
1179 unsigned addr = (unsigned int)strtoul(__s_addr, NULL, 16),
1180 length = (unsigned int)strtoul(s_length, NULL, 16);
1181
1182 DLOG("Xfer: type:%s;op:%s;annex:%s;addr:%d;length:%d\n",
1183 type, op, annex, addr, length);
1184
1185 const char* data;
1186 if (strcmp(op, "read")) {
1187 data = NULL;
1188 } else if (!strcmp(type, "memory-map")) {
1189 data = st->current_memory_map;
1190 } else if (!strcmp(type, "features")) {
1191 data = target_description;
1192 } else {
1193 data = NULL;
1194 }
1195
1196 if (data) {
1197 unsigned data_length = (unsigned int)strlen(data);
1198
1199 if (addr + length > data_length) { length = data_length - addr; }
1200
1201 if (length == 0) {
1202 reply = strdup("l");
1203 } else {
1204 reply = calloc(length + 2, 1);
1205 reply[0] = 'm';
1206 strncpy(&reply[1], data, length);
1207 }
1208 }
1209 } else if (!strncmp(queryName, "Rcmd,", 4)) {
1210 // Rcmd uses the wrong separator
1211 separator = strstr(packet, ",");
1212 params = "";
1213
1214 if (separator == NULL) {
1215 separator = packet + strlen(packet);
1216 } else {
1217 params = separator + 1;
1218 }
1219
1220 size_t hex_len = strlen(params);
1221 size_t alloc_size = (hex_len / 2) + 1;
1222 size_t cmd_len;
1223 char *cmd = malloc(alloc_size);
1224
1225 if (cmd == NULL) {
1226 DLOG("Rcmd unhexify allocation error\n");
1227 break;
1228 }
1229
1230 cmd_len = unhexify(params, cmd, alloc_size - 1);
1231 cmd[cmd_len] = 0;
1232
1233 DLOG("unhexified Rcmd: '%s'\n", cmd);
1234
1235 if (!strncmp(cmd, "resume", 6)) { // resume
1236 DLOG("Rcmd: resume\n");
1237 cache_sync(sl);
1238 ret = stlink_run(sl, RUN_NORMAL);
1239
1240 if (ret) {
1241 DLOG("Rcmd: resume failed\n");
1242 reply = strdup("E00");
1243 } else {
1244 reply = strdup("OK");
1245 }
1246
1247 } else if (!strncmp(cmd, "halt", 4)) { // halt
1248 ret = stlink_force_debug(sl);
1249
1250 if (ret) {
1251 DLOG("Rcmd: halt failed\n");
1252 reply = strdup("E00");
1253 } else {
1254 reply = strdup("OK");
1255 DLOG("Rcmd: halt\n");
1256 }
1257
1258 } else if (!strncmp(cmd, "jtag_reset", 10)) { // jtag_reset
1259 reply = strdup("OK");
1260
1261 ret = stlink_reset(sl, RESET_HARD);
1262 if (ret) {
1263 DLOG("Rcmd: jtag_reset failed with jtag_reset\n");
1264 reply = strdup("E00");
1265 }
1266
1267 ret = stlink_force_debug(sl);
1268 if (ret) {
1269 DLOG("Rcmd: jtag_reset failed with force_debug\n");
1270 reply = strdup("E00");
1271 }
1272
1273 if (strcmp(reply, "E00")) {
1274 // no errors have been found
1275 DLOG("Rcmd: jtag_reset\n");
1276 }
1277 } else if (!strncmp(cmd, "reset", 5)) { // reset
1278
1279 ret = stlink_force_debug(sl);
1280 if (ret) {
1281 DLOG("Rcmd: reset failed with force_debug\n");
1282 reply = strdup("E00");
1283 }
1284
1285 ret = stlink_reset(sl, RESET_AUTO);
1286 if (ret) {
1287 DLOG("Rcmd: reset failed with reset\n");
1288 reply = strdup("E00");
1289 }
1290
1291 init_code_breakpoints(sl);
1292 init_data_watchpoints(sl);
1293
1294 if (reply == NULL) {
1295 reply = strdup("OK");
1296 DLOG("Rcmd: reset\n");
1297 }
1298
1299 } else if (!strncmp(cmd, "semihosting ", 12)) {
1300 DLOG("Rcmd: got semihosting cmd '%s'", cmd);
1301 char *arg = cmd + 12;
1302
1303 while (isspace(*arg)) { arg++; } // skip whitespaces
1304
1305 if (!strncmp(arg, "enable", 6) || !strncmp(arg, "1", 1)) {
1306 st->semihosting = true;
1307 reply = strdup("OK");
1308 } else if (!strncmp(arg, "disable", 7) || !strncmp(arg, "0", 1)) {
1309 st->semihosting = false;
1310 reply = strdup("OK");
1311 } else {
1312 DLOG("Rcmd: unknown semihosting arg: '%s'\n", arg);
1313 }
1314 } else {
1315 DLOG("Rcmd: %s\n", cmd);
1316 }
1317
1318 free(cmd);
1319 }
1320
1321 if (reply == NULL) { reply = strdup(""); }
1322
1323 free(queryName);
1324 break;
1325 }
1326
1327 case 'v': {
1328 char *params = NULL;
1329 char *cmdName = strtok_r(packet, ":;", ¶ms);
1330
1331 cmdName++; // vCommand -> Command
1332
1333 if (!strcmp(cmdName, "FlashErase")) {
1334 char *__s_addr, *s_length;
1335 char *tok = params;
1336
1337 __s_addr = strsep(&tok, ",");
1338 s_length = tok;
1339
1340 unsigned addr = (unsigned int)strtoul(__s_addr, NULL, 16),
1341 length = (unsigned int)strtoul(s_length, NULL, 16);
1342
1343 DLOG("FlashErase: addr:%08x,len:%04x\n",
1344 addr, length);
1345
1346 if (flash_add_block(addr, length, sl) < 0) {
1347 reply = strdup("E00");
1348 } else {
1349 reply = strdup("OK");
1350 }
1351 } else if (!strcmp(cmdName, "FlashWrite")) {
1352 char *__s_addr, *data;
1353 char *tok = params;
1354
1355 __s_addr = strsep(&tok, ":");
1356 data = tok;
1357
1358 unsigned addr = (unsigned int)strtoul(__s_addr, NULL, 16);
1359 unsigned data_length = status - (unsigned int)(data - packet);
1360
1361 // Length of decoded data cannot be more than encoded, as escapes are removed.
1362 // Additional byte is reserved for alignment fix.
1363 uint8_t *decoded = calloc(data_length + 1, 1);
1364 unsigned dec_index = 0;
1365
1366 for (unsigned int i = 0; i < data_length; i++) {
1367 if (data[i] == 0x7d) {
1368 i++;
1369 decoded[dec_index++] = data[i] ^ 0x20;
1370 } else {
1371 decoded[dec_index++] = data[i];
1372 }
1373 }
1374
1375 // fix alignment
1376 if (dec_index % 2 != 0) { dec_index++; }
1377
1378 DLOG("binary packet %d -> %d\n", data_length, dec_index);
1379
1380 if (flash_populate(addr, decoded, dec_index) < 0) {
1381 reply = strdup("E00");
1382 } else {
1383 reply = strdup("OK");
1384 }
1385
1386 free(decoded);
1387 } else if (!strcmp(cmdName, "FlashDone")) {
1388 if (flash_go(sl, st)) {
1389 reply = strdup("E08");
1390 } else {
1391 reply = strdup("OK");
1392 }
1393 } else if (!strcmp(cmdName, "Kill")) {
1394 attached = 0;
1395 reply = strdup("OK");
1396 }
1397
1398 if (reply == NULL) { reply = strdup(""); }
1399
1400 break;
1401 }
1402
1403 case 'c':
1404 cache_sync(sl);
1405 ret = stlink_run(sl, RUN_NORMAL);
1406
1407 if (ret) { DLOG("Semihost: run failed\n"); }
1408
1409 while (1) {
1410 status = gdb_check_for_interrupt(client);
1411
1412 if (status < 0) {
1413 ELOG("cannot check for int: %d\n", status);
1414 close_socket(client);
1415 return(1);
1416 }
1417
1418 if (status == 1) {
1419 stlink_force_debug(sl);
1420 break;
1421 }
1422
1423 ret = stlink_status(sl);
1424
1425 if (ret) { DLOG("Semihost: status failed\n"); }
1426
1427 if (sl->core_stat == TARGET_HALTED) {
1428 struct stlink_reg reg;
1429 stm32_addr_t pc;
1430 stm32_addr_t addr;
1431 int offset = 0;
1432 uint16_t insn;
1433
1434 if (!st->semihosting) { break; }
1435
1436 ret = stlink_read_all_regs (sl, ®);
1437
1438 if (ret) { DLOG("Semihost: read_all_regs failed\n"); }
1439
1440 // read PC
1441 pc = reg.r[15];
1442
1443 // compute aligned value
1444 offset = pc % 4;
1445 addr = pc - offset;
1446
1447 // read instructions (address and length must be aligned).
1448 ret = stlink_read_mem32(sl, addr, (offset > 2 ? 8 : 4));
1449
1450 if (ret != 0) {
1451 DLOG("Semihost: cannot read instructions at: 0x%08x\n", addr);
1452 break;
1453 }
1454
1455 memcpy(&insn, &sl->q_buf[offset], sizeof(insn));
1456
1457 if (insn == 0xBEAB && !has_breakpoint(addr)) {
1458
1459 ret = do_semihosting (sl, reg.r[0], reg.r[1], ®.r[0]);
1460
1461 if (ret) { DLOG("Semihost: do_semihosting failed\n"); }
1462
1463 // write return value
1464 ret = stlink_write_reg(sl, reg.r[0], 0);
1465
1466 if (ret) { DLOG("Semihost: write_reg failed for return value\n"); }
1467
1468 // jump over the break instruction
1469 ret = stlink_write_reg(sl, reg.r[15] + 2, 15);
1470
1471 if (ret) { DLOG("Semihost: write_reg failed for jumping over break\n"); }
1472
1473 // continue execution
1474 cache_sync(sl);
1475 ret = stlink_run(sl, RUN_NORMAL);
1476
1477 if (ret) { DLOG("Semihost: continue execution failed with stlink_run\n"); }
1478 } else {
1479 break;
1480 }
1481 }
1482
1483 usleep(100000);
1484 }
1485
1486 reply = strdup("S05"); // TRAP
1487 break;
1488
1489 case 's':
1490 cache_sync(sl);
1491 ret = stlink_step(sl);
1492
1493 if (ret) {
1494 // ... having a problem sending step packet
1495 ELOG("Step: cannot send step request\n");
1496 reply = strdup("E00");
1497 critical_error = 1; // absolutely critical
1498 } else {
1499 reply = strdup("S05"); // TRAP
1500 }
1501
1502 break;
1503
1504 case '?':
1505
1506 if (attached) {
1507 reply = strdup("S05"); // TRAP
1508 } else {
1509 reply = strdup("OK"); // stub shall reply OK if not attached
1510 }
1511
1512 break;
1513
1514 case 'g':
1515 ret = stlink_read_all_regs(sl, ®p);
1516
1517 if (ret) { DLOG("g packet: read_all_regs failed\n"); }
1518
1519 reply = calloc(8 * 16 + 1, 1);
1520
1521 for (int i = 0; i < 16; i++) {
1522 sprintf(&reply[i * 8], "%08x", (uint32_t)htonl(regp.r[i]));
1523 }
1524
1525 break;
1526
1527 case 'p': {
1528 unsigned id = (unsigned int)strtoul(&packet[1], NULL, 16);
1529 unsigned myreg = 0xDEADDEAD;
1530
1531 if (id < 16) {
1532 ret = stlink_read_reg(sl, id, ®p);
1533 myreg = htonl(regp.r[id]);
1534 } else if (id == 0x19) {
1535 ret = stlink_read_reg(sl, 16, ®p);
1536 myreg = htonl(regp.xpsr);
1537 } else if (id == 0x1A) {
1538 ret = stlink_read_reg(sl, 17, ®p);
1539 myreg = htonl(regp.main_sp);
1540 } else if (id == 0x1B) {
1541 ret = stlink_read_reg(sl, 18, ®p);
1542 myreg = htonl(regp.process_sp);
1543 } else if (id == 0x1C) {
1544 ret = stlink_read_unsupported_reg(sl, id, ®p);
1545 myreg = htonl(regp.control);
1546 } else if (id == 0x1D) {
1547 ret = stlink_read_unsupported_reg(sl, id, ®p);
1548 myreg = htonl(regp.faultmask);
1549 } else if (id == 0x1E) {
1550 ret = stlink_read_unsupported_reg(sl, id, ®p);
1551 myreg = htonl(regp.basepri);
1552 } else if (id == 0x1F) {
1553 ret = stlink_read_unsupported_reg(sl, id, ®p);
1554 myreg = htonl(regp.primask);
1555 } else if (id >= 0x20 && id < 0x40) {
1556 ret = stlink_read_unsupported_reg(sl, id, ®p);
1557 myreg = htonl(regp.s[id - 0x20]);
1558 } else if (id == 0x40) {
1559 ret = stlink_read_unsupported_reg(sl, id, ®p);
1560 myreg = htonl(regp.fpscr);
1561 } else {
1562 ret = 1;
1563 reply = strdup("E00");
1564 }
1565
1566 if (ret) { DLOG("p packet: could not read register with id %u\n", id); }
1567
1568 if (reply == NULL) {
1569 // if reply is set to "E00", skip
1570 reply = calloc(8 + 1, 1);
1571 sprintf(reply, "%08x", myreg);
1572 }
1573
1574 break;
1575 }
1576
1577 case 'P': {
1578 char* s_reg = &packet[1];
1579 char* s_value = strstr(&packet[1], "=") + 1;
1580
1581 unsigned reg = (unsigned int)strtoul(s_reg, NULL, 16);
1582 unsigned value = (unsigned int)strtoul(s_value, NULL, 16);
1583
1584
1585 if (reg < 16) {
1586 ret = stlink_write_reg(sl, ntohl(value), reg);
1587 } else if (reg == 0x19) {
1588 ret = stlink_write_reg(sl, ntohl(value), 16);
1589 } else if (reg == 0x1A) {
1590 ret = stlink_write_reg(sl, ntohl(value), 17);
1591 } else if (reg == 0x1B) {
1592 ret = stlink_write_reg(sl, ntohl(value), 18);
1593 } else if (reg == 0x1C) {
1594 ret = stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
1595 } else if (reg == 0x1D) {
1596 ret = stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
1597 } else if (reg == 0x1E) {
1598 ret = stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
1599 } else if (reg == 0x1F) {
1600 ret = stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
1601 } else if (reg >= 0x20 && reg < 0x40) {
1602 ret = stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
1603 } else if (reg == 0x40) {
1604 ret = stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
1605 } else {
1606 ret = 1;
1607 reply = strdup("E00");
1608 }
1609
1610 if (ret) { DLOG("P packet: stlink_write_unsupported_reg failed with reg %u\n", reg); }
1611
1612 if (reply == NULL) { reply = strdup("OK"); /* Note: NULL may not be zero */ }
1613
1614 break;
1615 }
1616
1617 case 'G':
1618
1619 for (int i = 0; i < 16; i++) {
1620 char str[9] = {0};
1621 strncpy(str, &packet[1 + i * 8], 8);
1622 uint32_t reg = (uint32_t)strtoul(str, NULL, 16);
1623 ret = stlink_write_reg(sl, ntohl(reg), i);
1624
1625 if (ret) { DLOG("G packet: stlink_write_reg failed"); }
1626 }
1627
1628 reply = strdup("OK");
1629 break;
1630
1631 case 'm': {
1632 char* s_start = &packet[1];
1633 char* s_count = strstr(&packet[1], ",") + 1;
1634
1635 stm32_addr_t start = (stm32_addr_t)strtoul(s_start, NULL, 16);
1636 unsigned count = (unsigned int)strtoul(s_count, NULL, 16);
1637
1638 unsigned adj_start = start % 4;
1639 unsigned count_rnd = (count + adj_start + 4 - 1) / 4 * 4;
1640
1641 if (count_rnd > sl->flash_pgsz) { count_rnd = (unsigned int)sl->flash_pgsz; }
1642
1643 if (count_rnd > 0x1800) { count_rnd = 0x1800; }
1644
1645 if (count_rnd < count) { count = count_rnd; }
1646
1647 if (stlink_read_mem32(sl, start - adj_start, count_rnd) != 0) { count = 0; }
1648
1649 // read failed somehow, don't return stale buffer
1650
1651 reply = calloc(count * 2 + 1, 1);
1652
1653 for (unsigned int i = 0; i < count; i++) {
1654 reply[i * 2 + 0] = hex[sl->q_buf[i + adj_start] >> 4];
1655 reply[i * 2 + 1] = hex[sl->q_buf[i + adj_start] & 0xf];
1656 }
1657
1658 break;
1659 }
1660
1661 case 'M': {
1662 char* s_start = &packet[1];
1663 char* s_count = strstr(&packet[1], ",") + 1;
1664 char* hexdata = strstr(packet, ":") + 1;
1665
1666 stm32_addr_t start = (stm32_addr_t)strtoul(s_start, NULL, 16);
1667 unsigned count = (unsigned int)strtoul(s_count, NULL, 16);
1668 int err = 0;
1669
1670 if (start % 4) {
1671 unsigned align_count = 4 - start % 4;
1672
1673 if (align_count > count) { align_count = count; }
1674
1675 for (unsigned int i = 0; i < align_count; i++) {
1676 char hextmp[3] = { hexdata[i * 2], hexdata[i * 2 + 1], 0 };
1677 uint8_t byte = strtoul(hextmp, NULL, 16);
1678 sl->q_buf[i] = byte;
1679 }
1680
1681 err |= stlink_write_mem8(sl, start, align_count);
1682 cache_change(start, align_count);
1683 start += align_count;
1684 count -= align_count;
1685 hexdata += 2 * align_count;
1686 }
1687
1688 if (count - count % 4) {
1689 unsigned aligned_count = count - count % 4;
1690
1691 for (unsigned int i = 0; i < aligned_count; i++) {
1692 char hextmp[3] = { hexdata[i * 2], hexdata[i * 2 + 1], 0 };
1693 uint8_t byte = strtoul(hextmp, NULL, 16);
1694 sl->q_buf[i] = byte;
1695 }
1696
1697 err |= stlink_write_mem32(sl, start, aligned_count);
1698 cache_change(start, aligned_count);
1699 count -= aligned_count;
1700 start += aligned_count;
1701 hexdata += 2 * aligned_count;
1702 }
1703
1704 if (count) {
1705 for (unsigned int i = 0; i < count; i++) {
1706 char hextmp[3] = { hexdata[i * 2], hexdata[i * 2 + 1], 0 };
1707 uint8_t byte = strtoul(hextmp, NULL, 16);
1708 sl->q_buf[i] = byte;
1709 }
1710
1711 err |= stlink_write_mem8(sl, start, count);
1712 cache_change(start, count);
1713 }
1714
1715 reply = strdup(err ? "E00" : "OK");
1716 break;
1717 }
1718
1719 case 'Z': {
1720 char *endptr;
1721 stm32_addr_t addr = (stm32_addr_t)strtoul(&packet[3], &endptr, 16);
1722 stm32_addr_t len = (stm32_addr_t)strtoul(&endptr[1], NULL, 16);
1723
1724 switch (packet[1]) {
1725 case '1':
1726
1727 if (update_code_breakpoint(sl, addr, 1) < 0) {
1728 reply = strdup("E00");
1729 } else {
1730 reply = strdup("OK");
1731 }
1732
1733 break;
1734
1735 case '2': // insert write watchpoint
1736 case '3': // insert read watchpoint
1737 case '4': { // insert access watchpoint
1738 enum watchfun wf;
1739
1740 if (packet[1] == '2') {
1741 wf = WATCHWRITE;
1742 } else if (packet[1] == '3') {
1743 wf = WATCHREAD;
1744 } else {
1745 wf = WATCHACCESS;
1746 }
1747
1748 if (add_data_watchpoint(sl, wf, addr, len) < 0) {
1749 reply = strdup("E00");
1750 } else {
1751 reply = strdup("OK");
1752 break;
1753 }
1754 }
1755 break;
1756
1757 default:
1758 reply = strdup("");
1759 }
1760 break;
1761 }
1762 case 'z': {
1763 char *endptr;
1764 stm32_addr_t addr = (stm32_addr_t)strtoul(&packet[3], &endptr, 16);
1765 // stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
1766
1767 switch (packet[1]) {
1768 case '1': // remove breakpoint
1769 update_code_breakpoint(sl, addr, 0);
1770 reply = strdup("OK");
1771 break;
1772
1773 case '2': // remove write watchpoint
1774 case '3': // remove read watchpoint
1775 case '4': // remove access watchpoint
1776
1777 if (delete_data_watchpoint(sl, addr) < 0) {
1778 reply = strdup("E00");
1779 break;
1780 } else {
1781 reply = strdup("OK");
1782 break;
1783 }
1784
1785 default:
1786 reply = strdup("");
1787 }
1788 break;
1789 }
1790
1791 case '!': {
1792 // enter extended mode which allows restarting. We do support that always.
1793 // also, set to persistent mode to allow GDB disconnect.
1794 st->persistent = 1;
1795
1796 reply = strdup("OK");
1797 break;
1798 }
1799
1800 case 'R': {
1801 // reset the core.
1802 ret = stlink_reset(sl, RESET_AUTO);
1803 if (ret) { DLOG("R packet : stlink_reset failed\n"); }
1804
1805 init_code_breakpoints(sl);
1806 init_data_watchpoints(sl);
1807
1808 attached = 1;
1809
1810 reply = strdup("OK");
1811 break;
1812 }
1813 case 'k':
1814 // kill request - reset the connection itself
1815 ret = stlink_run(sl, RUN_NORMAL);
1816 if (ret) { DLOG("Kill: stlink_run failed\n"); }
1817
1818 ret = stlink_exit_debug_mode(sl);
1819 if (ret) { DLOG("Kill: stlink_exit_debug_mode failed\n"); }
1820
1821 stlink_close(sl);
1822
1823 sl = stlink_open_usb(st->logging_level, st->connect_mode, st->serialnumber, st->freq);
1824 if (sl == NULL || sl->chip_id == STLINK_CHIPID_UNKNOWN) { cleanup(0); }
1825
1826 connected_stlink = sl;
1827
1828 ret = stlink_force_debug(sl);
1829 if (ret) { DLOG("Kill: stlink_force_debug failed\n"); }
1830
1831 init_cache(sl);
1832 init_code_breakpoints(sl);
1833 init_data_watchpoints(sl);
1834
1835 reply = NULL; // no response
1836 break;
1837
1838 default:
1839 reply = strdup("");
1840 }
1841
1842 if (reply) {
1843 DLOG("send: %s\n", reply);
1844
1845 int result = gdb_send_packet(client, reply);
1846
1847 if (result != 0) {
1848 ELOG("cannot send: %d\n", result);
1849 free(reply);
1850 free(packet);
1851 close_socket(client);
1852 return(1);
1853 }
1854
1855 free(reply);
1856 }
1857
1858 if (critical_error) {
1859 close_socket(client);
1860 return(1);
1861 }
1862
1863 free(packet);
1864 }
1865
1866 close_socket(client);
1867 return(0);
1868 }
1869