//
// GDBRDP.C
//
// Handler for GDB remote debugger interface commands
//
// Copyright (c) 2002, Jason Riffel - TotalEmbedded LLC.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without 
// modification, are permitted provided that the following conditions 
// are met:
//
// Redistributions of source code must retain the above copyright 
// notice, this list of conditions and the following disclaimer. 
//
// Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in 
// the documentation and/or other materials provided with the
// distribution. 
//
// Neither the name of TotalEmbedded nor the names of its 
// contributors may be used to endorse or promote products derived 
// from this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
// POSSIBILITY OF SUCH DAMAGE.
//

//
// INCLUDES
//
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/wait.h>
#include <netinet/in.h>
#include <netdb.h>
#include "jtagsock.h"
#include "gdbrdp.h"
#include "mips32.h"

//
// TYPEDEFS
//
typedef struct _sw_breakpoint_
{
  unsigned int  ui_address;
  unsigned int  ui_original_memory;
  unsigned char uc_enabled;
} sw_breakpoint;

typedef struct _hw_breakpoint_
{
  unsigned int  ui_address;
  unsigned char uc_enabled;
} hw_breakpoint;

//
// STATIC LOCAL VARS
//
static int           si_inputhandlerstate;
static unsigned char suc_calculatedchecksum;
static unsigned char suc_receivedchecksum;
static unsigned int  sui_numbytesreceived;
static char          sac_commandbuffer[dGDBRDP_MAX_CMD_BUFFER];
static sw_breakpoint swb_array[dGDBRDP_MAX_SW_BREAKPOINTS];
static hw_breakpoint hwb_inst_array[dMIPS32_MAX_HARDWARE_BKPTS];
static hw_breakpoint hwb_data_array[dMIPS32_MAX_HARDWARE_BKPTS];

//
// fGDBRDP_Handler
//
// The real 'main'.  This function handles executing the individual
// handlers.  (JTAG and GDB communications)
//
int fGDBRDP_Handler(int i_socket)
{
  char   c_byte;
  int    i_result;
  char   ac_responsebuffer[0x03]; // Only signal ever sent in this buffer

  // Mark all breakpoints disabled - Just use i_result
  for (i_result=0; i_result<dGDBRDP_MAX_SW_BREAKPOINTS; i_result++)
  {
    swb_array[i_result].uc_enabled=0x00;
  }
  for (i_result=0; i_result<dMIPS32_MAX_HARDWARE_BKPTS; i_result++)
  {
    hwb_inst_array[i_result].uc_enabled=0x00;
    hwb_data_array[i_result].uc_enabled=0x00;
  }
  
  // Init the target specific code and all code below it
  fMIPS32_Init();

  si_inputhandlerstate = dGDBRDP_INPUT_IDLE;

  // This routine does not return until the socket is closed
  for (;;)
  {
    i_result = fJTAGSOCK_ReadByte(i_socket, &c_byte);
    if (i_result > 0) 
    {
      if (-1 == fGDBRDP_InputHandler(i_socket, c_byte))
      {
        // End of debug statement received - Kill this socket
        // and return back waiting on another GDB session.
        close(i_socket);
        printf("GDB DETACHED.\n");
        return(1);
      }
    }

    // Run JTAG exception catcher task
    if (-1 == fMIPS32_EJTAG_Machine())
    {
      // -1 == Target entered debug mode - report to GDB a user break (0x12)
      sprintf(ac_responsebuffer, "S%02X", ui_last_signal);
      fGDBRDP_SendToGDB(i_socket, &ac_responsebuffer[0], 0x03);
    }
  }
}

