kgdb_stub.c

linux 内核源代码

	regs->regs[R14] = gdb_regs[R14];
	regs->regs[R15] = gdb_regs[R15];
	regs->pc = gdb_regs[PC];
	regs->pr = gdb_regs[PR];
	regs->gbr = gdb_regs[GBR];
	regs->mach = gdb_regs[MACH];
	regs->macl = gdb_regs[MACL];
	regs->sr = gdb_regs[SR];
	regs->vbr = gdb_regs[VBR];
}

/* Calculate the new address for after a step */
static short *get_step_address(void)
{
	short op = *(short *) trap_registers.pc;
	long addr;

	/* BT */
	if (OPCODE_BT(op)) {
		if (trap_registers.sr & SR_T_BIT_MASK)
			addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
		else
			addr = trap_registers.pc + 2;
	}

	/* BTS */
	else if (OPCODE_BTS(op)) {
		if (trap_registers.sr & SR_T_BIT_MASK)
			addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
		else
			addr = trap_registers.pc + 4;	/* Not in delay slot */
	}

	/* BF */
	else if (OPCODE_BF(op)) {
		if (!(trap_registers.sr & SR_T_BIT_MASK))
			addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
		else
			addr = trap_registers.pc + 2;
	}

	/* BFS */
	else if (OPCODE_BFS(op)) {
		if (!(trap_registers.sr & SR_T_BIT_MASK))
			addr = trap_registers.pc + 4 + OPCODE_BTF_DISP(op);
		else
			addr = trap_registers.pc + 4;	/* Not in delay slot */
	}

	/* BRA */
	else if (OPCODE_BRA(op))
		addr = trap_registers.pc + 4 + OPCODE_BRA_DISP(op);

	/* BRAF */
	else if (OPCODE_BRAF(op))
		addr = trap_registers.pc + 4
		    + trap_registers.regs[OPCODE_BRAF_REG(op)];

	/* BSR */
	else if (OPCODE_BSR(op))
		addr = trap_registers.pc + 4 + OPCODE_BSR_DISP(op);

	/* BSRF */
	else if (OPCODE_BSRF(op))
		addr = trap_registers.pc + 4
		    + trap_registers.regs[OPCODE_BSRF_REG(op)];

	/* JMP */
	else if (OPCODE_JMP(op))
		addr = trap_registers.regs[OPCODE_JMP_REG(op)];

	/* JSR */
	else if (OPCODE_JSR(op))
		addr = trap_registers.regs[OPCODE_JSR_REG(op)];

	/* RTS */
	else if (OPCODE_RTS(op))
		addr = trap_registers.pr;

	/* RTE */
	else if (OPCODE_RTE(op))
		addr = trap_registers.regs[15];

	/* Other */
	else
		addr = trap_registers.pc + 2;

	flush_icache_range(addr, addr + 2);
	return (short *) addr;
}

/* Set up a single-step.  Replace the instruction immediately after the
   current instruction (i.e. next in the expected flow of control) with a
   trap instruction, so that returning will cause only a single instruction
   to be executed. Note that this model is slightly broken for instructions
   with delay slots (e.g. B[TF]S, BSR, BRA etc), where both the branch
   and the instruction in the delay slot will be executed. */
static void do_single_step(void)
{
	unsigned short *addr = 0;

	/* Determine where the target instruction will send us to */
	addr = get_step_address();
	stepped_address = (int)addr;

	/* Replace it */
	stepped_opcode = *(short *)addr;
	*addr = STEP_OPCODE;

	/* Flush and return */
	flush_icache_range((long) addr, (long) addr + 2);
}

/* Undo a single step */
static void undo_single_step(void)
{
	/* If we have stepped, put back the old instruction */
	/* Use stepped_address in case we stopped elsewhere */
	if (stepped_opcode != 0) {
		*(short*)stepped_address = stepped_opcode;
		flush_icache_range(stepped_address, stepped_address + 2);
	}
	stepped_opcode = 0;
}

/* Send a signal message */
static void send_signal_msg(const int signum)
{
	out_buffer[0] = 'S';
	out_buffer[1] = highhex(signum);
	out_buffer[2] = lowhex(signum);
	out_buffer[3] = 0;
	put_packet(out_buffer);
}

/* Reply that all was well */
static void send_ok_msg(void)
{
	strcpy(out_buffer, "OK");
	put_packet(out_buffer);
}

/* Reply that an error occurred */
static void send_err_msg(void)
{
	strcpy(out_buffer, "E01");
	put_packet(out_buffer);
}

/* Empty message indicates unrecognised command */
static void send_empty_msg(void)
{
	put_packet("");
}

/* Read memory due to 'm' message */
static void read_mem_msg(void)
{
	char *ptr;
	int addr;
	int length;

	/* Jmp, disable bus error handler */
	if (setjmp(rem_com_env) == 0) {

		kgdb_nofault = 1;

		/* Walk through, have m<addr>,<length> */
		ptr = &in_buffer[1];
		if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
			if (hex_to_int(&ptr, &length)) {
				ptr = 0;
				if (length * 2 > OUTBUFMAX)
					length = OUTBUFMAX / 2;
				mem_to_hex((char *) addr, out_buffer, length);
			}
		if (ptr)
			send_err_msg();
		else
			put_packet(out_buffer);
	} else
		send_err_msg();

	/* Restore bus error handler */
	kgdb_nofault = 0;
}

/* Write memory due to 'M' or 'X' message */
static void write_mem_msg(int binary)
{
	char *ptr;
	int addr;
	int length;

	if (setjmp(rem_com_env) == 0) {

		kgdb_nofault = 1;

		/* Walk through, have M<addr>,<length>:<data> */
		ptr = &in_buffer[1];
		if (hex_to_int(&ptr, &addr) && (*ptr++ == ','))
			if (hex_to_int(&ptr, &length) && (*ptr++ == ':')) {
				if (binary)
					ebin_to_mem(ptr, (char*)addr, length);
				else
					hex_to_mem(ptr, (char*)addr, length);
				flush_icache_range(addr, addr + length);
				ptr = 0;
				send_ok_msg();
			}
		if (ptr)
			send_err_msg();
	} else
		send_err_msg();

	/* Restore bus error handler */
	kgdb_nofault = 0;
}

/* Continue message  */
static void continue_msg(void)
{
	/* Try to read optional parameter, PC unchanged if none */
	char *ptr = &in_buffer[1];
	int addr;

	if (hex_to_int(&ptr, &addr))
		trap_registers.pc = addr;
}

/* Continue message with signal */
static void continue_with_sig_msg(void)
{
	int signal;
	char *ptr = &in_buffer[1];
	int addr;

	/* Report limitation */
	kgdb_to_gdb("Cannot force signal in kgdb, continuing anyway.\n");

	/* Signal */
	hex_to_int(&ptr, &signal);
	if (*ptr == ';')
		ptr++;

	/* Optional address */
	if (hex_to_int(&ptr, &addr))
		trap_registers.pc = addr;
}

/* Step message */
static void step_msg(void)
{
	continue_msg();
	do_single_step();
}

/* Step message with signal */
static void step_with_sig_msg(void)
{
	continue_with_sig_msg();
	do_single_step();
}

/* Send register contents */
static void send_regs_msg(void)
{
	kgdb_regs_to_gdb_regs(&trap_registers, registers);
	mem_to_hex((char *) registers, out_buffer, NUMREGBYTES);
	put_packet(out_buffer);
}

/* Set register contents - currently can't set other thread's registers */
static void set_regs_msg(void)
{
	kgdb_regs_to_gdb_regs(&trap_registers, registers);
	hex_to_mem(&in_buffer[1], (char *) registers, NUMREGBYTES);
	gdb_regs_to_kgdb_regs(registers, &trap_registers);
	send_ok_msg();
}

#ifdef CONFIG_SH_KGDB_CONSOLE
/*
 * Bring up the ports..
 */
static int __init kgdb_serial_setup(void)
{
	struct console dummy;
	return kgdb_console_setup(&dummy, 0);
}
#else
#define kgdb_serial_setup()	0
#endif

/* The command loop, read and act on requests */
static void kgdb_command_loop(const int excep_code, const int trapa_value)
{
	int sigval;

	/* Enter GDB mode (e.g. after detach) */
	if (!kgdb_in_gdb_mode) {
		/* Do serial setup, notify user, issue preemptive ack */
		printk(KERN_NOTICE "KGDB: Waiting for GDB\n");
		kgdb_in_gdb_mode = 1;
		put_debug_char('+');
	}

	/* Reply to host that an exception has occurred */
	sigval = compute_signal(excep_code);
	send_signal_msg(sigval);

	/* TRAP_VEC exception indicates a software trap inserted in place of
	   code by GDB so back up PC by one instruction, as this instruction
	   will later be replaced by its original one.  Do NOT do this for
	   trap 0xff, since that indicates a compiled-in breakpoint which
	   will not be replaced (and we would retake the trap forever) */
	if ((excep_code == TRAP_VEC) && (trapa_value != (0x3c << 2)))
		trap_registers.pc -= 2;

	/* Undo any stepping we may have done */
	undo_single_step();

	while (1) {
		out_buffer[0] = 0;
		get_packet(in_buffer, BUFMAX);

		/* Examine first char of buffer to see what we need to do */
		switch (in_buffer[0]) {
		case '?':	/* Send which signal we've received */
			send_signal_msg(sigval);
			break;

		case 'g':	/* Return the values of the CPU registers */
			send_regs_msg();
			break;

		case 'G':	/* Set the value of the CPU registers */
			set_regs_msg();
			break;

		case 'm':	/* Read LLLL bytes address AA..AA */
			read_mem_msg();
			break;

		case 'M':	/* Write LLLL bytes address AA..AA, ret OK */
			write_mem_msg(0);	/* 0 = data in hex */
			break;

		case 'X':	/* Write LLLL bytes esc bin address AA..AA */
			if (kgdb_bits == '8')
				write_mem_msg(1); /* 1 = data in binary */
			else
				send_empty_msg();
			break;

		case 'C':	/* Continue, signum included, we ignore it */
			continue_with_sig_msg();
			return;

		case 'c':	/* Continue at address AA..AA (optional) */
			continue_msg();
			return;

		case 'S':	/* Step, signum included, we ignore it */
			step_with_sig_msg();
			return;

		case 's':	/* Step one instruction from AA..AA */
			step_msg();
			return;

		case 'k':	/* 'Kill the program' with a kernel ? */
			break;

		case 'D':	/* Detach from program, send reply OK */
			kgdb_in_gdb_mode = 0;
			send_ok_msg();
			get_debug_char();
			return;

		default:
			send_empty_msg();
			break;
		}
	}
}

/* There has been an exception, most likely a breakpoint. */
static void handle_exception(struct pt_regs *regs)
{
	int excep_code, vbr_val;
	int count;
	int trapa_value = ctrl_inl(TRA);

	/* Copy kernel regs (from stack) */
	for (count = 0; count < 16; count++)
		trap_registers.regs[count] = regs->regs[count];
	trap_registers.pc = regs->pc;
	trap_registers.pr = regs->pr;
	trap_registers.sr = regs->sr;
	trap_registers.gbr = regs->gbr;
	trap_registers.mach = regs->mach;
	trap_registers.macl = regs->macl;

	asm("stc vbr, %0":"=r"(vbr_val));
	trap_registers.vbr = vbr_val;

	/* Get excode for command loop call, user access */
	asm("stc r2_bank, %0":"=r"(excep_code));

	/* Act on the exception */
	kgdb_command_loop(excep_code, trapa_value);

	/* Copy back the (maybe modified) registers */
	for (count = 0; count < 16; count++)
		regs->regs[count] = trap_registers.regs[count];
	regs->pc = trap_registers.pc;
	regs->pr = trap_registers.pr;
	regs->sr = trap_registers.sr;
	regs->gbr = trap_registers.gbr;
	regs->mach = trap_registers.mach;
	regs->macl = trap_registers.macl;

	vbr_val = trap_registers.vbr;
	asm("ldc %0, vbr": :"r"(vbr_val));
}

asmlinkage void kgdb_handle_exception(unsigned long r4, unsigned long r5,
				      unsigned long r6, unsigned long r7,
				      struct pt_regs __regs)
{
	struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
	handle_exception(regs);
}

/* Initialise the KGDB data structures and serial configuration */
int __init kgdb_init(void)
{
	in_nmi = 0;
	kgdb_nofault = 0;
	stepped_opcode = 0;
	kgdb_in_gdb_mode = 0;

	if (kgdb_serial_setup() != 0) {
		printk(KERN_NOTICE "KGDB: serial setup error\n");
		return -1;
	}

	/* Init ptr to exception handler */
	kgdb_debug_hook = handle_exception;
	kgdb_bus_err_hook = kgdb_handle_bus_error;

	/* Enter kgdb now if requested, or just report init done */
	printk(KERN_NOTICE "KGDB: stub is initialized.\n");

	return 0;
}

/* Make function available for "user messages"; console will use it too. */

char gdbmsgbuf[BUFMAX];
#define MAXOUT ((BUFMAX-2)/2)

static void kgdb_msg_write(const char *s, unsigned count)
{
	int i;
	int wcount;
	char *bufptr;

	/* 'O'utput */
	gdbmsgbuf[0] = 'O';

	/* Fill and send buffers... */
	while (count > 0) {
		bufptr = gdbmsgbuf + 1;

		/* Calculate how many this time */
		wcount = (count > MAXOUT) ? MAXOUT : count;

		/* Pack in hex chars */
		for (i = 0; i < wcount; i++)
			bufptr = pack_hex_byte(bufptr, s[i]);
		*bufptr = '\0';

		/* Move up */
		s += wcount;
		count -= wcount;

		/* Write packet */
		put_packet(gdbmsgbuf);
	}
}

static void kgdb_to_gdb(const char *s)
{
	kgdb_msg_write(s, strlen(s));
}

#ifdef CONFIG_SH_KGDB_CONSOLE
void kgdb_console_write(struct console *co, const char *s, unsigned count)
{
	/* Bail if we're not talking to GDB */
	if (!kgdb_in_gdb_mode)
		return;

	kgdb_msg_write(s, count);
}
#endif

#ifdef CONFIG_KGDB_SYSRQ
static void sysrq_handle_gdb(int key, struct tty_struct *tty)
{
	printk("Entering GDB stub\n");
	breakpoint();
}

static struct sysrq_key_op sysrq_gdb_op = {
        .handler        = sysrq_handle_gdb,
        .help_msg       = "Gdb",
        .action_msg     = "GDB",
};

static int gdb_register_sysrq(void)
{
	printk("Registering GDB sysrq handler\n");
	register_sysrq_key('g', &sysrq_gdb_op);
	return 0;
}
module_init(gdb_register_sysrq);
#endif