Subversion Repositories openrisc_me

// ----------------------------------------------------------------------------
 
// SystemC OpenRISC 1000 Debug Unit: implementation
 
// Copyright (C) 2008  Embecosm Limited <info@embecosm.com>
// Copyright (C) 2009  ORSoC
 
// 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 "DebugUnitSC.h"
#include "Utils.h"
 
 
using sc_core::sc_event;
using sc_core::sc_fifo;
using sc_core::sc_module_name;
 
 
//-----------------------------------------------------------------------------
//! Constructor for the Debug Unit
 
//! This module is entirely subservient to the GDB server. It has no SystemC
//! processes of its own. It provides services via calls to its API.
 
//! The current scan chain is marked as undefined, and the current JTAG scan
//! chain is maked as undefined.
 
//! Caches for SPR and memory access are initialized.
 
//! 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)
 
//! @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
//-----------------------------------------------------------------------------
DebugUnitSC::DebugUnitSC (sc_module_name        name,
			  sc_fifo<TapAction *> *_tapActionQueue) :
  sc_module (name),
  tapActionQueue (_tapActionQueue),
  stallState (UNKNOWN),
  currentScanChain (OR1K_SC_UNDEF)
{
#ifdef NOCACHE
  npcCacheIsValid = false;		// Always cache NPC
#else
  sprCache = new SprCache ();
  memCache = new MemCache ();
#endif
 
}	// DebugUnitSC ()
 
 
//-----------------------------------------------------------------------------
//! Destructor
 
//! Free up data structures
//-----------------------------------------------------------------------------
DebugUnitSC::~DebugUnitSC ()
{
#ifndef NOCACHE
  delete  memCache;
  delete  sprCache;
#endif
 
}	// ~DebugUnitSC
 
 
//-----------------------------------------------------------------------------
//! Reset the Debug Unit
 
//! This is just a reset of the JTAG. It is quite possible to reset the debug
//! unit without resetting the whole target.
 
//! @note Must be called from a SystemC thread, because of the use of wait()
//-----------------------------------------------------------------------------
void
DebugUnitSC::resetDebugUnit ()
{
  sc_event       *done = new sc_event();
  TapActionReset *resetAction;
 
  // Create and queue the reset action and wait for it to complete
  resetAction = new TapActionReset (done);
  tapActionQueue->write (resetAction);
  wait (*done);
 
  delete resetAction;
  delete done;
 
}	// resetDebugUnit ()
 
 
//-----------------------------------------------------------------------------
//! Reset the processor
 
//! Read the RISCOP register, OR in the reset bit and write it back.
 
//! After reset, the processor is known to be unstalled.
//-----------------------------------------------------------------------------
void      
DebugUnitSC::reset ()
{
  writeRiscop (readRiscop () | RISCOP_RESET);
  stallState = UNKNOWN;
 
}	// reset ()
 
 
//-----------------------------------------------------------------------------
//! Stall the processor
 
//! Read the RISCOP register, OR in the stall bit and write it back.
//-----------------------------------------------------------------------------
void      
DebugUnitSC::stall ()
{
  writeRiscop (/*readRiscop () |*/ RISCOP_STALL);
  stallState = STALLED;
 
}	// stall ()
 
 
//-----------------------------------------------------------------------------
//! Unstall the processor
 
//! Read the RISCOP register, AND out the stall bit and write it back. After
//! this the NPC cache will be invalid.
 
//! @note Don't be tempted to read back for confirmation. Single stepping
//!       will already have stalled the processor again!
//-----------------------------------------------------------------------------
void      
DebugUnitSC::unstall ()
{
  writeRiscop (/*readRiscop () & ~RISCOP_STALL*/ 0);
  stallState  = UNKNOWN;
 
#ifdef NOCACHE
  npcCacheIsValid = false;		// Always cache NPC
#else
  // Clear the caches
  sprCache->clear ();
  memCache->clear ();
#endif
 
}	// unstall ()
 
 
//-----------------------------------------------------------------------------
//! Report if the processor is stalled.
 
//! A stalled processor cannot spontaneously "unstall", so if the stallState
//! flag is STALLED, that value is returned. Otherwise the target is
//! interrogated to determine the status.
 
//! @return  TRUE if the processor is known to be stalled
//-----------------------------------------------------------------------------
bool
DebugUnitSC::isStalled ()
{
  if (STALLED == stallState)
    {
      return  true;
    }
 
  uint32_t  riscop = readRiscop ();
  /* For some reason the reset bit is skipped over somewhere, so we should
     just get riscop = 1 if it's stalled */
 
  //stallState = (RISCOP_STALL == (riscop & RISCOP_STALL)) ? STALLED : UNKNOWN;
  stallState = riscop ? STALLED : UNKNOWN;
 
  return  STALLED == stallState;
 
}	// isStalled ()
 
 
//-----------------------------------------------------------------------------
//! Read the value of an OpenRISC 1000 Special Purpose Register
 