//
// fGDBRDP_InputHandler
//
// This function accumulates inbound commands from GDB and launches
// the GDB command intepreter when a full command is received.
//
int fGDBRDP_InputHandler (int i_socket, char c_byte)
{
  // Input processing machine
  switch(si_inputhandlerstate)
  {
    case dGDBRDP_INPUT_IDLE:
      if (c_byte == dGDBRDP_START_OF_COMMAND) // Start of command
      {
        si_inputhandlerstate = dGDBRDP_RECEIVE_COMMAND;
        sui_numbytesreceived=0; 
        suc_calculatedchecksum=0;
        break;
      }
      else if (c_byte == dGDBRDP_CTRLC)
      {
        if (ui_ejtag_state == dMIPS32_EJTAG_WAIT_FOR_DEBUG_MODE)
        {
          // Make the catcher perform a debug break
          ui_ejtag_state = dMIPS32_EJTAG_FORCE_DEBUG_MODE;
          return(1);
        }
        else
        {
          // Processor was IN debug mode.  Shouldn't be getting this.
          printf("JTAGEMUL: Received CTRL-C while not running.\n");
          ui_ejtag_state = dMIPS32_EJTAG_WAIT_FOR_DEBUG_MODE;
          return(1);
        }
      }
      else if (c_byte == dGDBRDP_END_OF_COMMAND) // Somehow in the middle of a command
      {
        si_inputhandlerstate = dGDBRDP_OUT_OF_SYNC;
        break;
      }
      // All other data just throw away (out of sync)
      break;

    case dGDBRDP_OUT_OF_SYNC:
      // When out of sync we need to receive the 2 checksum
      // bytes and then send a NACK.  This is the first byte.
      si_inputhandlerstate = dGDBRDP_OUT_OF_SYNC_2;
      break;

    case dGDBRDP_OUT_OF_SYNC_2:
      // Last byte of checksum of a command that we are not
      // in sync with.  Just send the NACK.
      fJTAGSOCK_WriteByte(i_socket, dGDBRDP_NACK);
      si_inputhandlerstate = dGDBRDP_INPUT_IDLE;
      break;

    case dGDBRDP_RECEIVE_COMMAND:
      // Receive data into buffer until '#' is received
      if (c_byte == dGDBRDP_END_OF_COMMAND)
      {
        si_inputhandlerstate = dGDBRDP_GET_CHECKSUM;
        break;
      }

      // New data received
      suc_calculatedchecksum = suc_calculatedchecksum + c_byte;
      sac_commandbuffer[sui_numbytesreceived++]=c_byte; 
      break;

    case dGDBRDP_GET_CHECKSUM:
      // Receive the first nibble of the checksum
      suc_receivedchecksum = fGDBRDP_ConvertNibble(c_byte) << 4;
      si_inputhandlerstate = dGDBRDP_GET_CHECKSUM_2;
      break;

    case dGDBRDP_GET_CHECKSUM_2:
      // Receive the second nibble of the checksum and check it.
      suc_receivedchecksum = suc_receivedchecksum + fGDBRDP_ConvertNibble(c_byte);

      // Back to idle regardless of checksum
      si_inputhandlerstate = dGDBRDP_INPUT_IDLE;

      if (suc_receivedchecksum != suc_calculatedchecksum)
      {
        // Bad checksum calculation - Report NACK
        fJTAGSOCK_WriteByte(i_socket, dGDBRDP_NACK);
        break;
      }

      // We have a valid command, ACK it and run the handler.
      fJTAGSOCK_WriteByte(i_socket, dGDBRDP_ACK);
      return(fGDBRDP_CommandInterpreter(i_socket, &sac_commandbuffer[0], sui_numbytesreceived));
      break;
  }

  return(1);
}
 
