gdb-stub.c

底层驱动开发

	asm volatile("	movgs	gr0,brr	\n"
		     "	ld%I2	%M2,%0	\n"
		     "	movsg	brr,%1	\n"
		     : "=r"(res), "=r"(brr)
		     : "m"(*(uint32_t *) addr));
	*_res = res;
	gdbstub_addr_unmap();
	return likely(!brr);
}

static inline int gdbstub_write_dword(void *addr, uint32_t val)
{
	unsigned long brr;

	if (!gdbstub_addr_map(addr))
		return 0;

	asm volatile("	movgs	gr0,brr	\n"
		     "	st%I2	%1,%M2	\n"
		     "	movsg	brr,%0	\n"
		     : "=r"(brr)
		     : "r"(val), "m"(*(uint32_t *) addr));
	gdbstub_addr_unmap();
	return likely(!brr);
}

static inline int gdbstub_read_word(const void *addr, uint16_t *_res)
{
	unsigned long brr;
	uint16_t res;

	if (!gdbstub_addr_map(addr))
		return 0;

	asm volatile("	movgs	gr0,brr	\n"
		     "	lduh%I2	%M2,%0	\n"
		     "	movsg	brr,%1	\n"
		     : "=r"(res), "=r"(brr)
		     : "m"(*(uint16_t *) addr));
	*_res = res;
	gdbstub_addr_unmap();
	return likely(!brr);
}

static inline int gdbstub_write_word(void *addr, uint16_t val)
{
	unsigned long brr;

	if (!gdbstub_addr_map(addr))
		return 0;

	asm volatile("	movgs	gr0,brr	\n"
		     "	sth%I2	%1,%M2	\n"
		     "	movsg	brr,%0	\n"
		     : "=r"(brr)
		     : "r"(val), "m"(*(uint16_t *) addr));
	gdbstub_addr_unmap();
	return likely(!brr);
}

static inline int gdbstub_read_byte(const void *addr, uint8_t *_res)
{
	unsigned long brr;
	uint8_t res;

	if (!gdbstub_addr_map(addr))
		return 0;

	asm volatile("	movgs	gr0,brr	\n"
		     "	ldub%I2	%M2,%0	\n"
		     "	movsg	brr,%1	\n"
		     : "=r"(res), "=r"(brr)
		     : "m"(*(uint8_t *) addr));
	*_res = res;
	gdbstub_addr_unmap();
	return likely(!brr);
}

static inline int gdbstub_write_byte(void *addr, uint8_t val)
{
	unsigned long brr;

	if (!gdbstub_addr_map(addr))
		return 0;

	asm volatile("	movgs	gr0,brr	\n"
		     "	stb%I2	%1,%M2	\n"
		     "	movsg	brr,%0	\n"
		     : "=r"(brr)
		     : "r"(val), "m"(*(uint8_t *) addr));
	gdbstub_addr_unmap();
	return likely(!brr);
}

static void __gdbstub_console_write(struct console *co, const char *p, unsigned n)
{
	char outbuf[26];
	int qty;

	outbuf[0] = 'O';

	while (n > 0) {
		qty = 1;

		while (n > 0 && qty < 20) {
			mem2hex(p, outbuf + qty, 2, 0);
			qty += 2;
			if (*p == 0x0a) {
				outbuf[qty++] = '0';
				outbuf[qty++] = 'd';
			}
			p++;
			n--;
		}

		outbuf[qty] = 0;
		gdbstub_send_packet(outbuf);
	}
}

#if 0
void debug_to_serial(const char *p, int n)
{
	gdbstub_console_write(NULL,p,n);
}
#endif

#ifdef CONFIG_GDBSTUB_CONSOLE

static kdev_t gdbstub_console_dev(struct console *con)
{
	return MKDEV(1,3); /* /dev/null */
}

static struct console gdbstub_console = {
	.name	= "gdb",
	.write	= gdbstub_console_write,	/* in break.S */
	.device	= gdbstub_console_dev,
	.flags	= CON_PRINTBUFFER,
	.index	= -1,
};

#endif

/*****************************************************************************/
/*
 * Convert the memory pointed to by mem into hex, placing result in buf.
 * - if successful, return a pointer to the last char put in buf (NUL)
 * - in case of mem fault, return NULL
 * may_fault is non-zero if we are reading from arbitrary memory, but is currently
 * not used.
 */