//! First see if we have the value in the cache, and if so return
//! it. Otherwise, select the RISC_DEBUG scan chain and read from JTAG,
//! storing the result in the cache.
 
//! @param[in]  sprNum  The SPR to read
 
//! @return  The value of the SPR
//-----------------------------------------------------------------------------
uint32_t  
DebugUnitSC::readSpr (uint16_t  sprNum)
{
  uint32_t  cachedValue;
 
#ifdef NOCACHE
  // Always check NPC cache
  if ((STALLED == stallState) && (sprNum == SPR_NPC) && npcCacheIsValid)
    {
      return  npcCachedValue;
    }
#else
  // Use any cached value if we are stalled.
  if ((STALLED == stallState) && sprCache->read (sprNum, cachedValue))
    {
      return  cachedValue;		// Already there, no more to do
    }
#endif
 
  // Read the value
  selectDebugModule (OR1K_SC_CPU0);
  cachedValue = readJtagReg (sprNum);
 
#ifdef NOCACHE
  // Always update the NPC cache
  if ((STALLED == stallState) && (sprNum == SPR_NPC))
    {
      npcCachedValue  = cachedValue;
      npcCacheIsValid = true;
    }
#else
  // Update the cache if we are stalled
  if (STALLED == stallState)
    {
      sprCache->write (sprNum, cachedValue, sprNum == SPR_NPC);
    }
#endif
 
  return  cachedValue;
 
}	// readSpr ()
 
 
//-----------------------------------------------------------------------------
//! Write the value of an OpenRISC 1000 Special Purpose Register
 
//! First look to see if we are stalled and the value is cached. If the value
//! has not changed, then we need to no more. Otherwise cache the value prior
//! to writing it.
 
//! Select the RISC_DEBUG scan chain and write to JTAG
 
//! @param[in] sprNum  The SPR to write
//! @param[in] value   The value to write
//-----------------------------------------------------------------------------
void      
DebugUnitSC::writeSpr (uint16_t  sprNum,
		       uint32_t  value)
{
#ifdef NOCACHE
  // Always cache the NPC
  if ((STALLED == stallState) && (SPR_NPC == sprNum))
    {
      // Have we already cached this NPC value?
      if (npcCacheIsValid && (value == npcCachedValue))
	{
	  return;
	}
      else
	{
	  npcCachedValue  = value;
	  npcCacheIsValid = true;
	}
    }
#else
  if (STALLED == stallState)
    {
      // Have we already cached this value?
      uint32_t  cachedValue;
      if (sprCache->read (sprNum, cachedValue) &&
	  (value == cachedValue))
	{
	  return;				// Already there, no more to do
	}
      else
	{
	  sprCache->write (sprNum, value, sprNum == SPR_NPC);
	}
    }
#endif
 
  // Write the SPR
  selectDebugModule (OR1K_SC_CPU0);
  writeJtagReg (sprNum, value);
 
}	// writeSpr ()
 
 
//-----------------------------------------------------------------------------
//! AND the contents of an SPR with a value
 
//! A convenience combination of read and write
 
//! @param[in] sprNum  The SPR to write
//! @param[in] value   The value to AND into the register
//-----------------------------------------------------------------------------
void      
DebugUnitSC::andSpr (uint16_t  sprNum,
		     uint32_t  value)
{
  writeSpr (sprNum, readSpr (sprNum) & value);
 
}	// andSpr ()
 
 
//-----------------------------------------------------------------------------
//! OR the contents of an SPR with a value
 
//! A convenience combination of read and write
 
//! @param[in] sprNum  The SPR to write
//! @param[in] value   The value to OR into the register
//-----------------------------------------------------------------------------
void      
DebugUnitSC::orSpr (uint16_t  sprNum,
		   uint32_t  value)
{
  writeSpr (sprNum, readSpr (sprNum) | value);
 
}	// orSpr ()
 
 
//-----------------------------------------------------------------------------
//! Read a 32-bit word from the OpenRISC 1000 memory
 
//! Select the WISHBONE scan chain, then write the register. The data is in
//! model endianness and passed on without modification.
 
//! @todo Provide code to check if the read was from a valid address.
 
//! @param[in] addr  The address to read from
 
