Subversion Repositories openrisc_2011-10-31

// ----------------------------------------------------------------------------
 
// SystemC GDB RSP server: implementation
 
// Copyright (C) 2008  Embecosm Limited <info@embecosm.com>
 
// Contributor Jeremy Bennett <jeremy.bennett@embecosm.com>
// Contributor Julius Baxter <julius@orsoc.se>
 
// This file is part of the GDB interface to the cycle accurate model of the
// OpenRISC 1000 based system-on-chip, ORPSoC, built using Verilator.
 
// This program is free software: you can redistribute it and/or modify it
// under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or (at your
// option) any later version.
 
// This program is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
// License for more details.
 
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
// ----------------------------------------------------------------------------
 
// $Id$
 
#include <iostream>
#include <iomanip>
 
#include "GdbServerSC.h"
#include "Utils.h"
 
#include <errno.h>
#include <fcntl.h>
extern int monitor_to_gdb_pipe[2][2];	// [0][] - monitor to gdb, [1][] - gdb to monitor, [][0] - read, [][1] - write
 
using std::cerr;
using std::dec;
using std::endl;
using std::hex;
 
using sc_core::sc_fifo;
using sc_core::sc_module_name;
using sc_core::sc_stop;
using sc_core::sc_time;
 
SC_HAS_PROCESS(GdbServerSC);
 
//-----------------------------------------------------------------------------
//! Constructor for the GDB RSP server.
 
//! We create a SC_THREAD which will act as the listener. Must be a
//! thread, since we need to wait for the actions to complete.
 
//! The current scan chain is marked as undefined.
 
//! This makes use of the Embecosm cycle accurate SystemC JTAG interface.
 
//! @see Embecosm Application Note 5 "Using JTAG with SystemC: Implementation
//!      of a Cycle Accurate Interface"
//!      (http://www.embecosm.com/download/ean5.html)
 
//! @todo We do not handle a user coded l.trap properly (i.e. one that is not
//!       a breakpoint). Effectively it is ignored, whereas we ought to set up
//!       the exception registers and redirect through the trap vector.
 
//! @param[in] name             Name of this module, passed to the parent
//!                             constructor. 
//! @param[in] _tapActionQueue  Pointer to fifo of actions to be performed by
//!                             the JTAG interface
//-----------------------------------------------------------------------------
GdbServerSC::GdbServerSC(sc_module_name name,
			 uint32_t _flashStart,
			 uint32_t _flashEnd,
			 int rspPort,
			 sc_fifo <
			 TapAction * >*tapActionQueue):sc_module(name),
flashStart(_flashStart), flashEnd(_flashEnd)
{
	pkt = new RspPacket(RSP_PKT_MAX);
	rsp = new RspConnection(rspPort);
	debugUnit = new DebugUnitSC("debug-unit", tapActionQueue);
	mpHash = new MpHash();
 
	/* Setup the pipes between or1200 monitor module and GDB stub */
	pipe(monitor_to_gdb_pipe[0]);
	pipe(monitor_to_gdb_pipe[1]);
 
	// Set non-blocking reads
#ifdef O_NONBLOCK		/* The POSIX way */
	fcntl(monitor_to_gdb_pipe[0][0], F_SETFL, O_NONBLOCK);
	fcntl(monitor_to_gdb_pipe[1][0], F_SETFL, O_NONBLOCK);
#elif defined (O_NDELAY)
	fcntl(monitor_to_gdb_pipe[0][0], F_SETFL, O_NDELAY);
	fcntl(monitor_to_gdb_pipe[1][0], F_SETFL, O_NDELAY);
#endif /* O_NONBLOCK */
 
	SC_THREAD(rspServer);
 
}				// GdbServerSC ()
 
//-----------------------------------------------------------------------------
//! Destructor
 
//! Free up data structures
//-----------------------------------------------------------------------------
GdbServerSC::~GdbServerSC()
{
	delete mpHash;
	delete debugUnit;
	delete rsp;
	delete pkt;
 
}				// ~GdbServerSC
 
//-----------------------------------------------------------------------------
//! SystemC thread to listen for RSP requests
 
//! JTAG actions will be queued as appropriate. Runs forever
 
//! We don't allow any actions until the target is out of its startup mode
//! (where the ROM is mapped into RAM space). This is determined by seeing if
//! the PPC is still in flash memory.
 
//! Have to use a thread, since we will end up waiting for actions to
//! complete.
//-----------------------------------------------------------------------------
void
 GdbServerSC::rspServer()
{
	// Reset the debug unit, and wait for ORPSoC to be ready, by noting when it
	// accesses an address outside of flash. Note that we use NPC, not PPC since
	// at reset PPC is zero, and would trigger a false positive.
 
	debugUnit->resetDebugUnit();
	rsp_sigval = TARGET_SIGNAL_NONE;
	/*
	   uint32_t npc;
	   do
	   {
	   npc = debugUnit->readSpr (SPR_NPC);      
	   }
	   while ((flashStart <= npc) && (npc <= flashEnd));
	 */
	debugUnit->stall();
	targetStopped = true;
 
	// Make sure we are connected.
	while (!rsp->isConnected()) {
		// Reconnect and stall the processor on a new connection
		if (!rsp->rspConnect()) {
			// Serious failure. Must abort execution.
			cerr << "*** Unable to continue: ABORTING" << endl;
			sc_stop();
		}
		// Stall the processor until we get a command to handle.
		if (!debugUnit->isStalled()) {
			debugUnit->stall();
		}
 
		targetStopped = true;	// Processor now not running
	}
 
	// Loop processing commands forever
	while (true) {
		if (!rsp->isConnected()) {
			sc_stop();
			return;
		}
		// Wait until the target has stopped. In this simplified implementation,
		// the only reasons for stopping are hitting a breakpoint (l.trap),
		// hardware single stepping or hitting a non-breakpoint l.trap. This
		// last is not cleanly handled at the moment (we ought to redirect the
		// restart through the trap exception vector).
		while (!targetStopped) {
 
			/* First check to see if the or1200 monitor module wants us to stall */
			if (checkMonitorPipe())
				break;
 
			rspCheckForException();
 
			if (debugUnit->isStalled()) {
				targetStopped = true;
 
				// If it's a breakpoint, then we need to back up one
				// instruction, so on restart we execute the actual
				// instruction.
				uint32_t ppc = debugUnit->readSpr(SPR_PPC);
 
				if (NULL != mpHash->lookup(BP_MEMORY, ppc)
				    && rsp_sigval == TARGET_SIGNAL_TRAP) {
					writeNpc(ppc);
				}
				// Tell the client we've stopped.
				rspReportException();
			} else if (rsp->rspSocketPeek() > 0) {
				if (rsp->rspSocketPeek() == 0x03)	// ETX, end of text control char
				{
					// Got an interrupt command from GDB, this function should
					// pull the packet off the socket and stall the processor.
					// and then send a stop reply packet with signal 
					// TARGET_SIGNAL_NONE.
					rspInterrupt();
					targetStopped = true;	// Processor now not running
				}
			}
		}
 
		// Get a RSP client request
		rspClientRequest();
	}
}				// rspServer ()
 
//-----------------------------------------------------------------------------
//! Deal with a request from the GDB client session
 
//! In general, apart from the simplest requests, this function replies on
//! other functions to implement the functionality.
 
//! @note It is the responsibility of the recipient to delete the packet when
//!       it is finished with. It is permissible to reuse the packet for a
//!       reply.
 
