BootStrap/Boot/mon.c

/****************************************************************************
/*  F i l e   D a t a                                                        
/*                                                                           
/*  Module       : BootStrap                                                   
/*  C.I. No.     :                                                           
/*  $Revision: 1.2 $
/*  $Date: 2003/08/22 07:49:03 $
/*  Belonging to :                                                           
/*               :                                                           
/*  $RCSfile: mon.c,v $
/*  Program Type :                                                           
/*  Sub-modules  :                                                           
/*                                                                           
/****************************************************************************
/*  S W   D e v e l o p m e n t   E n v i r o n m e n t                      
/*                                                                           
/*  Host system  :                                                           
/*  SW Compiler  :                                                           
/*  $Author: sebastiani $
/*               :                                                           
/****************************************************************************
/*  U p d a t i n g                                                          
/*                                                                           
/*  $Log: mon.c,v $
/*  Revision 1.2  2003/08/22 07:49:03  sebastiani
/*  write the magic number (usefull to undestand the log start)
/*
/*  Revision 1.1.1.1  2003/08/04 09:40:21  sebastiani
/*  Imported sources laben rel. 19.06.2003 integrated with pam2
/*
/*  Revision 1.5  2002/07/25 09:24:46  zulia
/*  FM PROM - Configurated version
/*
/*  Revision 1.4  2002/07/16 07:13:32  zulia
/*  setup for 24MHz
/*  Copy the eeprom in ram at boot
/*
/*  Revision 1.3  2002/05/09 08:16:34  zulia
/*  *  acceptance release
/*
/*                                                                           
/*****************************************************************************/


/****************************************************************************

        THIS SOFTWARE IS NOT COPYRIGHTED

   HP offers the following for use in the public domain.  HP makes no
   warranty with regard to the software or it's performance and the
   user accepts the software "AS IS" with all faults.

   HP DISCLAIMS ANY WARRANTIES, EXPRESS OR IMPLIED, WITH REGARD
   TO THIS SOFTWARE INCLUDING BUT NOT LIMITED TO THE WARRANTIES
   OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

****************************************************************************/
 
/****************************************************************************
 *  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 $
 *  ModuleState:     Experimental $
 *
 *  NOTES:           See Below $
 *
 *  Modified for SPARC by Stu Grossman, Cygnus Support.
 *
 *  Further modified by Jiri Gaisler, ESA/ESTEC, to work as
 *  an embedded monitor for ERC32 targets.
 *
 *  
 *  @ 19200
 *
 *************
 *
 *    The following gdb commands are supported:
 *
 * command          function                               Return value
 *
 *    g             return the value of the CPU registers  hex data or ENN
 *    G             set the value of the CPU registers     OK or ENN
 *
 *    mAA..AA,LLLL  Read LLLL bytes at address AA..AA      hex data or ENN
 *    MAA..AA,LLLL: Write LLLL bytes at address AA.AA      OK or ENN
 *
 *    c             Resume at current address              SNN   ( signal NN)
 *    cAA..AA       Continue at address AA..AA             SNN
 *
 *    s             Step one instruction                   SNN
 *    sAA..AA       Step one instruction from AA..AA       SNN
 *
 *    k             kill
 *
 *    ?             What was the last sigval ?             SNN   (signal NN)
 *
 *    bBB..BB       Set baud rate to BB..BB        OK or BNN, then sets
 *                             baud rate
 *
 * All commands and responses are sent with a packet which includes a
 * checksum.  A packet consists of
 *
 * $<packet info>#<checksum>.
 *
 * where
 * <packet info> :: <characters representing the command or response>
 * <checksum>    :: < two hex digits computed as modulo 256 sum of <packetinfo>>
 *
 * When a packet is received, it is first acknowledged with either '+' or '-'.
 * '+' indicates a successful transfer.  '-' indicates a failed transfer.
 *
 * Example:
 *
 * Host:                  Reply:
 * $m0,10#2a               +$00010203040506070809101112131415#42
 *
 ****************************************************************************/

#include <string.h>
#include <signal.h>
#include <src/BootStrap/Boot/la695e.h>

/*============== Macros for asynchronous interrupts enable/disable =========*/
/*
 *  Standard nop
 */

#define nop() \
  do { \
    asm volatile ( "nop" ); \
  } while ( 0 )


#define SPARC_PSR_PIL_MASK  0x00000F00   /* bits  8 - 11 */

/*
 *  Get and set the PSR
 */

#define sparc_get_psr( _psr ) \
  do { \
     (_psr) = 0; \
     asm volatile( "rd %%psr, %0" :  "=r" (_psr) : "0" (_psr) ); \
  } while ( 0 )

#define sparc_set_psr( _psr ) \
  do { \
    asm volatile ( "mov  %0, %%psr " : "=r" ((_psr)) : "0" ((_psr)) ); \
    nop(); \
    nop(); \
    nop(); \
  } while ( 0 )

#define sparc_disable_interrupts( _level ) \
  do { \
    register unsigned int _newlevel; \
    \
    sparc_get_psr( _level ); \
    (_newlevel) = (_level) | SPARC_PSR_PIL_MASK; \
    sparc_set_psr( _newlevel ); \
  } while ( 0 )

#define sparc_enable_interrupts( _level ) \
  do { \
    unsigned int _tmp; \
    \
    sparc_get_psr( _tmp ); \
    _tmp &= ~SPARC_PSR_PIL_MASK; \
    _tmp |= (_level) & SPARC_PSR_PIL_MASK; \
    sparc_set_psr( _tmp ); \
  } while ( 0 )


/*===== Constant for routine to copy the application program into EEPROM ======*/

typedef unsigned char UBYTE;
typedef unsigned int  UINT;