//! @return  The 32-bit value read
//-----------------------------------------------------------------------------
uint32_t  
DebugUnitSC::readMem32 (uint32_t  addr)
{
  uint32_t  cachedValue;
 
#ifndef NOCACHE
  // Use any cached value if we are stalled.
  if ((STALLED == stallState) && memCache->read (addr, cachedValue))
    {
      return  cachedValue;		// Already there, no more to do
    }
#endif
 
  // Read the value
  selectDebugModule (OR1K_SC_WISHBONE);
  cachedValue = readJtagReg (addr);
 
#ifndef NOCACHE
  // Update the cache if we are stalled
  if (STALLED == stallState)
    {
      memCache->write (addr, cachedValue);
    }
#endif
 
  return  cachedValue;
 
}	// readMem32 ()
 
 
//-----------------------------------------------------------------------------
//! Write a 32-bit word to the OpenRISC 1000 memory
 
//! Select the WISHBONE scan chain, then write the register. The data is in
//! model endianness and passed on without modification.
 
//! @todo Provide code to check if the write was to a valid address.
 
//! @param[in] addr   The address to write to
//! @param[in] value  The 32-bit value to write
 
//! @return  True if the write was successful. For now all writes are
//           successful.
//-----------------------------------------------------------------------------
bool      
DebugUnitSC::writeMem32 (uint32_t  addr,
			 uint32_t  value)
{
#ifndef NOCACHE
  if (STALLED == stallState)
    {
      // Have we already cached this value?
      uint32_t  cachedValue;
      if (memCache->read (addr, cachedValue) &&
	  (value == cachedValue))
	{
	  return  true;				// Already there, no more to do
	}
      else
	{
	  memCache->write (addr, value);	// Write for the future
	}
    }
#endif
 
  // Write the memory
  selectDebugModule (OR1K_SC_WISHBONE);
  writeJtagReg (addr, value);
 
  return  true;
 
}	// writeMem32 ()
 
 
//-----------------------------------------------------------------------------
//! Read a byte from the OpenRISC 1000 main memory
 
//! All we can get are 32-bits words, so we have to unpick the value.
 
//! The underlying 32-bit routines take target endian arguments and return
//! target endian results. We need to convert to host endianness to access the
//! relevant byte.
 
//! @todo Provide code to check if the read was from a valid address.
 
//! @note Read bytes from memory mapped devices at your peril!
 
//! @param[in] addr  The address to read from
//! @return  The byte read
//-----------------------------------------------------------------------------
uint8_t   
DebugUnitSC::readMem8 (uint32_t  addr)
{
  uint32_t  word   = Utils::ttohl (readMem32 (addr & 0xfffffffc));
  uint8_t  *bytes  = (uint8_t *)(&word);
  int       offset = addr & 0x3;
 
  return  bytes[offset];
 
}	// readMem8 ()
 
 
//-----------------------------------------------------------------------------
//! Write a byte to the OpenRISC 1000 main memory
 
//! All we can get are 32-bits words, so we have to read the current value and
//! construct the new value to write back.
 
//! The underlying 32-bit routines take target endian arguments and return
//! target endian results. We need to convert to host endianness to alter the
//! relevant byte.
 
//! @note Write bytes to memory mapped devices at your peril!
 
//! @todo Provide code to check if the write was to a valid address.
 
//! @param[in] addr   The address to write to
//! @param[in] value  The byte to write
 
//! @return  True if the write was successful. For now all writes are
//           successful.
//-----------------------------------------------------------------------------
bool      
DebugUnitSC::writeMem8 (uint32_t  addr,
			uint8_t   value)
{
  uint32_t  currWord  = Utils::ttohl (readMem32 (addr & 0xfffffffc));
  uint8_t  *currBytes = (uint8_t *)(&currWord);
  int       offset    = addr & 0x3;
 
  currBytes[offset] = value;
 
  return  writeMem32 (addr & 0xfffffffc, Utils::htotl (currWord));
 
}	// writeMem8 ()
 
 
//-----------------------------------------------------------------------------
//! Get the debug interface CPU0 control register value
 
