+ 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;
+ if (count_rnd > sl->flash_pgsz)
+ count_rnd = sl->flash_pgsz;
+ if (count_rnd > 0x1800)
+ count_rnd = 0x1800;
+ if (count_rnd < count)
+ count = count_rnd;
+
+ 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;
+ }
+
+ case '!': {
+ /*
+ * Enter extended mode which allows restarting.
+ * We do support that always.
+ */
+
+ /*
+ * Also, set to persistent mode
+ * to allow GDB disconnect.
+ */
+ st->persistent = 1;
+
+ 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) {
+ DLOG("send: %s\n", reply);
+
+ int result = gdb_send_packet(client, reply);
+ if(result != 0) {
+ ELOG("cannot send: %d\n", result);
+ free(reply);
+ free(packet);
+#ifdef __MINGW32__
+ win32_close_socket(sock);
+#endif
+ return 1;
+ }
+
+ free(reply);
+ }
+
+ free(packet);
+ }
+
+#ifdef __MINGW32__
+ win32_close_socket(sock);
+#endif
+
+ return 0;