//! @todo Is this the implementation of the 'D' packet really the intended
//!       meaning? Or does it just mean that only vAttach will be recognized
//!       after this?
 
//! @param[in] pkt  The received RSP packet
//-----------------------------------------------------------------------------
void GdbServerSC::rspClientRequest()
{
	if (!rsp->getPkt(pkt)) {
		rsp->rspClose();	// Comms failure
		return;
	}
	//Uncomment the next line for the RSP client to print out every packet it gets from GDB
	//cerr << "rspClientRequest: " << pkt->data/*[0]*/ << endl;
	switch (pkt->data[0]) {
	case '!':
		// Request for extended remote mode
		pkt->packStr("OK");
		rsp->putPkt(pkt);
		return;
 
	case '?':
		// Return last signal ID
		rspReportException();
		return;
 
	case 'A':
		// Initialization of argv not supported
		cerr << "Warning: RSP 'A' packet not supported: ignored" <<
		    endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
 
	case 'b':
		// Setting baud rate is deprecated
		cerr << "Warning: RSP 'b' packet is deprecated and not "
		    << "supported: ignored" << endl;
		return;
 
	case 'B':
		// Breakpoints should be set using Z packets
		cerr << "Warning: RSP 'B' packet is deprecated (use 'Z'/'z' "
		    << "packets instead): ignored" << endl;
		return;
 
	case 'c':
		// Continue
		rspContinue(EXCEPT_NONE);
		return;
 
	case 'C':
		// Continue with signal (in the packet)
		rspContinue();
		return;
 
	case 'd':
		// Disable debug using a general query
		cerr << "Warning: RSP 'd' packet is deprecated (define a 'Q' "
		    << "packet instead: ignored" << endl;
		return;
 
	case 'D':
		// Detach GDB. Do this by closing the client. The rules say that
		// execution should continue, so unstall the processor.
		pkt->packStr("OK");
		rsp->putPkt(pkt);
		rsp->rspClose();
		//debugUnit->unstall ();      
		return;
 
	case 'F':
		// File I/O is not currently supported
		cerr << "Warning: RSP file I/O not currently supported: 'F' "
		    << "packet ignored" << endl;
		return;
 
	case 'g':
		rspReadAllRegs();
		return;
 
	case 'G':
		rspWriteAllRegs();
		return;
 
	case 'H':
		// Set the thread number of subsequent operations. For now ignore
		// silently and just reply "OK"
		pkt->packStr("OK");
		rsp->putPkt(pkt);
		return;
 
	case 'i':
	case 'I':
		// Single cycle step not currently supported. Mark the target as
		// running, so that next time it will be detected as stopped (it is
		// still stalled in reality) and an ack sent back to the client.
		cerr << "Warning: RSP cycle stepping not supported: target "
		    << "stopped immediately" << endl;
		targetStopped = false;
		return;
 
	case 'k':
		// Kill request. Do nothing for now.
		return;
 
	case 'm':
		// Read memory (symbolic)
		rspReadMem();
		return;
 
	case 'M':
		// Write memory (symbolic)
		rspWriteMem();
		return;
 
	case 'p':
		// Read a register
		rspReadReg();
		return;
 
	case 'P':
		// Write a register
		rspWriteReg();
		return;
 
	case 'q':
		// Any one of a number of query packets
		rspQuery();
		return;
 
	case 'Q':
		// Any one of a number of set packets
		rspSet();
		return;
 
	case 'r':
		// Reset the system. Deprecated (use 'R' instead)
		cerr << "Warning: RSP 'r' packet is deprecated (use 'R' "
		    << "packet instead): ignored" << endl;
		return;
 
	case 'R':
		// Restart the program being debugged.
		rspRestart();
		return;
 
	case 's':
		// Single step one machine instruction.
		rspStep(EXCEPT_NONE);
		return;
 
	case 'S':
		// Single step one machine instruction.
		rspStep();
		return;
 
	case 't':
		// Search. This is not well defined in the manual and for now we don't
		// support it. No response is defined.
		cerr << "Warning: RSP 't' packet not supported: ignored"
		    << endl;
		return;
 
	case 'T':
		// Is the thread alive. We are bare metal, so don't have a thread
		// context. The answer is always "OK".
		pkt->packStr("OK");
		rsp->putPkt(pkt);
		return;
 
	case 'v':
		// Any one of a number of packets to control execution
		rspVpkt();
		return;
 
	case 'X':
		// Write memory (binary)
		rspWriteMemBin();
		return;
 
	case 'z':
		// Remove a breakpoint/watchpoint.
		rspRemoveMatchpoint();
		return;
 
	case 'Z':
		// Insert a breakpoint/watchpoint.
		rspInsertMatchpoint();
		return;
 
	default:
		// Unknown commands are ignored
		cerr << "Warning: Unknown RSP request" << pkt->data << endl;
		return;
	}
}				// rspClientRequest ()
 
//-----------------------------------------------------------------------------
//! Check if processor is stalled. If it is, read the DRR and return the
//! target signal code.
//-----------------------------------------------------------------------------
void GdbServerSC::rspCheckForException()
{
 
	uint32_t drr;
 
	if (!debugUnit->isStalled()) {
		// Processor not stalled. Just return;
		return;
	}
 
	switch ((debugUnit->readSpr(SPR_DRR) & 0xffffffff)) {
	case SPR_DRR_RSTE:
		rsp_sigval = TARGET_SIGNAL_PWR;
		break;
	case SPR_DRR_BUSEE:
		rsp_sigval = TARGET_SIGNAL_BUS;
		break;
	case SPR_DRR_DPFE:
		rsp_sigval = TARGET_SIGNAL_SEGV;
		break;
	case SPR_DRR_IPFE:
		rsp_sigval = TARGET_SIGNAL_SEGV;
		break;
	case SPR_DRR_TTE:
		rsp_sigval = TARGET_SIGNAL_ALRM;
		break;
	case SPR_DRR_AE:
		rsp_sigval = TARGET_SIGNAL_BUS;
		break;
	case SPR_DRR_IIE:
		rsp_sigval = TARGET_SIGNAL_ILL;
		break;
	case SPR_DRR_IE:
		rsp_sigval = TARGET_SIGNAL_INT;
		break;
	case SPR_DRR_DME:
		rsp_sigval = TARGET_SIGNAL_SEGV;
		break;
	case SPR_DRR_IME:
		rsp_sigval = TARGET_SIGNAL_SEGV;
		break;
	case SPR_DRR_RE:
		rsp_sigval = TARGET_SIGNAL_FPE;
		break;
	case SPR_DRR_SCE:
		rsp_sigval = TARGET_SIGNAL_USR2;
		break;
	case SPR_DRR_FPE:
		rsp_sigval = TARGET_SIGNAL_FPE;
		break;
	case SPR_DRR_TE:
		rsp_sigval = TARGET_SIGNAL_TRAP;
		break;
 
	default:
		// This must be the case of single step (which does not set DRR)
		rsp_sigval = TARGET_SIGNAL_TRAP;
		break;
	}
 
	return;
}
 
//-----------------------------------------------------------------------------
//! Send a packet acknowledging an exception has occurred
 
//! The only signal we ever see in this implementation is TRAP.
//-----------------------------------------------------------------------------
void GdbServerSC::rspReportException()
{
	// Construct a signal received packet
	pkt->data[0] = 'S';
	pkt->data[1] = Utils::hex2Char(rsp_sigval >> 4);
	pkt->data[2] = Utils::hex2Char(rsp_sigval % 16);
	pkt->data[3] = '\0';
	pkt->setLen(strlen(pkt->data));
 
	rsp->putPkt(pkt);
 
}				// rspReportException ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP continue request
 
