- int status = gdb_recv_packet(client, &packet);
- if(status < 0) {
- fprintf(stderr, "cannot recv: %d\n", status);
- return 1;
- }
-
- #ifdef DEBUG
- printf("recv: %s\n", packet);
- #endif
-
- char* reply = NULL;
- reg regp;
-
- switch(packet[0]) {
- case 'q': {
- if(packet[1] == 'P' || packet[1] == 'C' || packet[1] == 'L') {
- reply = strdup("");
- break;
- }
-
- char *separator = strstr(packet, ":"), *params = "";
- if(separator == NULL) {
- separator = packet + strlen(packet);
- } else {
- params = separator + 1;
- }
-
- unsigned queryNameLength = (separator - &packet[1]);
- char* queryName = calloc(queryNameLength + 1, 1);
- strncpy(queryName, &packet[1], queryNameLength);
-
- #ifdef DEBUG
- printf("query: %s;%s\n", queryName, params);
- #endif
-
- if(!strcmp(queryName, "Supported")) {
- reply = strdup("PacketSize=3fff;qXfer:memory-map:read+");
- } else if(!strcmp(queryName, "Xfer")) {
- char *type, *op, *s_addr, *s_length;
- char *tok = params;
- char *annex __attribute__((unused));
-
- type = strsep(&tok, ":");
- op = strsep(&tok, ":");
- annex = strsep(&tok, ":");
- s_addr = strsep(&tok, ",");
- s_length = tok;
-
- unsigned addr = strtoul(s_addr, NULL, 16),
- length = strtoul(s_length, NULL, 16);
-
- #ifdef DEBUG
- printf("Xfer: type:%s;op:%s;annex:%s;addr:%d;length:%d\n",
- type, op, annex, addr, length);
- #endif
-
- const char* data = NULL;
-
- if(!strcmp(type, "memory-map") && !strcmp(op, "read"))
- data = current_memory_map;
-
- if(data) {
- unsigned data_length = strlen(data);
- if(addr + length > data_length)
- length = data_length - addr;
-
- if(length == 0) {
- reply = strdup("l");
- } else {
- reply = calloc(length + 2, 1);
- reply[0] = 'm';
- strncpy(&reply[1], data, length);
- }
- }
- }
-
- if(reply == NULL)
- reply = strdup("");
-
- free(queryName);
-
- break;
- }
-
- case 'v': {
- char *params = NULL;
- char *cmdName = strtok_r(packet, ":;", ¶ms);
-
- cmdName++; // vCommand -> Command
-
- if(!strcmp(cmdName, "FlashErase")) {
- char *s_addr, *s_length;
- char *tok = params;
-
- s_addr = strsep(&tok, ",");
- s_length = tok;
-
- unsigned addr = strtoul(s_addr, NULL, 16),
- length = strtoul(s_length, NULL, 16);
-
- #ifdef DEBUG
- printf("FlashErase: addr:%08x,len:%04x\n",
- addr, length);
- #endif
-
- if(flash_add_block(addr, length, sl) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- } else if(!strcmp(cmdName, "FlashWrite")) {
- char *s_addr, *data;
- char *tok = params;
-
- s_addr = strsep(&tok, ":");
- data = tok;
-
- unsigned addr = strtoul(s_addr, NULL, 16);
- unsigned data_length = status - (data - packet);
-
- // Length of decoded data cannot be more than
- // encoded, as escapes are removed.
- // Additional byte is reserved for alignment fix.
- uint8_t *decoded = calloc(data_length + 1, 1);
- unsigned dec_index = 0;
- for(int i = 0; i < data_length; i++) {
- if(data[i] == 0x7d) {
- i++;
- decoded[dec_index++] = data[i] ^ 0x20;
- } else {
- decoded[dec_index++] = data[i];
- }
- }
-
- // Fix alignment
- if(dec_index % 2 != 0)
- dec_index++;
-
- #ifdef DEBUG
- printf("binary packet %d -> %d\n", data_length, dec_index);
- #endif
-
- if(flash_populate(addr, decoded, dec_index) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- } else if(!strcmp(cmdName, "FlashDone")) {
- if(flash_go(sl) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- } else if(!strcmp(cmdName, "Kill")) {
- attached = 0;
-
- reply = strdup("OK");
- }
-
- if(reply == NULL)
- reply = strdup("");
-
- break;
- }
-
- case 'c':
- stlink_run(sl);
-
- while(1) {
- int status = gdb_check_for_interrupt(client);
- if(status < 0) {
- fprintf(stderr, "cannot check for int: %d\n", status);
- return 1;
- }
-
- if(status == 1) {
- stlink_force_debug(sl);
- break;
- }
-
- stlink_status(sl);
- if(sl->core_stat == STLINK_CORE_HALTED) {
- break;
- }
-
- usleep(100000);
- }
-
- reply = strdup("S05"); // TRAP
- break;
-
- case 's':
- stlink_step(sl);
-
- reply = strdup("S05"); // TRAP
- break;
-
- case '?':
- if(attached) {
- reply = strdup("S05"); // TRAP
- } else {
- /* Stub shall reply OK if not attached. */
- reply = strdup("OK");
- }
- break;
-
- case 'g':
- stlink_read_all_regs(sl, ®p);
-
- reply = calloc(8 * 16 + 1, 1);
- for(int i = 0; i < 16; i++)
- sprintf(&reply[i * 8], "%08x", htonl(regp.r[i]));
-
- break;
-
- case 'p': {
- unsigned id = strtoul(&packet[1], NULL, 16);
- unsigned myreg = 0xDEADDEAD;
-
- if(id < 16) {
- stlink_read_reg(sl, id, ®p);
- myreg = htonl(regp.r[id]);
- } else if(id == 0x19) {
- stlink_read_reg(sl, 16, ®p);
- myreg = htonl(regp.xpsr);
- } else {
- reply = strdup("E00");
- }
-
- reply = calloc(8 + 1, 1);
- sprintf(reply, "%08x", myreg);
-
- break;
- }
-
- case 'P': {
- char* s_reg = &packet[1];
- char* s_value = strstr(&packet[1], "=") + 1;
-
- unsigned reg = strtoul(s_reg, NULL, 16);
- unsigned value = strtoul(s_value, NULL, 16);
-
- if(reg < 16) {
- stlink_write_reg(sl, ntohl(value), reg);
- } else if(reg == 0x19) {
- stlink_write_reg(sl, ntohl(value), 16);
- } else {
- reply = strdup("E00");
- }
-
- if(!reply) {
- reply = strdup("OK");
- }
-
- break;
- }
-
- case 'G':
- for(int i = 0; i < 16; i++) {
- char str[9] = {0};
- strncpy(str, &packet[1 + i * 8], 8);
- uint32_t reg = strtoul(str, NULL, 16);
- stlink_write_reg(sl, ntohl(reg), i);
- }
-
- reply = strdup("OK");
- break;
-
- case 'm': {
- char* s_start = &packet[1];
- char* s_count = strstr(&packet[1], ",") + 1;
-
- stm32_addr_t start = strtoul(s_start, NULL, 16);
- unsigned count = strtoul(s_count, NULL, 16);
-
- unsigned adj_start = start % 4;
-
- stlink_read_mem32(sl, start - adj_start, (count % 4 == 0) ?
- count : count + 4 - (count % 4));
-
- reply = calloc(count * 2 + 1, 1);
- for(int i = 0; i < count; i++) {
- reply[i * 2 + 0] = hex[sl->q_buf[i + adj_start] >> 4];
- reply[i * 2 + 1] = hex[sl->q_buf[i + adj_start] & 0xf];
- }
-
- break;
- }
-
- case 'M': {
- char* s_start = &packet[1];
- char* s_count = strstr(&packet[1], ",") + 1;
- char* hexdata = strstr(packet, ":") + 1;
-
- stm32_addr_t start = strtoul(s_start, NULL, 16);
- unsigned count = strtoul(s_count, NULL, 16);
-
- for(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;
- }
-
- if((count % 4) == 0 && (start % 4) == 0) {
- stlink_write_mem32(sl, start, count);
- } else {
- stlink_write_mem8(sl, start, count);
- }
-
- reply = strdup("OK");
-
- break;
- }
-
- case 'Z': {
- char *endptr;
- stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
- stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
-
- switch (packet[1]) {
- case '1':
- if(update_code_breakpoint(sl, addr, 1) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- }
- break;
-
- case '2': // insert write watchpoint
- case '3': // insert read watchpoint
- case '4': // insert access watchpoint
- {
- enum watchfun wf;
- if(packet[1] == '2') {
- wf = WATCHWRITE;
- } else if(packet[1] == '3') {
- wf = WATCHREAD;
- } else {
- wf = WATCHACCESS;
- if(add_data_watchpoint(sl, wf, addr, len) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- break;
- }
- }
- }
-
- default:
- reply = strdup("");
- }
- break;
- }
- case 'z': {
- char *endptr;
- stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
- //stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
-
- switch (packet[1]) {
- case '1': // remove breakpoint
- update_code_breakpoint(sl, addr, 0);
- reply = strdup("OK");
- break;
-
- case '2' : // remove write watchpoint
- case '3' : // remove read watchpoint
- case '4' : // remove access watchpoint
- if(delete_data_watchpoint(sl, addr) < 0) {
- reply = strdup("E00");
- } else {
- reply = strdup("OK");
- break;
- }
-
- default:
- reply = strdup("");
- }
- break;
- }
-
- case '!': {
- /*
- * Enter extended mode which allows restarting.
- * We do support that always.
- */
-
- reply = strdup("OK");
-
- break;
- }
-
- case 'R': {
- /* Reset the core. */
-
- stlink_reset(sl);
- init_code_breakpoints(sl);
- init_data_watchpoints(sl);
-
- attached = 1;
-
- reply = strdup("OK");
-
- break;
- }
-
- default:
- reply = strdup("");
- }
-
- if(reply) {
- #ifdef DEBUG
- printf("send: %s\n", reply);
- #endif
-
- int result = gdb_send_packet(client, reply);
- if(result != 0) {
- fprintf(stderr, "cannot send: %d\n", result);
- return 1;
- }
-
- free(reply);
- }
-
- free(packet);
- }
+int serve(stlink_t *sl, st_state_t *st) {
+ int sock = socket(AF_INET, SOCK_STREAM, 0);
+ if(sock < 0) {
+ perror("socket");
+ return 1;
+ }
+
+ unsigned int val = 1;
+ setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, (char *)&val, sizeof(val));
+
+ struct sockaddr_in serv_addr;
+ memset(&serv_addr,0,sizeof(struct sockaddr_in));
+ serv_addr.sin_family = AF_INET;
+ serv_addr.sin_addr.s_addr = INADDR_ANY;
+ serv_addr.sin_port = htons(st->listen_port);
+
+ if(bind(sock, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
+ perror("bind");
+ return 1;
+ }
+
+ if(listen(sock, 5) < 0) {
+ perror("listen");
+ return 1;
+ }
+
+ ILOG("Listening at *:%d...\n", st->listen_port);
+
+ int client = accept(sock, NULL, NULL);
+ //signal (SIGINT, SIG_DFL);
+ if(client < 0) {
+ perror("accept");
+ return 1;
+ }
+
+ close(sock);
+
+ stlink_force_debug(sl);
+ if (st->reset) {
+ stlink_reset(sl);
+ }
+ init_code_breakpoints(sl);
+ init_data_watchpoints(sl);
+
+ ILOG("GDB connected.\n");
+
+ /*
+ * To allow resetting the chip from GDB it is required to
+ * emulate attaching and detaching to target.
+ */
+ unsigned int attached = 1;
+
+ while(1) {
+ char* packet;
+
+ int status = gdb_recv_packet(client, &packet);
+ if(status < 0) {
+ ELOG("cannot recv: %d\n", status);
+#ifdef __MINGW32__
+ win32_close_socket(sock);
+#endif
+ return 1;
+ }
+
+ DLOG("recv: %s\n", packet);
+
+ char* reply = NULL;
+ reg regp;
+
+ switch(packet[0]) {
+ case 'q': {
+ if(packet[1] == 'P' || packet[1] == 'C' || packet[1] == 'L') {
+ reply = strdup("");
+ break;
+ }
+
+ char *separator = strstr(packet, ":"), *params = "";
+ if(separator == NULL) {
+ separator = packet + strlen(packet);
+ } else {
+ params = separator + 1;
+ }
+
+ unsigned queryNameLength = (separator - &packet[1]);
+ char* queryName = calloc(queryNameLength + 1, 1);
+ strncpy(queryName, &packet[1], queryNameLength);
+
+ DLOG("query: %s;%s\n", queryName, params);
+
+ if(!strcmp(queryName, "Supported")) {
+ if(sl->chip_id==STM32_CHIPID_F4
+ || sl->chip_id==STM32_CHIPID_F4_HD
+ || sl->chip_id==STM32_CHIPID_F7) {
+ reply = strdup("PacketSize=3fff;qXfer:memory-map:read+;qXfer:features:read+");
+ }
+ else {
+ reply = strdup("PacketSize=3fff;qXfer:memory-map:read+");
+ }
+ } else if(!strcmp(queryName, "Xfer")) {
+ char *type, *op, *__s_addr, *s_length;
+ char *tok = params;
+ char *annex __attribute__((unused));
+
+ type = strsep(&tok, ":");
+ op = strsep(&tok, ":");
+ annex = strsep(&tok, ":");
+ __s_addr = strsep(&tok, ",");
+ s_length = tok;
+
+ unsigned addr = strtoul(__s_addr, NULL, 16),
+ length = strtoul(s_length, NULL, 16);
+
+ DLOG("Xfer: type:%s;op:%s;annex:%s;addr:%d;length:%d\n",
+ type, op, annex, addr, length);
+
+ const char* data = NULL;
+
+ if(!strcmp(type, "memory-map") && !strcmp(op, "read"))
+ data = current_memory_map;
+
+ if(!strcmp(type, "features") && !strcmp(op, "read"))
+ data = target_description_F4;
+
+ if(data) {
+ unsigned data_length = strlen(data);
+ if(addr + length > data_length)
+ length = data_length - addr;
+
+ if(length == 0) {
+ reply = strdup("l");
+ } else {
+ reply = calloc(length + 2, 1);
+ reply[0] = 'm';
+ strncpy(&reply[1], data, length);
+ }
+ }
+ } else if(!strncmp(queryName, "Rcmd,",4)) {
+ // Rcmd uses the wrong separator
+ char *separator = strstr(packet, ","), *params = "";
+ if(separator == NULL) {
+ separator = packet + strlen(packet);
+ } else {
+ params = separator + 1;
+ }
+
+
+ if (!strncmp(params,"726573756d65",12)) {// resume
+ DLOG("Rcmd: resume\n");
+ cache_sync(sl);
+ stlink_run(sl);
+
+ reply = strdup("OK");
+ } else if (!strncmp(params,"68616c74",8)) { //halt
+ reply = strdup("OK");
+
+ stlink_force_debug(sl);
+
+ DLOG("Rcmd: halt\n");
+ } else if (!strncmp(params,"6a7461675f7265736574",20)) { //jtag_reset
+ reply = strdup("OK");
+
+ stlink_jtag_reset(sl, 0);
+ stlink_jtag_reset(sl, 1);
+ stlink_force_debug(sl);
+
+ DLOG("Rcmd: jtag_reset\n");
+ } else if (!strncmp(params,"7265736574",10)) { //reset
+ reply = strdup("OK");
+
+ stlink_force_debug(sl);
+ stlink_reset(sl);
+ init_code_breakpoints(sl);
+ init_data_watchpoints(sl);
+
+ DLOG("Rcmd: reset\n");
+ } else {
+ DLOG("Rcmd: %s\n", params);
+ }
+
+ }
+
+ if(reply == NULL)
+ reply = strdup("");
+
+ free(queryName);
+
+ break;
+ }
+
+ case 'v': {
+ char *params = NULL;
+ char *cmdName = strtok_r(packet, ":;", ¶ms);
+
+ cmdName++; // vCommand -> Command
+
+ if(!strcmp(cmdName, "FlashErase")) {
+ char *__s_addr, *s_length;
+ char *tok = params;
+
+ __s_addr = strsep(&tok, ",");
+ s_length = tok;
+
+ unsigned addr = strtoul(__s_addr, NULL, 16),
+ length = strtoul(s_length, NULL, 16);
+
+ DLOG("FlashErase: addr:%08x,len:%04x\n",
+ addr, length);
+
+ if(flash_add_block(addr, length, sl) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ } else if(!strcmp(cmdName, "FlashWrite")) {
+ char *__s_addr, *data;
+ char *tok = params;
+
+ __s_addr = strsep(&tok, ":");
+ data = tok;
+
+ unsigned addr = strtoul(__s_addr, NULL, 16);
+ unsigned data_length = status - (data - packet);
+
+ // Length of decoded data cannot be more than
+ // encoded, as escapes are removed.
+ // Additional byte is reserved for alignment fix.
+ uint8_t *decoded = calloc(data_length + 1, 1);
+ unsigned dec_index = 0;
+ for(unsigned int i = 0; i < data_length; i++) {
+ if(data[i] == 0x7d) {
+ i++;
+ decoded[dec_index++] = data[i] ^ 0x20;
+ } else {
+ decoded[dec_index++] = data[i];
+ }
+ }
+
+ // Fix alignment
+ if(dec_index % 2 != 0)
+ dec_index++;
+
+ DLOG("binary packet %d -> %d\n", data_length, dec_index);
+
+ if(flash_populate(addr, decoded, dec_index) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ } else if(!strcmp(cmdName, "FlashDone")) {
+ if(flash_go(sl) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ } else if(!strcmp(cmdName, "Kill")) {
+ attached = 0;
+
+ reply = strdup("OK");
+ }
+
+ if(reply == NULL)
+ reply = strdup("");
+
+ break;
+ }
+
+ case 'c':
+ cache_sync(sl);
+ stlink_run(sl);
+
+ while(1) {
+ int status = gdb_check_for_interrupt(client);
+ if(status < 0) {
+ ELOG("cannot check for int: %d\n", status);
+#ifdef __MINGW32__
+ win32_close_socket(sock);
+#endif
+ return 1;
+ }
+
+ if(status == 1) {
+ stlink_force_debug(sl);
+ break;
+ }
+
+ stlink_status(sl);
+ if(sl->core_stat == STLINK_CORE_HALTED) {
+ break;
+ }
+
+ usleep(100000);
+ }
+
+ reply = strdup("S05"); // TRAP
+ break;
+
+ case 's':
+ cache_sync(sl);
+ stlink_step(sl);
+
+ reply = strdup("S05"); // TRAP
+ break;
+
+ case '?':
+ if(attached) {
+ reply = strdup("S05"); // TRAP
+ } else {
+ /* Stub shall reply OK if not attached. */
+ reply = strdup("OK");
+ }
+ break;
+
+ case 'g':
+ stlink_read_all_regs(sl, ®p);
+
+ reply = calloc(8 * 16 + 1, 1);
+ for(int i = 0; i < 16; i++)
+ sprintf(&reply[i * 8], "%08x", htonl(regp.r[i]));
+
+ break;
+
+ case 'p': {
+ unsigned id = strtoul(&packet[1], NULL, 16);
+ unsigned myreg = 0xDEADDEAD;
+
+ if(id < 16) {
+ stlink_read_reg(sl, id, ®p);
+ myreg = htonl(regp.r[id]);
+ } else if(id == 0x19) {
+ stlink_read_reg(sl, 16, ®p);
+ myreg = htonl(regp.xpsr);
+ } else if(id == 0x1A) {
+ stlink_read_reg(sl, 17, ®p);
+ myreg = htonl(regp.main_sp);
+ } else if(id == 0x1B) {
+ stlink_read_reg(sl, 18, ®p);
+ myreg = htonl(regp.process_sp);
+ } else if(id == 0x1C) {
+ stlink_read_unsupported_reg(sl, id, ®p);
+ myreg = htonl(regp.control);
+ } else if(id == 0x1D) {
+ stlink_read_unsupported_reg(sl, id, ®p);
+ myreg = htonl(regp.faultmask);
+ } else if(id == 0x1E) {
+ stlink_read_unsupported_reg(sl, id, ®p);
+ myreg = htonl(regp.basepri);
+ } else if(id == 0x1F) {
+ stlink_read_unsupported_reg(sl, id, ®p);
+ myreg = htonl(regp.primask);
+ } else if(id >= 0x20 && id < 0x40) {
+ stlink_read_unsupported_reg(sl, id, ®p);
+ myreg = htonl(regp.s[id-0x20]);
+ } else if(id == 0x40) {
+ stlink_read_unsupported_reg(sl, id, ®p);
+ myreg = htonl(regp.fpscr);
+ } else {
+ reply = strdup("E00");
+ }
+
+ reply = calloc(8 + 1, 1);
+ sprintf(reply, "%08x", myreg);
+
+ break;
+ }
+
+ case 'P': {
+ char* s_reg = &packet[1];
+ char* s_value = strstr(&packet[1], "=") + 1;
+
+ unsigned reg = strtoul(s_reg, NULL, 16);
+ unsigned value = strtoul(s_value, NULL, 16);
+
+ if(reg < 16) {
+ stlink_write_reg(sl, ntohl(value), reg);
+ } else if(reg == 0x19) {
+ stlink_write_reg(sl, ntohl(value), 16);
+ } else if(reg == 0x1A) {
+ stlink_write_reg(sl, ntohl(value), 17);
+ } else if(reg == 0x1B) {
+ stlink_write_reg(sl, ntohl(value), 18);
+ } else if(reg == 0x1C) {
+ stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
+ } else if(reg == 0x1D) {
+ stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
+ } else if(reg == 0x1E) {
+ stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
+ } else if(reg == 0x1F) {
+ stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
+ } else if(reg >= 0x20 && reg < 0x40) {
+ stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
+ } else if(reg == 0x40) {
+ stlink_write_unsupported_reg(sl, ntohl(value), reg, ®p);
+ } else {
+ reply = strdup("E00");
+ }
+
+ if(!reply) {
+ reply = strdup("OK");
+ }
+
+ break;
+ }
+
+ case 'G':
+ for(int i = 0; i < 16; i++) {
+ char str[9] = {0};
+ strncpy(str, &packet[1 + i * 8], 8);
+ uint32_t reg = strtoul(str, NULL, 16);
+ stlink_write_reg(sl, ntohl(reg), i);
+ }
+
+ reply = strdup("OK");
+ break;
+
+ case 'm': {
+ char* s_start = &packet[1];
+ char* s_count = strstr(&packet[1], ",") + 1;
+
+ stm32_addr_t start = strtoul(s_start, NULL, 16);
+ 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_rnd);
+
+ reply = calloc(count * 2 + 1, 1);
+ for(unsigned int i = 0; i < count; i++) {
+ reply[i * 2 + 0] = hex[sl->q_buf[i + adj_start] >> 4];
+ reply[i * 2 + 1] = hex[sl->q_buf[i + adj_start] & 0xf];
+ }
+
+ break;
+ }
+
+ case 'M': {
+ char* s_start = &packet[1];
+ char* s_count = strstr(&packet[1], ",") + 1;
+ char* hexdata = strstr(packet, ":") + 1;
+
+ stm32_addr_t start = strtoul(s_start, NULL, 16);
+ unsigned count = strtoul(s_count, NULL, 16);
+
+ 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);
+ cache_change(start, align_count);
+ start += align_count;
+ count -= align_count;
+ hexdata += 2*align_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);
+ cache_change(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);
+ cache_change(start, count);
+ }
+ reply = strdup("OK");
+ break;
+ }
+
+ case 'Z': {
+ char *endptr;
+ stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
+ stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
+
+ switch (packet[1]) {
+ case '1':
+ if(update_code_breakpoint(sl, addr, 1) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ }
+ break;
+
+ case '2': // insert write watchpoint
+ case '3': // insert read watchpoint
+ case '4': { // insert access watchpoint
+ enum watchfun wf;
+ if(packet[1] == '2') {
+ wf = WATCHWRITE;
+ } else if(packet[1] == '3') {
+ wf = WATCHREAD;
+ } else {
+ wf = WATCHACCESS;
+ }
+
+ if(add_data_watchpoint(sl, wf, addr, len) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ break;
+ }
+ }
+
+ default:
+ reply = strdup("");
+ }
+ break;
+ }
+ case 'z': {
+ char *endptr;
+ stm32_addr_t addr = strtoul(&packet[3], &endptr, 16);
+ //stm32_addr_t len = strtoul(&endptr[1], NULL, 16);
+
+ switch (packet[1]) {
+ case '1': // remove breakpoint
+ update_code_breakpoint(sl, addr, 0);
+ reply = strdup("OK");
+ break;
+
+ case '2' : // remove write watchpoint
+ case '3' : // remove read watchpoint
+ case '4' : // remove access watchpoint
+ if(delete_data_watchpoint(sl, addr) < 0) {
+ reply = strdup("E00");
+ } else {
+ reply = strdup("OK");
+ break;
+ }
+
+ default:
+ reply = strdup("");
+ }
+ break;
+ }