//
// fGDBRDP_CommandInterpreter
//
// Takes a string of ui_size length that has the actual command contents from GDB
// with all the protocol stripped.  Processes the command and calls the required 
// JTAG functions and responds to GDB when necessary.
//
int fGDBRDP_CommandInterpreter(int i_socket, char *pc_command, unsigned int ui_size)
{
  unsigned int ui_responsesize;
  unsigned int ui_address;
  unsigned int ui_count;
  unsigned int ui_index;
  char         ac_responsebuffer[dGDBRDP_MAX_CMD_BUFFER];

  // Evaluate each command received from GDB  
  switch(*pc_command)
  {
    case 'k': // GDB is exiting - Kill thread
      // We are currently using this as indication that GDB is exiting.
      return(-1); // Forces exit
      break;

    case '?': // Get last signal
      // This is used to query the debug hardware as to what the last exception was
      // that entered debug mode.  We always use 05 as of now which is ABORT.
      sprintf(ac_responsebuffer, "S%02X", ui_last_signal);
      ui_responsesize=3;
      break;

    case 'g': // Read general registers
      // This command reads all the registers from the processor and returns them

      // Go read all the registers from the processor
      fMIPS32_ExecuteDebugModule(aui_readregisters_code);
      
      // Now the register array is accurate, pump it out
      for (ui_responsesize=0; ui_responsesize<(dMIPS32_NUM_DEBUG_REGISTERS*8); ui_responsesize+=8)
      {
      	sprintf(&ac_responsebuffer[ui_responsesize], "%08X", aui_registers[ui_responsesize/8]);
      }
      break;

    case 'P': // Write a single register
      // Format is Pxx=12345678 where xx is the register number
      pc_command++; // Get past the 'P'
      
      // Get the register number
      ui_index  = fGDBRDP_ConvertNibble(*pc_command++) << 4;
      ui_index += fGDBRDP_ConvertNibble(*pc_command++);

      if (*pc_command++ != '=')
      {
        printf("JTAGEMUL: Cannot sync with set single register command!\n");
        break;
      }
      
      if (ui_index < dMIPS32_NUM_DEBUG_REGISTERS)
      {
        // Valid register number so go ahead and update
        aui_registers[ui_index]  = fGDBRDP_ConvertNibble(*pc_command++) << 28;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 24;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 20;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 16;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 12;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 8;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 4;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++);

        fMIPS32_ExecuteDebugModule(aui_writeregisters_code);
      
        sprintf(ac_responsebuffer, "OK");
        ui_responsesize = 2;
      }
      else
      {
        printf("JTAGEMUL: Invalid register number %d.\n", ui_index);
      }
      break;
    
    case 'G': // Write general registers
      // This command writes all the registers in the processor
      pc_command++;
      
      for (ui_index=0; ui_index<dMIPS32_NUM_DEBUG_REGISTERS; ui_index++)
      {
        aui_registers[ui_index]  = fGDBRDP_ConvertNibble(*pc_command++) << 28;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 24;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 20;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 16;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 12;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 8;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++) << 4;
        aui_registers[ui_index] += fGDBRDP_ConvertNibble(*pc_command++);
      }
      fMIPS32_ExecuteDebugModule(aui_writeregisters_code);
      
      sprintf(ac_responsebuffer, "OK");
      ui_responsesize = 2;
      break;

    case 'H': // Set thread
      // For now just say OK!
      sprintf(ac_responsebuffer, "OK");
      ui_responsesize = 2;
      break;

    case 'q': // Get a general query
      // For now only support 'qC' that returns the current thread ID 16bit
      pc_command++;
      switch(*pc_command)
      {
        case 'C':
          // Just return a made up thread ID
          sprintf(ac_responsebuffer, "QC5A5A");
          ui_responsesize = 6;
          break;
          
        default:
          // Unsupported general query
          ui_responsesize = 0;
          break;
      }
      break;

    case 'm': // Read memory
      // OK - This command comes formatted m<addr>,<length> with both components 
      // encoded ascii hex MSB to LSB.
      ui_address = 0x00;
      ui_count   = 0x00;

      // Calculate the address
      for (ui_index=1; *(pc_command+ui_index)!=','; ui_index++)
      {
      	ui_address = ui_address << 4;
      	ui_address = ui_address + fGDBRDP_ConvertNibble(*(pc_command+ui_index));
      }
      
      // Increment the index past the ','
      ui_index++;
      
      // Calculate the count
      for ( ; ui_index<ui_size; ui_index++)
      {
      	ui_count = ui_count << 4;
      	ui_count = ui_count + fGDBRDP_ConvertNibble(*(pc_command+ui_index));
      }

      // Ok, now we have the address and count for the read.  For speed we want to
      // read as much as fast as we can.  We WANT to do 32 bit aligned WORD accesses
      // but we can't always do that.  What we will do is perform BYTE reads until
      // we are WORD aligned.  Then we will perform WORD reads as long as we can.
      // Then we will finish up with more BYTE reads until we have the entire range.
      
      ui_responsesize=0x00;
      
      // Check to see if we are WORD aligned, if not then do BYTE reads
      while (0x00 != ui_address % 0x04) // A word aligned address will result in 0 modulus
      {
        // Non zero means we are not WORD aligned and need to do byte reads
        ui_address_register=ui_address;
        fMIPS32_ExecuteDebugModule(aui_readbyte_code);
        sprintf(&ac_responsebuffer[ui_responsesize], "%02X", ui_data_register);

        ui_responsesize = ui_responsesize + 2;
        ui_address = ui_address + 1;
        ui_count = ui_count - 1;
        
        // Now we have to check to see if we are finished (for small accesses)
        if (0x00 == ui_count)
        {
          // All done, go ahead and exit
          break;
        }
      }

      // Now we are WORD aligned, we need to read WORDs as long as we have at least
      // WORDSIZE count of bytes left to read.
      while (ui_count > 0x03)
      {
        ui_address_register=ui_address;
        fMIPS32_ExecuteDebugModule(aui_readword_code);
        sprintf(&ac_responsebuffer[ui_responsesize], "%08X", ui_data_register);

        ui_responsesize = ui_responsesize + 8;
        ui_address = ui_address + 4;
        ui_count = ui_count - 4;
      }

      // Now we need to perform any BYTE reads necessary to finish the read block.
      while(ui_count)
      {
        ui_address_register=ui_address;
        fMIPS32_ExecuteDebugModule(aui_readbyte_code);
        sprintf(&ac_responsebuffer[ui_responsesize], "%02X", ui_data_register);

        ui_responsesize = ui_responsesize + 2;
        ui_address = ui_address + 1;
        ui_count = ui_count - 1;
      }

      // Finished.  Successful read of the entire block.
      break;
    
    case 'M': // Write memory
      // OK - This command comes formatted M<addr>,<length>:VALUE with all components 
      // encoded ascii hex MSB to LSB.
      ui_address = 0x00;
      ui_count   = 0x00;

      // Calculate the address
      for (ui_index=1; *(pc_command+ui_index)!=','; ui_index++)
      {
      	ui_address = ui_address << 4;
      	ui_address = ui_address + fGDBRDP_ConvertNibble(*(pc_command+ui_index));
      }
      
      // Increment the index past the ','
      ui_index++;
      
      // Calculate the count
      for ( ; *(pc_command+ui_index)!=':'; ui_index++)
      {
      	ui_count = ui_count << 4;
      	ui_count = ui_count + fGDBRDP_ConvertNibble(*(pc_command+ui_index));
      }

      // Increment the index past the ':'
      ui_index++;
      
      switch(ui_count)
      {
        case 0x01: // Byte write
          ui_address_register=ui_address;

          ui_data_register  = fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 4;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index));

          fMIPS32_ExecuteDebugModule(aui_writebyte_code);
          break;
          
        case 0x02: // Half word write
          ui_address_register=ui_address;

          ui_data_register  = fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 12;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 8;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 4;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index));

          fMIPS32_ExecuteDebugModule(aui_writehalf_code);
          break;
          
        case 0x04: // Word write
          ui_address_register=ui_address;

          ui_data_register  = fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 28;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 24;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 20;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 16;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 12;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 8;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index++)) << 4;
          ui_data_register += fGDBRDP_ConvertNibble(*(pc_command+ui_index));

          fMIPS32_ExecuteDebugModule(aui_writeword_code);
          break;
          
        default:   // Unsupported
          printf("GDBRDP ERROR: Unsupported write size of %d.\n", ui_count);
          ui_responsesize=0;
          break;
      }          

      sprintf(ac_responsebuffer, "OK");
      ui_responsesize = 2;
      break;
    
    case 'S': // This one has signal to step in it (ignore for now, just step)
    case 's': // Step (possible at address)
      // All we need to do is set the single step bit and continue - The chip will
      // only execute 1 instruction and then return to debug mode.  Just execute
      // the code to set the single step bit and then fall through into the continue
      // code that takes care of the rest.
      fMIPS32_ExecuteDebugModule(aui_singlestepset_code);
      
      // *** NOTE *** FALL THROUGH ON PURPOSE!
      
    case 'C': // This one has signal to resume in it (ignore for now, just continue)
    case 'c': // Continue (possible at address)
      // Continue is a different animal because it has no response back to GDB
      // until something happens that halts the system.  (such as a breakpoint or
      // signal)  All we need to do is have the machine exit debug mode at this 
      // point.  If it ever comes out on its own the debug mode catcher will 
      // notifiy GDB.
      if (ui_ejtag_state == dMIPS32_EJTAG_WAIT_FOR_CONTINUE)
      {
      	// Let the catcher take care of exiting debug mode.
        ui_ejtag_state = dMIPS32_EJTAG_EXIT_DEBUG_MODE;
        return(1);
      }
      else
      {
        // Processor was NOT in debug mode.  Shouldn't be getting this.
        printf("JTAGEMUL: Received continue while NOT debugging.\n");
        return(1);
      }
      break;

    case 'z': // The 'z' packet is for removing breakpoints
    case 'Z': // The 'Z' packet is for setting breakpoints
      // Z0 is for software breakpoints, Z1 is for hardware breakpoints
      if ('0' == *(pc_command+1))
      {
        // Handle software breakpoint
        if (1 == fGDBRDP_HandleSoftwareBreakpoint(pc_command, ui_size))
        { 
          // Success       
          sprintf(ac_responsebuffer, "OK");
          ui_responsesize = 2;
          break;
        }
        else
        { 
          // Failure
          sprintf(ac_responsebuffer, "E00");
          ui_responsesize = 3;
          break;
        }
      }
      if ('1' == *(pc_command+1))
      {
        // Handle hardware breakpoint
        if (1 == fGDBRDP_HandleHardwareBreakpoint(pc_command, ui_size))
        { 
          // Success       
          sprintf(ac_responsebuffer, "OK");
          ui_responsesize = 2;
          break;
        }
        else
        { 
          // Failure
          sprintf(ac_responsebuffer, "E00");
          ui_responsesize = 3;
          break;
        }
      }
      else
      {
        // Unknown - report error
        sprintf(ac_responsebuffer, "E00");
        ui_responsesize = 3;
        break;
      }
      break;
      
    default:  // Command not handled. 
      // Unhandled commands should be responded with $#00.  What we will do here
      // is set the response size to 0 and call the respond routine.  The respond
      // routine will send the $ first, then the message of nothing, then the # 
      // and the calculated checksum which will be 00.
      ui_responsesize=0;
      break;
  }

  // We always send a response to these processed commands.  If it fell through
  // to default we will send an empty packet which the spec says is correct for
  // unsupported options.
  fGDBRDP_SendToGDB(i_socket, &ac_responsebuffer[0], ui_responsesize);

  return(1);
}

