Subversion Repositories or1k

/* toplevel.c -- Top level simulator source file
   Copyright (C) 1999 Damjan Lampret, lampret@opencores.org
 
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. */
 
/* Simulator commands. Help and version output. SIGINT processing.
Stdout redirection is specific to linux (I need to fix this). */
 
#include "config.h"
 
#include <stdio.h>
#include <ctype.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <stdarg.h>
/* Added by CZ 24/05/01 */
#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>
 
#ifdef HAVE_LIBREADLINE
#include <readline/readline.h>
#include <readline/history.h>
#endif /* HAVE_LIBREADLINE */
 
#include "arch.h"
#include "parse.h"
#include "abstract.h"
#include "trace.h"
#include "execute.h"
#include "sim-config.h"
#include "spr_defs.h"
 
#include "coff.h"
 
/* Added by CZ 24/05/01 */
#include "gdb.h"
#include <signal.h>
#include <errno.h>
typedef enum {
  false = 0,
  true = 1,
} Boolean;
unsigned int serverIP = 0;
unsigned int serverPort = 0;
unsigned int server_fd = 0;
unsigned int gdb_fd = 0;
void HandleServerSocket(Boolean);
void JTAGRequest(void);
void GDBRequest(void);
void ProtocolClean(int,int32_t);
static int gdb_read(void*,int);
static int gdb_write(void*,int);
void BlockJTAG(void);
 
#ifndef DEBUGMOD_OFF
int GlobalMode = 0;   /* Start off in the orginal mode */
#else  /* no DEBUGMOD_OFF */
#define GlobalMode 0
#endif /* no DEBUGMOD_OFF */
 
/* CVS revision number. */
const char rcsrev[] = "$Revision: 1.21 $";
 
/* Continuos run versus single step tracing switch. */
int cont_run;
 
/* History of execution */
int histexec[HISTEXEC_LEN];
 
char *sim_commands [] = {
  "q",
  "t",
  "help",
  "de",
  "dm",
  "run",
  "pr",
  "pm",
  "pc",
  "reset",
  "break",
  "hist",
  "stats",
  "info",
  "r",
  "dv",
  0
};
 
inline void debug(const char *format, ...)
{
#ifndef DEBUGMOD_OFF
  char *p;
  va_list ap;
 
  if (config.simdebug) {
    if ((p = malloc(1000)) == NULL)
      return;
    va_start(ap, format);
    (void) vsnprintf(p, 1000, format, ap);
    va_end(ap);
    printf("%s\n", p);
    fflush(stdout);
    free(p);
  } else {
#if DEBUG
  if ((p = malloc(1000)) == NULL)
    return;
  va_start(ap, format);
  (void) vsnprintf(p, 1000, format, ap);
  va_end(ap);
  printf("%s\n", p);
  fflush(stdout);
  free(p);
#endif
  }
#endif /* no DEBUGMOD_OFF */
}
 
/* Strip whitespace from the start and end of STRING.  Return a pointer
   into STRING. */
#ifndef whitespace
#define	whitespace(a)	((a) == '\t' ? 1 : ((a) == ' '? 1 : 0))
#endif
 
char *
stripwhite (string)
     char *string;
{
  register char *s, *t;
 
  for (s = string; whitespace (*s); s++)
    ;
 
  if (*s == 0)
    return (s);
 
  t = s + strlen (s) - 1;
  while (t > s && whitespace (*t))
    t--;
  *++t = '\0';
 
  return s;
}
 
void 
ctrl_c(signum)
     int signum;
{
	cont_run = 1;
	config.iprompt = 1;
  signal(SIGINT, ctrl_c);
}
 
void 
version()
{
	printf ("\n");
	printf ("OpenRISC 1000 (OR16+OR32) Architectural Simulator, %s\n", rcsrev);
	printf ("Copyright (C) 1999 Damjan Lampret, lampret@opencores.org\n");
	printf ("Copyright (C) 2000 Damjan Lampret, lampret@opencores.org\n");
	printf ("                   Jimmy Chen-Min Chen, jimmy@ee.nctu.edu.tw\n");
	printf ("                   Johan Rydberg, johan.rydberg@insight.se\n");
	printf ("Visit http://www.opencores.org for more information about ");
	printf ("OpenRISC 1000 and\nother open source cores.\n\n");
	printf ("This software comes with ABSOLUTELY NO WARRANTY; for ");
	printf ("details see COPYING.\nThis is free software, and you ");
	printf ("are welcome to redistribute it under certain\nconditions; ");
	printf ("for details see COPYING.\n");
}
 
