/*
 *  arch/mips/kernel/gdb-stub.c
 *
 *  Originally written by Glenn Engel, Lake Stevens Instrument Division
 *
 *  Contributed by HP Systems
 *
 *  Modified for SPARC by Stu Grossman, Cygnus Support.
 *
 *  Modified for Linux/MIPS (and MIPS in general) by Andreas Busse
 *  Send complaints, suggestions etc. to <andy@waldorf-gmbh.de>
 *
 *  Copyright (C) 1995 Andreas Busse
 *
 *  Copyright (C) 2003 MontaVista Software Inc.
 *  Author: Jun Sun, jsun@mvista.com or jsun@junsun.net
 */

/*
 *  To enable debugger support, two things need to happen.  One, a
 *  call to set_debug_traps() is necessary in order to allow any breakpoints
 *  or error conditions to be properly intercepted and reported to gdb.
 *  Two, a breakpoint needs to be generated to begin communication.  This
 *  is most easily accomplished by a call to breakpoint().  Breakpoint()
 *  simulates a breakpoint by executing a BREAK instruction.
 *
 *
 *    The following gdb commands are supported:
 *
 * command          function                               Return value
 *
 *    g             return the value of the CPU registers  hex data or ENN
 *    G             set the value of the CPU registers     OK or ENN
 *
 *    mAA..AA,LLLL  Read LLLL bytes at address AA..AA      hex data or ENN
 *    MAA..AA,LLLL: Write LLLL bytes at address AA.AA      OK or ENN
 *
 *    c             Resume at current address              SNN   ( signal NN)
 *    cAA..AA       Continue at address AA..AA             SNN
 *
 *    s             Step one instruction                   SNN
 *    sAA..AA       Step one instruction from AA..AA       SNN
 *
 *    k             kill
 *
 *    ?             What was the last sigval ?             SNN   (signal NN)
 *
 *    bBB..BB	    Set baud rate to BB..BB		   OK or BNN, then sets
 *							   baud rate
 *
 * All commands and responses are sent with a packet which includes a
 * checksum.  A packet consists of
 *
 * $<packet info>#<checksum>.
 *
 * where
 * <packet info> :: <characters representing the command or response>
 * <checksum>    :: < two hex digits computed as modulo 256 sum of <packetinfo>>
 *
 * When a packet is received, it is first acknowledged with either '+' or '-'.
 * '+' indicates a successful transfer.  '-' indicates a failed transfer.
 *
 * Example:
 *
 * Host:                  Reply:
 * $m0,10#2a               +$00010203040506070809101112131415#42
 *
 *
 *  ==============
 *  MORE EXAMPLES:
 *  ==============
 *
 *  For reference -- the following are the steps that one
 *  company took (RidgeRun Inc) to get remote gdb debugging
 *  going. In this scenario the host machine was a PC and the
 *  target platform was a Galileo EVB64120A MIPS evaluation
 *  board.
 *
 *  Step 1:
 *  First download gdb-5.0.tar.gz from the internet.
 *  and then build/install the package.
 *
 *  Example:
 *    $ tar zxf gdb-5.0.tar.gz
 *    $ cd gdb-5.0
 *    $ ./configure --target=mips-linux-elf
 *    $ make
 *    $ install
 *    $ which mips-linux-elf-gdb
 *    /usr/local/bin/mips-linux-elf-gdb
 *
 *  Step 2:
 *  Configure linux for remote debugging and build it.
 *
 *  Example:
 *    $ cd ~/linux
 *    $ make menuconfig <go to "Kernel Hacking" and turn on remote debugging>
 *    $ make
 *
 *  Step 3:
 *  Download the kernel to the remote target and start
 *  the kernel running. It will promptly halt and wait
 *  for the host gdb session to connect. It does this
 *  since the "Kernel Hacking" option has defined
 *  CONFIG_KGDB which in turn enables your calls
 *  to:
 *     set_debug_traps();
 *     breakpoint();
 *
 *  Step 4:
 *  Start the gdb session on the host.
 *
 *  Example:
 *    $ mips-linux-elf-gdb vmlinux
 *    (gdb) set remotebaud 115200
 *    (gdb) target remote /dev/ttyS1
 *    ...at this point you are connected to
 *       the remote target and can use gdb
 *       in the normal fasion. Setting
 *       breakpoints, single stepping,
 *       printing variables, etc.
 */
