URL https://opencores.org/ocsvn/or1k/or1k/trunk
Subversion Repositories or1k

[/] [or1k/] [trunk/] [jtag/] [jp1.c] - Blame information for rev 1322

Go to most recent revision | Details | Compare with Previous | View Log

/* jp1-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"
 
 
#define Boolean int
#define false 0
#define true 1
 
#define GDB_IN "../sim/rtl_sim/run/gdb_in.dat"
#define GDB_OUT "../sim/rtl_sim/run/gdb_out.dat"
 
/* Libraries for JTAG proxy server.  */
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/poll.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/tcp.h>
#include <inttypes.h>
#include <errno.h>
 
#include "gdb.h" /* partially copied from gdb/config/or1k */
 
#ifdef DEBUG
#define debug printf
#else
#define debug
#endif
 
#ifdef DEBUG2
#define debug2 printf
#else
#define debug2
#endif
 
#if (DEBUG) || (DEBUG2)
#define flush_debug() fflush(stdout)
#else
#define flush_debug()
#endif
 
#define LPT_BASE (base)
#define LPT_READ (LPT_BASE+1)
#define LPT_WRITE LPT_BASE
#if RTL_SIM
#define TCLK_BIT (0x01) /* D0, pin #2 */
#define TRST_BIT (0x02) /* D1, pin #3 */
#define TDI_BIT  (0x04) /* D2, pin #4 */
#define TMS_BIT  (0x08) /* D0, pin #5 */
#define TDO_BIT  (0x20) /* PE, pin #12 */
#define TMS      (0x02)
#define TDI      (0x01)
#else
#ifdef XILINX_PARALLEL_CABLE_III
#define TCLK_BIT (0x02) /* D1 pin 3 */
#define TRST_BIT (0x10) /* Not used */
#define TDI_BIT  (0x01) /* D0 pin 2 */
#define TMS_BIT  (0x04) /* D2 pin 4 */
#define TDO_BIT  (0x10) /* S6 pin 13*/
#define TMS      (0x02)
#define TDI      (0x01)
#else
#ifdef XESS_PARALLEL_CABLE
#define TCLK_BIT (0x04) /* D2 pin 4 */
#define TRST_BIT (0x08) /* D3 pin 5 */
#define TDI_BIT  (0x10) /* D4 pin 6 */
#define TMS_BIT  (0x20) /* D5 pin 7 */
#define TDO_BIT  (0x20) /* S5 pin 12*/
#define TMS      (0x02)
#define TDI      (0x01)
#endif
#endif
#endif
 
#ifdef RTL_SIM
# define JTAG_WAIT() usleep(1000)
# define NUM_RETRIES (16)
# define JTAG_RETRY_WAIT() usleep (1000)
#else
# define JTAG_WAIT() {      \
  int i;        \
  volatile int j;   \
  for(i = 0; i < 1000; i++) \
    j = i;      \
  }
# define NUM_RETRIES (16)
# define JTAG_RETRY_WAIT() usleep (1000)
#endif
 
/* Selects crc trailer size in bits. Currently supported: 8 */
#define CRC_SIZE (8)
 
/* Scan chain size in bits.  */
#define SC_SIZE (4)
 
#ifndef ULONGEST
#define ULONGEST unsigned long
#endif
 
static int base = 0x378; /* FIXME: We should detect the address. */
int err = 0;
int set_pc = 0;
int set_step = 0;
int waiting = 0;
 
unsigned int serverIP = 0;
unsigned int serverPort = 0;
unsigned int server_fd = 0;
unsigned int gdb_fd = 0;
void HandleServerSocket(Boolean block);
void JTAGRequest(void);
void GDBRequest(void);
void ProtocolClean(int,int32_t);
static int gdb_read(void*,int);
static int gdb_write(void*,int);
static void jtag_set_chain (int);
 
/* Scan chain info. */
/* *INDENT-OFF* */
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};
/* *INDENT-OFF* */
 
/* Currently selected scan chain - just to prevent unnecessary
   transfers. */
static int current_chain;
 
/* Designates whether we are in SELECT_DR state, otherwise in
   RUN TEST/IDLE */
static int select_dr = 0;
 
/* 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 int
crc_calc (int crc, int input_bit)
{
  int c;
  int new_crc;
  int d;
 
#if (CRC_SIZE == 8)
  d = input_bit&1;
  c = crc;
 
  /* Move queue left.  */
  new_crc = crc << 1;
 
  /* Mask upper five bits.  */
  new_crc &= 0xF8;
 
  /* Set lower three bits */
  new_crc |= (d ^ ((c >> 7)&1));
  new_crc |= (d ^ ((c >> 0)&1) ^ ((c >> 7)&1)) << 1;
  new_crc |= (d ^ ((c >> 1)&1) ^ ((c >> 7)&1)) << 2;
  return new_crc;
#else
  return 0;
#endif
}
 
/* Send a byte to parallel port. */
 
inline static void
jp1_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
jp1_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
jp1_write_JTAG (packet)
  unsigned char packet;
{
  unsigned char data = TRST_BIT;
  if (packet & 1)
    data |= TDI_BIT;
  if (packet & 2)
    data |= TMS_BIT;
 
  jp1_out (data);
  JTAG_WAIT();
  crc_w = crc_calc (crc_w, packet&1);
 
  /* rise clock */
  jp1_out (data | TCLK_BIT);
  JTAG_WAIT();
}
 