void 
help()
{
	printf("q			 - quit simulator\n");
	printf("r			 - display all registers\n");
	printf("t			 - execute next instruction\n");
	printf("run <cycles> [<hush>]	 - execute <cycles> instructions, no reg dump if hush\n");
	printf("pr <r> <value>		 - patch register <r> with <value>\n");
	printf("dm <fromaddr> [<toaddr>] - display memory from <fromaddr> to <toaddr>\n");
	printf("de                       - debug insn memory\n");
	printf("pm <addr> <value>	 - patch memory location <addr> with <value>\n");
	printf("pc <value>		 - patch PC register with <value>\n");
	printf("break <addr>		 - toggle breakpoint at address <addr>\n");
	printf("reset		 	 - simulator reset\n");
	printf("hist		 	 - execution history\n");
	printf("stats <num|clear> 	 - execution statistics num or clear it.\n");
	printf("info		 	 - configuration info (caches etc.)\n");
	printf("dv <fromaddr> [<toaddr>] [<modname>] - dumps memory as verilog (use redirect)\n");
	printf("dh <fromaddr> [<toaddr>] - dumps memory as hex code (use redirect)\n");
	printf("<cmd> > <filename>	 - redirect simulator stdout to <filename> (and not emulated printf)\n");
	printf("debug			 - toggles simulator debug mode\n");
	printf("help			 - available commands (this list)\n");
}
 
void debugmem();
 