#include <linux/string.h>
#include <linux/kernel.h>
#include <linux/signal.h>
#include <linux/sched.h>
#include <linux/mm.h>
#include <linux/console.h>
#include <linux/init.h>
#include <linux/smp.h>
#include <linux/spinlock.h>
#include <linux/slab.h>
#include <linux/reboot.h>

#include <asm/asm.h>
#include <asm/cacheflush.h>
#include <asm/mipsregs.h>
#include <asm/pgtable.h>
#include <asm/system.h>
#include <asm/gdb-stub.h>
#include <asm/inst.h>
#include <asm/smp.h>

/*
 * external low-level support routines
 */

extern int putDebugChar(char c);    /* write a single character      */
extern char getDebugChar(void);     /* read and return a single char */
extern void trap_low(void);

/*
 * breakpoint and test functions
 */
extern void breakpoint(void);
extern void breakinst(void);
extern void async_breakpoint(void);
extern void async_breakinst(void);
extern void adel(void);

/*
 * local prototypes
 */

static void getpacket(char *buffer);
static void putpacket(char *buffer);
static int computeSignal(int tt);
static int hex(unsigned char ch);
static int hexToInt(char **ptr, int *intValue);
static int hexToLong(char **ptr, long *longValue);
static unsigned char *mem2hex(char *mem, char *buf, int count, int may_fault);
void handle_exception(struct gdb_regs *regs);

int kgdb_enabled;

/*
 * spin locks for smp case
 */
static DEFINE_SPINLOCK(kgdb_lock);
static raw_spinlock_t kgdb_cpulock[NR_CPUS] = {
	[0 ... NR_CPUS-1] = __RAW_SPIN_LOCK_UNLOCKED,
};

/*
 * BUFMAX defines the maximum number of characters in inbound/outbound buffers
 * at least NUMREGBYTES*2 are needed for register packets
 */
#define BUFMAX 2048

static char input_buffer[BUFMAX];
static char output_buffer[BUFMAX];
static int initialized;	/* !0 means we've been initialized */
static int kgdb_started;
static const char hexchars[]="0123456789abcdef";

/* Used to prevent crashes in memory access.  Note that they'll crash anyway if
   we haven't set up fault handlers yet... */
int kgdb_read_byte(unsigned char *address, unsigned char *dest);
int kgdb_write_byte(unsigned char val, unsigned char *dest);

/*
 * Convert ch from a hex digit to an int
 */
static int hex(unsigned char ch)
{
	if (ch >= 'a' && ch <= 'f')
		return ch-'a'+10;
	if (ch >= '0' && ch <= '9')
		return ch-'0';
	if (ch >= 'A' && ch <= 'F')
		return ch-'A'+10;
	return -1;
}

/*
 * scan for the sequence $<data>#<checksum>
 */
static void getpacket(char *buffer)
{
	unsigned char checksum;
	unsigned char xmitcsum;
	int i;
	int count;
	unsigned char ch;

	do {
		/*
		 * wait around for the start character,
		 * ignore all other characters
		 */
		while ((ch = (getDebugChar() & 0x7f)) != '$') ;

		checksum = 0;
		xmitcsum = -1;
		count = 0;

		/*
		 * now, read until a # or end of buffer is found
		 */
		while (count < BUFMAX) {
			ch = getDebugChar();
			if (ch == '#')
				break;
			checksum = checksum + ch;
			buffer[count] = ch;
			count = count + 1;
		}

		if (count >= BUFMAX)
			continue;

		buffer[count] = 0;

		if (ch == '#') {
			xmitcsum = hex(getDebugChar() & 0x7f) << 4;
			xmitcsum |= hex(getDebugChar() & 0x7f);

			if (checksum != xmitcsum)
				putDebugChar('-');	/* failed checksum */
			else {
				putDebugChar('+'); /* successful transfer */

				/*
				 * if a sequence char is present,
				 * reply the sequence ID
				 */
				if (buffer[2] == ':') {
					putDebugChar(buffer[0]);
					putDebugChar(buffer[1]);

					/*
					 * remove sequence chars from buffer
					 */
					count = strlen(buffer);
					for (i=3; i <= count; i++)
						buffer[i-3] = buffer[i];
				}
			}
		}
	}
	while (checksum != xmitcsum);
}