/* Reads TDI.  */
 
static inline int
jp1_read_JTAG ()
{
  int data;
  data = jp1_in ();
  crc_r = crc_calc (crc_r, data);
  return data;
}
 
/* Writes bitstream.  LS bit first.  */
 
static inline void
jp1_write_stream (stream, len, set_last_bit)
  ULONGEST stream;
  int len;
  int set_last_bit;
{
  int i;
  if (len <= 0) return;
  debug("\nwrite(");
  for (i = 0; i < len - 1; i++)
    jp1_write_JTAG ((stream >> i) & 1);
 
  if (set_last_bit)
    jp1_write_JTAG ((stream >> (len - 1))& 1 | TMS);
  else
    jp1_write_JTAG ((stream >> (len - 1))& 1);
  debug(")\n");
}
 
/* Gets bitstream.  LS bit first.  */
 
inline static ULONGEST
jp1_read_stream (stream, len, set_last_bit)
  unsigned long stream;
  int len;
  int set_last_bit;
{
  int i;
  ULONGEST data;
  debug("\nread(");
  if (len <= 0) return;
  data = 0;
  for (i = 0; i < len - 1; i++)
    {
      jp1_write_JTAG (stream & 1);   /* LSB first */
      stream >>= 1;
      data |= jp1_read_JTAG () << i; /* LSB first */
    }
 
  if (set_last_bit)
    jp1_write_JTAG (stream & 1 | TMS);
  else
    jp1_write_JTAG (stream & 1);
  data |= jp1_read_JTAG () << (len - 1);
  debug(")\n");
  return data;
}
 
/* Goes into SELECT_IR state. Should be called before every control write.  */
 
inline static void
jp1_prepare_control ()
{
  if (!select_dr)
    jp1_write_JTAG (TMS); /* SELECT_DR SCAN */
  jp1_write_JTAG (TMS);   /* SELECT_IR SCAN */
  select_dr = 0;
}
 
/* Resets JTAG.
   Writes TRST=0
   and    TRST=1 */
 
static void
jp1_reset_JTAG ()
{
  int i;
  debug2 ("\nreset(");
  jp1_out (0);
  JTAG_RETRY_WAIT();
  /* In case we don't have TRST reset it manually */
  for (i = 0; i < 8; i++)
    jp1_write_JTAG (TMS);
  jp1_out (TRST_BIT);
  JTAG_RETRY_WAIT();
  jp1_write_JTAG (0);
  debug2(")\n");
  select_dr = 0;
}
 
/* Sets register/memory regno to data.  */
 
/* CZ 08/06/01: I am not sure how error checking is intended to
   be implemented here. It appears that no indication is returned
   to the caller as you have in standard unix system calls. Therefore,
   I guess the only way to use these functions when you want to know
   the exact position of the error is to manually clear err, call the
   function, and then manually check err. I have also made some changes
   where necessary because no value was returned at all int jtag_read_reg.
*/
 
static void
jtag_write_reg_support (regno, data)
  int regno;
  ULONGEST data;
{
  int crc_read, crc_write, crc_ok, retry;
  int result;
  int tmp;
 
  debug("\n");
  debug2("write_reg %i(%08x) <- %08x \n", regno, regno, data);
  if (!select_dr)
    jp1_write_JTAG (TMS); /* SELECT_DR SCAN */
  select_dr = 1;
 
      /* If we don't have rw bit, we assume chain
   is read only. */
  if (!chain_has_rw[current_chain])
    error ("Internal: Chain not writable.");
 
  for (retry = 0; retry < NUM_RETRIES; retry++) {
    jp1_write_JTAG (0); /* CAPTURE_DR */
    jp1_write_JTAG (0); /* SHIFT_DR */
    crc_w = 0;
 
    /* write addr */
    jp1_write_stream (regno, chain_addr_size[current_chain], 0);
 
    /* write (R/W=1) - we tested that previously. */
    jp1_write_JTAG (TDI);
 
    if (chain_has_crc[current_chain])
      {
        /* write data */
        jp1_write_stream (data, chain_data_size[current_chain], 0);
        crc_write = crc_w;
 
        /* write CRC, EXIT1_DR */
        crc_read = jp1_read_stream (crc_write, CRC_SIZE + 1, 1) >> 1;
      }
    else
      {
        /* write data */
        jp1_write_stream (data, chain_data_size[current_chain], 1);
      }
    jp1_write_JTAG (TMS); /* UPDATE_DR */
    jp1_write_JTAG (TMS); /* SELECT_DR */
 
    /* Did JTAG receive packet correctly? */
    if (chain_has_crc[current_chain])
      crc_ok = crc_read == crc_write;
 
    if (chain_has_crc[current_chain])
      {
        if (crc_ok)
          return;
 
        debug2(", crc failed. read %08x, generated %08x\n", crc_read, crc_write);
        jp1_reset_JTAG();
        jp1_write_JTAG (TMS); /* SELECT_DR SCAN */
        select_dr = 1;
        tmp = current_chain;
        current_chain = -1;
        jtag_set_chain(tmp);
      }
    else
      return;
  }
  printf ("Invalid CRC\n");
  err = ERR_CRC;
}
 