//! @return  The value in the RISCOP register
//-----------------------------------------------------------------------------
uint32_t     
DebugUnitSC::readRiscop ()
{
  selectDebugModule (OR1K_SC_CPU0);
 
  int  drLen;			// Size of the data register
 
  uint32_t calc_recv_crc = 0, recv_crc, status_ret;
 
  drLen = 1+4+32+52+4+32;
 
  // Initialize the register fields
  uint64_t *dRegIn  = new uint64_t [(drLen + 63) / 64];
  uint64_t *dRegOut = new uint64_t [(drLen + 63) / 64];
 
  // Write the READ command
  clearBits (dRegIn, drLen);
 
  packBits (dRegIn, 0, 1, 0);
  packBits (dRegIn, 0+1, 4, BITREV(0x3,4)); // We're reading CPU0 control reg
  uint32_t crc32_send = crc32 (dRegIn, 0+1+4, 0);
  packBits (dRegIn, 0+1+4, 32, BITREV(crc32_send,32));
 
  // Allocate a SystemC completion event
  sc_event *done  = new sc_event();
 
  // Loop until status is OK and CRCs match.
  do
    {
      TapActionDRScan *dRScan = new TapActionDRScan (done, dRegIn, drLen);
      tapActionQueue->write (dRScan);
      wait (*done);
      dRScan->getDRegOut (dRegOut);
      delete dRScan;
      status_ret = unpackBits (dRegOut,1+4+32+52,4);
      calc_recv_crc = crc32(dRegOut,52+4,1+4+32);
      recv_crc = BITREV(unpackBits (dRegOut,1+4+32+52+4,32),32);
    }
  while ((0 != status_ret) || ( calc_recv_crc != recv_crc));
 
  // All done
  uint32_t  res = BITREV(unpackBits (dRegOut, (1+4+32),2),2);
 
  delete [] dRegIn;
  delete [] dRegOut;
  delete  done;
  return  res;
}	// readRiscop ()
 
 
//-----------------------------------------------------------------------------
//! Set the RISCOP control register
 
//! Convenience function. Select the REGISTER scan chain, write the new value.
 
//! @param[in] value  The value to write into the RISCOP register
//-----------------------------------------------------------------------------
void
DebugUnitSC::writeRiscop (uint32_t  value)
{
  selectDebugModule (OR1K_SC_CPU0);
 
  int  drLen;			// Size of the data register
 
  uint32_t calc_recv_crc = 0, recv_crc, status_ret;
 
  drLen = 1+4+32+52+4+32;
 
  // Initialize the register fields
  uint64_t *dRegIn  = new uint64_t [(drLen + 63) / 64];
  uint64_t *dRegOut = new uint64_t [(drLen + 63) / 64];
 
  // Write the READ command
  clearBits (dRegIn, drLen);
 
  packBits (dRegIn, 0, 1, 0);
  packBits (dRegIn, 0+1, 4, BITREV(0x4,4)); // We're writing CPU0 control reg
  packBits (dRegIn, 5, 1, value&RISCOP_RESET); // First bit is reset
  packBits (dRegIn, 6, 1, (value&RISCOP_STALL)>>1); // Next bit is stall
  /* Next 50 bits should be zero */
  uint32_t crc32_send = crc32 (dRegIn, 1+4+52, 0);
  packBits (dRegIn, 1+4+52, 32, BITREV(crc32_send,32));
 
  // Allocate a SystemC completion event
  sc_event *done  = new sc_event();
 
  // Loop until status is OK and CRCs match.
  do
    {
      TapActionDRScan *dRScan = new TapActionDRScan (done, dRegIn, drLen);
      tapActionQueue->write (dRScan);
      wait (*done);
      dRScan->getDRegOut (dRegOut);
      delete dRScan;
      status_ret = unpackBits (dRegOut,1+4+32+52,4);
      calc_recv_crc = crc32(dRegOut,4,1+4+52+32);
      recv_crc = BITREV(unpackBits (dRegOut,1+4+52+32+4,32),32);
    }
  while ((0 != status_ret) || ( calc_recv_crc != recv_crc));
 
  delete [] dRegIn;
  delete [] dRegOut;
  delete  done;
 
}	// writeRiscop ()
 
 
//-----------------------------------------------------------------------------
//! Select a module attached to the debug module
 
//! @note Must be called from a SystemC thread, because of the use of wait()
 