/*
 * send the packet in buffer.
 */
static void putpacket(char *buffer)
{
	unsigned char checksum;
	int count;
	unsigned char ch;

	/*
	 * $<packet info>#<checksum>.
	 */

	do {
		putDebugChar('$');
		checksum = 0;
		count = 0;

		while ((ch = buffer[count]) != 0) {
			if (!(putDebugChar(ch)))
				return;
			checksum += ch;
			count += 1;
		}

		putDebugChar('#');
		putDebugChar(hexchars[checksum >> 4]);
		putDebugChar(hexchars[checksum & 0xf]);

	}
	while ((getDebugChar() & 0x7f) != '+');
}


/*
 * Convert the memory pointed to by mem into hex, placing result in buf.
 * Return a pointer to the last char put in buf (null), in case of mem fault,
 * return 0.
 * may_fault is non-zero if we are reading from arbitrary memory, but is currently
 * not used.
 */
static unsigned char *mem2hex(char *mem, char *buf, int count, int may_fault)
{
	unsigned char ch;

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

	*buf = 0;

	return buf;
}

/*
 * 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 written
 * may_fault is non-zero if we are reading from arbitrary memory, but is currently
 * not used.
 */
static char *hex2mem(char *buf, char *mem, int count, int binary, int may_fault)
{
	int i;
	unsigned char ch;

	for (i=0; i<count; i++)
	{
		if (binary) {
			ch = *buf++;
			if (ch == 0x7d)
				ch = 0x20 ^ *buf++;
		}
		else {
			ch = hex(*buf++) << 4;
			ch |= hex(*buf++);
		}
		if (kgdb_write_byte(ch, mem++) != 0)
			return 0;
	}

	return mem;
}

/*
 * This table contains the mapping between SPARC hardware trap types, and
 * signals, which are primarily what GDB understands.  It also indicates
 * which hardware traps we need to commandeer when initializing the stub.
 */
static struct hard_trap_info {
	unsigned char tt;		/* Trap type code for MIPS R3xxx and R4xxx */
	unsigned char signo;		/* Signal that we map this trap into */
} hard_trap_info[] = {
	{ 6, SIGBUS },			/* instruction bus error */
	{ 7, SIGBUS },			/* data bus error */
	{ 9, SIGTRAP },			/* break */
	{ 10, SIGILL },			/* reserved instruction */
/*	{ 11, SIGILL },		*/	/* CPU unusable */
	{ 12, SIGFPE },			/* overflow */
	{ 13, SIGTRAP },		/* trap */
	{ 14, SIGSEGV },		/* virtual instruction cache coherency */
	{ 15, SIGFPE },			/* floating point exception */
	{ 23, SIGSEGV },		/* watch */
	{ 31, SIGSEGV },		/* virtual data cache coherency */
	{ 0, 0}				/* Must be last */
};

/* Save the normal trap handlers for user-mode traps. */
void *saved_vectors[32];

/*
 * Set up exception handlers for tracing and breakpoints
 */
void set_debug_traps(void)
{
	struct hard_trap_info *ht;
	unsigned long flags;
	unsigned char c;

	local_irq_save(flags);
	for (ht = hard_trap_info; ht->tt && ht->signo; ht++)
		saved_vectors[ht->tt] = set_except_vector(ht->tt, trap_low);

	putDebugChar('+'); /* 'hello world' */
	/*
	 * In case GDB is started before us, ack any packets
	 * (presumably "$?#xx") sitting there.
	 */
	while((c = getDebugChar()) != '$');
	while((c = getDebugChar()) != '#');
	c = getDebugChar(); /* eat first csum byte */
	c = getDebugChar(); /* eat second csum byte */
	putDebugChar('+'); /* ack it */

	initialized = 1;
	local_irq_restore(flags);
}