/* Reads register/memory from regno.
   Reading is a bit strange. Data is not available
   at the time we pass an address, but in successive
   read instead. Call jtag_read_reg twice to get correct
   data. */
 
static ULONGEST
jtag_read_reg (regno)
  unsigned int regno;
{
  ULONGEST data;
  int crc_read, crc_write, crc_actual_read,  retry, crc_ok;
  int result;
  int tmp;
 
  debug("\n");
  debug2("read_reg %i(%08x)", regno, regno);
  debug (" \n ");
  if (!select_dr)
    jp1_write_JTAG (TMS); /* SELECT_DR SCAN */
  select_dr = 1;
 
  for (retry = 0; retry < NUM_RETRIES; retry++) {
    jp1_write_JTAG (0); /* CAPTURE_DR */
    jp1_write_JTAG (0); /* SHIFT_DR */
    crc_w = 0;
 
    /* write addr */
    jp1_write_stream (regno, chain_addr_size[current_chain], 0);
 
    /* read (R/W=0) */
    if (chain_has_rw[current_chain])
      jp1_write_JTAG (0);
    if (chain_has_crc[current_chain])
      {
        crc_r = 0;
 
        /* data = 0 */
        data = jp1_read_stream (0, chain_data_size[current_chain], 0);
        crc_write = crc_w;
        crc_actual_read = crc_r;
 
        /* Send my crc, EXIT1_DR */
        crc_read = jp1_read_stream (crc_write, CRC_SIZE + 1, 1) >> 1;
      } else {
        /* data = 0 */
        data = jp1_read_stream (0, chain_data_size[current_chain], 1);
      }
    jp1_write_JTAG (TMS); /* UPDATE_DR */
    jp1_write_JTAG (TMS); /* SELECT_DR */
 
    /* Did JTAG receive packet correctly? */
    if (chain_has_crc[current_chain])
      crc_ok = jp1_read_JTAG ();
 
    if (chain_has_crc[current_chain])
      {
        if ((crc_read == crc_actual_read) && (crc_ok)) {
          debug2(" , read_reg %i(%08x) = %08x\n", regno, regno, data);
          prev_regno = regno;
          return data;
        }
        debug2(", crc failed. read %08x, generated %08x\n", crc_read, crc_actual_read);
        jp1_reset_JTAG();
        jp1_write_JTAG (TMS); /* SELECT_DR SCAN */
        select_dr = 1;
        tmp = current_chain;
        current_chain = -1;
        jtag_set_chain(tmp);
        jtag_read_reg (prev_regno);
        if (err) return -1;
      }
    else {
      debug2(" , read_reg %i(%08x) = %08x\n", regno, regno, data);
      prev_regno = regno;
      return data;
    }
  }
  printf ("Invalid CRC\n");
  err = ERR_CRC;
  return -1;
}
 
/* Sets scan chain.  */
 
static void
jtag_set_chain (chain)
  int chain;
{
  int crc_read, crc_write, crc_ok, retry;
  int result;
 
  debug("\n");
  debug2("set_chain %i\n", chain);
  if (current_chain != chain) {
    if (!chain_is_valid[chain])
      error ("Chain not valid.");
 
    current_chain = chain;
    jp1_prepare_control ();
 
    while (1) {
      jp1_write_JTAG (0); /* CAPTURE_IR */
      jp1_write_JTAG (0); /* SHIFT_IR */
 
      /* write data, EXIT1_IR */
      jp1_write_stream (JI_CHAIN_SELECT, JI_SIZE, 1);
 
      jp1_write_JTAG (TMS); /* UPDATE_IR */
      jp1_write_JTAG (TMS); /* SELECT_DR */
 
      jp1_write_JTAG (0); /* CAPTURE_DR */
      jp1_write_JTAG (0); /* SHIFT_DR */
 
      if (chain_has_crc[current_chain])
        {
          crc_w = 0;
          /* write data */
          jp1_write_stream (chain, SC_SIZE, 0);
 
          crc_write = crc_w;
          /* write CRC, EXIT1_DR */
          crc_read = jp1_read_stream (crc_write, CRC_SIZE + 1, 1) >> 1;
        } else {
          /* write data, EXIT1_DR */
          jp1_write_stream (chain, SC_SIZE, 1);
        }
      jp1_write_JTAG (TMS); /* UPDATE_DR */
      jp1_write_JTAG (TMS); /* SELECT_DR */
 
      /* Did JTAG receive packet correctly? */
      if (chain_has_crc[current_chain])
        crc_ok = crc_read == crc_write;
 
      if (chain_has_crc[current_chain])
        {
          if (!crc_ok)
            {
              debug2(", crc failed.\n");
              jp1_reset_JTAG();
              jp1_prepare_control ();
              continue;
            }
        }
 
      jp1_write_JTAG (TMS); /* SELECT_IR */
      jp1_write_JTAG (0); /* CAPTURE_IR */
      jp1_write_JTAG (0); /* SHIFT_IR */
      crc_w = 0;
 
      /* write data, EXIT1_IR */
      jp1_write_stream (JI_DEBUG, JI_SIZE, 1);
 
      jp1_write_JTAG (TMS); /* UPDATE_IR */
      jp1_write_JTAG (TMS); /* SELECT_DR */
      select_dr = 1;
 
      return;
    }
    printf ("Invalid CRC\n");
    err = ERR_CRC;
  } else
    debug2 ("Already set.\n");
}
 
