Subversion Repositories or1k

/* jp2-linux.c -- JTAG protocol via parallel port for linux
   Copyright(C) 2001 Marko Mlinar, markom@opencores.org
   Code for TCP/IP copied from gdb, by Chris Ziomkowski
 
This file is part of OpenRISC 1000 Architectural Simulator.
 
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 of the License, or
(at your option) any later version.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */
 
/* Establishes jtag proxy server and communicates with parallel
 port directly.  Requires root access. */
 
#include <stdio.h>
#include <ctype.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdarg.h>
#include <sys/stat.h>
#include <sys/types.h>
 
/* Dirty way to include inb and outb from, but they say it is
   a standard one.  */
#include <asm/io.h>
#include <asm/system.h>
#include "mc.h"
#include "gdb.h"
#include "jp2.h"
 
#define TC_RESET           0
#define TC_BRIGHT          1
#define TC_DIM             2
#define TC_UNDERLINE       3
#define TC_BLINK           4
#define TC_REVERSE         7
#define TC_HIDDEN          8
 
#define TC_BLACK           0
#define TC_RED             1
#define TC_GREEN           2
#define TC_YELLOW          3
#define TC_BLUE            4
#define TC_MAGENTA         5
#define TC_CYAN            6
#define TC_WHITE           7
 
static int base = 0x378; /* FIXME: We should detect the address. */
int err = 0;
int set_pc = 0;
int set_step = 0;
int waiting = 0;
 
/* Scan chain info. */
static int chain_addr_size[] = { 0,  32, 0,  0,  5,  32, 32};
static int chain_data_size[] = { 0,  32, 0,  32, 32, 32, 32};
static int chain_is_valid[]  = { 0,  1,  0,  1,  1,  1,   1};
static int chain_has_crc[]   = { 0,  1,  0,  1,  1,  1,   1};
static int chain_has_rw[]    = { 0,  1,  0,  0,  1,  1,   1};
 
/* Currently selected scan chain - just to prevent unnecessary
   transfers. */
static int current_chain = -1;
 
/* The chain that should be currently selected. */
static int dbg_chain = -1;
 
/* Crc of current read or written data.  */
static int crc_r, crc_w = 0;
 
/* Address of previous read */
static unsigned long prev_regno = 0;
 
/* Generates new crc, sending in new bit input_bit */
static unsigned long crc_calc(unsigned long crc, int input_bit) {
  unsigned long d = (input_bit&1) ? 0xfffffff : 0x0000000;
  unsigned long crc_32 = ((crc >> 31)&1) ? 0xfffffff : 0x0000000;
  crc <<= 1;
  return crc ^ (d ^ crc_32) & DBG_CRC_POLY;
}
 
/* Send a byte to parallel port. */
inline static void jp2_out(unsigned int value) {
#ifdef RTL_SIM
  time_t time;
  struct stat s;
  char buf[1000];
  FILE *fout;
  unsigned num_read;
  int r;
  fout = fopen(GDB_IN, "wt+");
  fprintf(fout, "F\n");
  fclose(fout);
  fout = fopen(GDB_OUT, "wt+");
  fprintf(fout, "%02X\n", value);
  fclose(fout);
error:
  fout = fopen(GDB_OUT, "rt");
  r = fscanf(fout,"%x", &num_read);
  fclose(fout);
  if(r == 0 || num_read !=(0x10 | value)) goto error;
#else
  outb(value, LPT_WRITE);
#endif /* RTL_SIM */
  if (!(value & TCLK_BIT)) {
    if (value & TMS_BIT) debug("%c[%d;%dm", 0x1b, TC_BRIGHT, TC_WHITE + 30);
    debug("%x",(value & TDI_BIT) != 0);
    debug("%c[%d;%dm", 0x1b, TC_RESET, TC_WHITE + 30);
  }
  flush_debug();
}
 
/* Receive a byte from parallel port.  */
inline static unsigned char jp2_in() {
  int data;
#ifndef RTL_SIM
  data = inb(LPT_READ);
#ifdef TDO_INV
  data =(data & TDO_BIT) != TDO_BIT;
#else
  data =(data & TDO_BIT) == TDO_BIT;
#endif
#else
  FILE *fin = 0;  
  char ch;
  time_t time;
  struct stat s;
  while(1) {
    fin = fopen(GDB_IN, "rt");
    if(fin == 0) continue;
    ch = fgetc(fin);
    if(ch != '0' && ch != '1') {
      fclose(fin);
      continue;
    } else break;
  }
  fclose(fin);
  data = ch == '1';
#endif /* !RTL_SIM */
  debug("%c[%d;%dm", 0x1b, TC_BRIGHT, TC_GREEN + 30);
  debug("%01X", data);
  debug("%c[%d;%dm", 0x1b, TC_RESET, TC_WHITE + 30);
  flush_debug();
  return data;
}
 