static unsigned char *mem2hex(const void *_mem, char *buf, int count, int may_fault)
{
	const uint8_t *mem = _mem;
	uint8_t ch[4] __attribute__((aligned(4)));

	if ((uint32_t)mem&1 && count>=1) {
		if (!gdbstub_read_byte(mem,ch))
			return NULL;
		*buf++ = hexchars[ch[0] >> 4];
		*buf++ = hexchars[ch[0] & 0xf];
		mem++;
		count--;
	}

	if ((uint32_t)mem&3 && count>=2) {
		if (!gdbstub_read_word(mem,(uint16_t *)ch))
			return NULL;
		*buf++ = hexchars[ch[0] >> 4];
		*buf++ = hexchars[ch[0] & 0xf];
		*buf++ = hexchars[ch[1] >> 4];
		*buf++ = hexchars[ch[1] & 0xf];
		mem += 2;
		count -= 2;
	}

	while (count>=4) {
		if (!gdbstub_read_dword(mem,(uint32_t *)ch))
			return NULL;
		*buf++ = hexchars[ch[0] >> 4];
		*buf++ = hexchars[ch[0] & 0xf];
		*buf++ = hexchars[ch[1] >> 4];
		*buf++ = hexchars[ch[1] & 0xf];
		*buf++ = hexchars[ch[2] >> 4];
		*buf++ = hexchars[ch[2] & 0xf];
		*buf++ = hexchars[ch[3] >> 4];
		*buf++ = hexchars[ch[3] & 0xf];
		mem += 4;
		count -= 4;
	}

	if (count>=2) {
		if (!gdbstub_read_word(mem,(uint16_t *)ch))
			return NULL;
		*buf++ = hexchars[ch[0] >> 4];
		*buf++ = hexchars[ch[0] & 0xf];
		*buf++ = hexchars[ch[1] >> 4];
		*buf++ = hexchars[ch[1] & 0xf];
		mem += 2;
		count -= 2;
	}

	if (count>=1) {
		if (!gdbstub_read_byte(mem,ch))
			return NULL;
		*buf++ = hexchars[ch[0] >> 4];
		*buf++ = hexchars[ch[0] & 0xf];
	}

	*buf = 0;

	return buf;
} /* end mem2hex() */

/*****************************************************************************/
/*
 * convert the hex array pointed to by buf into binary to be placed in mem
 * return a pointer to the character AFTER the last byte of buffer consumed
 */
static char *hex2mem(const char *buf, void *_mem, int count)
{
	uint8_t *mem = _mem;
	union {
		uint32_t l;
		uint16_t w;
		uint8_t  b[4];
	} ch;

	if ((u32)mem&1 && count>=1) {
		ch.b[0]  = hex(*buf++) << 4;
		ch.b[0] |= hex(*buf++);
		if (!gdbstub_write_byte(mem,ch.b[0]))
			return NULL;
		mem++;
		count--;
	}

	if ((u32)mem&3 && count>=2) {
		ch.b[0]  = hex(*buf++) << 4;
		ch.b[0] |= hex(*buf++);
		ch.b[1]  = hex(*buf++) << 4;
		ch.b[1] |= hex(*buf++);
		if (!gdbstub_write_word(mem,ch.w))
			return NULL;
		mem += 2;
		count -= 2;
	}

	while (count>=4) {
		ch.b[0]  = hex(*buf++) << 4;
		ch.b[0] |= hex(*buf++);
		ch.b[1]  = hex(*buf++) << 4;
		ch.b[1] |= hex(*buf++);
		ch.b[2]  = hex(*buf++) << 4;
		ch.b[2] |= hex(*buf++);
		ch.b[3]  = hex(*buf++) << 4;
		ch.b[3] |= hex(*buf++);
		if (!gdbstub_write_dword(mem,ch.l))
			return NULL;
		mem += 4;
		count -= 4;
	}

	if (count>=2) {
		ch.b[0]  = hex(*buf++) << 4;
		ch.b[0] |= hex(*buf++);
		ch.b[1]  = hex(*buf++) << 4;
		ch.b[1] |= hex(*buf++);
		if (!gdbstub_write_word(mem,ch.w))
			return NULL;
		mem += 2;
		count -= 2;
	}

	if (count>=1) {
		ch.b[0]  = hex(*buf++) << 4;
		ch.b[0] |= hex(*buf++);
		if (!gdbstub_write_byte(mem,ch.b[0]))
			return NULL;
	}

	return (char *) buf;
} /* end hex2mem() */

/*****************************************************************************/
/*
 * This table contains the mapping between FRV TBR.TT exception codes,
 * and signals, which are primarily what GDB understands.  It also
 * indicates which hardware traps we need to commandeer when
 * initializing the stub.
 */