void restore_debug_traps(void)
{
	struct hard_trap_info *ht;
	unsigned long flags;

	local_irq_save(flags);
	for (ht = hard_trap_info; ht->tt && ht->signo; ht++)
		set_except_vector(ht->tt, saved_vectors[ht->tt]);
	local_irq_restore(flags);
}

/*
 * Convert the MIPS hardware trap type code to a Unix signal number.
 */
static int computeSignal(int tt)
{
	struct hard_trap_info *ht;

	for (ht = hard_trap_info; ht->tt && ht->signo; ht++)
		if (ht->tt == tt)
			return ht->signo;

	return SIGHUP;		/* default for things we don't know about */
}

/*
 * While we find nice hex chars, build an int.
 * Return number of chars processed.
 */
static int hexToInt(char **ptr, int *intValue)
{
	int numChars = 0;
	int hexValue;

	*intValue = 0;

	while (**ptr) {
		hexValue = hex(**ptr);
		if (hexValue < 0)
			break;

		*intValue = (*intValue << 4) | hexValue;
		numChars ++;

		(*ptr)++;
	}

	return (numChars);
}

static int hexToLong(char **ptr, long *longValue)
{
	int numChars = 0;
	int hexValue;

	*longValue = 0;

	while (**ptr) {
		hexValue = hex(**ptr);
		if (hexValue < 0)
			break;

		*longValue = (*longValue << 4) | hexValue;
		numChars ++;

		(*ptr)++;
	}

	return numChars;
}


#if 0
/*
 * Print registers (on target console)
 * Used only to debug the stub...
 */
void show_gdbregs(struct gdb_regs * regs)
{
	/*
	 * Saved main processor registers
	 */
	printk("$0 : %08lx %08lx %08lx %08lx %08lx %08lx %08lx %08lx\n",
	       regs->reg0, regs->reg1, regs->reg2, regs->reg3,
	       regs->reg4, regs->reg5, regs->reg6, regs->reg7);
	printk("$8 : %08lx %08lx %08lx %08lx %08lx %08lx %08lx %08lx\n",
	       regs->reg8, regs->reg9, regs->reg10, regs->reg11,
	       regs->reg12, regs->reg13, regs->reg14, regs->reg15);
	printk("$16: %08lx %08lx %08lx %08lx %08lx %08lx %08lx %08lx\n",
	       regs->reg16, regs->reg17, regs->reg18, regs->reg19,
	       regs->reg20, regs->reg21, regs->reg22, regs->reg23);
	printk("$24: %08lx %08lx %08lx %08lx %08lx %08lx %08lx %08lx\n",
	       regs->reg24, regs->reg25, regs->reg26, regs->reg27,
	       regs->reg28, regs->reg29, regs->reg30, regs->reg31);

	/*
	 * Saved cp0 registers
	 */
	printk("epc  : %08lx\nStatus: %08lx\nCause : %08lx\n",
	       regs->cp0_epc, regs->cp0_status, regs->cp0_cause);
}
#endif /* dead code */

/*
 * We single-step by setting breakpoints. When an exception
 * is handled, we need to restore the instructions hoisted
 * when the breakpoints were set.
 *
 * This is where we save the original instructions.
 */
static struct gdb_bp_save {
	unsigned long addr;
	unsigned int val;
} step_bp[2];

#define BP 0x0000000d  /* break opcode */

/*
 * Set breakpoint instructions for single stepping.
 */
static void single_step(struct gdb_regs *regs)
{
	union mips_instruction insn;
	unsigned long targ;
	int is_branch, is_cond, i;

	targ = regs->cp0_epc;
	insn.word = *(unsigned int *)targ;
	is_branch = is_cond = 0;

	switch (insn.i_format.opcode) {
	/*
	 * jr and jalr are in r_format format.
	 */
	case spec_op:
		switch (insn.r_format.func) {
		case jalr_op:
		case jr_op:
			targ = *(&regs->reg0 + insn.r_format.rs);
			is_branch = 1;
			break;
		}
		break;

	/*
	 * This group contains:
	 * bltz_op, bgez_op, bltzl_op, bgezl_op,
	 * bltzal_op, bgezal_op, bltzall_op, bgezall_op.
	 */
	case bcond_op:
		is_branch = is_cond = 1;
		targ += 4 + (insn.i_format.simmediate << 2);
		break;

	/*
	 * These are unconditional and in j_format.
	 */
	case jal_op:
	case j_op:
		is_branch = 1;
		targ += 4;
		targ >>= 28;
		targ <<= 28;
		targ |= (insn.j_format.target << 2);
		break;

	/*
	 * These are conditional.
	 */
	case beq_op:
	case beql_op:
	case bne_op:
	case bnel_op:
	case blez_op:
	case blezl_op:
	case bgtz_op:
	case bgtzl_op:
	case cop0_op:
	case cop1_op:
	case cop2_op:
	case cop1x_op:
		is_branch = is_cond = 1;
		targ += 4 + (insn.i_format.simmediate << 2);
		break;
	}

	if (is_branch) {
		i = 0;
		if (is_cond && targ != (regs->cp0_epc + 8)) {
			step_bp[i].addr = regs->cp0_epc + 8;
			step_bp[i++].val = *(unsigned *)(regs->cp0_epc + 8);
			*(unsigned *)(regs->cp0_epc + 8) = BP;
		}
		step_bp[i].addr = targ;
		step_bp[i].val  = *(unsigned *)targ;
		*(unsigned *)targ = BP;
	} else {
		step_bp[0].addr = regs->cp0_epc + 4;
		step_bp[0].val  = *(unsigned *)(regs->cp0_epc + 4);
		*(unsigned *)(regs->cp0_epc + 4) = BP;
	}
}

/*
 *  If asynchronously interrupted by gdb, then we need to set a breakpoint
 *  at the interrupted instruction so that we wind up stopped with a
 *  reasonable stack frame.
 */
static struct gdb_bp_save async_bp;

/*
 * Swap the interrupted EPC with our asynchronous breakpoint routine.
 * This is safer than stuffing the breakpoint in-place, since no cache
 * flushes (or resulting smp_call_functions) are required.  The
 * assumption is that only one CPU will be handling asynchronous bp's,
 * and only one can be active at a time.
 */
extern spinlock_t smp_call_lock;

void set_async_breakpoint(unsigned long *epc)
{
	/* skip breaking into userland */
	if ((*epc & 0x80000000) == 0)
		return;

#ifdef CONFIG_SMP
	/* avoid deadlock if someone is make IPC */
	if (spin_is_locked(&smp_call_lock))
		return;
#endif

	async_bp.addr = *epc;
	*epc = (unsigned long)async_breakpoint;
}

static void kgdb_wait(void *arg)
{
	unsigned flags;
	int cpu = smp_processor_id();

	local_irq_save(flags);

	__raw_spin_lock(&kgdb_cpulock[cpu]);
	__raw_spin_unlock(&kgdb_cpulock[cpu]);

	local_irq_restore(flags);
}

/*
 * GDB stub needs to call kgdb_wait on all processor with interrupts
 * disabled, so it uses it's own special variant.
 */