/* Writes TCLK=0, TRST=1, TMS=bit1, TDI=bit0
   and    TCLK=1, TRST=1, TMS=bit1, TDI=bit0 */
static inline void jp2_write_JTAG(unsigned char packet) {
  unsigned char data = TRST_BIT;
  if(packet & 1) data |= TDI_BIT;
  if(packet & 2) data |= TMS_BIT;
 
  jp2_out(data);
  JTAG_WAIT();
  crc_w = crc_calc(crc_w, packet&1);
 
  /* rise clock */
  jp2_out(data | TCLK_BIT);
  JTAG_WAIT();
}
 
/* Reads TDI.  */
static inline int jp2_read_JTAG() {
  int data;
  data = jp2_in();
  crc_r = crc_calc(crc_r, data);
  return data;
}
 
/* Writes bitstream.  LS bit first if len < 0, MS bit first if len > 0.  */
static inline void jp2_write_stream(ULONGEST stream, int len, int set_last_bit) {
  int i;
  if (len < 0) {
    len = -len;
    debug("writeL%d(", len);
    for(i = 0; i < len - 1; i++)
      jp2_write_JTAG((stream >> i) & 1);
 
    if(set_last_bit) jp2_write_JTAG((stream >>(len - 1))& 1 | TMS);
    else jp2_write_JTAG((stream >>(len - 1))& 1);
  } else {
    debug("write%d(", len);
    for(i = 0; i < len - 1; i++)
      jp2_write_JTAG((stream >> (len - 1 - i)) & 1);
 
    if(set_last_bit) jp2_write_JTAG((stream >> 0) & 1 | TMS);
    else jp2_write_JTAG((stream >> 0)& 1);
  }
  debug(")\n");
}
 
/* Gets bitstream.  LS bit first if len < 0, MS bit first if len > 0.  */
inline static ULONGEST jp2_read_stream(unsigned long stream, int len, int set_last_bit) {
  int i;
  ULONGEST data = 0;
  if (len < 0) {
    debug("readL(");
    for(i = 0; i < len - 1; i++) {      
      jp2_write_JTAG((stream >> i) & 1);   /* LSB first */
      data |= jp2_read_JTAG() << i; /* LSB first */
    }
 
    if (set_last_bit) jp2_write_JTAG((stream >> (len - 1)) & 1 | TMS);
    else jp2_write_JTAG((stream >> (len - 1)) & 1);
    data |= jp2_read_JTAG() << (len - 1);
  } else {
    debug("read(");
    for(i = 0; i < len - 1; i++) {      
      jp2_write_JTAG((stream >> (len - 1 - i)) & 1);   /* MSB first */
      data |= jp2_read_JTAG() << (len - 1 - i); /* MSB first */
    }
 
    if (set_last_bit) jp2_write_JTAG((stream >> 0) & 1 | TMS);
    else jp2_write_JTAG((stream >> 0) & 1);
    data |= jp2_read_JTAG() << 0;
  }
  debug(")\n");
  return data;
}
 
/* Sets scan chain.  */
void jtag_set_ir(int ir) {
  jp2_write_JTAG(TMS); /* SELECT_DR SCAN */
  jp2_write_JTAG(TMS); /* SELECT_IR SCAN */
 
  jp2_write_JTAG(0); /* CAPTURE_IR */
  jp2_write_JTAG(0); /* SHIFT_IR */      
 
  /* write data, EXIT1_IR */
  jp2_write_stream(ir, -JI_SIZE, 1);
 
  jp2_write_JTAG(TMS); /* UPDATE_IR */
  jp2_write_JTAG(0); /* IDLE */  
  current_chain = -1;
}
 
/* Resets JTAG
   Writes TRST=0
   and    TRST=1 */
static void jp2_reset_JTAG() {
  int i;
  debug2("\nreset(");
  jp2_out(0);
  JTAG_RETRY_WAIT();
  /* In case we don't have TRST reset it manually */
  for(i = 0; i < 8; i++) jp2_write_JTAG(TMS);
  jp2_out(TRST_BIT);
  JTAG_RETRY_WAIT();
  jp2_write_JTAG(0);
  debug2(")\n");
}
 