/* Sets register/memory regno to data.  */
 
static void
jtag_write_reg (regno, data)
  int regno;
  ULONGEST data;
{
  /* Set PC */
//  if (current_chain == SC_RISC_DEBUG && regno == 0x10)
//    data = data - 4;
 
  jtag_write_reg_support (regno, data);
}
 
/* Stalls the CPU.  */
 
static void
or1k_stall ()
{
  int val;
  jtag_set_chain (SC_REGISTER);
  val = jtag_read_reg (JTAG_RISCOP);
  jtag_write_reg (JTAG_RISCOP, val | 1);
}
 
/* Unstalls the CPU.  */
 
static void
or1k_unstall ()
{
  unsigned int val;
 
  jtag_set_chain (SC_REGISTER);
  val = jtag_read_reg (JTAG_RISCOP);
  jtag_write_reg (JTAG_RISCOP, val & ~1);
}
 
/* Initialize a new connection to the or1k board, and make sure we are
   really connected.  */
 
static int
jtag_init () {
  int tmp, i;
  unsigned int npc, ppc, r1, insn, result;
  current_chain = -1;
  jp1_reset_JTAG ();
#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
 
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000001);
 
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (MC_BASE_ADD + MC_CSC(0), (((FLASH_BASE_ADD & 0xffff0000) >> 5) | 0x25));
  jtag_write_reg (MC_BASE_ADD + MC_TMS(0), FLASH_TMS_VAL);
 
  jtag_write_reg (MC_BASE_ADD + MC_BA_MASK, MC_MASK_VAL);
  jtag_write_reg (MC_BASE_ADD + MC_CSR, MC_CSR_VAL);
 
  jtag_write_reg (MC_BASE_ADD + MC_TMS(1), SDRAM_TMS_VAL);
  jtag_write_reg (MC_BASE_ADD + MC_CSC(1), (((SDRAM_BASE_ADD & 0xffff0000) >> 5) | 0x0411));
 
  sleep(1);
#endif
 
#if 1
 