static int kgdb_smp_call_kgdb_wait(void)
{
#ifdef CONFIG_SMP
	cpumask_t mask = cpu_online_map;
	struct call_data_struct data;
	int cpu = smp_processor_id();
	int cpus;

	/*
	 * Can die spectacularly if this CPU isn't yet marked online
	 */
	BUG_ON(!cpu_online(cpu));

	cpu_clear(cpu, mask);
	cpus = cpus_weight(mask);
	if (!cpus)
		return 0;

	if (spin_is_locked(&smp_call_lock)) {
		/*
		 * Some other processor is trying to make us do something
		 * but we're not going to respond... give up
		 */
		return -1;
		}

	/*
	 * We will continue here, accepting the fact that
	 * the kernel may deadlock if another CPU attempts
	 * to call smp_call_function now...
	 */

	data.func = kgdb_wait;
	data.info = NULL;
	atomic_set(&data.started, 0);
	data.wait = 0;

	spin_lock(&smp_call_lock);
	call_data = &data;
	mb();

	core_send_ipi_mask(mask, SMP_CALL_FUNCTION);

	/* Wait for response */
	/* FIXME: lock-up detection, backtrace on lock-up */
	while (atomic_read(&data.started) != cpus)
		barrier();

	call_data = NULL;
	spin_unlock(&smp_call_lock);
#endif

	return 0;
}

/*
 * This function does all command processing for interfacing to gdb.  It
 * returns 1 if you should skip the instruction at the trap address, 0
 * otherwise.
 */