static const struct brr_to_sig_map {
	unsigned long	brr_mask;	/* BRR bitmask */
	unsigned long	tbr_tt;		/* TBR.TT code (in BRR.EBTT) */
	unsigned int	signo;		/* Signal that we map this into */
} brr_to_sig_map[] = {
	{ BRR_EB,	TBR_TT_INSTR_ACC_ERROR,	SIGSEGV		},
	{ BRR_EB,	TBR_TT_ILLEGAL_INSTR,	SIGILL		},
	{ BRR_EB,	TBR_TT_PRIV_INSTR,	SIGILL		},
	{ BRR_EB,	TBR_TT_MP_EXCEPTION,	SIGFPE		},
	{ BRR_EB,	TBR_TT_DATA_ACC_ERROR,	SIGSEGV		},
	{ BRR_EB,	TBR_TT_DATA_STR_ERROR,	SIGSEGV		},
	{ BRR_EB,	TBR_TT_DIVISION_EXCEP,	SIGFPE		},
	{ BRR_EB,	TBR_TT_COMPOUND_EXCEP,	SIGSEGV		},
	{ BRR_EB,	TBR_TT_INTERRUPT_13,	SIGALRM		},	/* watchdog */
	{ BRR_EB,	TBR_TT_INTERRUPT_14,	SIGINT		},	/* GDB serial */
	{ BRR_EB,	TBR_TT_INTERRUPT_15,	SIGQUIT		},	/* NMI */
	{ BRR_CB,	0,			SIGUSR1		},
	{ BRR_TB,	0,			SIGUSR2		},
	{ BRR_DBNEx,	0,			SIGTRAP		},
	{ BRR_DBx,	0,			SIGTRAP		},	/* h/w watchpoint */
	{ BRR_IBx,	0,			SIGTRAP		},	/* h/w breakpoint */
	{ BRR_CBB,	0,			SIGTRAP		},
	{ BRR_SB,	0,			SIGTRAP		},
	{ BRR_ST,	0,			SIGTRAP		},	/* single step */
	{ 0,		0,			SIGHUP		}	/* default */
};

/*****************************************************************************/
/*
 * convert the FRV BRR register contents into a UNIX signal number
 */
static inline int gdbstub_compute_signal(unsigned long brr)
{
	const struct brr_to_sig_map *map;
	unsigned long tbr = (brr & BRR_EBTT) >> 12;

	for (map = brr_to_sig_map; map->brr_mask; map++)
		if (map->brr_mask & brr)
			if (!map->tbr_tt || map->tbr_tt == tbr)
				break;

	return map->signo;
} /* end gdbstub_compute_signal() */

/*****************************************************************************/
/*
 * set a software breakpoint or a hardware breakpoint or watchpoint
 */