/* Resets JTAG, and sets DEBUG scan chain */
static int dbg_reset() {
  int err;
  jp2_reset_JTAG();
  /* select debug scan chain and stay in it forever */
  jtag_set_ir(JI_DEBUG);
  current_chain = -1;
  return DBG_ERR_OK;
}
/* counts retries and returns zero if we should abort */
/* TODO: dinamically adjust timings for jp2 */
static int retry_no = 0;
int retry_do() {
  int i, err;
  printf("RETRY\n");
  if (retry_no >= NUM_SOFT_RETRIES) {
    if ((err = dbg_reset())) return err;
  } else { /* quick reset */
    for(i = 0; i < 8; i++) jp2_write_JTAG(TMS);
    jp2_write_JTAG(0); /* go into IDLE state */
  }
  if (retry_no >= NUM_SOFT_RETRIES + NUM_HARD_RETRIES) {
    retry_no = 0;
    return 0;
  }
  retry_no++;
  return 1;
}
 
/* resets retry counter */
void retry_ok() {
  retry_no = 0;
}
 
/* Sets scan chain.  */
int dbg_set_chain(int chain) {
  int status, crc_generated, crc_read;
  dbg_chain = chain;
 
try_again:
  if (current_chain == chain) return DBG_ERR_OK;
  current_chain = -1;
  debug("\n");
  debug2("set_chain %i\n", chain);
  jp2_write_JTAG(TMS); /* SELECT_DR SCAN */
  jp2_write_JTAG(0); /* CAPTURE_DR */
  jp2_write_JTAG(0); /* SHIFT_DR */      
 
  /* write data, EXIT1_DR */
  crc_w = 0xffffffff;
  jp2_write_stream((chain & 0x7 | 0x8), DC_SIZE + 1, 0);
  jp2_write_stream(crc_w, DBG_CRC_SIZE, 0);
  crc_r = 0xffffffff;
  status = jp2_read_stream(0, DC_STATUS_SIZE, 0);
  crc_generated = crc_r;
  crc_read = jp2_read_stream(0, DBG_CRC_SIZE, 1);
 
  /* invalid chain? */
  if (status == 0xf && crc_read == crc_generated) return DBG_ERR_INV_CHAIN;
 
  /* we should read 1101, otherwise retry */
  if (status != 0xd || crc_read != crc_generated) {
    if (retry_do()) goto try_again;
    else return DBG_ERR_CRC;
  }
 
  /* reset retry counter */
  retry_ok();
 
  jp2_write_JTAG(TMS); /* UPDATE_DR */
  jp2_write_JTAG(0); /* IDLE */  
  current_chain = chain;
  return DBG_ERR_OK;
}
 
/* sends out a command with 32bit address and 16bit length, if len >= 0 */
int dbg_command(unsigned long adr, int command, int expect_status, int len) {
  int status, crc_generated, crc_read;
 
try_again:
  dbg_set_chain(dbg_chain);
  debug("\n");
  debug2("wb_comm %i\n", command);
 
  /***** WRITEx *****/
  jp2_write_JTAG(TMS); /* SELECT_DR SCAN */
  jp2_write_JTAG(0); /* CAPTURE_DR */
  jp2_write_JTAG(0); /* SHIFT_DR */      
 
  /* write data, EXIT1_DR */
  crc_w = 0xffffffff;
  jp2_write_stream((command & 0x7), DC_SIZE + 1, 0);
  jp2_write_stream(adr, 32, 0);
  if (len >= 0) jp2_write_stream(len, 16, 0);
  jp2_write_stream(crc_w, DBG_CRC_SIZE, 0);
  crc_r = 0xffffffff;
  status = jp2_read_stream(0, DC_STATUS_SIZE, 0);
  crc_generated = crc_r;
  crc_read = jp2_read_stream(0, DBG_CRC_SIZE, 1);
 
  /* we should read expected status value, otherwise retry */
  if (status != expect_status || crc_read != crc_generated) {
    if (retry_do()) goto try_again;
    else return DBG_ERR_CRC;
  }
  jp2_write_JTAG(TMS); /* UPDATE_DR */
  jp2_write_JTAG(0); /* IDLE */  
 
  /* reset retry counter */
  retry_ok();
  return DBG_ERR_OK;
}
 
