226 lines
6.6 KiB
C
226 lines
6.6 KiB
C
/*
|
|
* Copyright (c) 2023 Marek Vedral <vedrama5@fel.cvut.cz>
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*/
|
|
|
|
#include <zephyr/kernel.h>
|
|
#include <kernel_internal.h>
|
|
#include <zephyr/arch/arm/gdbstub.h>
|
|
#include <zephyr/debug/gdbstub.h>
|
|
|
|
/* Position of each register in the packet - n-th register in the ctx.registers array needs to be
|
|
* the packet_pos[n]-th byte of the g (read all registers) packet. See struct arm_register_names in
|
|
* GDB file gdb/arm-tdep.c, which defines these positions.
|
|
*/
|
|
static const int packet_pos[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 41};
|
|
|
|
/* Required struct */
|
|
static struct gdb_ctx ctx;
|
|
|
|
/* Return true if BKPT instruction caused the current entry */
|
|
static int is_bkpt(unsigned int exc_cause)
|
|
{
|
|
int ret = 0;
|
|
|
|
if (exc_cause == GDB_EXCEPTION_BREAKPOINT) {
|
|
/* Get the instruction */
|
|
unsigned int instr = sys_read32(ctx.registers[PC]);
|
|
/* Try to check the instruction encoding */
|
|
int ist = ((ctx.registers[SPSR] & BIT(SPSR_J)) >> (SPSR_J - 1)) |
|
|
((ctx.registers[SPSR] & BIT(SPSR_T)) >> SPSR_T);
|
|
|
|
if (ist == SPSR_ISETSTATE_ARM) {
|
|
/* ARM instruction set state */
|
|
ret = ((instr & 0xFF00000) == 0x1200000) && ((instr & 0xF0) == 0x70);
|
|
} else if (ist != SPSR_ISETSTATE_JAZELLE) {
|
|
/* Thumb or ThumbEE encoding */
|
|
ret = ((instr & 0xFF00) == 0xBE00);
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/* Wrapper function to save and restore execution c */
|
|
void z_gdb_entry(struct arch_esf *esf, unsigned int exc_cause)
|
|
{
|
|
/* Disable the hardware breakpoint in case it was set */
|
|
__asm__ volatile("mcr p14, 0, %0, c0, c0, 5" ::"r"(0x0) :);
|
|
|
|
ctx.exception = exc_cause;
|
|
/* save the registers */
|
|
ctx.registers[R0] = esf->basic.r0;
|
|
ctx.registers[R1] = esf->basic.r1;
|
|
ctx.registers[R2] = esf->basic.r2;
|
|
ctx.registers[R3] = esf->basic.r3;
|
|
/* The EXTRA_EXCEPTION_INFO kernel option ensures these regs are set */
|
|
ctx.registers[R4] = esf->extra_info.callee->v1;
|
|
ctx.registers[R5] = esf->extra_info.callee->v2;
|
|
ctx.registers[R6] = esf->extra_info.callee->v3;
|
|
ctx.registers[R7] = esf->extra_info.callee->v4;
|
|
ctx.registers[R8] = esf->extra_info.callee->v5;
|
|
ctx.registers[R9] = esf->extra_info.callee->v6;
|
|
ctx.registers[R10] = esf->extra_info.callee->v7;
|
|
ctx.registers[R11] = esf->extra_info.callee->v8;
|
|
ctx.registers[R13] = esf->extra_info.callee->psp;
|
|
|
|
ctx.registers[R12] = esf->basic.r12;
|
|
ctx.registers[LR] = esf->basic.lr;
|
|
ctx.registers[PC] = esf->basic.pc;
|
|
ctx.registers[SPSR] = esf->basic.xpsr;
|
|
|
|
/* True if entering after a BKPT instruction */
|
|
const int bkpt_entry = is_bkpt(exc_cause);
|
|
|
|
z_gdb_main_loop(&ctx);
|
|
|
|
/* The registers part of EXTRA_EXCEPTION_INFO are read-only - the excpetion return code
|
|
* does not restore them, thus we don't need to do so here
|
|
*/
|
|
esf->basic.r0 = ctx.registers[R0];
|
|
esf->basic.r1 = ctx.registers[R1];
|
|
esf->basic.r2 = ctx.registers[R2];
|
|
esf->basic.r3 = ctx.registers[R3];
|
|
esf->basic.r12 = ctx.registers[R12];
|
|
esf->basic.lr = ctx.registers[LR];
|
|
esf->basic.pc = ctx.registers[PC];
|
|
esf->basic.xpsr = ctx.registers[SPSR];
|
|
/* TODO: restore regs from extra exc. info */
|
|
|
|
if (bkpt_entry) {
|
|
/* Apply this offset, so that the process won't be affected by the
|
|
* BKPT instruction
|
|
*/
|
|
esf->basic.pc += 0x4;
|
|
}
|
|
esf->basic.xpsr = ctx.registers[SPSR];
|
|
}
|
|
|
|
void arch_gdb_init(void)
|
|
{
|
|
uint32_t reg_val;
|
|
/* Enable the monitor debug mode */
|
|
__asm__ volatile("mrc p14, 0, %0, c0, c2, 2" : "=r"(reg_val)::);
|
|
reg_val |= DBGDSCR_MONITOR_MODE_EN;
|
|
__asm__ volatile("mcr p14, 0, %0, c0, c2, 2" ::"r"(reg_val) :);
|
|
|
|
/* Generate the Prefetch abort exception */
|
|
__asm__ volatile("BKPT");
|
|
}
|
|
|
|
void arch_gdb_continue(void)
|
|
{
|
|
/* No need to do anything, return to the code. */
|
|
}
|
|
|
|
void arch_gdb_step(void)
|
|
{
|
|
/* Set the hardware breakpoint */
|
|
uint32_t reg_val = ctx.registers[PC];
|
|
/* set BVR (Breakpoint value register) to PC, make sure it is word aligned */
|
|
reg_val &= ~(0x3);
|
|
__asm__ volatile("mcr p14, 0, %0, c0, c0, 4" ::"r"(reg_val) :);
|
|
|
|
reg_val = 0;
|
|
/* Address mismatch */
|
|
reg_val |= (DBGDBCR_MEANING_ADDR_MISMATCH & DBGDBCR_MEANING_MASK) << DBGDBCR_MEANING_SHIFT;
|
|
/* Match any other instruction */
|
|
reg_val |= (0xF & DBGDBCR_BYTE_ADDR_MASK) << DBGDBCR_BYTE_ADDR_SHIFT;
|
|
/* Breakpoint enable */
|
|
reg_val |= DBGDBCR_BRK_EN_MASK;
|
|
__asm__ volatile("mcr p14, 0, %0, c0, c0, 5" ::"r"(reg_val) :);
|
|
}
|
|
|
|
size_t arch_gdb_reg_readall(struct gdb_ctx *c, uint8_t *buf, size_t buflen)
|
|
{
|
|
int ret = 0;
|
|
/* All other registers are not supported */
|
|
memset(buf, 'x', buflen);
|
|
for (int i = 0; i < GDB_NUM_REGS; i++) {
|
|
/* offset inside the packet */
|
|
int pos = packet_pos[i] * 8;
|
|
int r = bin2hex((const uint8_t *)(c->registers + i), 4, buf + pos, buflen - pos);
|
|
/* remove the newline character placed by the bin2hex function */
|
|
buf[pos + 8] = 'x';
|
|
if (r == 0) {
|
|
ret = 0;
|
|
break;
|
|
}
|
|
ret += r;
|
|
}
|
|
|
|
if (ret) {
|
|
/* Since we don't support some floating point registers, set the packet size
|
|
* manually
|
|
*/
|
|
ret = GDB_READALL_PACKET_SIZE;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
size_t arch_gdb_reg_writeall(struct gdb_ctx *c, uint8_t *hex, size_t hexlen)
|
|
{
|
|
int ret = 0;
|
|
|
|
for (unsigned int i = 0; i < hexlen; i += 8) {
|
|
if (hex[i] != 'x') {
|
|
/* check if the stub supports this register */
|
|
for (unsigned int j = 0; j < GDB_NUM_REGS; j++) {
|
|
if (packet_pos[j] != i) {
|
|
continue;
|
|
}
|
|
int r = hex2bin(hex + i * 8, 8, (uint8_t *)(c->registers + j), 4);
|
|
|
|
if (r == 0) {
|
|
return 0;
|
|
}
|
|
ret += r;
|
|
}
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
size_t arch_gdb_reg_readone(struct gdb_ctx *c, uint8_t *buf, size_t buflen, uint32_t regno)
|
|
{
|
|
/* Reading four bytes (could be any return value except 0, which would indicate an error) */
|
|
int ret = 4;
|
|
/* Fill the buffer with 'x' in case the stub does not support the required register */
|
|
memset(buf, 'x', 8);
|
|
if (regno == SPSR_REG_IDX) {
|
|
/* The SPSR register is at the end, we have to check separately */
|
|
ret = bin2hex((uint8_t *)(c->registers + GDB_NUM_REGS - 1), 4, buf, buflen);
|
|
} else {
|
|
/* Check which of our registers corresponds to regnum */
|
|
for (int i = 0; i < GDB_NUM_REGS; i++) {
|
|
if (packet_pos[i] == regno) {
|
|
ret = bin2hex((uint8_t *)(c->registers + i), 4, buf, buflen);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
size_t arch_gdb_reg_writeone(struct gdb_ctx *c, uint8_t *hex, size_t hexlen, uint32_t regno)
|
|
{
|
|
int ret = 0;
|
|
/* Set the value of a register */
|
|
if (hexlen != 8) {
|
|
return ret;
|
|
}
|
|
|
|
if (regno < (GDB_NUM_REGS - 1)) {
|
|
/* Again, check the corresponding register index */
|
|
for (int i = 0; i < GDB_NUM_REGS; i++) {
|
|
if (packet_pos[i] == regno) {
|
|
ret = hex2bin(hex, hexlen, (uint8_t *)(c->registers + i), 4);
|
|
break;
|
|
}
|
|
}
|
|
} else if (regno == SPSR_REG_IDX) {
|
|
ret = hex2bin(hex, hexlen, (uint8_t *)(c->registers + GDB_NUM_REGS - 1), 4);
|
|
}
|
|
return ret;
|
|
}
|