main(argc, argv)
     int argc;
     char *argv[];
{
	char *linestr;
	char item1[500], b2[500], prev_str[500] = "";
	char *redirstr;
	int hush;
	unsigned long endaddr = 0xFFFFFFFF;
	int first_prompt = 1;
 
	srand(getpid());
	init_defconfig();
	if (parse_args(argc, argv)) {
		printf("Usage: %s [options] <filename>\n", argv[0]);
		printf("Options:\n");
		printf(" -v: version and copyright note\n");
		printf(" -i: enable interactive command prompt\n");
		printf(" -bpb: disable branch prediction buffer analysis\n");
		printf(" -btic: disable branch prediction target insn cache analysis\n");
		printf(" -hazards: disable dependency hazards analysis\n");
		printf(" -history: disable instruction stream history analysis\n");
		printf(" -superscalar: disable superscalar analysis\n");
		printf(" -fast: disable BPB, BTIC, SLP, dependency hazards, history"
		       " analysis etc.\n");
		printf(" -profile: generates profiling data.\n"); /* MM */
		printf(" -upr <n>: set UPR to n\n");
		printf(" -ver <n>: set VR[VER] to n\n");
		printf(" -rev <n>: set VR[REV] to n\n");
		printf(" -loadmem[@<n>] <filename>: load memory with file, "
		       "optionally at address <n>\n"); /* (CZ) */
		printf(" -nosrv: do not launch JTAG proxy server\n"); /* (CZ) */
		printf(" -srv <n>: launch JTAG proxy server on port <n>\n"); /* (CZ) */
		printf(" -initmem <n | random>: initialize memory to value "
		       "<n> or random\n"); /* (CZ) */
		exit(-1);
	}
 
#ifdef HAVE_LIBREADLINE
  initialize_readline ();	/* Bind our completer. */
#endif	
 
  if(!config.inhibit_server)
    {
      serverPort = config.server_port;
      if(server_fd = GetServerSocket("or1ksim","tcp",serverPort))
	printf("JTAG Proxy server started on port %d\n",serverPort);
    }
 
  if(config.profile) {
    config.fprof = fopen("sim-profile","wt+");
    if(!config.fprof) {
      config.profile = 0;
      printf("Problems opening profile file. Profiling disabled. \n");
    } else
      fprintf(config.fprof, "+00000000 FFFFFFFF FFFFFFFF main\n");
  }
 
	print_config();
	signal(SIGINT, ctrl_c);
	initstats();
	build_automata();
 
	/* Modified by CZ on 24/05/01 ... if a filename is
	   specified, behave as the simulator always has. This way,
	   no existing test suites should be broken. If a filename
	   is not specified, default to the new style behavior. Let
	   the simulator start up and execute garbage, the same way
	   a real CPU would. This should maximize the reality of
	   the capabilities. In this mode, we will expect that
	   someone will attach to us over the JTAG Proxy interface
	   and begin debugging that way. */
 
	if(config.filename)
	  {
	    endaddr = loadcode(config.filename, MEMORY_START, 0);
	    if (endaddr == -1) {
	      printf("Problems loading boot code.\n");
	      exit(1);
	    }
	  }
	else 
	  {
	    if(config.random_mem)
	      {
		int n = 0;
		int len = sizeof(mem);
		unsigned int* mptr = (unsigned int*)mem;
		unsigned int val = 0;
		int seed = time(NULL);
 
		srandom(seed);
		/* Print out the seed just in case we ever need to debug */
		printf("Seeding random generator with value %d\n",seed);
 
		for(n=0;n<len;n+=sizeof(unsigned int))
		  {
		    val = random();
		    if(random() > RAND_MAX/2)
		      val |= 0x80000000;
		    *mptr++ = val;
		  }
	      }
	    else if(config.pattern_mem)
	      {
		int n = 0;
		int len = sizeof(mem);
		unsigned int* mptr = (unsigned int*)mem;
 
		for(n=0;n<len;n+=sizeof(unsigned int))
		  *mptr++ = config.pattern_mem;
	      }
	    else
	      memset(mem,0,sizeof(mem));
 
	    if(config.memory)
	      {
		MemoryBlock* block = config.memory;
 
		while(block)
		  {
		    int fd = open(block->file,O_RDONLY);
		    int len,i;
		    struct stat buf;
		    char *mptr = (char*)mem;
		    char buffer[8192];
 
		    if(fd < 0)
		      {
			perror(block->file);
			exit(1);
		      }
		    if(fstat(fd,&buf) < 0)
		      {
			char sTemp[256];
 
			sprintf(sTemp,"stat(\"%s\")",block->file);
			perror(sTemp);
			exit(1);
		      }
		    if(!S_ISREG(buf.st_mode))
		      {
			fprintf(stderr,"File \"%s\" is not a regular file.\n",
				block->file);
			exit(1);
		      }
 
		    len = buf.st_size;
		    mptr += block->address;
		    for(i=0;i<len;)
		      {
			int n = read(fd,buffer,sizeof(buffer));
 
			switch(n)
			  {
			  case -1:
			    if(errno == EINTR)
			      continue;
			    perror(block->file);
			    exit(1);
			  case 0:
			    fprintf(stderr,"File \"%s\": premature end of file.\n",
				    block->file);
			    exit(1);
			  default:
			    memcpy(mptr,buffer,n);
			    i+= n;
			    break;
			  }
		      }
		    close(fd);
		  }
	      }
	  }
#ifndef DEBUGMOD_OFF
	GlobalMode = config.filename == NULL; /* Old mode = 0, New mode = 1 */
#endif
 
	uart_reset();
	tick_reset();
	pm_reset();
	pic_reset();
	reset();
	if(!GlobalMode)  /* Only in old mode */
	  set_reg32(3, endaddr);
 
	while(1) {
		if (config.iprompt) {
		  if(server_fd)
		    {
		      printf ("(sim) ");
		      fflush(stdout);
		      HandleServerSocket(true);  /* block & check_stdin = true */
		    }
#ifdef HAVE_LIBREADLINE
		  /* Must disable readline in new mode. It isn't compatible
		     with non blocking environments */
		  if(!server_fd) 
		    linestr = readline("(sim) ");
		  else
		    linestr = fgets(b2, sizeof b2, stdin);
#else
		  if(!server_fd)
		    printf ("(sim) ");
		  linestr = fgets(b2, sizeof b2, stdin);
#endif
		} else
			strcpy(linestr = b2, "run -1 hush");
 
		if (!linestr)
			break;
		linestr = stripwhite (linestr);
 
#ifdef HAVE_LIBREADLINE
		/* Readline only works in the old mode */
		if(!server_fd)
		  {
		    if (strlen(linestr) == 0) {
		      char *l = repeat_last_command ();
 
		      if (l) {
			free (linestr);
			linestr = l;
		      }
		    }
 
		    if (*linestr) {
		      add_history (linestr);
		    }
		  }
#endif /* HAVE_LIBREADLINE */
 
		if (redirstr = strstr(linestr, ">")) {
			*redirstr = '\0';
			strtoken(&redirstr[1], item1, 1);
			freopen(item1, "w+", stdout);
		}
 
		if (linestr[0] == '\n')
		  strcpy (linestr, &prev_str[0]);
		else
		  strcpy (&prev_str[0], linestr);
 
		strtoken(linestr, item1, 1);
		if (strcmp(item1, "q") == 0) {	/* quit */
		  printf ("\n");
		  if (config.profile) {
		    extern int cycles;
		    fprintf(config.fprof,"-%08X FFFFFFFF\n", cycles);
		    fclose(config.fprof);
		  }
		  exit(0);
		} else
		if (strcmp(item1, "help") == 0)	/* help */
			help();
		else
		if (strcmp(item1, "t") == 0) {	/* trace */
			cont_run = 1;
		} else
		if (strcmp(item1, "dm") == 0) {	/* dump memory */
			char item2[20];
			char item3[20];
			static int from = 0, to = 0;
 
			strtoken(linestr, item2, 2);
			strtoken(linestr, item3, 3);
 
			if (strlen(item2)) {
				if (item2[0] == '_')
					from = eval_label(item2);
				else
					from = strtoul(item2, NULL, 0);
				to = from + 0x40;
			}
			if (strlen(item3))
				to = strtoul(item3, NULL, 0);
			dumpmemory(from, to, 0);
      			printf("\n");
		} else
		if (strcmp(item1, "dv") == 0) {/* dump memory as verilog*/
			char item2[20];
			char item3[20];
			char item4[20];
			static int from = 0, to = 0;
 
			strtoken(linestr, item2, 2);
			strtoken(linestr, item3, 3);
			strtoken(linestr, item4, 4);
 
			if (strlen(item2)) {
				if (item2[0] == '_')
					from = eval_label(item2);
				else
					from = strtoul(item2, NULL, 0);
				to = from + 0x40;
			}
			if (strlen(item3))
				to = strtoul(item3, NULL, 0);
			if (!strlen(item4))
				strcpy(item4, "or1k_mem");
			dumpverilog(item4, from, to);
	  		printf("\n");
		} else
		if (strcmp(item1, "dh") == 0) {/* dump memory as hex*/
			char item2[20];
			char item3[20];
			static int from = 0, to = 0;
 
			strtoken(linestr, item2, 2);
			strtoken(linestr, item3, 3);
 
			if (strlen(item2)) {
				if (item2[0] == '_')
					from = eval_label(item2);
				else
					from = strtoul(item2, NULL, 0);
				to = from + 0x40;
			}
			if (strlen(item3))
				to = strtoul(item3, NULL, 0);
			dumphex(from, to);
	  		printf("\n");
		} else
		if (strcmp(item1, "pm") == 0) {	/* patch memory */
			char item2[20];
			char item3[20];
			static int addr = 0;
			int breakpoint = 0;
 
			strtoken(linestr, item2, 2);
			strtoken(linestr, item3, 3);
			if (strlen(item2))
				if (item2[0] == '_')
					addr = eval_label(item2);
				else
					addr = strtoul(item2, NULL, 0);
			set_mem32(addr, strtoul(item3, NULL, 0), &breakpoint);
		} else
		if (strcmp(item1, "pr") == 0) {	/* patch regs */
			char item2[20];
			char item3[20];
 
			strtoken(linestr, item2, 2);
			strtoken(linestr, item3, 3);
			set_reg32(strtoul(item2, NULL,0), strtoul(item3, NULL, 0));
		} else
		if (strcmp(item1, "pc") == 0) {	/* patch PC */
			char item2[20];
 
			strtoken(linestr, item2, 2);
			pcnext = strtoul(item2, NULL, 0);
		} else
		if (strcmp(item1, "break") == 0) {	/* set/clear breakpoint */
			char item2[20];
 
			strtoken(linestr, item2, 2);
			set_insnbrkpoint(strtoul(item2, NULL, 0));
		} else
		if (strcmp(item1, "r") == 0) {	/* dump regs */
			dumpreg();
		} else
		if (strcmp(item1, "de") == 0) {	/* reset simulator */
			debugmem();
		} else
		if (strcmp(item1, "reset") == 0) {	/* reset simulator */
			uart_reset();
			tick_reset();
			pm_reset();
			pic_reset();
			reset(); /* Old or new mode */
		} else
		if (strcmp(item1, "debug") == 0) {	/* debug mode */
			config.simdebug ^= 1;
		} else
		if (strcmp(item1, "hist") == 0) {	/* dump history */
			int i;
			for(i = HISTEXEC_LEN; i; i--)
				dumpmemory(histexec[i - 1], histexec[i - 1] + 4, 1);
			printf("\n");
		} else
		if (strcmp(item1, "run") == 0) { /* run */
			char item2[20];
			char item3[20];
 
			strtoken(linestr, item2, 2);
			strtoken(linestr, item3, 3);
			if (strcmp(item3, "hush") == 0)
				hush = 1;
			else
				hush = 0;
			cont_run = strtol(item2, NULL, 0);
		} else
		if (strcmp(item1, "stats") == 0) { /* stats */
			char item2[20];
			int i = 0;
 
			strtoken(linestr, item2, 2);
			if (strcmp(item2, "clear") == 0) {
				initstats();
				printf("Cleared.\n");
			} else {
				i = strtoul(item2, NULL, 0);
				printstats(i);
			}
		} else
		if (strcmp(item1, "info") == 0) { /* configuration info */
			itlb_status(-1);
			dtlb_status(-1);
			bpb_info();
			btic_info();
			ic_info();
			dc_info();
			uart_status();
			sprs_status();
		} else {
			printf("%s: Unknown command.\n", linestr);
		}
 
		/* MM: 'run -1' means endless execution.  */
		while(cont_run != 0) {
		  extern int cycle_delay;  /* Added by CZ 27/05/01. Set during exception. */
		  extern int cpu_stalled;  /* CZ from debug_interface */
 
		  if(cpu_stalled)
		    {
		      BlockJTAG();
		      HandleServerSocket(false);
		      continue;
		    }
 
			if (!testsprbits(SPR_PMR, SPR_PMR_DME | SPR_PMR_SME)) {
			  if(cycle_delay <= 0)
			    {
			      if (cont_run > 0) cont_run--;
			      if(fetch()) {
				cont_run = 0; /* memory breakpoint encountered */
				break;
			      }
			      decode_execute(&iqueue[0]);
			      update_pc();
			      analysis();
			      if (!hush)
				dumpreg();
			    }
			  else
			    cycle_delay--;
 
			  pic_clock();
			  dc_clock();
			  ic_clock();
			}
			if (!testsprbits(SPR_PMR, SPR_PMR_SME))
			  tick_clock();
			pm_clock();
			uart_clock();
#ifndef DEBUGMOD_OFF
			HandleServerSocket(false); /* block & check_stdin = false */
#endif
		}
		hush = 0;
		fflush(stdout);
		freopen("/dev/fd/0", "w+", stdout);
 
		if (!config.iprompt && !GlobalMode) {	/* non-interactive quit in old mode */
		  if (config.profile) {
		    extern int cycles;
		    fprintf(config.fprof,"-%08X FFFFFFFF\n", cycles);
		    fclose(config.fprof);
		  }
		  exit(0);
		}
#ifdef HAVE_LIBREADLINE
		if (linestr)
			free (linestr);
#endif
 
	}
	exit(0);
}
 