//! @param[in] chain  The desired module
//-----------------------------------------------------------------------------
void
DebugUnitSC::selectDebugModule (int module)
{
 
  if (module == currentScanChain)
    {
      return;
    }
  else
    {
      currentScanChain = module;
    }
 
  sc_event        *done = new sc_event();
  TapActionIRScan *iRScan;
  TapActionDRScan *dRScan;
 
  // Create and queue the IR-Scan action for DEBUG (no CRC)
  iRScan      = new TapActionIRScan (done, DEBUG_IR, JTAG_IR_LEN);
  tapActionQueue->write (iRScan);
  wait (*done);
 
  delete iRScan;
 
  // Initialize the register fields
  uint64_t *dRegIn  = new uint64_t [(DUSEL_DR_LEN + 63) / 64];
  uint64_t *dRegOut = new uint64_t [(DUSEL_DR_LEN + 63) / 64];
 
  clearBits (dRegIn, DUSEL_DR_LEN);
  packBits (dRegIn, DUSEL_SEL_OFF, DUSEL_SEL_LEN, 0x1);
  packBits (dRegIn, DUSEL_OPCODE_OFF, DUSEL_OPCODE_LEN, bit_reverse_data(module,4));
  uint32_t crc32_send = crc32 (dRegIn, DUSEL_CRC_OFF, 0);
  packBits (dRegIn, DUSEL_CRC_OFF, DUSEL_CRC_LEN, bit_reverse_data(crc32_send,32) );
  uint32_t calc_recv_crc = 0, recv_crc, status_ret;
  // Loop until status is OK and CRCs match.
  do
    {
      TapActionDRScan *dRScan = new TapActionDRScan (done, dRegIn, DUSEL_DR_LEN);
      tapActionQueue->write (dRScan);
      wait (*done);
      dRScan->getDRegOut (dRegOut);
      delete dRScan;
      status_ret = unpackBits (dRegOut, DUSEL_RESP_STATUS_OFF, DUSEL_RESP_STATUS_LEN);
      calc_recv_crc = crc32(dRegOut, DUSEL_RESP_STATUS_LEN, DUSEL_RESP_STATUS_OFF);
      recv_crc = bit_reverse_data(unpackBits (dRegOut, DUSEL_RESP_CRC_OFF, DUSEL_RESP_CRC_LEN),32);      
    }
  while ((0 != status_ret) || ( calc_recv_crc != recv_crc));
 
  delete [] dRegIn;
  delete [] dRegOut;
  delete done;
 
}	// selectDebugModule()
 
 
//-----------------------------------------------------------------------------
//! Read a 32-bit value via the debug interface
 
//! @note Must be called from a SystemC thread, because of the use of wait()
 
//! @param[in] addr  The address of the register
 
//! @return  The register value read
//-----------------------------------------------------------------------------
uint32_t
DebugUnitSC::readJtagReg (uint32_t  addr)
{
  int  drLen;			// Size of the data register
 
  uint32_t calc_recv_crc = 0, recv_crc, status_ret;
 
  drLen = 125; // Size of write command command (bigger than data read)
 
  // Initialize the register fields
  uint64_t *dRegIn  = new uint64_t [(drLen + 63) / 64];
  uint64_t *dRegOut = new uint64_t [(drLen + 63) / 64];
 
  // Write the READ command
  clearBits (dRegIn, drLen);
 
  packBits (dRegIn, 0, 1, 0);
  packBits (dRegIn, 0+1, 4, BITREV(0x2,4)); // We're writing a command
  packBits (dRegIn, 1+4, 4, BITREV(0x6,4)); // Access type, 0x6 = 32-bit READ
  packBits (dRegIn, 1+4+4, 32, BITREV(addr,32)); // Address
  packBits (dRegIn, 1+4+4+32, 16, BITREV(0x3,16)); // Length (always 32-bit,n=(32/8)-1=3)
  uint32_t crc32_send = crc32 (dRegIn, 1+4+4+32+16, 0);
  packBits (dRegIn, 1+4+4+32+16, 32, BITREV(crc32_send,32));
 
  // Allocate a SystemC completion event
  sc_event *done  = new sc_event();
 
  // Loop until status is OK and CRCs match.
  do
    {
      TapActionDRScan *dRScan = new TapActionDRScan (done, dRegIn, 125);
      tapActionQueue->write (dRScan);
      wait (*done);
      dRScan->getDRegOut (dRegOut);
      delete dRScan;
      status_ret = unpackBits (dRegOut,1+4+4+32+16+32,4);
      calc_recv_crc = crc32(dRegOut,4,1+4+4+32+16+32);
      recv_crc = BITREV(unpackBits (dRegOut,1+4+4+32+16+32+4,32),32);
    }
  while ((0 != status_ret) || ( calc_recv_crc != recv_crc));
 
  clearBits (dRegIn, drLen);
  packBits (dRegIn, 0, 1, 0);
  packBits (dRegIn, 0+1, 4, 0x0); // We're GO'ing command
  crc32_send = crc32 (dRegIn,1+4,0);
  packBits (dRegIn, 1+4, 32, BITREV(crc32_send,32)); // CRC
 
  uint32_t  result;
  // Loop until status is OK and CRCs match.
  do
    {
      TapActionDRScan *dRScan = new TapActionDRScan (done, dRegIn, (1+4+32+36+((3+1)*8)));
      tapActionQueue->write (dRScan);
      wait (*done);
      dRScan->getDRegOut (dRegOut);
      delete dRScan;
      status_ret = BITREV(unpackBits (dRegOut,1+4+32+((3+1)*8),4),4);
      if (status_ret)
	{
	  printf("readJtagReg(): Addr: 0x%.8x Status err: 0x%x\n",addr,status_ret);
	  result = 0;
	  break;
	}
      calc_recv_crc = crc32(dRegOut,((3+1)*8)+4,1+4+32);
      recv_crc = BITREV(unpackBits (dRegOut,1+4+32+((3+1)*8)+4,32),32);
      result = BITREV(unpackBits (dRegOut, (1+4+32), ((3+1)*8)),32);
 
    }
  while ((0 != status_ret) || ( calc_recv_crc != recv_crc));
 
  // All done
 
  delete [] dRegIn;
  delete [] dRegOut;
  delete  done;
  return  result;
 
}	// readJtagReg ()
 
 
//-----------------------------------------------------------------------------
//! Write a 32-bit value via the debug interface
 