#define RAM_BASE 0x00000000
  /* Stall risc */
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000001);
 
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (RAM_BASE + 0x00, 0x9c200000);                                /* l.addi  r1,r0,0x0       */
  jtag_write_reg (RAM_BASE + 0x04, 0x18400000 + (RAM_BASE >> 16));             /* l.movhi r2,0x4000       */
  jtag_write_reg (RAM_BASE + 0x08, 0xa8420000 + ((RAM_BASE + 0x30) & 0xffff)); /* l.ori   r2,r2,0x0000    */
  jtag_write_reg (RAM_BASE + 0x0c, 0x9c210001);                                /* l.addi  r1,r1,1         */
  jtag_write_reg (RAM_BASE + 0x10, 0x9c210001);                                /* l.addi  r1,r1,1         */
  jtag_write_reg (RAM_BASE + 0x14, 0xd4020800);                                /* l.sw    0(r2),r1        */
  jtag_write_reg (RAM_BASE + 0x18, 0x9c210001);                                /* l.addi  r1,r1,1         */
  jtag_write_reg (RAM_BASE + 0x1c, 0x84620000);                                /* l.lwz   r3,0(r2)        */
  jtag_write_reg (RAM_BASE + 0x20, 0x03fffffb);                                /* l.j     loop2           */
  jtag_write_reg (RAM_BASE + 0x24, 0xe0211800);                                /* l.add   r1,r1,r3        */
  jtag_write_reg (RAM_BASE + 0x24, 0xe0211800);                                /* l.add   r1,r1,r3        */
 
  /* Enable exceptions */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 17, 0x01);
 
  /* Trap causes stall */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((6 << 11) + 20, 0x2000);
 
  /* Set PC */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 16, RAM_BASE);
 
  /* Set step bit */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((6 << 11) + 16, 1 << 22);
 
 
  for (i = 0; i < 10; i++)
    {
      /* Unstall */
      jtag_set_chain (SC_REGISTER);
      jtag_write_reg (4, 0x00000000);
      jtag_set_chain (SC_RISC_DEBUG);
    }
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  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;
 
 
  /* Reset step bit */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((6 << 11) + 16, 0);
 
 
 
  /* Set trap insn in delay slot */
  jtag_set_chain (SC_WISHBONE);
  insn = jtag_read_reg (RAM_BASE + 0x24);
  insn = jtag_read_reg (RAM_BASE + 0x24);
  jtag_write_reg (RAM_BASE + 0x24, 0x21000001);
 
  /* Unstall */
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000000);
  jtag_set_chain (SC_RISC_DEBUG);
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  /* Set back original insn */
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (RAM_BASE + 0x24, 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;
 
 
  /* Set trap insn in place of branch insn */
  jtag_set_chain (SC_WISHBONE);
  insn = jtag_read_reg (RAM_BASE + 0x20);
  insn = jtag_read_reg (RAM_BASE + 0x20);
  jtag_write_reg (RAM_BASE + 0x20, 0x21000001);
 
  /* Set PC */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 16, RAM_BASE + 0x0c);
 
  /* Unstall */
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000000);
  jtag_set_chain (SC_RISC_DEBUG);
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  /* Set back original insn */
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (RAM_BASE + 0x20, 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;
 
 
  /* Set trap insn before branch insn */
  jtag_set_chain (SC_WISHBONE);
  insn = jtag_read_reg (RAM_BASE + 0x1c);
  insn = jtag_read_reg (RAM_BASE + 0x1c);
  jtag_write_reg (RAM_BASE + 0x1c, 0x21000001);
 
  /* Set PC */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 16, RAM_BASE + 0x20);
 
  /* Unstall */
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000000);
  jtag_set_chain (SC_RISC_DEBUG);
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  /* Set back original insn */
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (RAM_BASE + 0x1c, 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;
 
 
  /* Set trap insn behind lsu insn */
  jtag_set_chain (SC_WISHBONE);
  insn = jtag_read_reg (RAM_BASE + 0x18);
  insn = jtag_read_reg (RAM_BASE + 0x18);
  jtag_write_reg (RAM_BASE + 0x18, 0x21000001);
 
  /* Set PC */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 16, RAM_BASE + 0x1c);
 
  /* Unstall */
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000000);
  jtag_set_chain (SC_RISC_DEBUG);
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  /* Set back original insn */
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (RAM_BASE + 0x18, 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;
 
  /* Set trap insn very near previous one */
  jtag_set_chain (SC_WISHBONE);
  insn = jtag_read_reg (RAM_BASE + 0x1c);
  insn = jtag_read_reg (RAM_BASE + 0x1c);
  jtag_write_reg (RAM_BASE + 0x1c, 0x21000001);
 
  /* Set PC */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 16, RAM_BASE + 0x18);
 
  /* Unstall */
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000000);
  jtag_set_chain (SC_RISC_DEBUG);
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  /* Set back original insn */
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (RAM_BASE + 0x1c, 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;
 
 
  /* Set trap insn to the start */
  jtag_set_chain (SC_WISHBONE);
  insn = jtag_read_reg (RAM_BASE + 0x0c);
  insn = jtag_read_reg (RAM_BASE + 0x0c);
  jtag_write_reg (RAM_BASE + 0x0c, 0x21000001);
 
  /* Set PC */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 16, RAM_BASE + 0x1c);
 
  /* Unstall */
  jtag_set_chain (SC_REGISTER);
  jtag_write_reg (4, 0x00000000);
  jtag_set_chain (SC_RISC_DEBUG);
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  /* Set back original insn */
  jtag_set_chain (SC_WISHBONE);
  jtag_write_reg (RAM_BASE + 0x0c, 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;
 
 
  /* Set step bit */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((6 << 11) + 16, 1 << 22);
 
  for (i = 0; i < 5; i++)
    {
      /* Unstall */
      jtag_set_chain (SC_REGISTER);
      jtag_write_reg (4, 0x00000000);
      jtag_set_chain (SC_RISC_DEBUG);
    }
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  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;
 
  /* Set PC */
  jtag_set_chain (SC_RISC_DEBUG);
  jtag_write_reg ((0 << 11) + 16, RAM_BASE + 0x20);
 
  for (i = 0; i < 2; i++)
    {
      /* Unstall */
      jtag_set_chain (SC_REGISTER);
      jtag_write_reg (4, 0x00000000);
      jtag_set_chain (SC_RISC_DEBUG);
    }
 
  /* Read NPC */
  jtag_set_chain (SC_RISC_DEBUG);
  npc = jtag_read_reg ((0 << 11) + 16);
  npc = jtag_read_reg ((0 << 11) + 16);
 
  /* Read PPC */
  jtag_set_chain (SC_RISC_DEBUG);
  ppc = jtag_read_reg ((0 << 11) + 18);
  ppc = jtag_read_reg ((0 << 11) + 18);
 
  /* Read R1 */
  jtag_set_chain (SC_RISC_DEBUG);
  r1 = jtag_read_reg (0x401);
  r1 = jtag_read_reg (0x401);
 
  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
 
  return err;
}
 