#define PD_RAM_ADDR_START        RAM_START_EXE
#define PD_EEPROM_ADDR_START     EEPROM_START_EXE
#define PD_EEPROM_LENGHT         EEPROM_LEN_EXE
#define PD_EEPROM_PAGE           0x200   /* EEPROM page Lenght 128 words 32bit */
#define PD_EEPROM_PAGE_W         0x80    /* EEPROM page Lenght 128 word        */
#define PD_EEPROM_LAST_PAGE      (PD_EEPROM_LENGHT - PD_EEPROM_PAGE)
#define WAIT_FOR_20ms            0x19700 /* at 24 MHz */

#define CD_EEPROM_WRITE_ENABLE  (*(unsigned short* )0x10000310 =0x0020)
#define CD_EEPROM_WRITE_DISABLE (*(unsigned short* )0x10000310 =0x0120)

#define word  (unsigned int *)
#define MEC_REG_BASE_ADDR      0x01f80000
#define CRIMEA_REG_BASE_ADDR   0x10000000
unsigned int *MEM_CONF_REG =   word (MEC_REG_BASE_ADDR + 0x10);
unsigned int *IO_CONF_REG =    word (MEC_REG_BASE_ADDR + 0x14);
unsigned int *CONFIG_LOW =     word (CRIMEA_REG_BASE_ADDR + 0x310);
unsigned int *EE_WAIT =        word (CRIMEA_REG_BASE_ADDR + 0x340);

#ifdef LEON
#define START_ADDR 0x40000000
#else
#define START_ADDR 0x2000000
#endif

const char version[]  = "  rdbmon v1.0 by LABEN SpA  -   Starting monitor\n\n\r";
const char loadprg[]  = "  rdbmon v1.0 by LABEN SpA  -   Loading  application\n\n\r";
const char copyprg[]  = "  rdbmon v1.0 by LABEN SpA  -   Loading  application into EEPROM\n\n\r";
const char debugprg[] = "  rdbmon v1.0 by LABEN SpA  -   Loading  application into RAM\n\n\r";
const char startprg[] = "  rdbmon v1.0 by LABEN SpA  -   Starting application\n\n\r";

extern int aux;

/* ^^^^^^^^^^^^ p r o t o t y p e s ^^^^^^^^^^^^  */
unsigned short PD_ifcrcComp (unsigned char* adrs, unsigned short Crc);
void PD_ifCopyRamToEEprom   (UINT* Source, UINT* Dest, UINT Len);
void PD_ifLoad              (void);
void PD_ifLoadFromEeprom    (void);


/*=========================================================================*/
/*============================== START CODE ===============================*/

void exceptionHandler(ex_num,ex_add,tbr)
unsigned char   ex_num;
unsigned int    *ex_add,tbr;
{
    unsigned int *t_add;
    
    t_add = (unsigned int *) ((tbr & ~0x0fff) | ((unsigned int) ex_num << 4));
    *t_add = 0xA010000F;    /* or %o7,%g0,%l0 */
    t_add++;
    *t_add = (0x40000000 | (((unsigned int) (ex_add-t_add)) )); /* call _ex_add */
    t_add++;
    *t_add = 0x9E100010;    /* or %l0,%g0,%o7 */
}

void flush_i_cache()
{
/*  asm("flush");     */
}

unsigned char getDebugChar()
{
    char c;

    read(3,&c,1);
    return(c);
}

int putDebugChar(c)
        unsigned char c;
{
        return(write(4,&c,1));
}

void puts(s)
    char *s;
{
    while (*s) {
        write(1,s,1);
        s++;
    }
}

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

static int initialized = 0; /* !0 means we've been initialized */

static void set_mem_fault_trap();

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

#define NUMREGS 72

/* Number of bytes of registers.  */
#define NUMREGBYTES (NUMREGS * 4)
enum regnames {  G0, G1, G2, G3, G4, G5, G6, G7,
         O0, O1, O2, O3, O4, O5, SP, O7,
         L0, L1, L2, L3, L4, L5, L6, L7,
         I0, I1, I2, I3, I4, I5, FP, I7,

         F0, F1, F2, F3, F4, F5, F6, F7,
         F8, F9, F10, F11, F12, F13, F14, F15,
         F16, F17, F18, F19, F20, F21, F22, F23,
         F24, F25, F26, F27, F28, F29, F30, F31,
         Y, PSR, WIM, TBR, PC, NPC, FPSR, CPSR };

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

extern void trap128(), systrap(), trap_low();
unsigned int trapstack;