//
// fGDBRDP_SendToGDB
//
// Send a message to GDB.  Takes a string to send (pc_message) of uc_length size.
// Generates all the protocol required including checksum to send the data.
//
void fGDBRDP_SendToGDB(int i_socket, char *pc_message, unsigned int ui_length)
{
  unsigned char uc_calculatedchecksum;
  
  // Start checksum at 0
  uc_calculatedchecksum = 0;
  
  // Send the leader
  fJTAGSOCK_WriteByte(i_socket, dGDBRDP_START_OF_COMMAND);
  
  while(ui_length--)
  {
    // Send each byte in the message while calculating checksum.
    fJTAGSOCK_WriteByte(i_socket, *pc_message);
    uc_calculatedchecksum = uc_calculatedchecksum + *pc_message++;
  }
  
  // Send the trailer
  fJTAGSOCK_WriteByte(i_socket, dGDBRDP_END_OF_COMMAND);
  
  // Send the upper nibble of the checksum
  fJTAGSOCK_WriteByte(i_socket, fGDBRDP_CalculateNibble(uc_calculatedchecksum >> 4));
  
  // Send the lower nibble of the checksum
  fJTAGSOCK_WriteByte(i_socket, fGDBRDP_CalculateNibble(uc_calculatedchecksum & 0x0F));

  // Forget about the ACK or NACK - Hope this is good enough.  Time will tell.  The input 
  // handler will throw out the ack/nack while waiting on a leader.
  return;
}