static int gdbstub_set_breakpoint(unsigned long type, unsigned long addr, unsigned long len)
{
	unsigned long tmp;
	int bkpt, loop, xloop;

	union {
		struct {
			unsigned long mask0, mask1;
		};
		uint8_t bytes[8];
	} dbmr;

	//gdbstub_printk("setbkpt(%ld,%08lx,%ld)\n", type, addr, len);

	switch (type) {
		/* set software breakpoint */
	case 0:
		if (addr & 3 || len > 7*4)
			return -EINVAL;

		for (bkpt = 255; bkpt >= 0; bkpt--)
			if (!gdbstub_bkpts[bkpt].addr)
				break;
		if (bkpt < 0)
			return -ENOSPC;

		for (loop = 0; loop < len/4; loop++)
			if (!gdbstub_read_dword(&((uint32_t *) addr)[loop],
						&gdbstub_bkpts[bkpt].originsns[loop]))
				return -EFAULT;

		for (loop = 0; loop < len/4; loop++)
			if (!gdbstub_write_dword(&((uint32_t *) addr)[loop],
						 BREAK_INSN)
			    ) {
				/* need to undo the changes if possible */
				for (xloop = 0; xloop < loop; xloop++)
					gdbstub_write_dword(&((uint32_t *) addr)[xloop],
							    gdbstub_bkpts[bkpt].originsns[xloop]);
				return -EFAULT;
			}

		gdbstub_bkpts[bkpt].addr = addr;
		gdbstub_bkpts[bkpt].len = len;

#if 0
		gdbstub_printk("Set BKPT[%02x]: %08lx #%d {%04x, %04x} -> { %04x, %04x }\n",
			       bkpt,
			       gdbstub_bkpts[bkpt].addr,
			       gdbstub_bkpts[bkpt].len,
			       gdbstub_bkpts[bkpt].originsns[0],
			       gdbstub_bkpts[bkpt].originsns[1],
			       ((uint32_t *) addr)[0],
			       ((uint32_t *) addr)[1]
			       );
#endif
		return 0;

		/* set hardware breakpoint */
	case 1:
		if (addr & 3 || len != 4)
			return -EINVAL;

		if (!(__debug_regs->dcr & DCR_IBE0)) {
			//gdbstub_printk("set h/w break 0: %08lx\n", addr);
			__debug_regs->dcr |= DCR_IBE0;
			asm volatile("movgs %0,ibar0" : : "r"(addr));
			return 0;
		}

		if (!(__debug_regs->dcr & DCR_IBE1)) {
			//gdbstub_printk("set h/w break 1: %08lx\n", addr);
			__debug_regs->dcr |= DCR_IBE1;
			asm volatile("movgs %0,ibar1" : : "r"(addr));
			return 0;
		}

		if (!(__debug_regs->dcr & DCR_IBE2)) {
			//gdbstub_printk("set h/w break 2: %08lx\n", addr);
			__debug_regs->dcr |= DCR_IBE2;
			asm volatile("movgs %0,ibar2" : : "r"(addr));
			return 0;
		}

		if (!(__debug_regs->dcr & DCR_IBE3)) {
			//gdbstub_printk("set h/w break 3: %08lx\n", addr);
			__debug_regs->dcr |= DCR_IBE3;
			asm volatile("movgs %0,ibar3" : : "r"(addr));
			return 0;
		}

		return -ENOSPC;

		/* set data read/write/access watchpoint */
	case 2:
	case 3:
	case 4:
		if ((addr & ~7) != ((addr + len - 1) & ~7))
			return -EINVAL;

		tmp = addr & 7;

		memset(dbmr.bytes, 0xff, sizeof(dbmr.bytes));
		for (loop = 0; loop < len; loop++)
			dbmr.bytes[tmp + loop] = 0;

		addr &= ~7;

		if (!(__debug_regs->dcr & (DCR_DRBE0|DCR_DWBE0))) {
			//gdbstub_printk("set h/w watchpoint 0 type %ld: %08lx\n", type, addr);
			tmp = type==2 ? DCR_DWBE0 : type==3 ? DCR_DRBE0 : DCR_DRBE0|DCR_DWBE0;
			__debug_regs->dcr |= tmp;
			asm volatile("	movgs	%0,dbar0	\n"
				     "	movgs	%1,dbmr00	\n"
				     "	movgs	%2,dbmr01	\n"
				     "	movgs	gr0,dbdr00	\n"
				     "	movgs	gr0,dbdr01	\n"
				     : : "r"(addr), "r"(dbmr.mask0), "r"(dbmr.mask1));
			return 0;
		}

		if (!(__debug_regs->dcr & (DCR_DRBE1|DCR_DWBE1))) {
			//gdbstub_printk("set h/w watchpoint 1 type %ld: %08lx\n", type, addr);
			tmp = type==2 ? DCR_DWBE1 : type==3 ? DCR_DRBE1 : DCR_DRBE1|DCR_DWBE1;
			__debug_regs->dcr |= tmp;
			asm volatile("	movgs	%0,dbar1	\n"
				     "	movgs	%1,dbmr10	\n"
				     "	movgs	%2,dbmr11	\n"
				     "	movgs	gr0,dbdr10	\n"
				     "	movgs	gr0,dbdr11	\n"
				     : : "r"(addr), "r"(dbmr.mask0), "r"(dbmr.mask1));
			return 0;
		}

		return -ENOSPC;

	default:
		return -EINVAL;
	}

} /* end gdbstub_set_breakpoint() */

/*****************************************************************************/
/*
 * clear a breakpoint or watchpoint
 */
int gdbstub_clear_breakpoint(unsigned long type, unsigned long addr, unsigned long len)
{
	unsigned long tmp;
	int bkpt, loop;

	union {
		struct {
			unsigned long mask0, mask1;
		};
		uint8_t bytes[8];
	} dbmr;

	//gdbstub_printk("clearbkpt(%ld,%08lx,%ld)\n", type, addr, len);

	switch (type) {
		/* clear software breakpoint */
	case 0:
		for (bkpt = 255; bkpt >= 0; bkpt--)
			if (gdbstub_bkpts[bkpt].addr == addr && gdbstub_bkpts[bkpt].len == len)
				break;
		if (bkpt < 0)
			return -ENOENT;

		gdbstub_bkpts[bkpt].addr = 0;

		for (loop = 0; loop < len/4; loop++)
			if (!gdbstub_write_dword(&((uint32_t *) addr)[loop],
						 gdbstub_bkpts[bkpt].originsns[loop]))
				return -EFAULT;
		return 0;

		/* clear hardware breakpoint */
	case 1:
		if (addr & 3 || len != 4)
			return -EINVAL;