asm("

    .data
    .align  4

in_trap_handler:
    .word   0
tctrl:
    .word 0
    .word 0


! This function is called when any SPARC trap (except window overflow or
! underflow) occurs.  It makes sure that the invalid register window is still
! available before jumping into C code.  It will also restore the world if you
! return from handle_exception.

    ");

#ifdef LEON
    asm("
    .text

    .globl _trap_low
_trap_low:

    .align 4
    set 0x80000000, %l3     ! stop timers
    ld  [%l3 + 0x48], %l4
    set tctrl, %l5
    st  %g0, [%l3 + 0x48]
    st  %l4, [%l5]
    ld  [%l3 + 0x58], %l4
    st  %g0, [%l3 + 0x58]
    st  %l4, [%l5+4]
    ");
#else
    asm("
    .text

    .globl _trap_low
_trap_low:

    set 0x01f80000, %l3     ! stop timers
    st  %g0, [%l3 + 0x98]
    ");
#endif
    asm("
    .text

    mov %psr, %l0
    mov %wim, %l3

    srl %l3, %l0, %l4       ! wim >> cwp
    cmp %l4, 1
    bne window_fine     ! Branch if not in the invalid window
    nop

! Handle window overflow

    mov %g1, %l4        ! Save g1, we use it to hold the wim
    srl %l3, 1, %g1     ! Rotate wim right
    sll %l3, 8-1, %l5
    or  %l5, %g1, %g1

    save    %g0, %g0, %g0       ! Slip into next window
    mov %g1, %wim       ! Install the new wim

    std %l0, [%sp + 0 * 4]  ! save L & I registers
    std %l2, [%sp + 2 * 4]
    std %l4, [%sp + 4 * 4]
    std %l6, [%sp + 6 * 4]

    std %i0, [%sp + 8 * 4]
    std %i2, [%sp + 10 * 4]
    std %i4, [%sp + 12 * 4]
    std %i6, [%sp + 14 * 4]

    restore             ! Go back to trap window.
    mov %l4, %g1        ! Restore %g1

window_fine:
    sethi   %hi(in_trap_handler), %l4
    ld  [%lo(in_trap_handler) + %l4], %l5
    tst %l5
    bg  recursive_trap
    inc %l5

    set _trapstack, %sp ! Switch to trap stack
    ld  [%sp], %sp

recursive_trap:
    st  %l5, [%lo(in_trap_handler) + %l4]
    sub %sp,(16+1+6+1+72)*4,%sp ! Make room for input & locals
                    ! + hidden arg + arg spill
                    ! + doubleword alignment
                    ! + registers[72] local var

    std %g0, [%sp + (24 + 0) * 4] ! registers[Gx]
    std %g2, [%sp + (24 + 2) * 4]
    std %g4, [%sp + (24 + 4) * 4]
    std %g6, [%sp + (24 + 6) * 4]

    std %i0, [%sp + (24 + 8) * 4] ! registers[Ox]
    std %i2, [%sp + (24 + 10) * 4]
    std %i4, [%sp + (24 + 12) * 4]
    std %i6, [%sp + (24 + 14) * 4]
    mov %y, %l4
    mov %tbr, %l5
    st  %l4, [%sp + (24 + 64) * 4] ! Y
    st  %l0, [%sp + (24 + 65) * 4] ! PSR
    st  %l3, [%sp + (24 + 66) * 4] ! WIM
    st  %l5, [%sp + (24 + 67) * 4] ! TBR
    st  %l1, [%sp + (24 + 68) * 4] ! PC
    st  %l2, [%sp + (24 + 69) * 4] ! NPC

                    ! F0->F31 if FPU enabled
    srl %l0, 12, %l4
    andcc   %l4, 1, %g0
    be  1f
    nop
    std %f0, [%sp + (24 + 32) * 4]
    std %f2, [%sp + (24 + 34) * 4]
    std %f4, [%sp + (24 + 36) * 4]
    std %f6, [%sp + (24 + 38) * 4]
    std %f8, [%sp + (24 + 40) * 4]
    std %f10, [%sp + (24 + 42) * 4]
    std %f12, [%sp + (24 + 44) * 4]
    std %f14, [%sp + (24 + 46) * 4]
    std %f16, [%sp + (24 + 48) * 4]
    std %f18, [%sp + (24 + 50) * 4]
    std %f20, [%sp + (24 + 52) * 4]
    std %f22, [%sp + (24 + 54) * 4]
    std %f24, [%sp + (24 + 56) * 4]
    std %f26, [%sp + (24 + 58) * 4]
    std %f28, [%sp + (24 + 60) * 4]
    std %f30, [%sp + (24 + 62) * 4]
    st  %fsr, [%sp + (24 + 70) * 4] ! FSR
                    ! CPSR not impl

1:
    or  %l0, 0xf20, %l4
    mov %l4, %psr       ! Turn on traps, disable interrupts

    call    _handle_exception
    add %sp, 24 * 4, %o0    ! Pass address of registers

! Reload all of the registers that aren't on the stack

    ld  [%sp + (24 + 1) * 4], %g1 ! registers[Gx]
    ldd [%sp + (24 + 2) * 4], %g2
    ldd [%sp + (24 + 4) * 4], %g4
    ldd [%sp + (24 + 6) * 4], %g6

    ldd [%sp + (24 + 8) * 4], %i0 ! registers[Ox]
    ldd [%sp + (24 + 10) * 4], %i2
    ldd [%sp + (24 + 12) * 4], %i4
    ldd [%sp + (24 + 14) * 4], %i6

    
    ldd [%sp + (24 + 64) * 4], %l0 ! Y & PSR
    srl %l1, 12, %l1
    andcc   %l1, 1, %g0
    be  1f
    nop
    ldd [%sp + (24 + 32) * 4], %f0 ! F0->F31
    ldd [%sp + (24 + 34) * 4], %f2
    ldd [%sp + (24 + 36) * 4], %f4
    ldd [%sp + (24 + 38) * 4], %f6
    ldd [%sp + (24 + 40) * 4], %f8
    ldd [%sp + (24 + 42) * 4], %f10
    ldd [%sp + (24 + 44) * 4], %f12
    ldd [%sp + (24 + 46) * 4], %f14
    ldd [%sp + (24 + 48) * 4], %f16
    ldd [%sp + (24 + 50) * 4], %f18
    ldd [%sp + (24 + 52) * 4], %f20
    ldd [%sp + (24 + 54) * 4], %f22
    ldd [%sp + (24 + 56) * 4], %f24
    ldd [%sp + (24 + 58) * 4], %f26
    ldd [%sp + (24 + 60) * 4], %f28
    ldd [%sp + (24 + 62) * 4], %f30
    ld  [%sp + (24 + 70) * 4], %fsr
1:
    ldd [%sp + (24 + 64) * 4], %l0 ! Y & PSR
    ldd [%sp + (24 + 68) * 4], %l2 ! PC & NPC

    restore             ! Ensure that previous window is valid
    save    %g0, %g0, %g0       !  by causing a window_underflow trap

    mov %l0, %y
    mov %l1, %psr       ! Make sure that traps are disabled
                    ! for rett
    nop; nop; nop

    sethi   %hi(in_trap_handler), %l4
    ld  [%lo(in_trap_handler) + %l4], %l5
    dec %l5
    st  %l5, [%lo(in_trap_handler) + %l4]
    ");

#ifdef LEON
    asm("
    .text

    set tctrl, %l5
    ld  [%l5], %l4
    set 0x80000000, %l6     ! start timers
    st  %l4, [%l6 + 0x48]
    ld  [%l5 + 0x4], %l4
    st  %l4, [%l6 + 0x58]
    set 4, %l4
    st  %l4, [%l6 + 0x9C]
    ld  [%l6 + 0x90], %l5
    or  %l5, %l4, %l5
    st  %l5, [%l6 + 0x90]
    ");

#else
    asm("
    .text

    mov %tbr, %l4
    andn    %l4, 0x0fff, %l4
    or  %l4, 0x07c0, %l4
    ld  [%l4], %l4
    and %l4, 0x0505, %l4        ! Start timers
    set 0x01f80000, %l5
    st  %l4, [%l5 + 0x98]
    ");

#endif

    asm("
    .text

    jmpl    %l2, %g0        ! Restore old PC
    rett    %l3         ! Restore old nPC

    ! handle sys traps

    .globl _systrap
_systrap:
    mov %psr, %l0
    subcc   %g1, 1, %g0
    be  _trap_low
    mov %l0, %psr
    nop
    ba  _trap128
    nop
    .data
    .globl _trap128
_trap128:
    .word 0,0,0,0
    .text

");

/* Convert ch from a hex digit to an int */

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

/* scan for the sequence $<data>#<checksum>     */

static void
getpacket(buffer)
     char *buffer;
{
  unsigned char checksum;
  unsigned char xmitcsum;
  int i;
  int count;
  unsigned char ch;

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

      checksum = 0;
      xmitcsum = -1;

      count = 0;

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

      if (count >= BUFMAX)
    continue;

      buffer[count] = 0;

      if (ch == '#')
    {
      xmitcsum = hex(getDebugChar()) << 4;
      xmitcsum |= hex(getDebugChar());
#if 0
      /* Humans shouldn't have to figure out checksums to type to it. */
      putDebugChar ('+');
      return;
#endif
      if (checksum != xmitcsum)
        putDebugChar('-');  /* failed checksum */
      else
        {
          putDebugChar('+'); /* successful transfer */
          /* if a sequence char is present, reply the sequence ID */
          if (buffer[2] == ':')
        {
          putDebugChar(buffer[0]);
          putDebugChar(buffer[1]);
          /* remove sequence chars from buffer */
          count = strlen(buffer);
          for (i=3; i <= count; i++)
            buffer[i-3] = buffer[i];
        }
        }
    }
    }
  while (checksum != xmitcsum);
}

/* send the packet in buffer.  */

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

  /*  $<packet info>#<checksum>. */
  do
    {
      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 & 0xf]);

    }
  while (getDebugChar() != '+');
}

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

/* Indicate to caller of mem2hex or hex2mem that there has been an
   error.  */
static volatile int mem_err = 0;

/* 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.
 * If MAY_FAULT is non-zero, then we will handle memory faults by returning
 * a 0, else treat a fault like any other fault in the stub.
 */

static unsigned char *
mem2hex(mem, buf, count, may_fault)
     unsigned char *mem;
     unsigned char *buf;
     int count;
     int may_fault;
{
  unsigned char ch;

  set_mem_fault_trap(may_fault);

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

  *buf = 0;

  set_mem_fault_trap(0);

  return buf;
}

static unsigned char *
memw2hex(mem, buf, count, may_fault)
     unsigned char *mem;
     unsigned char *buf;
     int count;
     int may_fault;
{
  unsigned char ch;
  unsigned int memw, *memp;
  int i, first, last;

  if (!(((int) mem) & 3) && !(count & 3)) {
    set_mem_fault_trap(may_fault);

    memp = (int *) mem;
    while (count > 0)
    {
      memw = *memp++;
      if (mem_err) {
    set_mem_fault_trap(0);
    return 0;
      }
      for (i=28; i>=0; i-=4) {
    ch = (memw >> i) & 0xf;
        *buf++ = hexchars[ch];
      }
      count -= 4;
    }

    *buf = 0;

    set_mem_fault_trap(0);
    return buf;

  } else return(mem2hex(mem, buf, count, may_fault));
}

/* 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 */

static char *
hex2mem(buf, mem, count, may_fault)
     unsigned char *buf;
     unsigned char *mem;
     int count;
     int may_fault;
{
  int i;
  unsigned char ch;
  unsigned int memw;

  set_mem_fault_trap(may_fault);

  if ((count == 4) && ((((int) mem) & 3) == 0)) {
    memw = hex(*buf++);
    for (i=0; i<7; i++) {
      memw <<= 4;
      memw |= hex(*buf++);
    }
    *((int *)mem) = memw;
    dummy(memw);
    if (mem_err) {
      set_mem_fault_trap(0);
      return 0;
    }
  } else for (i=0; i<count; i++)
    {
      ch = hex(*buf++) << 4;
      ch |= hex(*buf++);
      *mem++ = ch;
      dummy(ch);
      if (mem_err) {
    set_mem_fault_trap(0);
    return 0;
      }
    }

  set_mem_fault_trap(0);

  return mem;
}

dummy(memw)  /* needed to avoid some gcc optimisations */
int memw;
{}

/* This table contains the mapping between SPARC hardware trap types, and
   signals, which are primarily what GDB understands.  It also indicates
   which hardware traps we need to commandeer when initializing the stub. */

static struct hard_trap_info
{
  unsigned char tt;     /* Trap type code for SPARClite */
  unsigned char signo;      /* Signal that we map this trap into */
} hard_trap_info[] = {
  {1, SIGSEGV},         /* instruction access error */
  {2, SIGILL},          /* illegal instruction */
  {3, SIGILL},          /* privileged instruction */
  {4, SIGEMT},          /* fp disabled */
  {36, SIGEMT},         /* cp disabled */
  {7, SIGBUS},          /* mem address not aligned */
  {9, SIGSEGV},         /* data access exception */
  {10, SIGEMT},         /* tag overflow */
#ifdef LEON
  {0x12, SIGINT},       /* IRQ 2, UART B (used for host interrupt)*/
#else
  {0x15, SIGINT},       /* IRQ 5, UART B (used for host interrupt)*/
#endif
/*  {128, SIGSYS},      /* ta 0 - systrap used for exit */
  {128+1, SIGTRAP},     /* ta 1 - normal breakpoint instruction */
  {128+16, SIGHUP},     /* user define ta 16 - initialise */
  {0, 0}            /* Must be last */
};

/* Set up exception handlers for tracing and breakpoints */

void
set_debug_traps()
{
  struct hard_trap_info *ht;

  for (ht = hard_trap_info; ht->tt; ht++)
    exceptionHandler(ht->tt, trap_low, rdtbr());

  /* In case GDB is started before us, ack any packets (presumably
     "$?#xx") sitting there.  */

  putDebugChar ('+');

  initialized = 1;
}

asm ("
! Trap handler for memory errors.  This just sets mem_err to be non-zero.  It
! assumes that %l1 is non-zero.  This should be safe, as it is doubtful that
! 0 would ever contain code that could mem fault.  This routine will skip
! past the faulting instruction after setting mem_err.

    .text
    .align 4

_fltr_set_mem_err:
    sethi %hi(_mem_err), %l0
    st %l1, [%l0 + %lo(_mem_err)]
    jmpl %l2, %g0
    rett %l2+4
");

static void
set_mem_fault_trap(enable)
     int enable;
{
  extern void fltr_set_mem_err();
  mem_err = 0;

  if (enable)
    exceptionHandler(9, fltr_set_mem_err,rdtbr());
  else
    exceptionHandler(9, trap_low,rdtbr());
}

/* Convert the SPARC hardware trap type code to a unix signal number. */

static int
computeSignal(tt)
     int tt;
{
  struct hard_trap_info *ht;

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

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

/*
 * While we find nice hex chars, build an int.
 * Return number of chars processed.
 */

static int
hexToInt(char **ptr, int *intValue)
{
  int numChars = 0;
  int hexValue;

  *intValue = 0;

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

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

      (*ptr)++;
    }

  return (numChars);
}

/*
 * This function does all command procesing for interfacing to gdb.  It
 * returns 1 if you should skip the instruction at the trap address, 0
 * otherwise.
 */

extern int text_start;

static void
handle_exception (registers)
     unsigned long *registers;
{
    int tt;         /* Trap type */
    int sigval;
    int addr;
    int length;
    int i;
    int ustat;
    char *ptr, resp;
    unsigned long *sp;
    static unsigned long userfp;
    static unsigned long usersp;
    static unsigned long freq;
    struct hard_trap_info *ht;
#ifdef LEON
    volatile unsigned int *leon = (unsigned int *) 0x80000000;
#else
    volatile unsigned int *mec = (unsigned int *) 0x01f80000;
#endif
    unsigned long *newsp, psr;
    int *t128new, *t128org;

/* First, we must force all of the windows to be spilled out */

    asm("   
    save %sp, -64, %sp
    save %sp, -64, %sp
    save %sp, -64, %sp
    save %sp, -64, %sp
    save %sp, -64, %sp
    save %sp, -64, %sp
    save %sp, -64, %sp
    save %sp, -64, %sp
    restore
    restore
    restore
    restore
    restore
    restore
    restore
    restore
    ");


    sp = (unsigned long *)registers[SP];

    tt = (registers[TBR] >> 4) & 0xff;
 
    sigval = computeSignal(tt);
    ptr = remcomOutBuffer;

    switch (tt) {
#ifdef LEON
    case 0x12:      /* UART B interrupt, used to break into running app  */
    /* If no data arrived just continue (TX empty irq) */
    ustat = leon[0x84/4];
    leon[0x84/4] = 0;   /* clear any pending errors */
    if ((ustat & 0x1) == 0) {
        return;
    }
#else
    case 0x15:      /* UART B interrupt, used to break into running app  */
    /* If no data arrived just continue (TX empty irq) */
    ustat = mec[0xE8/4];
    if ((ustat & 0x710000) == 0) {
        return;
    }
    /* Report that we stopped with signal = 0 */
    if (mec[0xE8/4] & 0x700000) {   /* Clear UART B if errors */
        mec[0xE8/4] = 0x800000;
        mec[0] = mec[0];        /* Re-program baudrate */
            putDebugChar('-');      /* Initiate retransmitt */
    }
#endif
    remcomOutBuffer[0] = 'S';
    remcomOutBuffer[1] = hexchars[sigval >> 4];
    remcomOutBuffer[2] = hexchars[sigval & 0xf];
    remcomOutBuffer[3] = 0;
        putpacket(remcomOutBuffer);
    break;
    case 0x03:      /* privileged instruction */
        for (ht = hard_trap_info; ht->tt; ht++)
            exceptionHandler(ht->tt, trap_low, registers[G1]);
        t128new = (int *) trap128;
        t128org = (int *) registers[G1];
    t128new[0] = t128org[512]; t128new[1] = t128org[513];
    t128new[2] = t128org[514]; t128new[3] = t128org[515];
        exceptionHandler(128, systrap, registers[G1]);
        registers[PSR] |= 0x40;
    *((int *)(registers[G1] + 0x7e0)) = freq;   /* pass freq to app */
        return;
        break;
    case (128 + 16) : /* Initialise */
        registers[PC] = START_ADDR;
        registers[NPC] = registers[PC] + 4;
        registers[PSR] &= ~0x40;
        userfp = sp[14] = (rdtbr() & ~0x0fff) - 16;      /* FP = user stack */
        usersp = registers[SP] = sp[14] - 96*4;
    freq = *((int *)((rdtbr() & ~0x0fff) + 0x7e0)); /* get current freq */
        sp[7] = 0;
        sigval = 0;
        resp = 'W';
        *ptr++ = resp;
        *ptr++ = hexchars[(registers[O0] >> 4) & 0xf];
        *ptr++ = hexchars[registers[O0] & 0xf];
        *ptr++ = 0;
        putpacket(remcomOutBuffer);
        break;
    case 128:
    switch (registers[G1]) {
        case (1) : /* exit() */
            resp = 'W';
            *ptr++ = resp;
            *ptr++ = hexchars[(registers[O0] >> 4) & 0xf];
            *ptr++ = hexchars[registers[O0] & 0xf];
            *ptr++ = 0;
        /*
            registers[PC] = 0x2000000;
            registers[NPC] = registers[PC] + 4;
            registers[PSR] &= ~0x40;
            sp[14] = userfp;
            registers[SP] = usersp;
        */
            break;
        case (4) :  /* write(fd, buf, size) */
            resp = 'O';
            *ptr++ = resp;
            for (i=0;i<registers[O2];i++) 
            *ptr++ = *((char *) (registers[O1]+i));
            *ptr++ = 0;
            putpacket(remcomOutBuffer);
            registers[PC] = registers[NPC];
            registers[NPC] += 4;
            registers[O0] = registers[O2];
            return;
        default:    /* Not supported systraps */
            resp = 'W';
            *ptr++ = resp;
            *ptr++ = hexchars[(registers[O0] >> 4) & 0xf];
            *ptr++ = hexchars[registers[O0] & 0xf];
            *ptr++ = 0;
            break;
    }


        putpacket(remcomOutBuffer);
        break;

    default:
  /* reply to host that an exception has occurred */

    ptr = remcomOutBuffer;

    *ptr++ = 'T';
    *ptr++ = hexchars[sigval >> 4];
    *ptr++ = hexchars[sigval & 0xf];

    *ptr++ = hexchars[PC >> 4];
    *ptr++ = hexchars[PC & 0xf];
    *ptr++ = ':';
    ptr = mem2hex((char *)&registers[PC], ptr, 4, 0);
    *ptr++ = ';';

    *ptr++ = hexchars[FP >> 4];
    *ptr++ = hexchars[FP & 0xf];
    *ptr++ = ':';
    ptr = mem2hex(sp + 8 + 6, ptr, 4, 0); /* FP */
    *ptr++ = ';';

    *ptr++ = hexchars[SP >> 4];
    *ptr++ = hexchars[SP & 0xf];
    *ptr++ = ':';
    ptr = mem2hex((char *)&sp, ptr, 4, 0);
    *ptr++ = ';';

    *ptr++ = hexchars[NPC >> 4];
    *ptr++ = hexchars[NPC & 0xf];
    *ptr++ = ':';
    ptr = mem2hex((char *)&registers[NPC], ptr, 4, 0);
    *ptr++ = ';';

    *ptr++ = hexchars[O7 >> 4];
    *ptr++ = hexchars[O7 & 0xf];
    *ptr++ = ':';
    ptr = mem2hex((char *)&registers[O7], ptr, 4, 0);
    *ptr++ = ';';

    *ptr++ = 0;

    putpacket(remcomOutBuffer);
    
    }


    while (1) {
        remcomOutBuffer[0] = 0;

        getpacket(remcomInBuffer);
        switch (remcomInBuffer[0]) {
    case '?':       /* Status */
        remcomOutBuffer[0] = 'S';
        remcomOutBuffer[1] = hexchars[sigval >> 4];
        remcomOutBuffer[2] = hexchars[sigval & 0xf];
        remcomOutBuffer[3] = 0;
        break;

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

    case 'g':       /* return the value of the CPU registers */
        ptr = remcomOutBuffer;
        ptr = mem2hex((char *)registers, ptr, 16 * 4, 0); /* G & O regs */
        ptr = mem2hex(sp + 0, ptr, 16 * 4, 0); /* L & I regs */
        ptr = mem2hex((char *)&registers[F0], ptr, 32 * 4, 0);/* FLOAT */   
                /* Y, PSR, WIM, TBR, PC, NPC, FPSR, CPSR */
        mem2hex((char *)&registers[Y], ptr, 8 * 4, 0);  
        break;
    case 'G':      /* set the value of the CPU registers - return OK */

        psr = registers[PSR];

        ptr = &remcomInBuffer[1];
        hex2mem(ptr, (char *)registers, 16 * 4, 0); /* G & O regs */
        hex2mem(ptr + 16 * 4 * 2, sp + 0, 16 * 4, 0); /* L & I regs */
        hex2mem(ptr + 32 * 4 * 2, (char *)&registers[F0], 32 * 4, 0); /* Floating point */
        hex2mem(ptr + 64 * 4 * 2, (char *)&registers[Y],
            8 * 4, 0);  /* Y, PSR, WIM, TBR, PC, NPC, FPSR, CPSR */

        /* See if the stack pointer has moved.  If so, then copy the saved
           locals and ins to the new location.  This keeps the window
           overflow and underflow routines happy.  */

        newsp = (unsigned long *)registers[SP];
        if (sp != newsp)
          sp = memcpy(newsp, sp, 16 * 4);

        /* Don't allow CWP to be modified. */

        if (psr != registers[PSR])
          registers[PSR] = (psr & 0x1f) | (registers[PSR] & ~0x1f);

        strcpy(remcomOutBuffer,"OK");
        break;
    case 'm':     /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
      /* Try to read %x,%x.  */

        ptr = &remcomInBuffer[1];
        if (hexToInt(&ptr, &addr) && *ptr++ == ','
            && hexToInt(&ptr, &length))
        {
            if (memw2hex((char *)addr, remcomOutBuffer, length, 1)) break;
            strcpy (remcomOutBuffer, "E03");
        } else
            strcpy(remcomOutBuffer,"E01");
        break;

    case 'M': /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
      /* Try to read '%x,%x:'.  */

        ptr = &remcomInBuffer[1];
        if (hexToInt(&ptr, &addr) && *ptr++ == ','
          && hexToInt(&ptr, &length) && *ptr++ == ':')
        {
            if (hex2mem(ptr, (char *)addr, length, 1))
            strcpy(remcomOutBuffer, "OK");
            else
        strcpy(remcomOutBuffer, "E03");
        } else
            strcpy(remcomOutBuffer, "E02");
        break;

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

        if (*((unsigned int*)0x23f7ffc)==0x210553)
        {
            puts(copyprg);
            PD_ifLoad();
            *((unsigned int*)0x23f7ffc)=0x00;
        }
        ptr = &remcomInBuffer[1];
        if (hexToInt(&ptr, &addr)) 
        {
            registers[PC] = addr;
            registers[NPC] = addr + 4;
        }
        if (registers[PC] == START_ADDR) 
        {
        registers[PSR] &= ~0x40;
            for (ht = hard_trap_info; ht->tt; ht++)
                exceptionHandler(ht->tt, trap_low, rdtbr());
            sp[14] = userfp;
        sp[7] = START_ADDR;
            registers[SP] = usersp;
        }
        
#ifdef LEON
        leon[0x9c/4] = 4;   /* clear uart b irq */
#else
            mec[0x50/4] = 0x20; 
            mec[0x4C/4] &= ~0x20; 
#endif

/* Need to flush the instruction cache here, as we may have deposited a
   breakpoint, and the icache probably has no way of knowing that a data ref to
   some location may have changed something that is in the instruction cache.
 */

        flush_i_cache();
        return;

      /* kill the program */
    case 'k' :      /* do nothing */
        sigval = 0;
        registers[PC] = START_ADDR;
        registers[NPC] = registers[PC] + 4;
        registers[PSR] &= ~0x40;
        registers[FP] = userfp;
        registers[SP] = usersp;
        sp[7] = 0;

        break;

    case 'r':       /* Reset */

        asm("
            call 0;
            nop;
        ");
        break;

#if 0
Disabled until we can unscrew this properly

    case 'b':     /* bBB...  Set baud rate to BB... */
        int baudrate;
        extern void set_timer_3();

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

        /* Convert baud rate to uart clock divider */
        switch (baudrate)
          {
          case 38400:
        baudrate = 16;
        break;
          case 19200:
        baudrate = 33;
        break;
          case 9600:
        baudrate = 65;
        break;
          default:
        strcpy(remcomOutBuffer,"B02");
        goto x1;
          }

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

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

/*****************************************************************************/
/* @Function: PD_ifLoad                                                      */
/* @Purpose :                                                                */
/*  This function copies the application program into EEPROM device.         */
/*  The number of bytes copied and the CRC are written into last but one and */
/*  last word location.                                                      */
/*                                                                           */
/* @@                                                                        */
/* @Parameter Name      @Mode   @Description                                 */
/* @@                                                                        */
/*****************************************************************************/
void PD_ifLoad(void)
{
unsigned short  crc;
UINT*           pEE;
UBYTE*          pR;
UBYTE           ch;
unsigned int    i,c,cntWord;
UINT            temp;
UBYTE*          pRam;
unsigned int    level;

    /* Disable asynchronous interrupts                          */
    sparc_disable_interrupts(level);                

    *MEM_CONF_REG |=0x6000; /* EDAC/parity enabled                     */
    *IO_CONF_REG   =0x1f;   /* Enable CRIMEA Register access           */
    *EE_WAIT       =0x500;  /* Read/write EEPROM 5 wait state SW (+1HW)*/

    CD_EEPROM_WRITE_ENABLE;
    
    pR  =(UBYTE *)(PD_RAM_ADDR_START);
    pEE =(UINT  *)(PD_EEPROM_ADDR_START);
    /* Copy program application into EEPROM (0x80000 Kbytes)    */
    for (i=0, crc=0xffff; i< PD_EEPROM_LENGHT; i +=512, pEE +=128, pR +=512)
    {
        /* Into EEPROM last page do not count the last 8 byte   */
        /* of CRC (room for EEPROM lenght and CRC)              */
        cntWord =512;
        if (i==PD_EEPROM_LAST_PAGE)
        {
            cntWord =504;
        }

        for (c=0; c<cntWord; c++)
        {
            crc  =PD_ifcrcComp(pR+c,crc);
        }
        /* Write specified number of words into EEPROM          */
        PD_ifCopyRamToEEprom((UINT*)pR,pEE,PD_EEPROM_PAGE_W);
    }

    /* Write number of bytes copied into EEPROM                 */
    *((UINT*)(PD_EEPROM_ADDR_START + PD_EEPROM_LENGHT - 8)) =PD_EEPROM_LENGHT;
    /* Write CRC                                                */
    *((UINT*)(PD_EEPROM_ADDR_START + PD_EEPROM_LENGHT - 4)) =crc;

    /* Waiting for internal EEPROM writing                      */
    for (c=0; c<= WAIT_FOR_20ms ;c++);


    /* Disable EEPROM writing by programming CRIMEA register    */
    CD_EEPROM_WRITE_DISABLE;

    pEE =(UINT*)(PD_EEPROM_ADDR_START);
    for (i=0, crc=0xffff; i< PD_EEPROM_LENGHT-8; i +=4,pEE++ )
    {
        temp =*pEE;
        ch  =(UBYTE)((temp>>24) & 0x000000ff);
        crc =PD_ifcrcComp(&ch,crc);
        ch  =(UBYTE)((temp>>16) & 0x000000ff);
        crc =PD_ifcrcComp(&ch,crc);
        ch  =(UBYTE)((temp>>8) & 0x000000ff);
        crc =PD_ifcrcComp(&ch,crc);
        ch  =(UBYTE)(temp & 0x000000ff);
        crc =PD_ifcrcComp(&ch,crc);
    }
    if (crc == *((unsigned int*)(PD_EEPROM_ADDR_START + PD_EEPROM_LENGHT - 4)) )
    {
        puts("\n\rThe EEPROM copy is OK\n\r");
    }
    else
    {
        puts("\n\rThe EEPROM copy is FAILED\n\r");
    }
    /* Enable asynchronous interrupts                           */
    sparc_enable_interrupts(level);             
}


/*****************************************************************************/
/* @Function: PD_ifcrcComp                                                   */
/* @Purpose :                                                                */
/*  This function calculates the CRC.                                        */
/*                                                                           */
/* @@                                                                        */
/* @Parameter Name      @Mode   @Description                                 */
/* @@                                                                        */
/*****************************************************************************/
unsigned short PD_ifcrcComp (unsigned char* adrs, unsigned short Crc) 
{
unsigned int Crc16Value;

    Crc16Value =Crc;
    // Calculating the CRC16 by using a LookUp table
    Crc16Value = (unsigned int)(Crc16Value << 8) ^ 
                 (unsigned int)(Crc_lookup[ (unsigned int) ( ((Crc16Value >>
                 8) & 0xFF) ^ *adrs) ]);
    return ((unsigned short)Crc16Value);             
    
}

/*****************************************************************************/
/* @Function: PD_ifCopyRamToEEprom                                           */
/* @Purpose :                                                                */
/*  This function writes a specified number of words from a source buffer    */
/*  a source buffer to an EEPROM memory address. The number of written words */
/*  into the EEPROM must not exceed the page boundary. At the end of the     */
/*  writing procedure the task is suspended for 10 milliseconds.             */
/*                                                                           */
/* @@                                                                        */
/* @Parameter Name      @Mode   @Description                                 */
/* Dest                 IN      write destination address                    */
/* Source               IN      read source address                          */
/* Len                  IN      number of words (32 bit) to be written       */
/* @@                                                                        */
/*****************************************************************************/
void PD_ifCopyRamToEEprom(UINT* Source, UINT* Dest, UINT Len)
{
unsigned int c;

    /* Move data from source to destination                 */
    for (c=0 ; c<Len; c++, Source++,Dest++)
    {
        *Dest =*Source;
    }
    
    /* Waiting for internal EEPROM writing                  */
    for (c=0; c<= WAIT_FOR_20ms; c++);
}

/*****************************************************************************/
/* @Function: PD_ifCopyRamToEEprom                                           */
/* @Purpose :                                                                */
/*  This function writes                                                     */
/*                                                                           */
/* @@                                                                        */
/* @Parameter Name      @Mode   @Description                                 */
/* @@                                                                        */
/*****************************************************************************/
void PD_ifLoadFromEeprom(void)
{
unsigned int    level;
UINT*           pEE;
UINT*           pR;
UINT            len;
UINT            i;

    pR  = (UINT *)(PD_RAM_ADDR_START);
    pEE = (UINT  *)(PD_EEPROM_ADDR_START);
    len = (PD_EEPROM_LENGHT-8) >> 2;

    /* Disable asynchronous interrupts                          */
    sparc_disable_interrupts(level);            

    /* copy EEPROM to RAM */
    i = 0;
    while (i < len) {
        *pR++ = *pEE++;
        i++;
    }

    /* Enable asynchronous interrupts                           */
    sparc_enable_interrupts(level);
}


/* This function will generate a breakpoint exception.  It is used at the
   beginning of a program to sync up with a debugger and can be used
   otherwise as a quick means to stop program execution and "break" into
   the debugger. */

main()
{
    volatile unsigned int *mec = (unsigned int *) 0x1f80000;

    mec[0x20 / 4] = 0;
    mec[0x24 / 4] = 0;
    mec[0x14 / 4] =0x0000001f;

    puts(version);
    puts(debugprg);

    /* write the magic number FAFEDEFA in order to start loggin properly by
     the analizer */
    putchar(0xFA);
    putchar(0xFE);
    putchar(0xDE);
    putchar(0xFA);

    PD_ifLoadFromEeprom();

    set_debug_traps();
    asm("       

        set _trapstack, %o0
        sub     %sp, 32, %o1
        andn    %o1, 0x0f, %o1
        st      %o1, [%o0]

        .globl _syncinst

        _syncinst: ta 16
        nop
      ");

}