//
// fGDBRDP_ConvertNibble
//
// Takes one ascii encoded hex nibble and converts it to its
// decimal equivelent.
//
int fGDBRDP_ConvertNibble (char c_nibble)
{
  switch(c_nibble)
  {
    case '0': return(0);
    case '1': return(1);
    case '2': return(2);
    case '3': return(3);
    case '4': return(4);
    case '5': return(5);
    case '6': return(6);
    case '7': return(7);
    case '8': return(8);
    case '9': return(9);
    case 'a': return(10);
    case 'A': return(10);
    case 'b': return(11);
    case 'B': return(11);
    case 'c': return(12);
    case 'C': return(12);
    case 'd': return(13);
    case 'D': return(13);
    case 'e': return(14);
    case 'E': return(14);
    case 'f': return(15);
    case 'F': return(15);
    default:  return(0);
  }
}
    
//
// fGDBRDP_CalculateNibble
//
// Takes one decimal nibble and converts it to the ascii
// encoded hex equivelent.
//
char fGDBRDP_CalculateNibble (int i_nibble)
{
  switch(i_nibble)
  {
    case 0x00: return('0');
    case 0x01: return('1');
    case 0x02: return('2');
    case 0x03: return('3');
    case 0x04: return('4');
    case 0x05: return('5');
    case 0x06: return('6');
    case 0x07: return('7');
    case 0x08: return('8');
    case 0x09: return('9');
    case 0x0A: return('a');
    case 0x0B: return('b');
    case 0x0C: return('c');
    case 0x0D: return('d');
    case 0x0E: return('e');
    case 0x0F: return('f');
    default:   return('0');
  }
}