//! This version is typically used for the 'c' packet, to continue without
//! signal, in which case EXCEPT_NONE is passed in as the exception to use.
 
//! At present other exceptions are not supported
 
//! @param[in] except  The OpenRISC 1000 exception to use
//-----------------------------------------------------------------------------
void GdbServerSC::rspContinue(uint32_t except)
{
	uint32_t addr;		// Address to continue from, if any
 
	// Reject all except 'c' packets
	if ('c' != pkt->data[0]) {
		cerr <<
		    "Warning: Continue with signal not currently supported: " <<
		    "ignored" << endl;
		return;
	}
	// Get an address if we have one
	if (0 == strcmp("c", pkt->data)) {
		addr = readNpc();	// Default uses current NPC
	} else if (1 != sscanf(pkt->data, "c%lx", &addr)) {
		cerr << "Warning: RSP continue address " << pkt->data
		    << " not recognized: ignored" << endl;
		addr = readNpc();	// Default uses current NPC
	}
 
	rspContinue(addr, EXCEPT_NONE);
 
}				// rspContinue ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP continue with signal request
 
//! @todo Currently does nothing. Will use the underlying generic continue
//!       function. 
//-----------------------------------------------------------------------------
void GdbServerSC::rspContinue()
{
	cerr << "RSP continue with signal '" << pkt->data
	    << "' received" << endl;
 
}				// rspContinue ()
 
//-----------------------------------------------------------------------------
//! Generic processing of a continue request
 
//! The signal may be EXCEPT_NONE if there is no exception to be
//! handled. Currently the exception is ignored.
 
//! The single step flag is cleared in the debug registers and then the
//! processor is unstalled.
 
//! @param[in] addr    Address from which to step
//! @param[in] except  The exception to use (if any)                         
//-----------------------------------------------------------------------------
void GdbServerSC::rspContinue(uint32_t addr, uint32_t except)
{
	// Set the address as the value of the next program counter
	writeNpc(addr);
 
	/*
	   // If we're continuing from a breakpoint, replace that instruction in the memory
	   // ... actually no, I was wrong about this.
	   if (NULL != mpHash->lookup (BP_MEMORY, addr) && rsp_sigval == TARGET_SIGNAL_TRAP)
	   {
	   MpEntry *entry = mpHash->lookup (BP_MEMORY, addr);
	   debugUnit->writeMem32(entry->addr, entry->instr);
	   }
	 */
 
	// Clear Debug Reason Register and watchpoint break generation in Debug Mode
	// Register 2 for watchpoints that we triggered to stop this time.
	debugUnit->writeSpr(SPR_DRR, 0);
	if (rsp_sigval == TARGET_SIGNAL_TRAP) {
		/* 
		   Disable last trap generation on watchpoint if this is why we stopped
		   last time.
		 */
		uint32_t temp_dmr2 = debugUnit->readSpr(SPR_DMR2);
		if (temp_dmr2 & SPR_DMR2_WBS) {
			/* 
			   One of these breakpoints is responsible for our stopping, so
			   disable it this time we start. GDB will send a packet re-enabling
			   it next time we continue.
			 */
			debugUnit->writeSpr(SPR_DMR2,
					    temp_dmr2 &
					    ~((temp_dmr2 & SPR_DMR2_WBS) >>
					      10));
		}
	}
	// Clear the single step trigger in Debug Mode Register 1 and set traps to
	// be handled by the debug unit in the Debug Stop Register
	debugUnit->andSpr(SPR_DMR1, ~SPR_DMR1_ST);
	debugUnit->orSpr(SPR_DSR, SPR_DSR_TE);
 
	// Unstall the processor. Note the GDB client is now waiting for a reply,
	// which it will get as soon as the processor stalls again.
	debugUnit->unstall();
	targetStopped = false;
 
}				// rspContinue ()
 
//------------------------------------------------------------------------------
//!Handle an interrupt from GDB
 
//! Detect an interrupt from GDB and stall the processor                        
//------------------------------------------------------------------------------
void GdbServerSC::rspInterrupt()
{
	unsigned char c;
 
	c = rsp->getRspChar();
	if (c < 0) {
		// Had issues, just return
		return;
	}
	// Ensure this is a ETX control char (0x3), currently, we only call this
	// function when we've peeked and seen it, otherwise, ignore, return and pray
	// things go back to normal...
	if (c != 0x03) {
		cerr <<
		    "* Warning: Interrupt character expected but not found on socket"
		    << endl;
		return;
	}
	// Otherwise, it's an interrupt packet, stall the processor, and upon return
	// to the main handle_rsp() loop, it will inform GDB.
 
	debugUnit->stall();
 
	// Send a stop reply response, manually set rsp.sigval to TARGET_SIGNAL_NONE
	rsp_sigval = TARGET_SIGNAL_NONE;
	rspReportException();
 
	return;
 
}
 
//-----------------------------------------------------------------------------
//! Handle a RSP read all registers request
 
//! The registers follow the GDB sequence for OR1K: GPR0 through GPR31, PPC
//! (i.e. SPR PPC), NPC (i.e. SPR NPC) and SR (i.e. SPR SR). Each register is
//! returned as a sequence of bytes in target endian order.
 
//! Each byte is packed as a pair of hex digits.                             
//-----------------------------------------------------------------------------
void GdbServerSC::rspReadAllRegs()
{
	// The GPRs
	for (int r = 0; r < max_gprs; r++) {
		Utils::reg2Hex(readGpr(r), &(pkt->data[r * 8]));
	}
 
	// PPC, NPC and SR
	Utils::reg2Hex(debugUnit->readSpr(SPR_PPC),
		       &(pkt->data[PPC_REGNUM * 8]));
	Utils::reg2Hex(readNpc(), &(pkt->data[NPC_REGNUM * 8]));
	Utils::reg2Hex(debugUnit->readSpr(SPR_SR), &(pkt->data[SR_REGNUM * 8]));
 
	// Finalize the packet and send it
	pkt->data[NUM_REGS * 8] = 0;
	pkt->setLen(NUM_REGS * 8);
	rsp->putPkt(pkt);
 
}				// rspReadAllRegs ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP write all registers request
 
//! The registers follow the GDB sequence for OR1K: GPR0 through GPR31, PPC
//! (i.e. SPR PPC), NPC (i.e. SPR NPC) and SR (i.e. SPR SR). Each register is
//! supplied as a sequence of bytes in target endian order.
 
//! Each byte is packed as a pair of hex digits.
 
//! @todo There is no error checking at present. Non-hex chars will generate a
//!       warning message, but there is no other check that the right amount
//!       of data is present. The result is always "OK".
//-----------------------------------------------------------------------------
void GdbServerSC::rspWriteAllRegs()
{
	// The GPRs
	for (int r = 0; r < max_gprs; r++) {
		writeGpr(r, Utils::hex2Reg(&(pkt->data[r * 8])));
	}
 
	// PPC, NPC and SR
	debugUnit->writeSpr(SPR_PPC,
			    Utils::hex2Reg(&(pkt->data[PPC_REGNUM * 8])));
	debugUnit->writeSpr(SPR_SR,
			    Utils::hex2Reg(&(pkt->data[SR_REGNUM * 8])));
	writeNpc(Utils::hex2Reg(&(pkt->data[NPC_REGNUM * 8])));
 
	// Acknowledge (always OK for now).
	pkt->packStr("OK");
	rsp->putPkt(pkt);
 
}				// rspWriteAllRegs ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP read memory (symbolic) request
 