void handle_exception(struct gdb_regs *regs)
{
	int trap;			/* Trap type */
	int sigval;
	long addr;
	int length;
	char *ptr;
	unsigned long *stack;
	int i;
	int bflag = 0;

	kgdb_started = 1;

	/*
	 * acquire the big kgdb spinlock
	 */
	if (!spin_trylock(&kgdb_lock)) {
		/*
		 * some other CPU has the lock, we should go back to
		 * receive the gdb_wait IPC
		 */
		return;
	}

	/*
	 * If we're in async_breakpoint(), restore the real EPC from
	 * the breakpoint.
	 */
	if (regs->cp0_epc == (unsigned long)async_breakinst) {
		regs->cp0_epc = async_bp.addr;
		async_bp.addr = 0;
	}

	/*
	 * acquire the CPU spinlocks
	 */
	for_each_online_cpu(i)
		if (__raw_spin_trylock(&kgdb_cpulock[i]) == 0)
			panic("kgdb: couldn't get cpulock %d\n", i);

	/*
	 * force other cpus to enter kgdb
	 */
	kgdb_smp_call_kgdb_wait();

	/*
	 * If we're in breakpoint() increment the PC
	 */
	trap = (regs->cp0_cause & 0x7c) >> 2;
	if (trap == 9 && regs->cp0_epc == (unsigned long)breakinst)
		regs->cp0_epc += 4;

	/*
	 * If we were single_stepping, restore the opcodes hoisted
	 * for the breakpoint[s].
	 */
	if (step_bp[0].addr) {
		*(unsigned *)step_bp[0].addr = step_bp[0].val;
		step_bp[0].addr = 0;

		if (step_bp[1].addr) {
			*(unsigned *)step_bp[1].addr = step_bp[1].val;
			step_bp[1].addr = 0;
		}
	}

	stack = (long *)regs->reg29;			/* stack ptr */
	sigval = computeSignal(trap);

	/*
	 * reply to host that an exception has occurred
	 */
	ptr = output_buffer;

	/*
	 * Send trap type (converted to signal)
	 */
	*ptr++ = 'T';
	*ptr++ = hexchars[sigval >> 4];
	*ptr++ = hexchars[sigval & 0xf];

	/*
	 * Send Error PC
	 */
	*ptr++ = hexchars[REG_EPC >> 4];
	*ptr++ = hexchars[REG_EPC & 0xf];
	*ptr++ = ':';
	ptr = mem2hex((char *)&regs->cp0_epc, ptr, sizeof(long), 0);
	*ptr++ = ';';

	/*
	 * Send frame pointer
	 */
	*ptr++ = hexchars[REG_FP >> 4];
	*ptr++ = hexchars[REG_FP & 0xf];
	*ptr++ = ':';
	ptr = mem2hex((char *)&regs->reg30, ptr, sizeof(long), 0);
	*ptr++ = ';';

	/*
	 * Send stack pointer
	 */
	*ptr++ = hexchars[REG_SP >> 4];
	*ptr++ = hexchars[REG_SP & 0xf];
	*ptr++ = ':';
	ptr = mem2hex((char *)&regs->reg29, ptr, sizeof(long), 0);
	*ptr++ = ';';

	*ptr++ = 0;
	putpacket(output_buffer);	/* send it off... */

	/*
	 * Wait for input from remote GDB
	 */
	while (1) {
		output_buffer[0] = 0;
		getpacket(input_buffer);

		switch (input_buffer[0])
		{
		case '?':
			output_buffer[0] = 'S';
			output_buffer[1] = hexchars[sigval >> 4];
			output_buffer[2] = hexchars[sigval & 0xf];
			output_buffer[3] = 0;
			break;

		/*
		 * Detach debugger; let CPU run
		 */
		case 'D':
			putpacket(output_buffer);
			goto finish_kgdb;
			break;

		case 'd':
			/* toggle debug flag */
			break;

		/*
		 * Return the value of the CPU registers
		 */
		case 'g':
			ptr = output_buffer;
			ptr = mem2hex((char *)&regs->reg0, ptr, 32*sizeof(long), 0); /* r0...r31 */
			ptr = mem2hex((char *)&regs->cp0_status, ptr, 6*sizeof(long), 0); /* cp0 */
			ptr = mem2hex((char *)&regs->fpr0, ptr, 32*sizeof(long), 0); /* f0...31 */
			ptr = mem2hex((char *)&regs->cp1_fsr, ptr, 2*sizeof(long), 0); /* cp1 */
			ptr = mem2hex((char *)&regs->frame_ptr, ptr, 2*sizeof(long), 0); /* frp */
			ptr = mem2hex((char *)&regs->cp0_index, ptr, 16*sizeof(long), 0); /* cp0 */
			break;

		/*
		 * set the value of the CPU registers - return OK
		 */
		case 'G':
		{
			ptr = &input_buffer[1];
			hex2mem(ptr, (char *)&regs->reg0, 32*sizeof(long), 0, 0);
			ptr += 32*(2*sizeof(long));
			hex2mem(ptr, (char *)&regs->cp0_status, 6*sizeof(long), 0, 0);
			ptr += 6*(2*sizeof(long));
			hex2mem(ptr, (char *)&regs->fpr0, 32*sizeof(long), 0, 0);
			ptr += 32*(2*sizeof(long));
			hex2mem(ptr, (char *)&regs->cp1_fsr, 2*sizeof(long), 0, 0);
			ptr += 2*(2*sizeof(long));
			hex2mem(ptr, (char *)&regs->frame_ptr, 2*sizeof(long), 0, 0);
			ptr += 2*(2*sizeof(long));
			hex2mem(ptr, (char *)&regs->cp0_index, 16*sizeof(long), 0, 0);
			strcpy(output_buffer, "OK");
		 }
		break;

		/*
		 * mAA..AA,LLLL  Read LLLL bytes at address AA..AA
		 */
		case 'm':
			ptr = &input_buffer[1];

			if (hexToLong(&ptr, &addr)
				&& *ptr++ == ','
				&& hexToInt(&ptr, &length)) {
				if (mem2hex((char *)addr, output_buffer, length, 1))
					break;
				strcpy(output_buffer, "E03");
			} else
				strcpy(output_buffer, "E01");
			break;

		/*
		 * XAA..AA,LLLL: Write LLLL escaped binary bytes at address AA.AA
		 */
		case 'X':
			bflag = 1;
			/* fall through */

		/*
		 * MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK
		 */
		case 'M':
			ptr = &input_buffer[1];

			if (hexToLong(&ptr, &addr)
				&& *ptr++ == ','
				&& hexToInt(&ptr, &length)
				&& *ptr++ == ':') {
				if (hex2mem(ptr, (char *)addr, length, bflag, 1))
					strcpy(output_buffer, "OK");
				else
					strcpy(output_buffer, "E03");
			}
			else
				strcpy(output_buffer, "E02");
			break;

		/*
		 * cAA..AA    Continue at address AA..AA(optional)
		 */
		case 'c':
			/* try to read optional parameter, pc unchanged if no parm */

			ptr = &input_buffer[1];
			if (hexToLong(&ptr, &addr))
				regs->cp0_epc = addr;

			goto exit_kgdb_exception;
			break;

		/*
		 * kill the program; let us try to restart the machine
		 * Reset the whole machine.
		 */
		case 'k':
		case 'r':
			machine_restart("kgdb restarts machine");
			break;

		/*
		 * Step to next instruction
		 */
		case 's':
			/*
			 * There is no single step insn in the MIPS ISA, so we
			 * use breakpoints and continue, instead.
			 */
			single_step(regs);
			goto exit_kgdb_exception;
			/* NOTREACHED */
			break;

		/*
		 * Set baud rate (bBB)
		 * FIXME: Needs to be written
		 */
		case 'b':
		{
#if 0
			int baudrate;
			extern void set_timer_3();

			ptr = &input_buffer[1];
			if (!hexToInt(&ptr, &baudrate))
			{
				strcpy(output_buffer, "B01");
				break;
			}

			/* Convert baud rate to uart clock divider */

			switch (baudrate)
			{
				case 38400:
					baudrate = 16;
					break;
				case 19200:
					baudrate = 33;
					break;
				case 9600:
					baudrate = 65;
					break;
				default:
					baudrate = 0;
					strcpy(output_buffer, "B02");
					goto x1;
			}

			if (baudrate) {
				putpacket("OK");	/* Ack before changing speed */
				set_timer_3(baudrate); /* Set it */
			}
#endif
		}
		break;

		}			/* switch */

		/*
		 * reply to the request
		 */

		putpacket(output_buffer);

	} /* while */

	return;

finish_kgdb:
	restore_debug_traps();

exit_kgdb_exception:
	/* release locks so other CPUs can go */
	for_each_online_cpu(i)
		__raw_spin_unlock(&kgdb_cpulock[i]);
	spin_unlock(&kgdb_lock);

	__flush_cache_all();
	return;
}