main(argc, argv)
  int argc;
  char *argv[];
{
  char *redirstr;
  int trace_fd = 0;
  char *s;
 
  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("Connected to 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
 
  /* Test the connection.  */
  if (jtag_init()) {
    fprintf(stderr,"Connection with jtag via parallel port failed.\n");
    exit(-1);
  }
 
  /* 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);
}
 
/************************
   JTAG Server Routines
 ************************/
 
static int tcp_level = 0;
 
/* Added by CZ 24/05/01 */
int GetServerSocket(const char* name,const char* proto,int port)
{
  struct servent *service;
  struct protoent *protocol;
  struct sockaddr_in sa;
  struct hostent *hp;
  int sockfd;
  char myname[256];
  int flags;
  char sTemp[256];
 
  /* First, get the protocol number of TCP */
  if(!(protocol = getprotobyname(proto)))
    {
      sprintf(sTemp,"Unable to load protocol \"%s\"",proto);
      perror(sTemp);
      return 0;
    }
  tcp_level = protocol->p_proto; /* Save for later */
 
  /* If we weren't passed a non standard port, get the port
     from the services directory. */
  if(!port)
    {
      if(service = getservbyname(name,protocol->p_name))
  port = ntohs(service->s_port);
    }
 
  /* Create the socket using the TCP protocol */
  if((sockfd = socket(PF_INET,SOCK_STREAM,protocol->p_proto)) < 0)
    {
      perror("Unable to create socket");
      return 0;
    }
 
  flags = 1;
  if(setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,(const char*)&flags,sizeof(int))
 < 0)
    {
      sprintf(sTemp,"Can not set SO_REUSEADDR option on socket %d",sockfd);
      perror(sTemp);
      close(sockfd);
      return 0;
    }
 
  /* The server should also be non blocking. Get the current flags. */
  if((flags = fcntl(sockfd,F_GETFL,0)) < 0)
    {
      sprintf(sTemp,"Unable to get flags for socket %d",sockfd);
      perror(sTemp);
      close(sockfd);
      return 0;
    }
 
  /* Set the nonblocking flag */
  if(fcntl(sockfd,F_SETFL, flags | O_NONBLOCK) < 0)
    {
      sprintf(sTemp,"Unable to set flags for socket %d to value 0x%08x",
        sockfd,flags | O_NONBLOCK);
      perror(sTemp);
      close(sockfd);
      return 0;
    }
 
  /* Find out what our address is */
  memset(&sa,0,sizeof(struct sockaddr_in));
  gethostname(myname,sizeof(myname));
  if(!(hp = gethostbyname(myname)))
    {
      perror("Unable to read hostname");
      close(sockfd);
      return 0;
    }
 
  /* Bind our socket to the appropriate address */
  sa.sin_family = hp->h_addrtype;
  sa.sin_port = htons(port);
  if(bind(sockfd,(struct sockaddr*)&sa,sizeof(struct sockaddr_in)) < 0)
    {
      sprintf(sTemp,"Unable to bind socket %d to port %d",sockfd,port);
      perror(sTemp);
      close(sockfd);
      return 0;
    }
  serverIP = sa.sin_addr.s_addr;
  flags = sizeof(struct sockaddr_in);
  if(getsockname(sockfd,(struct sockaddr*)&sa,&flags) < 0)
    {
      sprintf(sTemp,"Unable to get socket information for socket %d",sockfd);
      perror(sTemp);
      close(sockfd);
      return 0;
    }
  serverPort = ntohs(sa.sin_port);
 
  /* Set the backlog to 1 connections */
  if(listen(sockfd,1) < 0)
    {
      sprintf(sTemp,"Unable to set backlog on socket %d to %d",sockfd,1);
      perror(sTemp);
      close(sockfd);
      return 0;
    }
 
  return sockfd;
}
 
void HandleServerSocket(Boolean block)
{
  struct pollfd fds[2];
  int n;
 
rebuild:
  n = 0;
  if(!server_fd && !gdb_fd)
    return;
 
  if(server_fd)
    {
      fds[n].fd = server_fd;
      fds[n].events = POLLIN;
      fds[n++].revents = 0;
    }
  if(gdb_fd)
    {
      fds[n].fd = gdb_fd;
      fds[n].events = POLLIN;
      fds[n++].revents = 0;
    }
 
  while(1)
    {
      switch(poll(fds, n, -1))
        {
          case 0:
          case -1:
            if(errno == EINTR)
              continue;
            perror("poll");
            server_fd = 0;
            return;
          default:
            /* Make sure to handle the gdb port first! */
            if (gdb_fd && (fds[0].revents && !server_fd ||
                           fds[1].revents && server_fd))
              {
                int revents = server_fd ? fds[1].revents : fds[0].revents;
 
                if (revents & POLLIN)
                   GDBRequest();
                else /* Error Occurred */
                  {
                    fprintf(stderr,"Received flags 0x%08x on gdb socket. Shutting down.\n",revents);
                    close(gdb_fd);
                    gdb_fd = 0;
                  }
               }
            if(fds[0].revents && server_fd)
              {
                if(fds[0].revents & POLLIN) {
                  JTAGRequest();
                  goto rebuild;
                } else /* Error Occurred */
                  {
                    fprintf(stderr,"Received flags 0x%08x on server. Shutting down.\n",fds[0].revents);
                    close(server_fd);
                    server_fd = 0;
                    serverPort = 0;
                    serverIP = 0;
                    return;
                  }
              }
           if (fds[1].revents && !gdb_fd) {
                   /* gdb_fd was cleared because connection was closed,
                    * but fd still in poll set */
                   fds[1].fd = -1;
                   n = 1;
           }
            break;
        } /* End of switch statement */
    } /* End of while statement */
}
 
void JTAGRequest()
{
  struct sockaddr_in sa;
  struct sockaddr* addr = (struct sockaddr*)&sa;
  int n = sizeof(struct sockaddr_in);
  int fd = accept(server_fd,addr,&n);
  int on_off = 0; /* Turn off Nagel's algorithm on the socket */
  int flags;
  char sTemp[256];
 
  if(fd < 0)
    {
      /* This is valid, because a connection could have started,
         and then terminated due to a protocol error or user
         initiation before the accept could take place. */
      if(errno != EWOULDBLOCK && errno != EAGAIN)
        {
          perror("accept");
          close(server_fd);
          server_fd = 0;
          serverPort = 0;
          serverIP = 0;
        }
      return;
    }
 
  if(gdb_fd)
    {
      close(fd);
      return;
    }
 
  if((flags = fcntl(fd,F_GETFL,0)) < 0)
    {
      sprintf(sTemp,"Unable to get flags for gdb socket %d",fd);
      perror(sTemp);
      close(fd);
      return;
    }
 
  if(fcntl(fd,F_SETFL, flags | O_NONBLOCK) < 0)
    {
      sprintf(sTemp,"Unable to set flags for gdb socket %d to value 0x%08x",
        fd,flags | O_NONBLOCK);
      perror(sTemp);
      close(fd);
      return;
    }
 
  if(setsockopt(fd,tcp_level,TCP_NODELAY,&on_off,sizeof(int)) < 0)
    {
      sprintf(sTemp,"Unable to disable Nagel's algorithm for socket %d.\nsetsockopt",fd);
      perror(sTemp);
      close(fd);
      return;
    }
 
  gdb_fd = fd;
}
 
void GDBRequest()
{
  JTAGProxyWriteMessage msg_write;
  JTAGProxyReadMessage msg_read;
  JTAGProxyChainMessage msg_chain;
  JTAGProxyWriteResponse resp_write;
  JTAGProxyReadResponse resp_read;
  JTAGProxyChainResponse resp_chain;
  JTAGProxyBlockWriteMessage *msg_bwrite;
  JTAGProxyBlockReadMessage msg_bread;
  JTAGProxyBlockWriteResponse resp_bwrite;
  JTAGProxyBlockReadResponse *resp_bread;
  char *buf;
  unsigned long long data;
  uint32_t command,length;
  int len,i;
  err = 0;
 
  /* First, we must read the incomming command */
  if(gdb_read(&command,sizeof(uint32_t)) < 0)
    {
      if(gdb_fd)
        {
          perror("gdb socket - 1");
          close(gdb_fd);
          gdb_fd = 0;
        }
      return;
    }
  if(gdb_read(&length,sizeof(uint32_t)) < 0)
    {
      if(gdb_fd)
        {
          perror("gdb socket - 2");
          close(gdb_fd);
          gdb_fd = 0;
        }
      return;
    }
  length = ntohl(length);
 
  /* Now, verify the protocol and implement the command */
  switch(ntohl(command))
    {
    case JTAG_COMMAND_WRITE:
      if(length != sizeof(msg_write) - 8)
        {
          ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);
          return;
        }
      buf = (char*)&msg_write;
      if(gdb_read(&buf[8],length) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 3");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      msg_write.address = ntohl(msg_write.address);
      msg_write.data_H = ntohl(msg_write.data_H);
      msg_write.data_L = ntohl(msg_write.data_L);
      jtag_write_reg(msg_write.address,msg_write.data_L);
      resp_write.status = htonl(err);
      if(gdb_write(&resp_write,sizeof(resp_write)) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 4");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      break;
    case JTAG_COMMAND_READ:
      if(length != sizeof(msg_read) - 8)
        {
          ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);
          return;
        }
      buf = (char*)&msg_read;
      if(gdb_read(&buf[8],length) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 5");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      msg_read.address = ntohl(msg_read.address);
      jtag_read_reg(msg_read.address); /* Data not ready at this time, repeat. */
      resp_read.data_L = jtag_read_reg(msg_read.address);
      resp_read.status = htonl(err);
      resp_read.data_H = 0;
      resp_read.data_L = htonl(resp_read.data_L);
      if(gdb_write(&resp_read,sizeof(resp_read)) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 6");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      break;
    case JTAG_COMMAND_BLOCK_WRITE:
      if(length < sizeof(JTAGProxyBlockWriteMessage)-8)
        {
          ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);
          return;
        }
      if(!(buf = (char*)malloc(8+length)))
        {
          ProtocolClean(length,JTAG_PROXY_OUT_OF_MEMORY);
          return;
        }
      msg_bwrite = (JTAGProxyBlockWriteMessage*)buf;
      if(gdb_read(&buf[8],length) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 5");
              close(gdb_fd);
              gdb_fd = 0;
            }
          free(buf);
          return;
        }
      msg_bwrite->address = ntohl(msg_bwrite->address);
      msg_bwrite->nRegisters = ntohl(msg_bwrite->nRegisters);
      for(i=0;i<msg_bwrite->nRegisters;i++)
        {
          msg_bwrite->data[i] = ntohl(msg_bwrite->data[i]);
          jtag_write_reg(msg_bwrite->address + i * 4,msg_bwrite->data[i]);
        }
      resp_bwrite.status = htonl(err);
      free(buf);
      msg_bwrite = (JTAGProxyBlockWriteMessage *)NULL;
      buf = (char *)msg_bwrite;
      if(gdb_write(&resp_bwrite,sizeof(resp_bwrite)) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 4");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      break;
    case JTAG_COMMAND_BLOCK_READ:
      if(length != sizeof(msg_bread) - 8)
        {
          ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);
          return;
        }
      buf = (char*)&msg_bread;
      if(gdb_read(&buf[8],length) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 5");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      msg_bread.address = ntohl(msg_bread.address);
      msg_bread.nRegisters = ntohl(msg_bread.nRegisters);
      len = sizeof(JTAGProxyBlockReadResponse) + 4*(msg_bread.nRegisters-1);
      if(!(buf = (char*)malloc(len)))
        {
          ProtocolClean(0,JTAG_PROXY_OUT_OF_MEMORY);
          return;
        }
      resp_bread = (JTAGProxyBlockReadResponse*)buf;
      jtag_read_reg(msg_bread.address); /* Prepare for reading. */
      for(i=0;i<msg_bread.nRegisters;i++)
        {
          /* Read previous, address next one. */
          resp_bread->data[i] = jtag_read_reg(msg_bread.address + (i + 1) * 4);
          resp_bread->data[i] = htonl(resp_bread->data[i]);
        }
      resp_bread->status = htonl(err);
      resp_bread->nRegisters = htonl(msg_bread.nRegisters);
      if(gdb_write(resp_bread,len) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 6");
              close(gdb_fd);
              gdb_fd = 0;
            }
          free(buf);
          return;
        }
      free(buf);
      resp_bread = (JTAGProxyBlockReadResponse *)NULL;
      buf = (char *)resp_bread;
      break;
    case JTAG_COMMAND_CHAIN:
      if(length != sizeof(msg_chain) - 8)
        {
          ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);
          return;
        }
      buf = (char*)&msg_chain;
      if(gdb_read(&buf[8],sizeof(msg_chain)-8) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 7");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      msg_chain.chain = htonl(msg_chain.chain);
      jtag_set_chain(msg_chain.chain);
      resp_chain.status = htonl(err);
      if(gdb_write(&resp_chain,sizeof(resp_chain)) < 0)
        {
          if(gdb_fd)
            {
              perror("gdb socket - 8");
              close(gdb_fd);
              gdb_fd = 0;
            }
          return;
        }
      break;
    default:
      perror("Unknown JTAG command.");
      ProtocolClean(length,JTAG_PROXY_COMMAND_NOT_IMPLEMENTED);
      break;
    }
}
 
void ProtocolClean(int length,int32_t err)
{
  char buf[4096];
 
  err = htonl(err);
  if((gdb_read(buf,length) < 0) ||
      (gdb_write(&err,sizeof(err)) < 0) && gdb_fd)
    {
      perror("gdb socket - 9");
      close(gdb_fd);
      gdb_fd = 0;
    }
}
 
static int gdb_write(void* buf,int len)
{
  int n;
  char* w_buf = (char*)buf;
  struct pollfd block;
 
  while(len)
    {
      if((n = write(gdb_fd,w_buf,len)) < 0)
        {
          switch(errno)
            {
            case EWOULDBLOCK: /* or EAGAIN */
              /* We've been called on a descriptor marked
           for nonblocking I/O. We better simulate
           blocking behavior. */
              block.fd = gdb_fd;
              block.events = POLLOUT;
              block.revents = 0;
              poll(&block,1,-1);
              continue;
            case EINTR:
              continue;
            case EPIPE:
              close(gdb_fd);
              gdb_fd = 0;
              return -1;
            default:
              return -1;
            }
        }
      else
        {
          len -= n;
          w_buf += n;
        }
    }
  return 0;
}
 
static int gdb_read(void* buf,int len)
{
  int n;
  char* r_buf = (char*)buf;
  struct pollfd block;
 
  while(len)
    {
      if((n = read(gdb_fd,r_buf,len)) < 0)
        {
          switch(errno)
            {
            case EWOULDBLOCK: /* or EAGAIN */
              /* We've been called on a descriptor marked
           for nonblocking I/O. We better simulate
           blocking behavior. */
              block.fd = gdb_fd;
              block.events = POLLIN;
              block.revents = 0;
              poll(&block,1,-1);
              continue;
            case EINTR:
              continue;
            default:
              return -1;
            }
        }
      else if(n == 0)
        {
          close(gdb_fd);
          gdb_fd = 0;
          return -1;
        }
      else
        {
          len -= n;
          r_buf += n;
        }
    }
  return 0;
}
Browse

Tools

Subversion Repositories or1k

[/] [or1k/] [trunk/] [jtag/] [jp1.c] - Blame information for rev 1322

Line No.	Rev	Author	Line
1	435	simons	`/* jp1-linux.c -- JTAG protocol via parallel port for linux`
2			`Copyright (C) 2001 Marko Mlinar, markom@opencores.org`
3			`Code for TCP/IP copied from gdb, by Chris Ziomkowski`
4
5			`This file is part of OpenRISC 1000 Architectural Simulator.`
6
7			`This program is free software; you can redistribute it and/or modify`
8			`it under the terms of the GNU General Public License as published by`
9			`the Free Software Foundation; either version 2 of the License, or`
10			`(at your option) any later version.`
11
12			`This program is distributed in the hope that it will be useful,`
13			`but WITHOUT ANY WARRANTY; without even the implied warranty of`
14			`MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the`
15			`GNU General Public License for more details.`
16
17			`You should have received a copy of the GNU General Public License`
18			`along with this program; if not, write to the Free Software`
19			`Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */`
20
21			`/* Establishes jtag proxy server and communicates with parallel`
22			`port directly. Requires root access. */`
23
24			`#include <stdio.h>`
25			`#include <ctype.h>`
26			`#include <string.h>`
27			`#include <stdlib.h>`
28			`#include <unistd.h>`
29			`#include <stdarg.h>`
30
31			`/* Dirty way to include inb and outb from, but they say it is`
32			`a standard one. */`
33			`#include <asm/io.h>`
34			`#include <asm/system.h>`
35	588	simons	`#include "mc.h"`
36	435	simons
37	602	lampret
38			`#define Boolean int`
39			`#define false 0`
40			`#define true 1`