//
// fGDBRDP_HandleHardwareBreakpoint
//
// Sets or clears a hardware breakpoint
//
int  fGDBRDP_HandleHardwareBreakpoint (char *pc_command, unsigned int ui_size)
{
  unsigned int ui_index;
  unsigned int ui_address;
  
  // Calculate the address now - Its the same location for all packets.
  // Packet is formated <z/Z><0/1><,><ADDR><,><LENGTH>
  ui_address = 0x00;
  for (ui_index=3; ui_index<11; ui_index++)
  {
    ui_address = ui_address << 4;
    ui_address = ui_address + fGDBRDP_ConvertNibble(*(pc_command+ui_index));
  }

  // Make sure address is 32bit aligned
  if (0x00 != (ui_address % 4))
  {
    // Unaligned, not allowed
    printf("BREAKPOINT NOT ALIGNED!\n");
    return(-1);
  }

  // Check to see if it is a SET or REMOVE breakpoint command
  if ('Z' == *pc_command) // Then its a set command
  {
    // Set breakpoint - First check to make sure this address doesn't already
    // have a software breakpoint.  We only allow 1 breakpoint per address.
    for(ui_index=0; ui_index<dMIPS32_MAX_HARDWARE_BKPTS; ui_index++)
    {
      if (hwb_inst_array[ui_index].uc_enabled)
      {
        // Breakpoint is enabled, check to see the address
        if (hwb_inst_array[ui_index].ui_address == ui_address)
        {
          // Bad bad bad!  Already have a breakpoint at this address!
          printf("DUPLICATE HARDWARE BREAKPOINT!\n");
          return(-1);
        }
      }
    }
    
    // OK - Breakpoint is NOT a duplicate - Find an open slot
    for(ui_index=0; ui_index<dMIPS32_MAX_HARDWARE_BKPTS; ui_index++)
    {
      if (!(hwb_inst_array[ui_index].uc_enabled))
      {
        // Ok - This is an open slot - Mark enabled - Read old value - 
        // Write new value - Store address
        hwb_inst_array[ui_index].uc_enabled = 0x01;
        hwb_inst_array[ui_index].ui_address = ui_address;

        // We have to mangle the address because the au1500 only supports
        // 64 bit aligned breakpoint addresses.  This means the lower 3 bits
        // are cropped from the address.  In software at the time of the
        // debug exception we will have to look at the PC and check for an
        // address match on this breakpoint.
        ui_address &= 0xFFFFFFF8;
        
        // Now we need to set the enable bit in the watch register which is
        // in bit 2 with the address.  Just OR this bit right into the register.
        ui_cp0instwatchlo_register = ui_address;
        ui_cp0instwatchlo_register |= 0x04;

        // Set ONLY the global bit in the ui_cp0instwatchhi_register to make
        // this a global instruction watchpoing.
        ui_cp0instwatchhi_register = 0x40000000;
        
        // Write out the breakpoint registers
        fMIPS32_ExecuteDebugModule(aui_hwbkptwrite_code);

        return(1); // Success!
      }
    }

    // No open slots!
    printf("NO OPEN HARDWARE BREAKPOINT SLOTS!\n");
    return(-1);
  }
  else if ('z' == *pc_command) // Then its a remove command
  {
    // Search for the breakpoint
    for(ui_index=0; ui_index<dMIPS32_MAX_HARDWARE_BKPTS; ui_index++)
    {
      if (hwb_inst_array[ui_index].uc_enabled)
      {
        if (hwb_inst_array[ui_index].ui_address == ui_address)
        {
          // Found the breakpoint - Mark unused - Delete - Disable
          hwb_inst_array[ui_index].uc_enabled = 0x00;

          ui_cp0instwatchlo_register = 0x00;
          ui_cp0instwatchhi_register = 0x00;
        
          // Write out the breakpoint registers
          fMIPS32_ExecuteDebugModule(aui_hwbkptwrite_code);

          return(1);
        }
      }
    }
    
    // Never found the breakpoint
    printf("BREAKPOINT NOT FOUND!\n");
    return(-1);
  }

  // Should never get here
  return (-1);
}