//! @note Must be called from a SystemC thread, because of the use of wait()
 
//! @param[in] addr  The address of the register
//! @param[in] data  The register data to write
//-----------------------------------------------------------------------------
void
DebugUnitSC::writeJtagReg (uint32_t  addr,
			   uint32_t  data)
{
  int  drLen;			// Size of the data register
 
  uint32_t calc_recv_crc = 0, recv_crc, status_ret;
 
  drLen = 125; // Size of write command command (bigger than data read)
 
  // Initialize the register fields
  uint64_t *dRegIn  = new uint64_t [(drLen + 63) / 64];
  uint64_t *dRegOut = new uint64_t [(drLen + 63) / 64];
 
  // Write the READ command
  clearBits (dRegIn, drLen);
 
  packBits (dRegIn, 0, 1, 0);
  packBits (dRegIn, 0+1, 4, BITREV(0x2,4)); // We're writing a command
  packBits (dRegIn, 1+4, 4, BITREV(0x2,4)); // Access type, 0x2 = 32-bit WRITE
  packBits (dRegIn, 1+4+4, 32, BITREV(addr,32)); // Address
  packBits (dRegIn, 1+4+4+32, 16, BITREV(0x3,16)); // Length (always 32-bit,n=(32/8)-1=3)
  uint32_t crc32_send = crc32 (dRegIn, 1+4+4+32+16, 0);
  packBits (dRegIn, 1+4+4+32+16, 32, BITREV(crc32_send,32));
 
  // Allocate a SystemC completion event
  sc_event *done  = new sc_event();
 
  // Loop until status is OK and CRCs match.
  do
    {
      TapActionDRScan *dRScan = new TapActionDRScan (done, dRegIn, 125);
      tapActionQueue->write (dRScan);
      wait (*done);
      dRScan->getDRegOut (dRegOut);
      delete dRScan;
      status_ret = unpackBits (dRegOut,1+4+4+32+16+32,4);
      calc_recv_crc = crc32(dRegOut,4,1+4+4+32+16+32);
      recv_crc = BITREV(unpackBits (dRegOut,1+4+4+32+16+32+4,32),32);
    }
  while ((0 != status_ret) || ( calc_recv_crc != recv_crc));
 
  clearBits (dRegIn, drLen);
  packBits (dRegIn, 0, 1, 0);
  packBits (dRegIn, 0+1, 4, 0x0); // We're GO'ing command
  packBits (dRegIn, 0+1+4, 32, BITREV(data,32)); // Add in data
  crc32_send = crc32 (dRegIn,1+4+32,0);
  packBits (dRegIn, 1+4+32, 32, BITREV(crc32_send,32)); // CRC
 
  // Loop until status is OK and CRCs match.
  do
    {
      TapActionDRScan *dRScan = new TapActionDRScan (done, dRegIn, (1+4+((3+1)*8)+32+36));
      tapActionQueue->write (dRScan);
      wait (*done);
      dRScan->getDRegOut (dRegOut);
      delete dRScan;
      status_ret = unpackBits (dRegOut,1+4+32+32,4);
      calc_recv_crc = crc32(dRegOut,4,1+4+32+32);
      recv_crc = BITREV(unpackBits (dRegOut,1+4+32+32+4,32),32);
    }
  while ((0 != status_ret) || ( calc_recv_crc != recv_crc));
 
  delete [] dRegIn;
  delete [] dRegOut;
  delete  done;
 
}	// writeJtagReg ()
 
 
//-----------------------------------------------------------------------------
//! Clear the bits in a data register
 