//! Syntax is:
 
//!   m<addr>,<length>:
 
//! The response is the bytes, lowest address first, encoded as pairs of hex
//! digits.
 
//! The length given is the number of bytes to be read.
//-----------------------------------------------------------------------------
void GdbServerSC::rspReadMem()
{
	unsigned int addr;	// Where to read the memory
	int len;		// Number of bytes to read
	int off;		// Offset into the memory
 
	if (2 != sscanf(pkt->data, "m%x,%x:", &addr, &len)) {
		cerr << "Warning: Failed to recognize RSP read memory command: "
		    << pkt->data << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Make sure we won't overflow the buffer (2 chars per byte)
	if ((len * 2) >= pkt->getBufSize()) {
		cerr << "Warning: Memory read " << pkt->data
		    << " too large for RSP packet: truncated" << endl;
		len = (pkt->getBufSize() - 1) / 2;
	}
	//cerr << "rspReadMem: " << len << " bytes@0x"<< hex << addr << endl;
	// Refill the buffer with the reply
	for (off = 0; off < len; off++) {
		unsigned char ch = debugUnit->readMem8(addr + off);
 
		pkt->data[off * 2] = Utils::hex2Char(ch >> 4);
		pkt->data[off * 2 + 1] = Utils::hex2Char(ch & 0xf);
	}
 
	pkt->data[off * 2] = '\0';	// End of string
	pkt->setLen(strlen(pkt->data));
	rsp->putPkt(pkt);
 
}				// rsp_read_mem ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP write memory (symbolic) request
 
//! Syntax is:
 
//!   m<addr>,<length>:<data>
 
//! The data is the bytes, lowest address first, encoded as pairs of hex
//! digits.
 
//! The length given is the number of bytes to be written.
//-----------------------------------------------------------------------------
void GdbServerSC::rspWriteMem()
{
	uint32_t addr;		// Where to write the memory
	int len;		// Number of bytes to write
 
	if (2 != sscanf(pkt->data, "M%x,%x:", &addr, &len)) {
		cerr << "Warning: Failed to recognize RSP write memory "
		    << pkt->data << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Find the start of the data and check there is the amount we expect.
	char *symDat = (char *)(memchr(pkt->data, ':', pkt->getBufSize())) + 1;
	int datLen = pkt->getLen() - (symDat - pkt->data);
 
	// Sanity check
	if (len * 2 != datLen) {
		cerr << "Warning: Write of " << len *
		    2 << "digits requested, but " << datLen <<
		    " digits supplied: packet ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Write the bytes to memory (no check the address is OK here)
	for (int off = 0; off < len; off++) {
		uint8_t nyb1 = Utils::char2Hex(symDat[off * 2]);
		uint8_t nyb2 = Utils::char2Hex(symDat[off * 2 + 1]);
 
		if (!debugUnit->writeMem8(addr + off, (nyb1 << 4) | nyb2)) {
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
	}
 
	pkt->packStr("OK");
	rsp->putPkt(pkt);
 
}				// rspWriteMem ()
 
//-----------------------------------------------------------------------------
//! Read a single register
 
//! The registers follow the GDB sequence for OR1K: GPR0 through GPR31, PC
//! (i.e. SPR NPC) and SR (i.e. SPR SR). The register is returned as a
//! sequence of bytes in target endian order.
 
//! Each byte is packed as a pair of hex digits.
//-----------------------------------------------------------------------------
void GdbServerSC::rspReadReg()
{
	unsigned int regNum;
 
	// Break out the fields from the data
	if (1 != sscanf(pkt->data, "p%x", &regNum)) {
		cerr <<
		    "Warning: Failed to recognize RSP read register command: "
		    << pkt->data << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Get the relevant register
	if (regNum < max_gprs) {
		Utils::Utils::reg2Hex(readGpr(regNum), pkt->data);
	} else if (PPC_REGNUM == regNum) {
		Utils::Utils::reg2Hex(debugUnit->readSpr(SPR_PPC), pkt->data);
	} else if (NPC_REGNUM == regNum) {
		Utils::Utils::reg2Hex(readNpc(), pkt->data);
	} else if (SR_REGNUM == regNum) {
		Utils::Utils::reg2Hex(debugUnit->readSpr(SPR_SR), pkt->data);
	} else {
		// Error response if we don't know the register
		cerr << "Warning: Attempt to read unknown register" << regNum
		    << ": ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
 
	pkt->setLen(strlen(pkt->data));
	rsp->putPkt(pkt);
 
}				// rspWriteReg ()
 
//-----------------------------------------------------------------------------
//! Write a single register
 
//! The registers follow the GDB sequence for OR1K: GPR0 through GPR31, PC
//! (i.e. SPR NPC) and SR (i.e. SPR SR). The register is specified as a
//! sequence of bytes in target endian order.
 
//! Each byte is packed as a pair of hex digits.
//-----------------------------------------------------------------------------
void GdbServerSC::rspWriteReg()
{
	unsigned int regNum;
	char valstr[9];		// Allow for EOS on the string
 
	// Break out the fields from the data
	if (2 != sscanf(pkt->data, "P%x=%8s", &regNum, valstr)) {
		cerr <<
		    "Warning: Failed to recognize RSP write register command "
		    << pkt->data << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Set the relevant register
	if (regNum < max_gprs) {
		writeGpr(regNum, Utils::hex2Reg(valstr));
	} else if (PPC_REGNUM == regNum) {
		debugUnit->writeSpr(SPR_PPC, Utils::hex2Reg(valstr));
	} else if (NPC_REGNUM == regNum) {
		writeNpc(Utils::hex2Reg(valstr));
	} else if (SR_REGNUM == regNum) {
		debugUnit->writeSpr(SPR_SR, Utils::hex2Reg(valstr));
	} else {
		// Error response if we don't know the register
		cerr << "Warning: Attempt to write unknown register " << regNum
		    << ": ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
 
	pkt->packStr("OK");
	rsp->putPkt(pkt);
 
}				// rspWriteReg ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP query request
//-----------------------------------------------------------------------------
void GdbServerSC::rspQuery()
{
	if (0 == strcmp("qAttached", pkt->data)) {
		// We are always attaching to an existing process with the bare metal
		// embedded system. 
		pkt->packStr("1");
		rsp->putPkt(pkt);
	} else if (0 == strcmp("qC", pkt->data)) {
		// Return the current thread ID (unsigned hex). A null response
		// indicates to use the previously selected thread. We use the constant
		// OR1KSIM_TID to represent our single thread of control.
		sprintf(pkt->data, "QC%x", OR1KSIM_TID);
		pkt->setLen(strlen(pkt->data));
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qCRC", pkt->data, strlen("qCRC"))) {
		// Return CRC of memory area
		cerr << "Warning: RSP CRC query not supported" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
	} else if (0 == strcmp("qfThreadInfo", pkt->data)) {
		// Return info about active threads. We return just the constant
		// OR1KSIM_TID to represent our single thread of control.
		sprintf(pkt->data, "m%x", OR1KSIM_TID);
		pkt->setLen(strlen(pkt->data));
		rsp->putPkt(pkt);
	} else if (0 == strcmp("qsThreadInfo", pkt->data)) {
		// Return info about more active threads. We have no more, so return the
		// end of list marker, 'l'
		pkt->packStr("l");
		rsp->putPkt(pkt);
	} else if (0 ==
		   strncmp("qGetTLSAddr:", pkt->data, strlen("qGetTLSAddr:"))) {
		// We don't support this feature
		pkt->packStr("");
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qL", pkt->data, strlen("qL"))) {
		// Deprecated and replaced by 'qfThreadInfo'
		cerr << "Warning: RSP qL deprecated: no info returned" << endl;
		pkt->packStr("qM001");
		rsp->putPkt(pkt);
	} else if (0 == strcmp("qOffsets", pkt->data)) {
		// Report any relocation
		pkt->packStr("Text=0;Data=0;Bss=0");
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qP", pkt->data, strlen("qP"))) {
		// Deprecated and replaced by 'qThreadExtraInfo'
		cerr << "Warning: RSP qP deprecated: no info returned" << endl;
		pkt->packStr("");
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qRcmd,", pkt->data, strlen("qRcmd,"))) {
		// This is used to interface to commands to do "stuff"
		rspCommand();
	} else if (0 == strncmp("qSupported", pkt->data, strlen("qSupported"))) {
		// Report a list of the features we support. For now we just ignore any
		// supplied specific feature queries, but in the future these may be
		// supported as well. Note that the packet size allows for 'G' + all the
		// registers sent to us, or a reply to 'g' with all the registers and an
		// EOS so the buffer is a well formed string.
		sprintf(pkt->data, "PacketSize=%x", pkt->getBufSize());
		pkt->setLen(strlen(pkt->data));
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qSymbol:", pkt->data, strlen("qSymbol:"))) {
		// Offer to look up symbols. Nothing we want (for now). TODO. This just
		// ignores any replies to symbols we looked up, but we didn't want to
		// do that anyway!
		pkt->packStr("OK");
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qThreadExtraInfo,", pkt->data,
				strlen("qThreadExtraInfo,"))) {
		// Report that we are runnable, but the text must be hex ASCI
		// digits. For now do this by steam, reusing the original packet
		sprintf(pkt->data, "%02x%02x%02x%02x%02x%02x%02x%02x%02x",
			'R', 'u', 'n', 'n', 'a', 'b', 'l', 'e', 0);
		pkt->setLen(strlen(pkt->data));
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qTStatus", pkt->data, strlen("qTstatus"))) {
		// We don't support tracing, return empty packet
		pkt->packStr("");
		rsp->putPkt(pkt);
	} else if (0 == strncmp("qXfer:", pkt->data, strlen("qXfer:"))) {
		// For now we support no 'qXfer' requests, but these should not be
		// expected, since they were not reported by 'qSupported'
		cerr << "Warning: RSP 'qXfer' not supported: ignored" << endl;
		pkt->packStr("");
		rsp->putPkt(pkt);
	} else {
		cerr << "Unrecognized RSP query: ignored" << endl;
	}
}				// rspQuery ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP qRcmd request
 
//! The actual command follows the "qRcmd," in ASCII encoded to hex
//-----------------------------------------------------------------------------
void GdbServerSC::rspCommand()
{
	char cmd[RSP_PKT_MAX];
 
	Utils::hex2Ascii(cmd, &(pkt->data[strlen("qRcmd,")]));
 
	// Work out which command it is
	if (0 == strncmp("readspr ", cmd, strlen("readspr"))) {
		unsigned int sprNum;
 
		// Parse and return error if we fail
		if (1 != sscanf(cmd, "readspr %4x", &sprNum)) {
			cerr << "Warning: qRcmd " << cmd
			    << "not recognized: ignored" << endl;
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
		// SPR out of range
		if (sprNum > MAX_SPRS) {
			cerr << "Warning: qRcmd readspr " << hex << sprNum
			    << dec << " too large: ignored" << endl;
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
		// Construct the reply
		sprintf(cmd, "%8lx", debugUnit->readSpr(sprNum));
		Utils::ascii2Hex(pkt->data, cmd);
		pkt->setLen(strlen(pkt->data));
		rsp->putPkt(pkt);
	} else if (0 == strncmp("writespr ", cmd, strlen("writespr"))) {
		unsigned int sprNum;
		uint32_t val;
 
		// Parse and return error if we fail
		if (2 != sscanf(cmd, "writespr %4x %8lx", &sprNum, &val)) {
			cerr << "Warning: qRcmd " << cmd <<
			    " not recognized: ignored" << endl;
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
		// SPR out of range
		if (sprNum > MAX_SPRS) {
			cerr << "Warning: qRcmd writespr " << hex << sprNum
			    << dec << " too large: ignored" << endl;
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
		// Update the SPR and reply "OK"
		debugUnit->writeSpr(sprNum, val);
		pkt->packStr("OK");
		rsp->putPkt(pkt);
	}
 
}				// rspCommand ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP set request
//-----------------------------------------------------------------------------
void GdbServerSC::rspSet()
{
	if (0 == strncmp("QPassSignals:", pkt->data, strlen("QPassSignals:"))) {
		// Passing signals not supported
		pkt->packStr("");
		rsp->putPkt(pkt);
	} else if ((0 == strncmp("QTDP", pkt->data, strlen("QTDP"))) ||
		   (0 == strncmp("QFrame", pkt->data, strlen("QFrame"))) ||
		   (0 == strcmp("QTStart", pkt->data)) ||
		   (0 == strcmp("QTStop", pkt->data)) ||
		   (0 == strcmp("QTinit", pkt->data)) ||
		   (0 == strncmp("QTro", pkt->data, strlen("QTro")))) {
		// All tracepoint features are not supported. This reply is really only
		// needed to 'QTDP', since with that the others should not be
		// generated.
		pkt->packStr("");
		rsp->putPkt(pkt);
	} else {
		cerr << "Unrecognized RSP set request: ignored" << endl;
		delete pkt;
	}
}				// rspSet ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP restart request
 
//! For now we just put the program counter back to the reset vector. If we
//! supported the vRun request, we should use the address specified
//! there. There is no point in unstalling the processor, since we'll never
//! get control back.
//-----------------------------------------------------------------------------
void GdbServerSC::rspRestart()
{
	writeNpc(EXCEPT_RESET);
 
}				// rspRestart ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP step request
 
//! This version is typically used for the 's' packet, to continue without
//! signal, in which case EXCEPT_NONE is passed in as the exception to use.
 
//! @param[in] except  The exception to use. Only EXCEPT_NONE should be set
//!                    this way.
//-----------------------------------------------------------------------------
void GdbServerSC::rspStep(uint32_t except)
{
	uint32_t addr;		// The address to step from, if any
 
	// Reject all except 's' packets
	if ('s' != pkt->data[0]) {
		cerr << "Warning: Step with signal not currently supported: "
		    << "ignored" << endl;
		return;
	}
 
	if (0 == strcmp("s", pkt->data)) {
		addr = readNpc();	// Default uses current NPC
	} else if (1 != sscanf(pkt->data, "s%lx", &addr)) {
		cerr << "Warning: RSP step address " << pkt->data
		    << " not recognized: ignored" << endl;
		addr = readNpc();	// Default uses current NPC
	}
 
	rspStep(addr, EXCEPT_NONE);
 
}				// rspStep ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP step with signal request
 
//! @todo Currently null. Will use the underlying generic step function.
//-----------------------------------------------------------------------------
void GdbServerSC::rspStep()
{
	cerr << "RSP step with signal '" << pkt->data << "' received" << endl;
 
}				// rspStep ()
 
//-----------------------------------------------------------------------------
//! Generic processing of a step request
 
//! The signal may be EXCEPT_NONE if there is no exception to be
//! handled. Currently the exception is ignored.
 
//! The single step flag is set in the debug registers and then the processor
//! is unstalled.
 
//! @todo There appears to be a bug in the ORPSoC debug unit, whereby multiple
//!       single steps make a mess of the pipeline, leading to multiple
//!       executions of the same instruction. A fix would be to use l.trap (as
//!       for continue) for any consecutive calls to step.
 
//! @param[in] addr    Address from which to step
//! @param[in] except  The exception to use (if any)                         
//-----------------------------------------------------------------------------
void GdbServerSC::rspStep(uint32_t addr, uint32_t except)
{
	// Set the address as the value of the next program counter
	writeNpc(addr);
 
	/*
	   // If we're continuing from a breakpoint, replace that instruction in the memory
	   // ... actually no, I was wrong about this.
	   if (NULL != mpHash->lookup (BP_MEMORY, addr) && rsp_sigval == TARGET_SIGNAL_TRAP)
	   {
	   MpEntry *entry = mpHash->lookup (BP_MEMORY, addr);
	   debugUnit->writeMem32(entry->addr, entry->instr);
	   }
	 */
 
	// Clear Debug Reason Register and watchpoint break generation in Debug Mode
	// Register 2 for watchpoints that we triggered to stop this time.
	debugUnit->writeSpr(SPR_DRR, 0);
	if (rsp_sigval == TARGET_SIGNAL_TRAP) {
		/* 
		   Disable last trap generation on watchpoint if this is why we stopped
		   last time.
		 */
		uint32_t temp_dmr2 = debugUnit->readSpr(SPR_DMR2);
		if (temp_dmr2 & SPR_DMR2_WBS) {
			/* 
			   One of these breakpoints is responsible for our stopping, so
			   disable it this time we start. GDB will send a packet re-enabling
			   it next time we continue.
			 */
			debugUnit->writeSpr(SPR_DMR2,
					    temp_dmr2 &
					    ~((temp_dmr2 & SPR_DMR2_WBS) >>
					      10));
		}
	}
	// Set the single step trigger in Debug Mode Register 1 and set traps to be
	// handled by the debug unit in the Debug Stop Register
	debugUnit->orSpr(SPR_DMR1, SPR_DMR1_ST);
	debugUnit->orSpr(SPR_DSR, SPR_DSR_TE);
 
	// Unstall the processor. Note the GDB client is now waiting for a reply,
	// which it will get as soon as the processor stalls again.
	debugUnit->unstall();
	targetStopped = false;
 
}				// rspStep ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP 'v' packet
 
//! These are commands associated with executing the code on the target
//-----------------------------------------------------------------------------
void GdbServerSC::rspVpkt()
{
	if (0 == strncmp("vAttach;", pkt->data, strlen("vAttach;"))) {
		// Attaching is a null action, since we have no other process. We just
		// return a stop packet (using TRAP) to indicate we are stopped.
		pkt->packStr("S05");
		rsp->putPkt(pkt);
		return;
	} else if (0 == strcmp("vCont?", pkt->data)) {
		// For now we don't support this.
		pkt->packStr("");
		rsp->putPkt(pkt);
		return;
	} else if (0 == strncmp("vCont", pkt->data, strlen("vCont"))) {
		// This shouldn't happen, because we've reported non-support via vCont?
		// above
		cerr << "Warning: RSP vCont not supported: ignored" << endl;
		return;
	} else if (0 == strncmp("vFile:", pkt->data, strlen("vFile:"))) {
		// For now we don't support this.
		cerr << "Warning: RSP vFile not supported: ignored" << endl;
		pkt->packStr("");
		rsp->putPkt(pkt);
		return;
	} else if (0 ==
		   strncmp("vFlashErase:", pkt->data, strlen("vFlashErase:"))) {
		// For now we don't support this.
		cerr << "Warning: RSP vFlashErase not supported: ignored" <<
		    endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	} else if (0 ==
		   strncmp("vFlashWrite:", pkt->data, strlen("vFlashWrite:"))) {
		// For now we don't support this.
		cerr << "Warning: RSP vFlashWrite not supported: ignored" <<
		    endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	} else if (0 == strcmp("vFlashDone", pkt->data)) {
		// For now we don't support this.
		cerr << "Warning: RSP vFlashDone not supported: ignored" <<
		    endl;;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	} else if (0 == strncmp("vRun;", pkt->data, strlen("vRun;"))) {
		// We shouldn't be given any args, but check for this
		if (pkt->getLen() > strlen("vRun;")) {
			cerr << "Warning: Unexpected arguments to RSP vRun "
			    "command: ignored" << endl;
		}
		// Restart the current program. However unlike a "R" packet, "vRun"
		// should behave as though it has just stopped. We use signal 5 (TRAP).
		rspRestart();
		pkt->packStr("S05");
		rsp->putPkt(pkt);
	} else {
		cerr << "Warning: Unknown RSP 'v' packet type " << pkt->data
		    << ": ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
}				// rspVpkt ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP write memory (binary) request
 
//! Syntax is:
 
//!   X<addr>,<length>:
 
//! Followed by the specified number of bytes as raw binary. Response should be
//! "OK" if all copied OK, E<nn> if error <nn> has occurred.
 
//! The length given is the number of bytes to be written. The data buffer has
//! already been unescaped, so will hold this number of bytes.
 
//! The data is in model-endian format, so no transformation is needed.
//-----------------------------------------------------------------------------
void GdbServerSC::rspWriteMemBin()
{
	uint32_t addr;		// Where to write the memory
	int len;		// Number of bytes to write
 
	if (2 != sscanf(pkt->data, "X%x,%x:", &addr, &len)) {
		cerr <<
		    "Warning: Failed to recognize RSP write memory command: %s"
		    << pkt->data << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Find the start of the data and "unescape" it. Bindat must be unsigned, or
	// all sorts of horrible sign extensions will happen when val is computed
	// below!
	uint8_t *bindat = (uint8_t *) (memchr(pkt->data, ':',
					      pkt->getBufSize())) + 1;
	int off = (char *)bindat - pkt->data;
	int newLen = Utils::rspUnescape((char *)bindat, pkt->getLen() - off);
 
	// Sanity check
	if (newLen != len) {
		int minLen = len < newLen ? len : newLen;
 
		cerr << "Warning: Write of " << len << " bytes requested, but "
		    << newLen << " bytes supplied. " << minLen <<
		    " will be written" << endl;
		len = minLen;
	}
	// Write the bytes to memory. More efficent to do this in 32-bit chunks
	int startBytes = addr & 0x3;
	int endBytes = (addr + len) & 0x3;
 
	// First partial word. Access bindat in an endian independent fashion.
	for (off = 0; off < startBytes; off++) {
		if (!debugUnit->writeMem8(addr + off, bindat[off])) {
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
	}
 
	// The bulk as words. Convert to model endian before writing.
	for (off = startBytes; off < len; off += 4) {
		uint32_t val = *((uint32_t *) (&(bindat[off])));
 
		if (!debugUnit->writeMem32(addr + off, Utils::htotl(val))) {
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
	}
 
	// Last partial word. Access bindat in an endian independent fashion.
	for (off = len - endBytes; off < len; off++) {
		uint32_t base = (addr + len) & 0xfffffffc;
 
		if (!debugUnit->writeMem8(base + off, bindat[off])) {
			pkt->packStr("E01");
			rsp->putPkt(pkt);
			return;
		}
	}
 
	pkt->packStr("OK");
	rsp->putPkt(pkt);
 
}				// rspWriteMemBin ()
 
//-----------------------------------------------------------------------------
//! Handle a RSP remove breakpoint or matchpoint request
 
//! For now only memory breakpoints are implemented, which are implemented by
//! substituting a breakpoint at the specified address. The implementation must
//! cope with the possibility of duplicate packets.
 
//! @todo This doesn't work with icache/immu yet
//-----------------------------------------------------------------------------
void GdbServerSC::rspRemoveMatchpoint()
{
	MpType type;		// What sort of matchpoint
	uint32_t addr;		// Address specified
	uint32_t instr;		// Instruction value found
	int len;		// Matchpoint length (not used)
 
	// Break out the instruction
	if (3 != sscanf(pkt->data, "z%1d,%lx,%1d", (int *)&type, &addr, &len)) {
		cerr << "Warning: RSP matchpoint deletion request not "
		    << "recognized: ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Sanity check that the length is 4
	if (4 != len) {
		cerr << "Warning: RSP matchpoint deletion length " << len
		    << "not valid: 4 assumed" << endl;
		len = 4;
	}
	// Sort out the type of matchpoint
	switch (type) {
	case BP_MEMORY:
		// Memory breakpoint - replace the original instruction.
		if (mpHash->remove(type, addr, &instr)) {
			//cerr << "rspRemoveMatchpoint at 0x" << hex << addr << " restoring instruction: 0x" << hex << instr <<endl;
			debugUnit->writeMem32(addr, instr);
		}
 
		pkt->packStr("OK");
		rsp->putPkt(pkt);
		return;
 
	case BP_HARDWARE:
		int off;
		for (off = 0; off < 8; off++)
			if ((debugUnit->readSpr(SPR_DCR0 + off) == (0x23)) &&
			    (debugUnit->readSpr(SPR_DVR0 + off) == addr))
				break;
		if (off > 7) {
			pkt->packStr("E02");	// Failed ot find breakpoint
			rsp->putPkt(pkt);
			return;
		}
		// Clear DCR's CT and DVR, WGB bit
		debugUnit->writeSpr(SPR_DCR0 + off, 0);
		debugUnit->writeSpr(SPR_DVR0 + off, 0);
		debugUnit->writeSpr(SPR_DMR2,
				    debugUnit->readSpr(SPR_DMR2) & ~((1 << off)
								     <<
								     SPR_DMR2_WGB_SHIFT));
		pkt->packStr("OK");
		rsp->putPkt(pkt);
		return;
 
	case WP_WRITE:
		{
			int off;
			for (off = 0; off < 8; off++) {
				if ((debugUnit->readSpr(SPR_DCR0 + off) ==
				     (0x63))
				    && (debugUnit->readSpr(SPR_DVR0 + off) ==
					addr))
					break;
			}
			if (off > 7) {
				pkt->packStr("E02");	// Failed ot find breakpoint
				rsp->putPkt(pkt);
				return;
			}
			// Clear DCR's CT and DVR, WGB bit
			debugUnit->writeSpr(SPR_DCR0 + off, 0);
			debugUnit->writeSpr(SPR_DVR0 + off, 0);
			debugUnit->writeSpr(SPR_DMR2,
					    debugUnit->readSpr(SPR_DMR2) &
					    ~((1 << off) <<
					      SPR_DMR2_WGB_SHIFT));
			pkt->packStr("OK");
			rsp->putPkt(pkt);
			return;
		}
 
	case WP_READ:
		{
			int off;
			for (off = 0; off < 8; off++) {
				if ((debugUnit->readSpr(SPR_DCR0 + off) ==
				     (0x43))
				    && (debugUnit->readSpr(SPR_DVR0 + off) ==
					addr))
					break;
			}
			if (off > 7) {
				pkt->packStr("E02");	// Failed ot find breakpoint
				rsp->putPkt(pkt);
				return;
			}
			// Clear DCR's CT and DVR, WGB bit
			debugUnit->writeSpr(SPR_DCR0 + off, 0);
			debugUnit->writeSpr(SPR_DVR0 + off, 0);
			debugUnit->writeSpr(SPR_DMR2,
					    debugUnit->readSpr(SPR_DMR2) &
					    ~((1 << off) <<
					      SPR_DMR2_WGB_SHIFT));
			pkt->packStr("OK");
			rsp->putPkt(pkt);
			return;
		}
 
	case WP_ACCESS:
		{
			int off;
			for (off = 0; off < 8; off++) {
				//printf("WP_ACCESS remove check off=%d DCR=0x%.8x DVR=0x%.8x\n",
				//off,debugUnit->readSpr (SPR_DCR0+off),debugUnit->readSpr (SPR_DVR0+off));
				if ((debugUnit->readSpr(SPR_DCR0 + off) ==
				     (0xc3))
				    && (debugUnit->readSpr(SPR_DVR0 + off) ==
					addr))
					break;
			}
			if (off > 7) {
				//printf("rspRemoveWatchpoint: WP_ACCESS remove ERROR, regpair %d for 0x%.8x\n",off, addr);
				pkt->packStr("E02");	// Failed ot find breakpoint
				rsp->putPkt(pkt);
				return;
			}
			//printf("rspRemoveWatchpoint: WP_ACCESS remove, regpair %d for 0x%.8x\n",off, addr);
 
			// Clear DCR's CT and DVR, WGB bit
			debugUnit->writeSpr(SPR_DCR0 + off, 0);
			debugUnit->writeSpr(SPR_DVR0 + off, 0);
			debugUnit->writeSpr(SPR_DMR2,
					    debugUnit->readSpr(SPR_DMR2) &
					    ~((1 << off) <<
					      SPR_DMR2_WGB_SHIFT));
			pkt->packStr("OK");
			rsp->putPkt(pkt);
			return;
		}
	default:
		cerr << "Warning: RSP matchpoint type " << type
		    << " not recognized: ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
}				// rspRemoveMatchpoint ()
 
//---------------------------------------------------------------------------*/
//! Handle a RSP insert breakpoint or matchpoint request
 
//! For now only memory breakpoints are implemented, which are implemented by
//! substituting a breakpoint at the specified address. The implementation must
//! cope with the possibility of duplicate packets.
 
//! @todo This doesn't work with icache/immu yet
//---------------------------------------------------------------------------*/
void GdbServerSC::rspInsertMatchpoint()
{
	MpType type;		// What sort of matchpoint
	uint32_t addr;		// Address specified
	int len;		// Matchpoint length (not used)
 
	// Break out the instruction
	if (3 != sscanf(pkt->data, "Z%1d,%lx,%1d", (int *)&type, &addr, &len)) {
		cerr << "Warning: RSP matchpoint insertion request not "
		    << "recognized: ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
	// Sanity check that the length is 4
	if (4 != len) {
		cerr << "Warning: RSP matchpoint insertion length " << len
		    << "not valid: 4 assumed" << endl;
		len = 4;
	}
	// Sort out the type of matchpoint
	switch (type) {
	case BP_MEMORY:
		// Memory breakpoint - substitute a TRAP instruction
		mpHash->add(type, addr, debugUnit->readMem32(addr));
		debugUnit->writeMem32(addr, OR1K_TRAP_INSTR);
		pkt->packStr("OK");
		rsp->putPkt(pkt);
		return;
 
	case BP_HARDWARE:
		{
			int off;
			for (off = 0; off < 8; off++)
				if (!
				    (debugUnit->readSpr(SPR_DCR0 + off) &
				     SPR_DCR_CT_MASK))
					break;
			if (off > 7) {
				pkt->packStr("");	// No room
				rsp->putPkt(pkt);
				return;
			}
			// CC = equal, CT = Instruction fetch EA, set WGB bit
			debugUnit->writeSpr(SPR_DCR0 + off, 0x22);
			debugUnit->writeSpr(SPR_DVR0 + off, addr);
			debugUnit->writeSpr(SPR_DMR2,
					    debugUnit->readSpr(SPR_DMR2) |
					    ((1 << off) << SPR_DMR2_WGB_SHIFT));
			pkt->packStr("OK");
			rsp->putPkt(pkt);
			return;
		}
 
	case WP_WRITE:
		{
			int off;
			for (off = 0; off < 8; off++)
				if (!
				    (debugUnit->readSpr(SPR_DCR0 + off) &
				     SPR_DCR_CT_MASK))
					break;
			//printf("rspInsertWatchpoint: WP_WRITE, regpair %d for 0x%.8x\n",off, addr);
			if (off > 7) {
				pkt->packStr("");	// No room
				rsp->putPkt(pkt);
				return;
			}
			// CC = equal, CT = Store EA, set WGB bit
			debugUnit->writeSpr(SPR_DCR0 + off, 0x62);
			debugUnit->writeSpr(SPR_DVR0 + off, addr);
			debugUnit->writeSpr(SPR_DMR2,
					    debugUnit->readSpr(SPR_DMR2) |
					    ((1 << off) << SPR_DMR2_WGB_SHIFT));
			pkt->packStr("OK");
			rsp->putPkt(pkt);
			return;
		}
 
	case WP_READ:
		{
			int off;
			for (off = 0; off < 8; off++)
				if (!
				    (debugUnit->readSpr(SPR_DCR0 + off) &
				     SPR_DCR_CT_MASK))
					break;
			//printf("rspInsertWatchpoint: WP_WRITE, regpair %d for 0x%.8x\n",off, addr);
			if (off > 7) {
				pkt->packStr("");	// No room
				rsp->putPkt(pkt);
				return;
			}
			// CC = equal, CT = Load EA, set WGB bit
			debugUnit->writeSpr(SPR_DCR0 + off, 0x42);
			debugUnit->writeSpr(SPR_DVR0 + off, addr);
			debugUnit->writeSpr(SPR_DMR2,
					    debugUnit->readSpr(SPR_DMR2) |
					    ((1 << off) << SPR_DMR2_WGB_SHIFT));
			pkt->packStr("OK");
			rsp->putPkt(pkt);
			return;
		}
 
		pkt->packStr("");	// Not supported
		rsp->putPkt(pkt);
		return;
 
	case WP_ACCESS:
		{
			int off;
			for (off = 0; off < 8; off++)
				if (!
				    (debugUnit->readSpr(SPR_DCR0 + off) &
				     SPR_DCR_CT_MASK))
					break;
			//printf("rspInsertWatchpoint: WP_ACCESS, regpair %d for 0x%.8x\n",off, addr);
			if (off > 7) {
				pkt->packStr("");	// No room
				rsp->putPkt(pkt);
				return;
			}
			// CC = equal, CT = Load/Store EA, set WGB bit
			debugUnit->writeSpr(SPR_DCR0 + off, 0xc2);
			debugUnit->writeSpr(SPR_DVR0 + off, addr);
			debugUnit->writeSpr(SPR_DMR2,
					    debugUnit->readSpr(SPR_DMR2) |
					    ((1 << off) << SPR_DMR2_WGB_SHIFT));
			pkt->packStr("OK");
			rsp->putPkt(pkt);
			return;
		}
 
	default:
		cerr << "Warning: RSP matchpoint type " << type
		    << "not recognized: ignored" << endl;
		pkt->packStr("E01");
		rsp->putPkt(pkt);
		return;
	}
}				// rspInsertMatchpoint ()
 
//-----------------------------------------------------------------------------
//! Read the value of the Next Program Counter (a SPR)
 
//! A convenience routine.
 
//! Setting the NPC flushes the pipeline, so subsequent reads will return
//! zero until the processor has refilled the pipeline. This will not be
//! happening if the processor is stalled (as it is when GDB had control).
 
//! However for debugging we always want to know what the effective value of
//! the NPC will be (i.e. the value that will be used once the pipeline has
//! refilled). Fortunately SPR cacheing in the debug unit silently solves this
//! for us.
 
//! @return  The value of the NPC
//-----------------------------------------------------------------------------
uint32_t GdbServerSC::readNpc()
{
	return debugUnit->readSpr(SPR_NPC);
 
}				// readNpc ()
 
//-----------------------------------------------------------------------------
//! Write the value of the Next Program Counter (a SPR)
 
//! A convenience function.
 
//! Setting the NPC flushes the pipeline, so subsequent reads will return
//! zero until the processor has refilled the pipeline. This will not be
//! happening if the processor is stalled (as it is when GDB had control).
 
//! However for debugging we always want to know what the effective value of
//! the NPC will be (i.e. the value that will be used once the pipeline has
//! refilled). Fortunately SPR cacheing in the debug unit silently solves this
//! for us.
 
//! There is one other caveat for the NPC. We do not wish to write it (whether
//! or not it is cached) if it has not changed. So unlike all other SPRs we
//! always read it first before writing.
 
//! @param[in]  The address to write into the NPC
//-----------------------------------------------------------------------------
void GdbServerSC::writeNpc(uint32_t addr)
{
	if (addr != readNpc()) {
		debugUnit->writeSpr(SPR_NPC, addr);
	}
}				// writeNpc ()
 
//-----------------------------------------------------------------------------
//! Read the value of an OpenRISC 1000 General Purpose Register
 
//! A convenience function. This is just a wrapper for reading a SPR, since
//! the GPR's are mapped into SPR space
 
//! @param[in]  regNum  The GPR to read
 
//! @return  The value of the GPR
//-----------------------------------------------------------------------------
uint32_t GdbServerSC::readGpr(int regNum)
{
	return debugUnit->readSpr(SPR_GPR0 + regNum);
 
}				// readGpr ()
 
//-----------------------------------------------------------------------------
//! Write the value of an OpenRISC 1000 General Purpose Register
 
//! A convenience function. This is just a wrapper for writing a SPR, since
//! the GPR's are mapped into SPR space
 
//! @param[in]  regNum  The GPR to read
 
//! @return  The value of the GPR
//-----------------------------------------------------------------------------
void GdbServerSC::writeGpr(int regNum, uint32_t value)
{
	debugUnit->writeSpr(SPR_GPR0 + regNum, value);
 
}				// writeGpr ()
 
//-----------------------------------------------------------------------------
//! Check if we received anything via the pipe from the or1200 monitor
 
//! We stall the processor, and behave in a manner similar to if an interrupt
//! had been received. Perhaps the sigval should be set differently/more
//! more appropriately.
//! Read from the pipe should be NON-blocking.
 
//! @return  false if nothing received, else true
//-----------------------------------------------------------------------------
bool GdbServerSC::checkMonitorPipe()
{
	char readChar;
	int n = read(monitor_to_gdb_pipe[0][0], &readChar, sizeof(char));
	if (!(((n < 0) && (errno == EAGAIN)) || (n == 0)) && !targetStopped) {
		debugUnit->stall();
		// Send a stop reply response, manually set rsp.sigval to TARGET_SIGNAL_NONE
		rsp_sigval = TARGET_SIGNAL_NONE;
		rspReportException();
		targetStopped = true;	// Processor now not running
		write(monitor_to_gdb_pipe[1][1], &readChar, sizeof(char));
		return true;
	}
 
	return false;
 
}				// checkMonitorPipe ()