/*
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2, 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
 * General Public License for more details.
 *
 * RTLinux Debugger modifications are written by Michael Barabanov
 * (baraban@fsmlabs.com) and
 * are Copyright (C) 2000, Finite State Machine Labs Inc.
 *
 *
 * StandAlone RTLinux integration
 * New GDB Agent Functionalities. breakpoint_dif_time and FCache commands.
 * written by Vicente Esteve LLoret <viesllo@inf.upv.es> 
 * Copyright (C) Feb, 2003 OCERA Consortium.
 * 
 *
 */
 
/****************************************************************************
 *  Header: remcom.c,v 1.34 91/03/09 12:29:49 glenne Exp $
 *
 *  Module name: remcom.c $
 *  Revision: 1.34 $
 *  Date: 91/03/09 12:29:49 $
 *  Contributor:     Lake Stevens Instrument Division$
 *
 *  Description:     low level support for gdb debugger. $
 *
 *  Considerations:  only works on target hardware $
 *
 *  Written by:      Glenn Engel $
 *  Updated by:	     David Grothe <dave@gcom.com>
 *  ModuleState:     Experimental $
 *
 *  NOTES:           See Below $
 *
 *  Modified for 386 by Jim Kingdon, Cygnus Support.
 *  Compatibility with 2.1.xx kernel by David Grothe <dave@gcom.com>
 *  Integrated into 2.2.5 kernel by Tigran Aivazian <tigran@sco.com>
 *  Modified for RTLinux by Michael Barabanov (baraban@fsmlabs.com)
 *
 *  To enable debugger support, two things need to happen.  One, a
 *  call to set_debug_traps() is necessary in order to allow any breakpoints
 *  or error conditions to be properly intercepted and reported to gdb.
 *  Two, a breakpoint needs to be generated to begin communication.  This
 *  is most easily accomplished by a call to breakpoint().  Breakpoint()
 *  simulates a breakpoint by executing an int 3.
 *
 *************
 *
 *    The following gdb commands are supported:
 *
 * command          function                               Return value
 *
 *    g             return the value of the CPU registers  hex data or ENN
 *    G             set the value of the CPU registers     OK or ENN
 *
 *    mAA..AA,LLLL  Read LLLL bytes at address AA..AA      hex data or ENN
 *    MAA..AA,LLLL: Write LLLL bytes at address AA.AA      OK or ENN
 *
 *    c             Resume at current address              SNN   ( signal NN)
 *    cAA..AA       Continue at address AA..AA             SNN
 *
 *    s             Step one instruction                   SNN
 *    sAA..AA       Step one instruction from AA..AA       SNN
 *
 *    k             kill
 *
 *    ?             What was the last sigval ?             SNN   (signal NN)
 *
 * All commands and responses are sent with a packet which includes a
 * checksum.  A packet consists of
 *
 * $<packet info>#<checksum>.
 *
 * where
 * <packet info> :: <characters representing the command or response>
 * <checksum>    :: < two hex digits computed as modulo 256 sum of <packetinfo>>
 *
 * When a packet is received, it is first acknowledged with either '+' or '-'.
 * '+' indicates a successful transfer.  '-' indicates a failed transfer.
 *
 * Example:
 *
 * Host:                  Reply:
 * $m0,10#2a               +$00010203040506070809101112131415#42
 *
 ****************************************************************************/

#include <rtl_conf.h>

#if CONFIG_RTL_GDBAGENT 

#include <deblin/vhal.h>
#include <arch/hw_irq.h>
#include <arch/linkage.h>
#include <arch/cachetlb.h>
#include <arch/rtl_io.h>
#include <arch/mprot.h>

#include <string.h>
#include <rtl_sync.h>
#include <rtl_sched.h>
#include <arch/segment.h>
#include <arch/ptrace.h>
#include <arch/rtl_irqs.h>

#define strtoul 



extern unsigned int cached_irq_mask; 

#define __byte(x,y) 	(((unsigned char *)&(y))[x])
#define cached_21	(__byte(0,cached_irq_mask))
#define cached_A1	(__byte(1,cached_irq_mask))


int rtl_debug_initialized = 0;

/************************************************************************
 *
 * external low-level support routines
 * This will Get and Put Chars from/to Host Machine
 */ 

typedef void (*Function)(void);           /* pointer to a function */

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

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

#define BUFMAX 800

static int     remote_debug = 0;
static int     Exception_info = 0;