//! We always clear whole 64-bit words, not just the minimum number of
//! bytes. It saves all sorts of confusion when debugging code.
 
//! @note It is the caller's responsibility to make sure the date register
//!       array is large enough.
 
//! @param[in,out] regArray  The data register to clear
//! @param[in]     regBits  Size of the data register (in bits)
//-----------------------------------------------------------------------------
void
DebugUnitSC::clearBits (uint64_t  regArray[],
			int       regBits)
{
  memset ((char *)regArray, 0, ((regBits + 63) / 64) * 8);
 
}	// clearBits ()
 
 
//-----------------------------------------------------------------------------
//! Set a bit field in a data register
 
//! The field is cleared, the supplied value masked and then ored into the
//! vector.
 
//! @note It is the caller's responsibility to make sure the date register
//!       array is large enough.
 
//! @param[in,out] regArray     The data register
//! @param[in]     fieldOffset  Start of the field (in bits)
//! @param[in]     fieldBits    Size of the field (in bits)
//! @param[in]     fieldVal     Value to set in the field
//-----------------------------------------------------------------------------
void
DebugUnitSC::packBits (uint64_t  regArray[],
		       int       fieldOffset,
		       int       fieldBits,
		       uint64_t  fieldVal)
{
  fieldVal &= (1ULL << fieldBits) - 1ULL;	// Mask the supplied value
 
  int  startWord =  fieldOffset / 64;
  int  endWord   = (fieldOffset + fieldBits - 1) / 64;
 
  fieldOffset = fieldOffset % 64;		// Now refers to target word
 
  // Deal with the startWord. Get enough bits for the mask and put them in the
  // right place
  uint64_t startMask   = ((1ULL << fieldBits) - 1ULL) << fieldOffset;
 
  regArray[startWord] &= ~startMask;
  regArray[startWord] |= fieldVal << fieldOffset;
 
  // If we were all in one word, we can give up now.
  if (startWord == endWord)
    {
      return;
    }
 
  // Deal with the endWord. Get enough bits for the mask. No need to shift
  // these up - they're always at the bottom of the word
  int       bitsToDo = (fieldOffset + fieldBits) % 64;
  uint64_t  endMask  = (1ULL << bitsToDo) - 1ULL;
 
  regArray[endWord] &= ~endMask;
  regArray[endWord] |= fieldVal >> (fieldBits - bitsToDo);
 
}	// packBits ()
 
 
//-----------------------------------------------------------------------------
//! Extract a bit field from a data register
 
//! The field is cleared, the supplied value masked and then ored into the
//! vector.
 
//! @note It is the caller's responsibility to make sure the date register
//!       array is large enough.
 
//! @param[in,out] regArray     The data register
//! @param[in]     fieldOffset  Start of the field (in bits)
//! @param[in]     fieldBits    Size of the field (in bits)
 
//! @return  The value in the field
//-----------------------------------------------------------------------------
uint64_t
DebugUnitSC::unpackBits (uint64_t  regArray[],
			 int       fieldOffset,
			 int       fieldBits)
{
  int  startWord = fieldOffset / 64;
  int  endWord   = (fieldOffset + fieldBits - 1) / 64;
 
  fieldOffset = fieldOffset % 64;		// Now refers to target word
 
  // Deal with the startWord. Get enough bits for the mask and put them in the
  // right place
  uint64_t  startMask = ((1ULL << fieldBits) - 1ULL) << fieldOffset;
  uint64_t  res       = (regArray[startWord] & startMask) >> fieldOffset;
 
  // If we were all in one word, we can give up now.
  if (startWord == endWord)
    {
      res &= (1ULL << fieldBits) - 1ULL;	// Mask off any unwanted bits
      return  res;
    }
 
  // Deal with the endWord. Get enough bits for the mask. No need to shift
  // these up - they're always at the bottom of the word
  int       bitsToDo = (fieldOffset + fieldBits) % 64;
  uint64_t  endMask  = (1ULL << bitsToDo) - 1ULL;
 
  res = res | ((regArray[endWord] & endMask) << (fieldBits - bitsToDo));
  res &= (1ULL << fieldBits) - 1ULL;		// Mask off any unwanted bits
  return  res;
 
}	// unpackBits ()
 
 
//-----------------------------------------------------------------------------
//! Compute CRC-8-ATM
 
//! The data is in an array of uint64_t, for which we use the first size bits
//! to compute the CRC.
 
//! @Note I am using the same algorithm as the ORPSoC debug unit, but I
//!       believe its function is broken! I don't believe the data bit should
//!       feature in the computation of bits 2 & 1 of the new CRC.
 