/* issues a burst read/write */
int dbg_go(int go_command, unsigned char *data, unsigned short len, int read, int expected_status) {
  int status, crc_generated, crc_read;
  int i;
 
try_again:
  dbg_set_chain(dbg_chain);
  debug("\n");
  debug2("go len = %d\n", len);
 
  jp2_write_JTAG(TMS); /* SELECT_DR SCAN */
  jp2_write_JTAG(0); /* CAPTURE_DR */
  jp2_write_JTAG(0); /* SHIFT_DR */      
 
  /* write data, EXIT1_DR */
  crc_w = 0xffffffff;
  jp2_write_stream(go_command, DC_SIZE + 1, 0);
  if (!read) {
    /* reverse byte ordering, since we must send in big endian */
    for (i = 0; i < len; i++)
      jp2_write_stream(data[i], 8, 0);
  }
  jp2_write_stream(crc_w, DBG_CRC_SIZE, 0);
  crc_r = 0xffffffff;
  if (read) {
    /* reverse byte ordering, since we must send in big endian */
    for (i = 0; i < len; i++)
      data[i] = jp2_read_stream(data[i], 8, 0);
  }
  status = jp2_read_stream(0, DC_STATUS_SIZE, 0);
  crc_generated = crc_r;
  crc_read = jp2_read_stream(0, DBG_CRC_SIZE, 1);
 
  /* was there an bus error -- wishbone only */
  if (status == 0x9 && crc_read == crc_generated) return DBG_ERR_BUSERR;
  //printf("[status %x %x %x %x]\n", status, expected_status, crc_read, crc_generated);
 
  /* we should read expected status, otherwise retry */
  if (status != expected_status || crc_read != crc_generated) {
    if (retry_do()) goto try_again;
    else return DBG_ERR_CRC;
  }
  jp2_write_JTAG(TMS); /* UPDATE_DR */
  jp2_write_JTAG(0); /* IDLE */  
 
  /* reset retry counter */
  retry_ok();
  return DBG_ERR_OK;
}
 
/* check the status */
int dbg_wb_status() {
  int status, crc_generated, crc_read;
  int wait_retries = 0;
 
try_again:
  dbg_set_chain(dbg_chain);
  debug("\n");
  debug2("status\n");
 
  jp2_write_JTAG(TMS); /* SELECT_DR SCAN */
  jp2_write_JTAG(0); /* CAPTURE_DR */
  jp2_write_JTAG(0); /* SHIFT_DR */      
 
  /* write data, EXIT1_DR */
  crc_w = 0xffffffff;
  jp2_write_stream(DI_WB_STATUS, DC_SIZE + 1, 0);
  jp2_write_stream(crc_w, DBG_CRC_SIZE, 0);
  crc_r = 0xffffffff;
  status = jp2_read_stream(0, DC_STATUS_SIZE, 0);
  crc_generated = crc_r;
  crc_read = jp2_read_stream(0, DBG_CRC_SIZE, 1);
 
  /* is cycle still in progress */
  if (status == 0xc && crc_read == crc_generated) {
    jp2_write_JTAG(TMS); /* UPDATE_DR */
    jp2_write_JTAG(0); /* IDLE */  
    JTAG_RETRY_WAIT();
    wait_retries++;
    if (wait_retries > NUM_ACCESS_RETRIES) goto try_again;
  }
 
  /* we should read 1000, otherwise retry */
  if (status != 0x8 || crc_read != crc_generated) {
    if (retry_do()) goto try_again;
    else return DBG_ERR_CRC;
  }
 
  jp2_write_JTAG(TMS); /* UPDATE_DR */
  jp2_write_JTAG(0); /* IDLE */
 
  /* reset retry counter */
  retry_ok();
  return DBG_ERR_OK;
}
 
/* read a block from wishbone */
int dbg_wb_read(unsigned long adr, int command, unsigned char *data, unsigned short len) {
  int err;
  if ((err = dbg_set_chain(DC_WISHBONE))) return err;
  if ((err = dbg_command(adr, command, 0x8, len))) return err;
  if ((err = dbg_go(DI_WB_GO, data, len, 1, 0x8))) return err;
  if ((err = dbg_wb_status())) return err;
  return DBG_ERR_OK;
}
 
/* write a block to wishbone */
int dbg_wb_write(unsigned long adr, int command, unsigned char *data, unsigned short len) {
  int err;
  if ((err = dbg_set_chain(DC_WISHBONE))) return err;
  if ((err = dbg_command(adr, command, 0x8, len))) return err;
  if ((err = dbg_go(DI_WB_GO, data, len, 0, 0x8))) return err;
  if ((err = dbg_wb_status())) return err;
  return DBG_ERR_OK;
}
 
/* read a register from cpu */
int dbg_cpu_read(unsigned long adr, int command, unsigned long *data, int len) {
  int err;
  if ((err = dbg_set_chain(DC_CPU))) return err;
  if ((err = dbg_command(adr, command, 0xa, -1))) return err;
  if ((err = dbg_go(DI_CPU_GO, (unsigned char*)data, len, 1, 0xa))) return err;
  return DBG_ERR_OK;
}
 
/* write a cpu register */
int dbg_cpu_write(unsigned long adr, int command, unsigned long data, int len) {
  int err;
  if ((err = dbg_set_chain(DC_CPU))) return err;
  if ((err = dbg_command(adr, command, 0xa, -1))) return err;
  if ((err = dbg_go(DI_CPU_GO, (unsigned char *)&data, len, 0, 0xa))) return err;
  return DBG_ERR_OK;
}
 
/* read a word from wishbone */
int dbg_wb_read32(unsigned long adr, unsigned long *data) {
  int err = dbg_wb_read(adr, DI_WB_READ32, (unsigned char*)data, 4);
  *data = ntohl(*data);
  return err;
}
 
/* write a word to wishbone */
int dbg_wb_write32(unsigned long adr, unsigned long data) {
  data = ntohl(data);
  return dbg_wb_write(adr, DI_WB_WRITE32, (unsigned char*)&data, 4);
}
 
/* read a block from wishbone */
int dbg_wb_read_block32(unsigned long adr, unsigned long *data, int len) {
  int i, err;
  //printf("%08x %08x\n", adr, len);
  err = dbg_wb_read(adr, DI_WB_READ32, (unsigned char*)data, len);
  for (i = 0; i < len / 4; i ++) data[i] = ntohl(data[i]);
  //printf("%08x\n", err);
  return err;
}
 
/* write a block to wishbone */
int dbg_wb_write_block32(unsigned long adr, unsigned long *data, int len) {
  int i;
  for (i = 0; i < len / 4; i ++) data[i] = ntohl(data[i]);
  return dbg_wb_write(adr, DI_WB_WRITE32, (unsigned char*)data, len);
}
 
/* read a register from cpu */
int dbg_cpu_read32(unsigned long adr, unsigned long *data) {
  int err = dbg_cpu_read(adr, DI_CPU_READ32, data, 4);
  *data = ntohl(*data);
  return err;
}
 
/* read a register from cpu module */
int dbg_cpu_read_reg(unsigned long adr, unsigned char *data) {
  return dbg_cpu_read(adr, DI_CPU_READ_REG, (unsigned long *)data, 1);
}
 
/* write a cpu register */
int dbg_cpu_write32(unsigned long adr, unsigned long data) {
  data = ntohl(data);
  return dbg_cpu_write(adr, DI_CPU_WRITE32, data, 4);
}
 
/* write a cpu module register */
int dbg_cpu_write_reg(unsigned long adr, unsigned char data) {
  unsigned long l;
  *((unsigned char*)&l) = data;
  return dbg_cpu_write(adr, DI_CPU_WRITE_REG, data, 1);
}
 
void check(char *fn, int l, int i) {
  if (i != DBG_ERR_OK) {
    fprintf(stderr, "%s:%d: Jtag error %d occured; exiting.\n", fn, l, i);
    exit(1);
  }
}
 
void dbg_test() {
  int i;
  unsigned long npc, ppc, r1, insn, result;
  unsigned char stalled;
#if 1
#define MC_BASE_ADDR     0x93000000
#define FLASH_BASE_ADDR  0xf0000000
#define FLASH_BAR_VAL    FLASH_BASE_ADDR
#define FLASH_AMR_VAL    0xf0000000
#define FLASH_WTR_VAL    0x00011009
#define FLASH_RTR_VAL    0x01002009
#define SDRAM_BASE_ADDR  0x00000000
#define SDRAM_BAR_VAL    SDRAM_BASE_ADDR
#define SDRAM_SIZE       0x02000000
#define SDRAM_AMR_VAL    (~(SDRAM_SIZE -1))
#define SDRAM_RATR_VAL   0x00000006
#define SDRAM_RCDR_VAL   0x00000002
#define SDRAM_RCTR_VAL   0x00000006
#define SDRAM_REFCTR_VAL 0x00000006
#define SDRAM_PTR_VAL    0x00000001
#define SDRAM_RRDR_VAL   0x00000000
#define SDRAM_RIR_VAL    0x000000C0
 
#define MC_BAR_0         0x00
#define MC_AMR_0         0x04
#define MC_WTR_0         0x30
#define MC_RTR_0         0x34
#define MC_OSR           0xe8
#define MC_BAR_4         0x80
#define MC_AMR_4         0x84
#define MC_CCR_4         0xa0
#define MC_RATR          0xb0
#define MC_RCDR          0xc8
#define MC_RCTR          0xb4
#define MC_REFCTR        0xc4
#define MC_PTR           0xbc
#define MC_RRDR          0xb8
#define MC_RIR           0xcc
#define MC_ORR           0xe4
 
  CHECK(dbg_cpu_write_reg(0, 0x01));
 
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_BAR_0, FLASH_BAR_VAL & 0xffff0000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_AMR_0, FLASH_AMR_VAL & 0xffff0000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_WTR_0, FLASH_WTR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_RTR_0, FLASH_RTR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_OSR, 0x40000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_BAR_4, SDRAM_BAR_VAL & 0xffff0000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_AMR_4, SDRAM_AMR_VAL & 0xffff0000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_CCR_4, 0x00ef0004));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_RATR, SDRAM_RATR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_RCDR, SDRAM_RCDR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_RCTR, SDRAM_RCTR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_REFCTR, SDRAM_REFCTR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_PTR, SDRAM_PTR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_RRDR, SDRAM_RRDR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_RIR, SDRAM_RIR_VAL));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_OSR, 0x5e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x5e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_OSR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x6e000000));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_OSR, 0x7e000033));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_ORR, 0x7e000033));
  CHECK(dbg_wb_write32(MC_BASE_ADDR + MC_CCR_4, 0xc0ef0004));
 
  CHECK(dbg_wb_read32(MC_BASE_ADDR+MC_CCR_4, &insn));
  printf("expected %x, read %x\n", 0xc0be0004, insn);
#endif
 
#if 1
#define CPU_OP_ADR  0
#define CPU_SEL_ADR 1
#define RAM_BASE 0x00000000
 
  /* Select cpu 0 */
  CHECK(dbg_cpu_write_reg(CPU_SEL_ADR, 0x01));
  /* Stall all cpus and selected cpu 0 */
  CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x05));
 
  /* unstall the or1200 in highland_sys */
  CHECK(dbg_wb_write32(0xb8070000, 2));
 
  usleep(1000);
  CHECK(dbg_wb_write32(RAM_BASE+0x00, 0x12345678));
  CHECK(dbg_wb_read32(RAM_BASE+0x00, &insn));
  printf("expected %x, read %x\n", 0x12345678, insn);
  if (insn != 0x12345678) exit(1);
 
  CHECK(dbg_wb_write32(RAM_BASE+0x0000, 0x11112222));
  CHECK(dbg_wb_read32(RAM_BASE+0x0000, &insn));
  printf("expected %x, read %x\n", 0x11112222, insn);
 
  CHECK(dbg_wb_write32(RAM_BASE+0x0004, 0x33334444));
  CHECK(dbg_wb_write32(RAM_BASE+0x0008, 0x55556666));
  CHECK(dbg_wb_write32(RAM_BASE+0x000c, 0x77778888));
  CHECK(dbg_wb_write32(RAM_BASE+0x0010, 0x9999aaaa));
  CHECK(dbg_wb_write32(RAM_BASE+0x0014, 0xbbbbcccc));
  CHECK(dbg_wb_write32(RAM_BASE+0x0018, 0xddddeeee));
  CHECK(dbg_wb_write32(RAM_BASE+0x001c, 0xffff0000));
  CHECK(dbg_wb_write32(RAM_BASE+0x0020, 0xdeadbeaf));
 
  CHECK(dbg_wb_read32(RAM_BASE+0x0004, &insn));
  printf("expected %x, read %x\n", 0x33334444, insn);
  CHECK(dbg_wb_read32(RAM_BASE+0x0000, &insn));
  printf("expected %x, read %x\n", 0x11112222, insn);
  CHECK(dbg_wb_read32(RAM_BASE+0x0008, &insn));
  printf("expected %x, read %x\n", 0x55556666, insn);
  CHECK(dbg_wb_read32(RAM_BASE+0x000c, &insn));
  printf("expected %x, read %x\n", 0x77778888, insn);
  CHECK(dbg_wb_read32(RAM_BASE+0x0010, &insn));
  printf("expected %x, read %x\n", 0x9999aaaa, insn);
  CHECK(dbg_wb_read32(RAM_BASE+0x0014, &insn));
  printf("expected %x, read %x\n", 0xbbbbcccc, insn);
  CHECK(dbg_wb_read32(RAM_BASE+0x0018, &insn));
  printf("expected %x, read %x\n", 0xddddeeee, insn);
 
  CHECK(dbg_wb_read32(RAM_BASE+0x001c, &insn));
  printf("expected %x, read %x\n", 0xffff0000, insn);
 
  CHECK(dbg_wb_read32(RAM_BASE+0x0020, &insn));
  printf("expected %x, read %x\n", 0xdeadbeaf, insn);
 
  if (insn != 0xdeadbeaf) exit(1);
 
  CHECK(dbg_wb_write32(RAM_BASE+0x00, 0xe0000005));   /* l.xor   r0,r0,r0   */
  CHECK(dbg_wb_write32(RAM_BASE+0x04, 0x9c200000));   /* l.addi  r1,r0,0x0  */
  CHECK(dbg_wb_write32(RAM_BASE+0x08, 0x18400000));   /* l.movhi r2,0x4000  */
  CHECK(dbg_wb_write32(RAM_BASE+0x0c, 0xa8420030));   /* l.ori   r2,r2,0x30 */
  CHECK(dbg_wb_write32(RAM_BASE+0x10, 0x9c210001));   /* l.addi  r1,r1,1    */
  CHECK(dbg_wb_write32(RAM_BASE+0x14, 0x9c210001));   /* l.addi  r1,r1,1    */
  CHECK(dbg_wb_write32(RAM_BASE+0x18, 0xd4020800));   /* l.sw    0(r2),r1   */
  CHECK(dbg_wb_write32(RAM_BASE+0x1c, 0x9c210001));   /* l.addi  r1,r1,1    */
  CHECK(dbg_wb_write32(RAM_BASE+0x20, 0x84620000));   /* l.lwz   r3,0(r2)   */
  CHECK(dbg_wb_write32(RAM_BASE+0x24, 0x03fffffb));   /* l.j     loop2      */
  CHECK(dbg_wb_write32(RAM_BASE+0x28, 0xe0211800));   /* l.add   r1,r1,r3   */
 
  CHECK(dbg_cpu_write32((0 << 11) + 17, 0x01));  /* Enable exceptions */
  CHECK(dbg_cpu_write32((6 << 11) + 20, 0x2000));  /* Trap causes stall */
  CHECK(dbg_cpu_write32((0 << 11) + 16, RAM_BASE));  /* Set PC */
  CHECK(dbg_cpu_write32((6 << 11) + 16, 1 << 22));  /* Set step bit */
  for(i = 0; i < 11; i++) {
    CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* 11x Unstall */
    do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  }
 
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000010, 0x00000028, 5);
  result = npc + ppc + r1;
 
  CHECK(dbg_cpu_write32((6 << 11) + 16, 0));  /* Reset step bit */
  CHECK(dbg_wb_read32(RAM_BASE + 0x28, &insn));  /* Set trap insn in delay slot */
  CHECK(dbg_wb_write32(RAM_BASE + 0x28, 0x21000001));
  CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
  do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  CHECK(dbg_wb_write32(RAM_BASE + 0x28, insn));  /* Set back original insn */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000010, 0x00000028, 8);
  result = npc + ppc + r1 + result;
 
  CHECK(dbg_wb_read32(RAM_BASE + 0x24, &insn));  /* Set trap insn in place of branch insn */
  CHECK(dbg_wb_write32(RAM_BASE + 0x24, 0x21000001));
  CHECK(dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x10));  /* Set PC */
  CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
  do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  CHECK(dbg_wb_write32(RAM_BASE + 0x24, insn));  /* Set back original insn */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000028, 0x00000024, 11);
  result = npc + ppc + r1 + result;
 
  CHECK(dbg_wb_read32(RAM_BASE + 0x20, &insn));  /* Set trap insn before branch insn */
  CHECK(dbg_wb_write32(RAM_BASE + 0x20, 0x21000001));
  CHECK(dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x24));  /* Set PC */
  CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
  do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  CHECK(dbg_wb_write32(RAM_BASE + 0x20, insn));  /* Set back original insn */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000024, 0x00000020, 24);
  result = npc + ppc + r1 + result;
 
  CHECK(dbg_wb_read32(RAM_BASE + 0x1c, &insn));  /* Set trap insn behind lsu insn */
  CHECK(dbg_wb_write32(RAM_BASE + 0x1c, 0x21000001));
  CHECK(dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x20));  /* Set PC */
  CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
  do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  CHECK(dbg_wb_write32(RAM_BASE + 0x1c, insn));  /* Set back original insn */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000020, 0x0000001c, 49);
  result = npc + ppc + r1 + result;
 
  CHECK(dbg_wb_read32(RAM_BASE + 0x20, &insn));  /* Set trap insn very near previous one */
  CHECK(dbg_wb_write32(RAM_BASE + 0x20, 0x21000001));
  CHECK(dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x1c));  /* Set PC */
  CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
  do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  CHECK(dbg_wb_write32(RAM_BASE + 0x20, insn));  /* Set back original insn */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000024, 0x00000020, 50);
  result = npc + ppc + r1 + result;
 
  CHECK(dbg_wb_read32(RAM_BASE + 0x10, &insn));  /* Set trap insn to the start */
  CHECK(dbg_wb_write32(RAM_BASE + 0x10, 0x21000001));
  CHECK(dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x20)  /* Set PC */);
  CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
  do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  CHECK(dbg_wb_write32(RAM_BASE + 0x10, insn));  /* Set back original insn */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000014, 0x00000010, 99);
  result = npc + ppc + r1 + result;
 
  CHECK(dbg_cpu_write32((6 << 11) + 16, 1 << 22));  /* Set step bit */
  for(i = 0; i < 5; i++) {
    CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
    do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  }
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000028, 0x00000024, 101);
  result = npc + ppc + r1 + result;
 
  CHECK(dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x24));  /* Set PC */
  for(i = 0; i < 2; i++) {
    CHECK(dbg_cpu_write_reg(CPU_OP_ADR, 0x00));  /* Unstall */
    do CHECK(dbg_cpu_read_reg(CPU_OP_ADR, &stalled)); while (!(stalled & 1));
  }
  CHECK(dbg_cpu_read32((0 << 11) + 16, &npc));  /* Read NPC */
  CHECK(dbg_cpu_read32((0 << 11) + 18, &ppc));  /* Read PPC */
  CHECK(dbg_cpu_read32(0x401, &r1));  /* Read R1 */
  printf("Read      npc = %.8lx ppc = %.8lx r1 = %.8lx\n", npc, ppc, r1);
  printf("Expected  npc = %.8lx ppc = %.8lx r1 = %.8lx\n", 0x00000010, 0x00000028, 201);
  result = npc + ppc + r1 + result;
 
  printf("result = %.8lx\n", result ^ 0xdeaddae1);
#endif
}
 
int main(int argc,  char *argv[]) {
  char *redirstr;
  int trace_fd = 0;
  char *s;
  int err = DBG_ERR_OK;
 
  srand(getpid());
  if(argc != 2) {
    printf("JTAG protocol via parallel port for linux.\n");
    printf("Copyright(C) 2001 Marko Mlinar, markom@opencores.org\n\n");
    printf("Usage: %s JTAG port_number\n", argv[0]);
    return -1;
  }
 
#ifndef RTL_SIM
  if(ioperm(LPT_BASE, 3, 1)) {
    fprintf(stderr, "Couldn't get the port at %x\n", LPT_BASE);
    perror("Root privileges are required.\n"); 
    return -1;
  }
  printf("Using parallel port at %x\n", LPT_BASE);
#else
  {
    FILE *fin = fopen(GDB_IN, "wt+");
    if(fin == 0) {
      fprintf(stderr, "Can not open %s\n", GDB_IN);
      exit(1);
    }
    fclose(fin);
 
  }
#endif
#ifndef RTL_SIM
  /* Get rid of root privileges.  */
  setreuid(getuid(), getuid());
#endif
 
  /* Initialize a new connection to the or1k board, and make sure we are
     really connected.  */
  current_chain = -1;
  if ((err = dbg_reset())) goto error;
 
  /* Test the connection.  */
  dbg_test();
 
  /* We have a connection.  Establish server.  */
  argv++; argc--;
  printf("Dropping root privileges.\n");
  serverPort = strtol(*(argv),&s,10);
  if(*s) return -1;
  argv++; argc--;
 
  if(server_fd = GetServerSocket("or1ksim","tcp", serverPort)) {
    printf("JTAG Proxy server started on port %d\n", serverPort);
    printf("Press CTRL+c to exit.\n");
  } else {
    fprintf(stderr,"Cannot start JTAG Proxy server on port %d\n", serverPort);
    exit(-1);
  }
 
  /* Do endless loop of checking and handle GDB requests.  Ctrl-c exits.  */
  HandleServerSocket(true);
  return 0;
error:
  fprintf(stderr,"Connection with jtag via parallel port failed (err = %d).\n", err);
  exit(-1);
}