- }
-
- }
-
- 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(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++;
-
- #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 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;
-
- stlink_read_mem32(sl, start - adj_start, (count % 4 == 0) ?
- count : count + 4 - (count % 4));
-
- 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) {