/*
 * This function will generate a breakpoint exception.  It is used at the
 * beginning of a program to sync up with a debugger and can be used
 * otherwise as a quick means to stop program execution and "break" into
 * the debugger.
 */
void breakpoint(void)
{
	if (!initialized)
		return;

	__asm__ __volatile__(
			".globl	breakinst\n\t"
			".set\tnoreorder\n\t"
			"nop\n"
			"breakinst:\tbreak\n\t"
			"nop\n\t"
			".set\treorder"
			);
}

/* Nothing but the break; don't pollute any registers */
void async_breakpoint(void)
{
	__asm__ __volatile__(
			".globl	async_breakinst\n\t"
			".set\tnoreorder\n\t"
			"nop\n"
			"async_breakinst:\tbreak\n\t"
			"nop\n\t"
			".set\treorder"
			);
}

void adel(void)
{
	__asm__ __volatile__(
			".globl\tadel\n\t"
			"lui\t$8,0x8000\n\t"
			"lw\t$9,1($8)\n\t"
			);
}

/*
 * malloc is needed by gdb client in "call func()", even a private one
 * will make gdb happy
 */
static void __used *malloc(size_t size)
{
	return kmalloc(size, GFP_ATOMIC);
}

static void __used free(void *where)
{
	kfree(where);
}

#ifdef CONFIG_GDB_CONSOLE

void gdb_putsn(const char *str, int l)
{
	char outbuf[18];

	if (!kgdb_started)
		return;

	outbuf[0]='O';

	while(l) {
		int i = (l>8)?8:l;
		mem2hex((char *)str, &outbuf[1], i, 0);
		outbuf[(i*2)+1]=0;
		putpacket(outbuf);
		str += i;
		l -= i;
	}
}

static void gdb_console_write(struct console *con, const char *s, unsigned n)
{
	gdb_putsn(s, n);
}

static struct console gdb_console = {
	.name	= "gdb",
	.write	= gdb_console_write,
	.flags	= CON_PRINTBUFFER,
	.index	= -1
};

static int __init register_gdb_console(void)
{
	register_console(&gdb_console);

	return 0;
}

console_initcall(register_gdb_console);

#endif