#ifdef HAVE_LIBREADLINE
char *command_generator ();
char **sim_completion ();
 
/* Tell the GNU readline library how to complete.  We want to try to complete
   on command names if this is the first word in the line, or on filenames
   if not. */
void initialize_readline ()
{
  /* Allow conditional parsing of the ~/.inputrc file. */
  rl_readline_name = "or1ksim";
 
  /* Tell the completer that we want a crack first. */
  rl_attempted_completion_function = (CPPFunction *)sim_completion;
}
 
/* Attempt to complete on the contents of TEXT.  START and END bound the
   region of rl_line_buffer that contains the word to complete.  TEXT is
   the word to complete.  We can use the entire contents of rl_line_buffer
   in case we want to do some simple parsing.  Return the array of matches,
   or NULL if there aren't any. */
char **
sim_completion (text, start, end)
     char *text;
     int start, end;
{
  char **matches;
 
  matches = (char **)NULL;
 
  /* If this word is at the start of the line, then it is a command
     to complete.  Otherwise it is the name of a file in the current
     directory. */
  if (start == 0)
    matches = completion_matches (text, command_generator);
 
  return (matches);
}
 
/* Generator function for command completion.  STATE lets us know whether
   to start from scratch; without any state (i.e. STATE == 0), then we
   start at the top of the list. */