// Breakpoint Time Measurement
static unsigned long long breakpoint_end_time = 0x0;
static unsigned long long breakpoint_start_time = 0x0;
static unsigned long long breakpoint_dif_time = 0x0;
static unsigned long long rdtsc_overhead = 0x0;

/*  debug >  0 prints ill-formed commands in valid packets & checksum errors */

static const char hexchars[]="0123456789abcdef";

/* Number of bytes of registers.  */
#define NUMREGBYTES 64
/*
 * Note that this register image is in a different order than
 * the register image that Linux produces at interrupt time.
 *
 * Linux's register image is defined by struct pt_regs in ptrace.h.
 * Just why GDB uses a different order is a historical mystery.
 */
enum regnames {_EAX,		/* 0 */
	       _ECX,		/* 1 */
	       _EDX,		/* 2 */
	       _EBX,		/* 3 */
	       _ESP,		/* 4 */
	       _EBP,		/* 5 */
	       _ESI,		/* 6 */
	       _EDI,		/* 7 */
	       _PC 		/* 8 also known as eip */,
	       _PS		/* 9 also known as eflags */,
	       _CS,		/* 10 */
	       _SS,		/* 11 */
	       _DS,		/* 12 */
	       _ES,		/* 13 */
	       _FS,		/* 14 */
	       _GS};		/* 15 */



/***************************  ASSEMBLY CODE MACROS *************************/
/* 									   */


/* Put the error code here just in case the user cares.  */
int gdb_i386errcode;

/* Likewise, the vector number here (since GDB only gets the signal
   number through the usual means, and that's not very specific).  */
int gdb_i386vector = -1;

int hex(char ch)
{
  if ((ch >= 'a') && (ch <= 'f')) return (ch-'a'+10);
  if ((ch >= '0') && (ch <= '9')) return (ch-'0');
  if ((ch >= 'A') && (ch <= 'F')) return (ch-'A'+10);
  return (-1);
}


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

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

    count = 0;

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

      if (checksum != xmitcsum)
      {
	putDebugChar('-');  /* failed checksum */
      }
      else 
      {
	 putDebugChar('+');  /* successful transfer */
	 /* if a sequence char is present, reply the sequence ID */
	 if (buffer[2] == ':') 
	 {
	    putDebugChar( buffer[0] );
	    putDebugChar( buffer[1] );
	    /* remove sequence chars from buffer */
	    count = strlen(buffer);
	    for (i=3; i <= count; i++) 
	      buffer[i-3] = buffer[i];
         }
      }
    }
  } while (checksum != xmitcsum);

//  if (remote_debug)
//    printk("R:%s\n", buffer) ;
}

/* send the packet in buffer.  */


