+ 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);
+ 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) {
+ 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");
+ 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, 1);
+ stlink_jtag_reset(sl, 0);
+ 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':
+ stlink_run(sl);
+
+ while(1) {
+ int status = gdb_check_for_interrupt(client);
+ if(status < 0) {
+ ELOG("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 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);
+ 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);
+ count -= aligned_count;
+ start += aligned_count;
+ hexdata += 2*aligned_count;
+ }
+
+ if(count) {
+ for(unsigned int i = 0; i < count; i ++) {
+ char hex[3] = { hexdata[i*2], hexdata[i*2+1], 0 };
+ uint8_t byte = strtoul(hex, NULL, 16);
+ sl->q_buf[i] = byte;
+ }
+ stlink_write_mem8(sl, start, count);
+ }
+ reply = strdup("OK");
+ break;
+ }
+
+ 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");