//
// fGDBRDP_HandleHardwareBreakpoint
//
// Sets or clears a software breakpoint
//
int  fGDBRDP_HandleSoftwareBreakpoint (char *pc_command, unsigned int ui_size)
{
  unsigned int ui_index;
  unsigned int ui_address;
  
  // Calculate the address now - Its the same location for all packets.
  // Packet is formated <z/Z><0/1><,><ADDR><,><LENGTH>
  ui_address = 0x00;
  for (ui_index=3; *(pc_command+ui_index)!=','; ui_index++)
  {
    ui_address = ui_address << 4;
    ui_address = ui_address + fGDBRDP_ConvertNibble(*(pc_command+ui_index));
  }

  // Make sure address is 32bit aligned
  if (0x00 != (ui_address % 4))
  {
    // Unaligned, not allowed
    printf("BREAKPOINT NOT ALIGNED!\n");
    return(-1);
  }

  // Check to see if it is a SET or REMOVE breakpoint command
  if ('Z' == *pc_command) // Then its a set command
  {
    // Set breakpoint - First check to make sure this address doesn't already
    // have a software breakpoint.  We only allow 1 breakpoint per address.
    for(ui_index=0; ui_index<dGDBRDP_MAX_SW_BREAKPOINTS; ui_index++)
    {
      if (swb_array[ui_index].uc_enabled)
      {
        // Breakpoint is enabled, check to see the address
        if (swb_array[ui_index].ui_address == ui_address)
        {
          // Bad bad bad!  Already have a breakpoint at this address!
          printf("DUPLICATE BREAKPOINT!\n");
          return(-1);
        }
      }
    }
    
    // OK - Breakpoint is NOT a duplicate - Find an open slot
    for(ui_index=0; ui_index<dGDBRDP_MAX_SW_BREAKPOINTS; ui_index++)
    {
      if (!(swb_array[ui_index].uc_enabled))
      {
        // Ok - This is an open slot - Mark enabled - Read old value - 
        // Write new value - Store address
        swb_array[ui_index].uc_enabled = 0x01;
        
        ui_address_register=ui_address;
        fMIPS32_ExecuteDebugModule(aui_readword_code);
        swb_array[ui_index].ui_original_memory = ui_data_register;

        ui_data_register = dMIPS32_SW_BREAKPOINT_INSTRUCTION;
        fMIPS32_ExecuteDebugModule(aui_writeword_code);

        swb_array[ui_index].ui_address = ui_address;

        return(1); // Success!
      }
    }

    // No open slots!
    printf("NO OPEN SLOTS!\n");
    return(-1);
  }
  else if ('z' == *pc_command) // Then its a remove command
  {
    // Search for the breakpoint
    for(ui_index=0; ui_index<dGDBRDP_MAX_SW_BREAKPOINTS; ui_index++)
    {
      if (swb_array[ui_index].uc_enabled)
      {
        if (swb_array[ui_index].ui_address == ui_address)
        {
          // Found the breakpoint - Replace memory - Mark unused
          ui_address_register = ui_address;
          ui_data_register = swb_array[ui_index].ui_original_memory;
          fMIPS32_ExecuteDebugModule(aui_writeword_code);
          
          swb_array[ui_index].uc_enabled = 0x00;

          return(1);
        }
      }
    }
    
    // Never found the breakpoint
    printf("BREAKPOINT NOT FOUND!\n");
    return(-1);
  }
  else // Should never reach this
  {
    // Error
    return(-1);
  }
  
  // Should never reach here
  return(-1);
}