char *
command_generator (text, state)
     char *text;
     int state;
{
  static int list_index, len;
  char *name;
 
  /* If this is a new word to complete, initialize now.  This includes
     saving the length of TEXT for efficiency, and initializing the index
     variable to 0. */
  if (!state)
    {
      list_index = 0;
      len = strlen (text);
    }
 
  /* Return the next name which partially matches from the command list. */
  while (name = sim_commands[list_index])
    {
      list_index++;
 
      if (strncmp (name, text, len) == 0)
        return (dupstr(name));
    }
 
  /* If no names matched, then return NULL. */
  return ((char *)NULL);
}
 
char *
dupstr (s)
     char *s;
{
  char *r;
 
  r = xmalloc (strlen (s) + 1);
  strcpy (r, s);
  return (r);
}
 
/* Repeats the last command.  */
char *
repeat_last_command ()
{
  int offset = where_history ();
  HIST_ENTRY *hist;
 
  if (hist = history_get (offset))
    {
      return dupstr (hist->line);
    }
  return 0;
}
 
#endif
 
extern char *disassembled;
void debugmem() {
  int i;
  printf("starting to dump mem...\n");
  for(i=0; i<500; i++) {
    printf("i=%x :: ", i);
    if (mem[i].label)
      printf("label: %s |", mem[i].label->name);
    {
      unsigned int _insn;
      _insn = ((unsigned long)mem[i].data << 24);
      _insn |= ((unsigned long)mem[i + 1].data << 16);
      _insn |= ((unsigned long)mem[i + 2].data << 8);
      _insn |= ((unsigned long)mem[i + 3].data);
      iqueue[0].insn_index = insn_decode(_insn);
      iqueue[0].insn = _insn;
      disassemble_insn (_insn);
      printf("%s\n", disassembled);
    }
  }
}
 
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(fcntl(sockfd,F_GETFL,&flags) < 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 BlockJTAG()
{
  struct pollfd fds[2];
  int n = 0;
 
  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;
    }
  poll(fds,n,-1);
}
 
void HandleServerSocket(Boolean block)
{
  struct pollfd fds[3];
  int n = 0;
  int timeout = block ? -1 : 0;
  int server_index = -1;
  int gdb_index = -1;
  Boolean data_on_stdin = false;
  int o_serv_fd = server_fd;
 
  if(!o_serv_fd && !gdb_fd)
    return;
 
  if(o_serv_fd)
    {
      fds[n].fd = o_serv_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;
    }
  if(block)
    {
      fds[n].fd = 0;
      fds[n].events = POLLIN;
      fds[n++].revents = 0;
    }
 
  while(!data_on_stdin)
    {
      switch(poll(fds,n,timeout))
	{
	case -1:
	  if(errno == EINTR)
	    continue;
	  perror("poll");
	  server_fd = 0;
	  break;
	case 0: /* Nothing interesting going on */
	  data_on_stdin = true; /* Can only get here if nonblocking */
	  break;
	default:
	  /* Make sure to handle the gdb port first! */
	  if((fds[0].revents && (gdb_fd && !o_serv_fd) ||
	      fds[1].revents && (server_fd && gdb_fd)))
	    {
	      int revents = o_serv_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 && o_serv_fd)
	    {
	      if(fds[0].revents & POLLIN)
		JTAGRequest();
	      else /* Error Occurred */
		{
		  fprintf(stderr,"Received flags 0x%08x on server. Shutting down.\n",fds[0].revents);
		  close(o_serv_fd);
		  server_fd = 0;
		  serverPort = 0;
		  serverIP = 0;
		}
	    }
	  if(fds[2].revents || (fds[1].revents && !gdb_fd))
	    data_on_stdin = true;
	  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(fcntl(fd,F_GETFL,&flags) < 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;
  int err = 0;
  uint32_t command,length;
  int len,i;
 
  /* 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);
      err = DebugSetRegister(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);
      err = DebugGetRegister(msg_read.address,&resp_read.data_L);
      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++)
	{
	  int t_err;
 
	  msg_bwrite->data[i] = ntohl(msg_bwrite->data[i]);
	  t_err = DebugSetRegister(msg_bwrite->address+i,msg_bwrite->data[i]);
	  err = err ? err : t_err;
	}
      resp_bwrite.status = htonl(err);
      free(buf);
      buf = msg_bwrite = NULL;
      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;
      for(i=0;i<msg_bread.nRegisters;i++)
	{
	  int t_err;
 
	  t_err = DebugGetRegister(msg_bread.address+i,&resp_bread->data[i]);
	  resp_bread->data[i] = htonl(resp_bread->data[i]);
	  err = err ? err : t_err;
	}
      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);
      buf = resp_bread = NULL;
      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);
      err = DebugSetChain(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:
      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;
}