//! @Note I've realized that this is an algorithm for LSB first, so maybe it
//!       is correct!
 
//! @param dataArray  The array of data whose CRC is desired
//! @param size       The number of bits in the data
//-----------------------------------------------------------------------------
uint8_t
DebugUnitSC::crc8 (uint64_t  dataArray[],
		   int       size)
{
  uint8_t    crc = 0;
 
  for (int  i = 0; i < size; i++)
    {
      uint8_t  d       = (dataArray[i / 64] >> (i % 64)) & 1;
      uint8_t  oldCrc7 = (crc >> 7) & 1;
      uint8_t  oldCrc1 = (crc >> 1) & 1;
      uint8_t  oldCrc0 = (crc >> 0) & 1;
      uint8_t  newCrc2 = d ^ oldCrc1 ^ oldCrc7;		// Why d?
      uint8_t  newCrc1 = d ^ oldCrc0 ^ oldCrc7;		// Why d?
      uint8_t  newCrc0 = d ^ oldCrc7;
 
      crc = ((crc << 1) & 0xf8) | (newCrc2 << 2) | (newCrc1 << 1) | newCrc0;
    }
 
  return crc;
 
}	// crc8 ()
 
/* Crc of current read or written data.  */
uint32_t crc_r, crc_w = 0;
 
/* Generates new crc, sending in new bit input_bit */
uint32_t 
DebugUnitSC::crc32(uint64_t dataArray[],
		   int size,
		   int offset)
{
  uint32_t crc = 0xffffffff;
  for (int  i = offset; i < size+offset; i++)
    {
      uint32_t d = ((dataArray[i / 64] >> (i % 64)) & 1) ? 0xfffffff : 0x0000000;
      uint32_t crc_32 = ((crc >> 31)&1) ? 0xfffffff : 0x0000000;
      crc <<= 1;
      crc = crc ^ ((d ^ crc_32) & DBG_CRC32_POLY);
    }
 
  return crc;
}
 
uint32_t 
DebugUnitSC::bit_reverse_swar_2(uint32_t x)
{
  return (((x&0xaaaaaaaa)>>1)|((x&0x55555555)<<1));
}
uint32_t 
DebugUnitSC::bit_reverse_swar_4(uint32_t x)
{
  x=(((x&0xaaaaaaaa)>>1)|((x&0x55555555)<<1));
  x=(((x&0xcccccccc)>>2)|((x&0x33333333)<<2));
  return x;
}
uint32_t 
DebugUnitSC::bit_reverse_swar_8(uint32_t x)
{
  x=(((x&0xaaaaaaaa)>>1)|((x&0x55555555)<<1));
  x=(((x&0xcccccccc)>>2)|((x&0x33333333)<<2));
  x=(((x&0xf0f0f0f0)>>4)|((x&0x0f0f0f0f)<<4));
  return x;
}
uint32_t 
DebugUnitSC::bit_reverse_swar_16(uint32_t x)
{
  x=(((x&0xaaaaaaaa)>>1)|((x&0x55555555)<<1));
  x=(((x&0xcccccccc)>>2)|((x&0x33333333)<<2));
  x=(((x&0xf0f0f0f0)>>4)|((x&0x0f0f0f0f)<<4));
  x=(((x&0xff00ff00)>>8)|((x&0x00ff00ff)<<8));
  return x;
}
uint32_t 
DebugUnitSC::bit_reverse_swar_32(uint32_t x)
{
  x=(((x&0xaaaaaaaa)>>1)|((x&0x55555555)<<1));
  x=(((x&0xcccccccc)>>2)|((x&0x33333333)<<2));
  x=(((x&0xf0f0f0f0)>>4)|((x&0x0f0f0f0f)<<4));
  x=(((x&0xff00ff00)>>8)|((x&0x00ff00ff)<<8));
  x=(((x&0xffff0000)>>16)|((x&0x0000ffff)<<16)); // We could be on 64-bit arch!
  return x;
}
 
uint32_t 
DebugUnitSC::bit_reverse_data(uint32_t data, int length){
  if (length == 2) return bit_reverse_swar_2(data);
  if (length == 4) return bit_reverse_swar_4(data);
  if (length == 8) return bit_reverse_swar_8(data);
  if (length == 16) return bit_reverse_swar_16(data);
  if (length == 32) return bit_reverse_swar_32(data);
  // Long and laborious way - hopefully never gets called anymore!
  uint32_t reverse=0;
  for (int i=0;i<length;i++) reverse |= (((data>>i)&1)<<(length-1-i));
  return reverse;
}