void putpacket(char * buffer)
{
  unsigned char checksum;
  int  count;
  char ch;

  /*  $<packet info>#<checksum>. */
  do {
//  if (remote_debug)
//    printk("T:%s\n", buffer) ;
    putDebugChar('$');
    checksum = 0;
    count    = 0;

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

    putDebugChar('#');
    putDebugChar(hexchars[checksum >> 4]);
    putDebugChar(hexchars[checksum % 16]);

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

}

static char  remcomInBuffer[BUFMAX];
static char  remcomOutBuffer[BUFMAX];
static short error;


void debug_error( char * format, char * parm)
{
//  if (remote_debug) printk (format,parm);
}

static void regs_to_gdb_regs(int *gdb_regs, struct pt_regs *regs)
{
    gdb_regs[_EAX] =  regs->eax;
    gdb_regs[_EBX] =  regs->ebx;
    gdb_regs[_ECX] =  regs->ecx;
    gdb_regs[_EDX] =  regs->edx;
    gdb_regs[_ESI] =  regs->esi;
    gdb_regs[_EDI] =  regs->edi;
    gdb_regs[_EBP] =  regs->ebp;
    gdb_regs[ _DS] =  regs->xds;
    gdb_regs[ _ES] =  regs->xes;
    gdb_regs[ _PS] =  regs->eflags;
    gdb_regs[ _CS] =  regs->xcs;
    gdb_regs[ _PC] =  regs->eip;
    gdb_regs[_ESP] =  (int) (&regs->esp) ;
    gdb_regs[ _SS] =  __KERNEL_DS;
    gdb_regs[ _FS] =  0xFFFF;
    gdb_regs[ _GS] =  0xFFFF;
} /* regs_to_gdb_regs */

static void gdb_regs_to_regs(int *gdb_regs, struct pt_regs *regs)
{
    regs->eax	=     gdb_regs[_EAX] ;
    regs->ebx	=     gdb_regs[_EBX] ;
    regs->ecx	=     gdb_regs[_ECX] ;
    regs->edx	=     gdb_regs[_EDX] ;
    regs->esi	=     gdb_regs[_ESI] ;
    regs->edi	=     gdb_regs[_EDI] ;
    regs->ebp	=     gdb_regs[_EBP] ;
    regs->xds	=     gdb_regs[ _DS] ;
    regs->xes	=     gdb_regs[ _ES] ;
    regs->eflags=     gdb_regs[ _PS] ;
    regs->xcs	=     gdb_regs[ _CS] ;
    regs->eip	=     gdb_regs[ _PC] ;
#if 0					/* can't change these */
    regs->esp	=     gdb_regs[_ESP] ;
    regs->xss	=     gdb_regs[ _SS] ;
    regs->fs	=     gdb_regs[ _FS] ;
    regs->gs	=     gdb_regs[ _GS] ;
#endif

} /* gdb_regs_to_regs */


int
get_char (char *addr)
{
  return *addr;
}

void
set_char ( char *addr, int val)
{
  *addr = val;
}


static volatile int real_mem_err [RTL_NR_CPUS];
static volatile int real_mem_err_expected [RTL_NR_CPUS];
#define mem_err (real_mem_err[rtl_getcpuid()])
#define mem_err_expected (real_mem_err_expected[rtl_getcpuid()])





/* Convert the memory pointed to by mem into hex, placing result in buf.
 * Return a pointer to the last char put in buf (null), in case of mem fault,
 * return 0.
 */
char* mem2hex( char* mem, char* buf, int   count)
	       
{
      int i;
      unsigned char ch;
      int may_fault = 1;

      if (may_fault)
      {
	  mem_err_expected = 1 ;
	  mem_err = 0 ;
      }
      for (i=0;i<count;i++) {
	  /* printk("%lx = ", mem) ; */

	  ch = get_char (mem++);

	  /* printk("%02x\n", ch & 0xFF) ; */
	  if (may_fault && mem_err)
	  {
//	    if (remote_debug)
//		printk("Mem fault fetching from addr %lx\n", (long)(mem-1));
	    *buf = 0 ;				/* truncate buffer */
	    return 0;
	  }
          *buf++ = hexchars[ch >> 4];
          *buf++ = hexchars[ch % 16];
      }
      *buf = 0;
      if (may_fault)
	  mem_err_expected = 0 ;
      return(buf);
}

/* convert the hex array pointed to by buf into binary to be placed in mem */
/* return a pointer to the character AFTER the last byte written */
char* hex2mem( char* buf,
	       char* mem,
	       int   count)

{
      int i;
      unsigned char ch;
      int may_fault = 1;

      if (may_fault)
      {
	  mem_err_expected = 1 ;
	  mem_err = 0 ;
      }
      for (i=0;i<count;i++) {
          ch = hex(*buf++) << 4;
          ch = ch + hex(*buf++);

	  set_char (mem++, ch);

	  if (may_fault && mem_err)
	  {
//	    if (remote_debug)
//		printk("Mem fault storing to addr %lx\n", (long)(mem-1));
	    return 0;
	  }
      }
      if (may_fault)
	  mem_err_expected = 0 ;
      return(mem);
}


/**********************************************/
/* WHILE WE FIND NICE HEX CHARS, BUILD AN INT */
/* RETURN NUMBER OF CHARS PROCESSED           */
/**********************************************/
int hexToInt(char **ptr, int *intValue)
{
    int numChars = 0;
    int hexValue;

    *intValue = 0;

    while (**ptr)
    {
        hexValue = hex(**ptr);
        if (hexValue >=0)
        {
            *intValue = (*intValue <<4) | hexValue;
            numChars ++;
        }
        else
            break;

        (*ptr)++;
    }

    return (numChars);
}

// This is the maxim number of breakpoints
#define RTL_MAX_BP 1024

static struct bp_cache_entry {
	char *mem;
	unsigned char val;
	struct bp_cache_entry *next;
} bp_cache [RTL_MAX_BP];

static struct bp_cache_entry *cache_start = 0;

int insert_bp(char *mem)
{
  int i;
  struct bp_cache_entry *e;
  int old;
  char buf[3];

  if (!mem2hex(mem, buf, 1)) {
    return EINVAL; /* memory error */
  }

  old = strtoul(buf, 0, 16);

  for (e = cache_start; e; e = e->next) {
     if (e->mem == mem) {
       return EINVAL; /* already there */
     }
  }
  
  for (i = 0; i < RTL_MAX_BP; i++) {
     if (bp_cache[i].mem == 0) {
       break;
     }
  }
  
  if (i == RTL_MAX_BP) {
    return EINVAL; /* no space */
  }
  
  bp_cache[i] . val = old;
  bp_cache[i] . mem = mem;
  bp_cache[i] . next = cache_start;
  cache_start = &bp_cache[i];

  set_char (mem, 0xcc);
  return 0;
}


int remove_bp (char *mem)
{
  struct bp_cache_entry *e = cache_start;
  struct bp_cache_entry *f = 0;
  if (!e) {
    return EINVAL;
  }
  
  if (e->mem == mem) {
    cache_start = e->next;
    f = e;
  } else {
    for (; e->next; e = e->next) {
       if (e->next->mem == mem) {
         f = e->next;
	 e->next = f->next;
	 break;
       }
    }
  }
  
  if (!f) {
    return EINVAL;
  }

  mem_err_expected = 1;
  set_char (f->mem, f->val);
  if (mem_err) {
    return EINVAL;
  }
  mem_err_expected = 0;
  return 0;
}

#define CONFIG_RTL_DEBUGGER_THREADS
#define CONFIG_RTL_DEBUGGER_Z_PROTOCOL

extern int rtl_send_exception_info;

void Process_GDB_Messages(struct pt_regs *regs)
{
  int    addr, length;
  char * ptr;
  int    newPC;
  int    gdb_regs[NUMREGBYTES/4];
  int    signo;

  if (!rtl_debug_initialized)  
  {
    breakpoint_dif_time = breakpoint_end_time - breakpoint_start_time;
    rdtsc_overhead = breakpoint_dif_time;
  } else {
    breakpoint_dif_time = breakpoint_end_time - 
	                  breakpoint_start_time - 
			  rdtsc_overhead;;
  };

  breakpoint_start_time = breakpoint_end_time;
  
  while (rtl_debug_initialized) 
  {
    error = 0;
    remcomOutBuffer[0] = 0;
    if (Exception_info) {
	strcpy(remcomInBuffer, "?");
    } else {
     getpacket(remcomInBuffer);
    };
    Exception_info = 0;
    switch (remcomInBuffer[0]) {
      case 'q' : 
         break;
      case 'H':
         break;
      case 'T':
         break;
      case 'Z': 
	 break;
      case '?' :
	 signo = SIGTRAP;
         remcomOutBuffer[0] = 'S';
         remcomOutBuffer[1] =  hexchars[signo >> 4];
         remcomOutBuffer[2] =  hexchars[signo % 16];
         remcomOutBuffer[3] = 0;
         break;
	 break;
      case 'd' : 
         break;
      case 'g' : /* return the value of the CPU registers */
         regs_to_gdb_regs(gdb_regs, regs);
         mem2hex((char*) gdb_regs, remcomOutBuffer, NUMREGBYTES);
         break;
      case 'G' : /* set the value of the CPU registers - return OK */
         hex2mem(&remcomInBuffer[1], (char*) gdb_regs, NUMREGBYTES);
         gdb_regs_to_regs(gdb_regs, regs) ;
         strcpy(remcomOutBuffer,"OK");
         break;

      /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
      case 'm' :
         ptr = &remcomInBuffer[1];
         if (hexToInt(&ptr,&addr))
           if (*(ptr++) == ',')
             if (hexToInt(&ptr,&length))
             {
               ptr = 0;
               if (!mem2hex((char*) addr, remcomOutBuffer, length)) 
	       {
	         strcpy (remcomOutBuffer, "E03");
               }
             }
          if (ptr)
          {
            strcpy(remcomOutBuffer,"E01");
          }
          break;

      /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
      case 'M' :
          ptr = &remcomInBuffer[1];
          if (hexToInt(&ptr,&addr))
            if (*(ptr++) == ',')
              if (hexToInt(&ptr,&length))
                if (*(ptr++) == ':')
                {
                  if (!hex2mem(ptr, (char*) addr, length)) 
		  {
                    strcpy (remcomOutBuffer, "E03");
                  } else {
		    strcpy(remcomOutBuffer,"OK");
                  }
                  ptr = 0;
                }
           if (ptr)
           {
             strcpy(remcomOutBuffer,"E02");
           }
           break;

     /* cAA..AA    Continue at address AA..AA(optional) */
     /* sAA..AA   Step one instruction from AA..AA(optional) */
     case 'c' :
     case 's' :
        ptr = &remcomInBuffer[1];
        if (hexToInt(&ptr,&addr))
        {
          regs->eip = addr;
        }

        newPC = regs->eip ;

          /* clear the trace bit */
        regs->eflags &= 0xfffffeff;

          /* set the trace bit if we're stepping */
        if (remcomInBuffer[0] == 's') regs->eflags |= 0x100;

        goto cleanup;

     case 'F' :
         if (!strncmp(&remcomInBuffer[1], "cacheon",7))
	 {
	    ENABLECACHE;   
	    strcpy(remcomOutBuffer,"Cache Enable");
	    break;
	 };
	 
	 if (!strncmp(&remcomInBuffer[1], "cacheoff",8))
	 {
	    DISABLECACHE;
	    strcpy(remcomOutBuffer,"Cache Disable");
	    break;
	 };
       	 
	 if (!strncmp(&remcomInBuffer[1], "simirqon",8))
	 {
            rtl_outb(0xff, 0x21);	/* restore master IRQ mask */
            rtl_outb(0xff, 0xA1);	/* restore slave IRQ mask */
	    strcpy(remcomOutBuffer,"IRQ sim on");
	    break;
	 };

	 if (!strncmp(&remcomInBuffer[1], "simirqoff",9))
	 {
            rtl_outb(cached_21, 0x21);	/* restore master IRQ mask */
            rtl_outb(cached_A1, 0xA1);	/* restore slave IRQ mask */
	    strcpy(remcomOutBuffer,"IRQ sim off");
	    break;
	 };
	 
	 if (!strncmp(&remcomInBuffer[1], "irq",3))
	 {
	    unsigned int irq_mask_bak = cached_irq_mask;
            char *ptr = &remcomInBuffer[4];
	    int numirq;
	    remcomInBuffer[6] = 0;
	    hexToInt(ptr,&numirq);
            DebugValue(numirq);	    
            cached_irq_mask=~(1<<numirq);	 
            
	    rtl_outb(cached_21, 0x21);	/* restore master IRQ mask */
            rtl_outb(cached_A1, 0xA1);	/* restore slave IRQ mask */
	    
	    cached_irq_mask = irq_mask_bak;
	    strcpy(remcomOutBuffer,"Simultating IRQ ");
	    break;
	 };
	 
	 
         
	 break;
 
      /* kill the program */
     case 'k' :
/* 	  last_module = 0; */
//	  goto cleanup;
/*      default:
	  rtl_debug_handler(); */
      } /* switch */

    /* reply to the request */
    putpacket(remcomOutBuffer);
  }

cleanup:// pthread_cleanup_pop(1);
//  rtl_hard_restore_flags(flags) ;
}

asmlinkage unsigned int StepbyStep_Handler(struct pt_regs regs)
{
  
  Exception_info = 1;
  Process_GDB_Messages(&regs);
  
  return 0;
}

asmlinkage unsigned int Breakpoint_Handler(struct pt_regs regs)
{

  Exception_info = 1;
  Process_GDB_Messages(&regs);
  
  return 0;
};

BUILD_STEP_INT();
BUILD_BREAKPOINT_INT();

void calc_rdtsc_overhead(void)
{
  __asm("int $3\n\t");
  __asm("int $3\n\t");
};

int set_debug_traps(void)
{
  if (rtl_debug_initialized) 
  {
    return -1;
  }
  set_intr_gate(1,&stepbystep_interrupt);

  set_intr_gate(3,&breakpoint_interrupt);

  calc_rdtsc_overhead();
  
//  rtl_request_traps(&rtl_debug_exception);
  rtl_debug_initialized = 1;
  return 0;
}


void unset_debug_traps(void)
{
#if 0
  for (i = 0; i < nbreak; i++) 
  {
    debugpr ("unpatching leftover breakpoints\n");
    set_char(bp_cache[i].mem, bp_cache[i].val);
  }
#endif
  // rtl_request_traps(0);
  rtl_debug_initialized = 0;
}


#endif //CONFIG_RTL_GDBAGENT