X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fgdb-server.c;h=d44f20f18cb6cab1b7b77543ea6dda357a24781f;hb=08a79ed0fe770d11a06c7b661f9ee542a7d8d85b;hp=ef30a029cc11b34dd0a4d756510f78a6c0ef5969;hpb=fc7b68944dab0d3cd695722eb430260935e51716;p=fw%2Fstlink diff --git a/src/gdb-server.c b/src/gdb-server.c index ef30a02..d44f20f 100644 --- a/src/gdb-server.c +++ b/src/gdb-server.c @@ -19,7 +19,34 @@ static const char hex[] = "0123456789abcdef"; +static const char* current_memory_map = NULL; + +struct chip_params { + uint32_t chip_id; + char* description; + uint32_t max_flash_size, flash_pagesize; + uint32_t sram_size; + uint32_t bootrom_base, bootrom_size; +} const devices[] = { + { 0x412, "Low-density device", + 0x8000, 0x400, 0x2800, 0x1ffff000, 0x800 }, + { 0x410, "Medium-density device", + 0x20000, 0x400, 0x5000, 0x1ffff000, 0x800 }, + { 0x414, "High-density device", + 0x80000, 0x800, 0x10000, 0x1ffff000, 0x800 }, + { 0x418, "Connectivity line device", + 0x40000, 0x800, 0x10000, 0x1fffb000, 0x4800 }, + { 0x420, "Medium-density value line device", + 0x20000, 0x400, 0x2000, 0x1ffff000, 0x800 }, + { 0x428, "High-density value line device", + 0x80000, 0x800, 0x8000, 0x1ffff000, 0x800 }, + { 0x430, "XL-density device", + 0x100000, 0x800, 0x18000, 0x1fffe000, 0x1800 }, + { 0 } +}; + int serve(struct stlink* sl, int port); +char* make_memory_map(const struct chip_params *params, uint32_t flash_size); int main(int argc, char** argv) { if(argc != 3) { @@ -34,8 +61,40 @@ int main(int argc, char** argv) { if(stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE) stlink_enter_swd_mode(sl); - stlink_core_id(sl); - printf("Debugging ARM core %08x.\n", sl->core_id); + uint32_t chip_id; + + stlink_read_mem32(sl, 0xE0042000, 4); + chip_id = sl->q_buf[0] | (sl->q_buf[1] << 8) | (sl->q_buf[2] << 16) | + (sl->q_buf[3] << 24); + + printf("Chip ID is %08x.\n", chip_id); + + const struct chip_params* params = NULL; + + for(int i = 0; i < sizeof(devices) / sizeof(devices[0]); i++) { + if(devices[i].chip_id == (chip_id & 0xFFF)) { + params = &devices[i]; + break; + } + } + + if(params == NULL) { + fprintf(stderr, "Cannot recognize the connected device!\n"); + return 0; + } + + printf("Device connected: %s\n", params->description); + printf("Device parameters: SRAM: 0x%x bytes, Flash: up to 0x%x bytes in pages of 0x%x bytes\n", + params->sram_size, params->max_flash_size, params->flash_pagesize); + + uint32_t flash_size; + + stlink_read_mem32(sl, 0x1FFFF7E0, 4); + flash_size = sl->q_buf[0] | (sl->q_buf[1] << 8); + + printf("Flash size is %d KiB.\n", flash_size); + + current_memory_map = make_memory_map(params, flash_size * 0x400); int port = atoi(argv[1]); @@ -46,6 +105,237 @@ int main(int argc, char** argv) { return 0; } +static const char* const memory_map_template = + "" + "" + "" + " " // code = sram, bootrom or flash; flash is bigger + " " // sram 8k + " " + " 0x%x" + " " + " " // peripheral regs + " " // cortex regs + " " // bootrom + " " // option byte area + ""; + +char* make_memory_map(const struct chip_params *params, uint32_t flash_size) { + /* This will be freed in serve() */ + char* map = malloc(4096); + map[0] = '\0'; + + snprintf(map, 4096, memory_map_template, + flash_size, + params->sram_size, + flash_size, params->flash_pagesize, + params->bootrom_base, params->bootrom_size); + + return map; +} + +#define CODE_BREAK_NUM 6 + +#define CODE_BREAK_LOW 0x01 +#define CODE_BREAK_HIGH 0x02 + +struct code_hw_breakpoint { + stm32_addr_t addr; + int type; +}; + +struct code_hw_breakpoint code_breaks[CODE_BREAK_NUM]; + +static void init_code_breakpoints(struct stlink* sl) { + memset(sl->q_buf, 0, 4); + sl->q_buf[0] = 0x03; // KEY | ENABLE + stlink_write_mem32(sl, 0xe0002000, 4); + + memset(sl->q_buf, 0, 4); + for(int i = 0; i < CODE_BREAK_NUM; i++) { + code_breaks[i].type = 0; + stlink_write_mem32(sl, 0xe0002008 + i * 4, 4); + } +} + +static int update_code_breakpoint(struct stlink* sl, stm32_addr_t addr, int set) { + stm32_addr_t fpb_addr = addr & ~0x3; + int type = addr & 0x2 ? CODE_BREAK_HIGH : CODE_BREAK_LOW; + + if(addr & 1) { + fprintf(stderr, "update_code_breakpoint: unaligned address %08x\n", addr); + return -1; + } + + int id = -1; + for(int i = 0; i < CODE_BREAK_NUM; i++) { + if(fpb_addr == code_breaks[i].addr || + (set && code_breaks[i].type == 0)) { + id = i; + break; + } + } + + if(id == -1) { + if(set) return -1; // Free slot not found + else return 0; // Breakpoint is already removed + } + + struct code_hw_breakpoint* brk = &code_breaks[id]; + + brk->addr = fpb_addr; + + if(set) brk->type |= type; + else brk->type &= ~type; + + memset(sl->q_buf, 0, 4); + + if(brk->type == 0) { + #ifdef DEBUG + printf("clearing hw break %d\n", id); + #endif + + stlink_write_mem32(sl, 0xe0002008 + id * 4, 4); + } else { + sl->q_buf[0] = ( brk->addr & 0xff) | 1; + sl->q_buf[1] = ((brk->addr >> 8) & 0xff); + sl->q_buf[2] = ((brk->addr >> 16) & 0xff); + sl->q_buf[3] = ((brk->addr >> 24) & 0xff) | (brk->type << 6); + + #ifdef DEBUG + printf("setting hw break %d at %08x (%d)\n", + id, brk->addr, brk->type); + printf("reg %02x %02x %02x %02x\n", + sl->q_buf[3], sl->q_buf[2], sl->q_buf[1], sl->q_buf[0]); + #endif + + stlink_write_mem32(sl, 0xe0002008 + id * 4, 4); + } + + return 0; +} + +#define FLASH_BASE 0x08000000 +#define FLASH_PAGE 0x400 +#define FLASH_PAGE_MASK (~((1 << 10) - 1)) +#define FLASH_SIZE (FLASH_PAGE * 128) + +struct flash_block { + stm32_addr_t addr; + unsigned length; + uint8_t* data; + + struct flash_block* next; +}; + +static struct flash_block* flash_root; + +static int flash_add_block(stm32_addr_t addr, unsigned length) { + if(addr < FLASH_BASE || addr + length > FLASH_BASE + FLASH_SIZE) { + fprintf(stderr, "flash_add_block: incorrect bounds\n"); + return -1; + } + + if(addr % FLASH_PAGE != 0 || length % FLASH_PAGE != 0) { + fprintf(stderr, "flash_add_block: unaligned block\n"); + return -1; + } + + struct flash_block* new = malloc(sizeof(struct flash_block)); + new->next = flash_root; + + new->addr = addr; + new->length = length; + new->data = calloc(length, 1); + + flash_root = new; + + return 0; +} + +static int flash_populate(stm32_addr_t addr, uint8_t* data, unsigned length) { + int fit_blocks = 0, fit_length = 0; + + for(struct flash_block* fb = flash_root; fb; fb = fb->next) { + /* Block: ------X------Y-------- + * Data: a-----b + * a--b + * a-----------b + * Block intersects with data, if: + * a < Y && b > x + */ + + unsigned X = fb->addr, Y = fb->addr + fb->length; + unsigned a = addr, b = addr + length; + if(a < Y && b > X) { + // from start of the block + unsigned start = (a > X ? a : X) - X; + unsigned end = (b > Y ? Y : b) - X; + + memcpy(fb->data + start, data, end - start); + + fit_blocks++; + fit_length += end - start; + } + } + + if(fit_blocks == 0) { + fprintf(stderr, "Unfit data block %08x -> %04x\n", addr, length); + return -1; + } + + if(fit_length != length) { + fprintf(stderr, "warning: data block %08x -> %04x truncated to %04x\n", + addr, length, fit_length); + fprintf(stderr, "(this is not an error, just a GDB glitch)\n"); + } + + return 0; +} + +static int flash_go(struct stlink* sl) { + int error = -1; + + // Some kinds of clock settings do not allow writing to flash. + stlink_reset(sl); + + for(struct flash_block* fb = flash_root; fb; fb = fb->next) { + #ifdef DEBUG + printf("flash_do: block %08x -> %04x\n", fb->addr, fb->length); + #endif + + unsigned length = fb->length; + for(stm32_addr_t page = fb->addr; page < fb->addr + fb->length; page += 0x400) { + #ifdef DEBUG + printf("flash_do: page %08x\n", page); + #endif + + stlink_erase_flash_page(sl, page); + + if(stlink_write_flash(sl, page, fb->data + (page - fb->addr), + length > 0x400 ? 0x400 : length) < 0) + goto error; + } + + } + + stlink_reset(sl); + + error = 0; + +error: + for(struct flash_block* fb = flash_root, *next; fb; fb = next) { + next = fb->next; + free(fb->data); + free(fb); + } + + flash_root = NULL; + + return error; +} + int serve(struct stlink* sl, int port) { int sock = socket(AF_INET, SOCK_STREAM, 0); if(sock < 0) { @@ -73,6 +363,7 @@ int serve(struct stlink* sl, int port) { stlink_force_debug(sl); stlink_reset(sl); + init_code_breakpoints(sl); printf("Listening at *:%d...\n", port); @@ -86,6 +377,12 @@ int serve(struct stlink* sl, int port) { printf("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; @@ -102,18 +399,178 @@ int serve(struct stlink* sl, int port) { char* reply = NULL; 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, *annex, *s_addr, *s_length; + char *tok = params; + + 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) < 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); - printf("Core running, waiting for interrupt.\n"); + while(1) { + int status = gdb_check_for_interrupt(client); + if(status < 0) { + fprintf(stderr, "cannot check for int: %d\n", status); + return 1; + } - int status = gdb_wait_for_interrupt(client); - if(status < 0) { - fprintf(stderr, "cannot wait for int: %d\n", status); - return 1; - } + if(status == 1) { + stlink_force_debug(sl); + break; + } - stlink_force_debug(sl); + stlink_status(sl); + if(sl->core_stat == STLINK_CORE_HALTED) { + break; + } + + usleep(100000); + } reply = strdup("S05"); // TRAP break; @@ -125,7 +582,12 @@ int serve(struct stlink* sl, int port) { break; case '?': - reply = strdup("S05"); // TRAP + if(attached) { + reply = strdup("S05"); // TRAP + } else { + /* Stub shall reply OK if not attached. */ + reply = strdup("OK"); + } break; case 'g': @@ -235,12 +697,56 @@ int serve(struct stlink* sl, int port) { break; } - case 'k': { - // After this function will be entered afterwards, the - // chip will be reset anyway. So this is a no-op. + case 'Z': { + if(packet[1] == '1') { + stm32_addr_t addr = strtoul(&packet[3], NULL, 16); + if(update_code_breakpoint(sl, addr, 1) < 0) { + reply = strdup("E00"); + } else { + reply = strdup("OK"); + } + } else { + reply = strdup(""); + } + + break; + } + + case 'z': { + if(packet[1] == '1') { + stm32_addr_t addr = strtoul(&packet[3], NULL, 16); + update_code_breakpoint(sl, addr, 0); + + reply = strdup("OK"); + } else { + 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); - close(client); - return 0; + attached = 1; + + reply = strdup("OK"); + + break; } default: