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>
 
/* 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"
 
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 & 1)) debug("[%x%c]",(value & TDI_BIT) != 0,(value & TMS_BIT)?'^':'_');
  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(" R%01X ", data);
  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("\nwriteL(");
    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("\nwrite(");
    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("\nreadL(");
    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("\nread(");
    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 nonzero if we should abort */
/* TODO: dinamically adjust timings for jp2 */
static int retry_no = 0;
int retry_do() {
  int i, err;
  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 1;
  }
  retry_no++;
  return DBG_ERR_OK;
}
 
/* 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 = 0;
  jp2_write_stream((chain & 0x7 | 0x8), DC_SIZE + 1, 0);
  jp2_write_stream(crc_w, DBG_CRC_SIZE, 0);
  crc_r = 0;
  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 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 = 0;
  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 = 0;
  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 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;
}
 
/* issues a burst read/write */
int dbg_go(int go_command, unsigned char *data, unsigned short len, int read) {
  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 = 0;
  jp2_write_stream(go_command, DC_SIZE + 1, 0);
  for (i = 0; i < len; i++)
    if (read) data[i] = jp2_read_stream(data[i], 8, 0);
    else jp2_write_stream(data[i], 8, 0);
  jp2_write_stream(crc_w, DBG_CRC_SIZE, 0);
  crc_r = 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 */
  if (status == 0x9 || crc_read == crc_generated) return DBG_ERR_BUSERR;
 
  /* 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;
}
 
/* 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 = 0;
  jp2_write_stream(DI_WB_STATUS, DC_SIZE + 1, 0);
  jp2_write_stream(crc_w, DBG_CRC_SIZE, 0);
  crc_r = 0;
  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, len))) return err;
  if ((err = dbg_go(DI_WB_GO, data, len, 1))) 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, len))) return err;
  if ((err = dbg_go(DI_WB_GO, data, len, 0))) 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 err;
  if ((err = dbg_set_chain(DC_CPU))) return err;
  if ((err = dbg_command(adr, command, -1))) return err;
  if ((err = dbg_go(DI_CPU_GO, (unsigned char*)data, 4, 1))) return err;
  return DBG_ERR_OK;
}
 
/* write a cpu register */
int dbg_cpu_write(unsigned long adr, int command, unsigned long data) {
  int err;
  if ((err = dbg_set_chain(DC_CPU))) return err;
  if ((err = dbg_command(adr, command, -1))) return err;
  if ((err = dbg_go(DI_CPU_GO, (unsigned char *)&data, 4, 0))) return err;
  return DBG_ERR_OK;
}
 
/* read a word from wishbone */
int dbg_wb_read32(unsigned long adr, unsigned long *data) {
  return dbg_wb_read(adr, DI_WB_READ32, (unsigned char*)data, 4);
}
 
/* write a word to wishbone */
int dbg_wb_write32(unsigned long adr, unsigned long data) {
  return dbg_wb_write(adr, DI_WB_WRITE32, (unsigned char*)&data, 4);
}
 
/* read a block from wishbone */
int dbg_wb_read_block_32(unsigned long adr, unsigned long *data, int len) {
  return dbg_wb_read(adr, DI_WB_READ32, (unsigned char*)data, len);
}
 
/* write a block to wishbone */
int dbg_wb_write_block32(unsigned long adr, unsigned long *data, int len) {
  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) {
  return dbg_cpu_read(adr, DI_CPU_READ32, data);
}
 
/* read a register from cpu module */
int dbg_cpu_read_reg(unsigned long adr, unsigned long *data) {
  return dbg_cpu_read(adr, DI_CPU_READ_REG, data);
}
 
/* write a cpu register */
int dbg_cpu_write32(unsigned long adr, unsigned long data) {
  return dbg_cpu_write(adr, DI_CPU_WRITE32, data);
}
 
/* write a cpu module register */
int dbg_cpu_write_reg(unsigned long adr, unsigned long data) {
  return dbg_cpu_write(adr, DI_CPU_WRITE_REG, data);
}
 
void dbg_test() {
  int i;
  unsigned long npc, ppc, r1, insn, result;
#if 0
#define MC_BASE_ADD     0x60000000
#define MC_CSR_VAL      0x0f300300
#define MC_MASK_VAL     0x000000e0
#define FLASH_BASE_ADD  0x04000000
#define FLASH_TMS_VAL   0x0010a10a
#define SDRAM_BASE_ADD  0x00000000
#define SDRAM_TMS_VAL   0x07248230
 
  dbg_cpu_write_reg(0, 0x01);
 
  dbg_wb_write32(MC_BASE_ADD + MC_CSC(0),(((FLASH_BASE_ADD & 0xffff0000) >> 5) | 0x25));
  dbg_wb_write32(MC_BASE_ADD + MC_TMS(0), FLASH_TMS_VAL);
 
  dbg_wb_write32(MC_BASE_ADD + MC_BA_MASK, MC_MASK_VAL);
  dbg_wb_write32(MC_BASE_ADD + MC_CSR, MC_CSR_VAL);
 
  dbg_wb_write32(MC_BASE_ADD + MC_TMS(1), SDRAM_TMS_VAL);
  dbg_wb_write32(MC_BASE_ADD + MC_CSC(1),(((SDRAM_BASE_ADD & 0xffff0000) >> 5) | 0x0411));
 
  sleep(1);
#endif
 
#if 1
 
#define RAM_BASE 0x00000000
  /* Stall risc */
  dbg_cpu_write_reg(0, 0x01);
 
  dbg_wb_write32(RAM_BASE+0x00, 0x9c200000);                             /* l.addi  r1,r0,0x0*/
  dbg_wb_write32(RAM_BASE+0x04, 0x18400000+(RAM_BASE >> 16));            /* l.movhi r2,0x4000*/
  dbg_wb_write32(RAM_BASE+0x08, 0xa8420000+((RAM_BASE + 0x30) & 0xffff));/* l.ori   r2,r2,0x0*/
  dbg_wb_write32(RAM_BASE+0x0c, 0x9c210001);                             /* l.addi  r1,r1,1  */
  dbg_wb_write32(RAM_BASE+0x10, 0x9c210001);                             /* l.addi  r1,r1,1  */
  dbg_wb_write32(RAM_BASE+0x14, 0xd4020800);                             /* l.sw    0(r2),r1 */
  dbg_wb_write32(RAM_BASE+0x18, 0x9c210001);                             /* l.addi  r1,r1,1  */
  dbg_wb_write32(RAM_BASE+0x1c, 0x84620000);                             /* l.lwz   r3,0(r2) */
  dbg_wb_write32(RAM_BASE+0x20, 0x03fffffb);                             /* l.j     loop2    */
  dbg_wb_write32(RAM_BASE+0x24, 0xe0211800);                             /* l.add   r1,r1,r3 */
  dbg_wb_write32(RAM_BASE+0x24, 0xe0211800);                             /* l.add   r1,r1,r3 */
 
  dbg_cpu_write32((0 << 11) + 17, 0x01);  /* Enable exceptions */
  dbg_cpu_write32((6 << 11) + 20, 0x2000);  /* Trap causes stall */
  dbg_cpu_write32((0 << 11) + 16, RAM_BASE);  /* Set PC */
  dbg_cpu_write32((6 << 11) + 16, 1 << 22);  /* Set step bit */
  for(i = 0; i < 10; i++) dbg_cpu_write_reg(0, 0x00);  /* 10x Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  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", 0x4000000c, 0x40000024, 5);
  result = npc + ppc + r1;
 
  dbg_cpu_write32((6 << 11) + 16, 0);  /* Reset step bit */
  dbg_wb_read32(RAM_BASE + 0x24, &insn);  /* Set trap insn in delay slot */
  dbg_wb_write32(RAM_BASE + 0x24, 0x21000001);
  dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  dbg_cpu_read32(0x401, &r1);  /* Read R1 */
  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", 0x4000000c, 0x40000024, 8);
  result = npc + ppc + r1 + result;
 
  dbg_wb_read32(RAM_BASE + 0x20, &insn);  /* Set trap insn in place of branch insn */
  dbg_wb_write32(RAM_BASE + 0x20, 0x21000001);
  dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x0c);  /* Set PC */
  dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  dbg_cpu_read32(0x401, &r1);  /* Read R1 */
  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", 0x40000024, 0x40000020, 11);
  result = npc + ppc + r1 + result;
 
  dbg_wb_read32(RAM_BASE + 0x1c, &insn);  /* Set trap insn before branch insn */
  dbg_wb_write32(RAM_BASE + 0x1c, 0x21000001);
  dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x20);  /* Set PC */
  dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  dbg_cpu_read32(0x401, &r1);  /* Read R1 */
  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", 0x40000020, 0x4000001c, 24);
  result = npc + ppc + r1 + result;
 
 
  dbg_wb_read32(RAM_BASE + 0x18, &insn);  /* Set trap insn behind lsu insn */
  dbg_wb_write32(RAM_BASE + 0x18, 0x21000001);
  dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x1c);  /* Set PC */
  dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  dbg_cpu_read32(0x401, &r1);  /* Read R1 */
  dbg_wb_write32(RAM_BASE + 0x18, 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", 0x4000001c, 0x40000018, 49);
  result = npc + ppc + r1 + result;
 
  dbg_wb_read32(RAM_BASE + 0x1c, &insn);  /* Set trap insn very near previous one */
  dbg_wb_write32(RAM_BASE + 0x1c, 0x21000001);
  dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x18);  /* Set PC */
  dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  dbg_cpu_read32(0x401, &r1);  /* Read R1 */
  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", 0x40000020, 0x4000001c, 50);
  result = npc + ppc + r1 + result;
 
  dbg_wb_read32(RAM_BASE + 0x0c, &insn);  /* Set trap insn to the start */
  dbg_wb_write32(RAM_BASE + 0x0c, 0x21000001);
  dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x1c)  /* Set PC */;
  dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  dbg_cpu_read32(0x401, &r1);  /* Read R1 */
  dbg_wb_write32(RAM_BASE + 0x0c, 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", 0x40000010, 0x4000000c, 99);
  result = npc + ppc + r1 + result;
 
  dbg_cpu_write32((6 << 11) + 16, 1 << 22);  /* Set step bit */
  for(i = 0; i < 5; i++) dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  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", 0x40000024, 0x40000020, 101);
  result = npc + ppc + r1 + result;
 
  dbg_cpu_write32((0 << 11) + 16, RAM_BASE + 0x20);  /* Set PC */
  for(i = 0; i < 2; i++) dbg_cpu_write_reg(0, 0x00);  /* Unstall */
  dbg_cpu_read32((0 << 11) + 16, &npc);  /* Read NPC */
  dbg_cpu_read32((0 << 11) + 18, &ppc);  /* Read PPC */
  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", 0x4000000c, 0x40000024, 201);
  result = npc + ppc + r1 + result;
 
  printf("result = %.8lx\n", result + 0x5eaddaa9);
#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);
}