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

Subversion Repositories or1k

[/] [or1k/] [tags/] [nog_patch_47/] [or1ksim/] [debug/] [gdbcomm.c] - Blame information for rev 439

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


/* gdbcomm.c -- Communication routines for gdb
         Copyright (C) 2001 by Marko Mlinar, markom@opencores.org
         Code copied from toplevel.c
 
         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.
*/
 
#include <stdlib.h>
#include <stdio.h>
#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 "gdb.h"
#include "gdbcomm.h"
#include "vapi.h"
#include "sim-config.h"
 
static unsigned int serverIP = 0;
static unsigned int serverPort = 0;
static unsigned int server_fd = 0;
static unsigned int gdb_fd = 0;
 
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 = 0;
 
          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 = NULL;
      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 = NULL;
      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(const void* buf,int len)
{
  int n, log_n = 0;
  const char* w_buf = (const char*)buf;
  const uint32_t* log_buf = (const uint32_t*)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;
      if ( config.debug.vapi_id )
        for ( log_n += n; log_n >= 4; log_n -= 4, ++ log_buf )
          vapi_write_log_file( VAPI_COMMAND_SEND, config.debug.vapi_id, ntohl(*log_buf) );
    }
  }
  return 0;
}
 
static int gdb_read(void* buf,int len)
{
  int n, log_n = 0;
  char* r_buf = (char*)buf;
  uint32_t* log_buf = (uint32_t*)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;
      if ( config.debug.vapi_id )
        for ( log_n += n; log_n >= 4; log_n -= 4, ++ log_buf )
          vapi_write_log_file( VAPI_COMMAND_REQUEST, config.debug.vapi_id, ntohl(*log_buf) );
    }
  }
  return 0;
}
 
int gdbcomm_init ()
{
  serverPort = config.debug.server_port;
  if(server_fd = GetServerSocket("or1ksim","tcp",serverPort))
          printf("JTAG Proxy server started on port %d\n",serverPort);
}

Browse

Tools

Subversion Repositories or1k

[/] [or1k/] [tags/] [nog_patch_47/] [or1ksim/] [debug/] [gdbcomm.c] - Blame information for rev 439

Line No.	Rev	Author	Line
1	321	markom	`/* gdbcomm.c -- Communication routines for gdb`
2			`Copyright (C) 2001 by Marko Mlinar, markom@opencores.org`
3			`Code copied from toplevel.c`
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
22			`#include <stdlib.h>`
23			`#include <stdio.h>`
24			`#include <sys/stat.h>`
25			`#include <sys/types.h>`
26			`#include <sys/socket.h>`
27			`#include <netinet/in.h>`
28			`#include <sys/select.h>`
29			`#include <sys/poll.h>`
30			`#include <fcntl.h>`
31			`#include <netdb.h>`
32			`#include <netinet/tcp.h>`
33			`#include <inttypes.h>`
34
35			`#include "gdb.h"`
36			`#include "gdbcomm.h"`
37	439	erez	`#include "vapi.h"`
38	321	markom	`#include "sim-config.h"`
39
40			`static unsigned int serverIP = 0;`
41			`static unsigned int serverPort = 0;`
42			`static unsigned int server_fd = 0;`
43			`static unsigned int gdb_fd = 0;`
44
45			`static int tcp_level = 0;`
46
47			`/* Added by CZ 24/05/01 */`
48			`int GetServerSocket(const char* name,const char* proto,int port)`
49			`{`
50			`struct servent *service;`
51			`struct protoent *protocol;`
52			`struct sockaddr_in sa;`
53			`struct hostent *hp;`
54			`int sockfd;`
55			`char myname[256];`
56			`int flags;`
57			`char sTemp[256];`
58
59			`/* First, get the protocol number of TCP */`
60			`if(!(protocol = getprotobyname(proto)))`
61			`{`
62			`sprintf(sTemp,"Unable to load protocol \"%s\"",proto);`
63			`perror(sTemp);`
64			`return 0;`
65			`}`
66			`tcp_level = protocol->p_proto; /* Save for later */`
67
68			`/* If we weren't passed a non standard port, get the port`
69			`from the services directory. */`
70			`if(!port)`
71			`{`
72			`if(service = getservbyname(name,protocol->p_name))`
73			`port = ntohs(service->s_port);`
74			`}`
75
76			`/* Create the socket using the TCP protocol */`
77			`if((sockfd = socket(PF_INET,SOCK_STREAM,protocol->p_proto)) < 0)`
78			`{`
79			`perror("Unable to create socket");`
80			`return 0;`
81			`}`
82
83			`flags = 1;`
84			`if(setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,(const char*)&flags,sizeof(int)) < 0)`
85			`{`
86			`sprintf(sTemp,"Can not set SO_REUSEADDR option on socket %d",sockfd);`
87			`perror(sTemp);`
88			`close(sockfd);`
89			`return 0;`
90			`}`
91
92			`/* The server should also be non blocking. Get the current flags. */`
93			`if(fcntl(sockfd,F_GETFL,&flags) < 0)`
94			`{`
95			`sprintf(sTemp,"Unable to get flags for socket %d",sockfd);`
96			`perror(sTemp);`
97			`close(sockfd);`
98			`return 0;`
99			`}`
100
101			`/* Set the nonblocking flag */`
102			`if(fcntl(sockfd,F_SETFL, flags \| O_NONBLOCK) < 0)`
103			`{`
104			`sprintf(sTemp,"Unable to set flags for socket %d to value 0x%08x",`
105			`sockfd,flags \| O_NONBLOCK);`
106			`perror(sTemp);`
107			`close(sockfd);`
108			`return 0;`
109			`}`
110
111			`/* Find out what our address is */`
112			`memset(&sa,0,sizeof(struct sockaddr_in));`
113			`gethostname(myname,sizeof(myname));`
114			`if(!(hp = gethostbyname(myname)))`
115			`{`
116			`perror("Unable to read hostname");`
117			`close(sockfd);`
118			`return 0;`
119			`}`
120
121			`/* Bind our socket to the appropriate address */`
122			`sa.sin_family = hp->h_addrtype;`
123			`sa.sin_port = htons(port);`
124			`if(bind(sockfd,(struct sockaddr*)&sa,sizeof(struct sockaddr_in)) < 0)`
125			`{`
126			`sprintf(sTemp,"Unable to bind socket %d to port %d",sockfd,port);`
127			`perror(sTemp);`
128			`close(sockfd);`
129			`return 0;`
130			`}`
131			`serverIP = sa.sin_addr.s_addr;`
132			`flags = sizeof(struct sockaddr_in);`
133			`if(getsockname(sockfd,(struct sockaddr*)&sa,&flags) < 0)`
134			`{`
135			`sprintf(sTemp,"Unable to get socket information for socket %d",sockfd);`
136			`perror(sTemp);`
137			`close(sockfd);`
138			`return 0;`
139			`}`
140			`serverPort = ntohs(sa.sin_port);`
141
142			`/* Set the backlog to 1 connections */`
143			`if(listen(sockfd,1) < 0)`
144			`{`
145			`sprintf(sTemp,"Unable to set backlog on socket %d to %d",sockfd,1);`
146			`perror(sTemp);`
147			`close(sockfd);`
148			`return 0;`
149			`}`
150
151			`return sockfd;`
152			`}`
153
154			`void BlockJTAG()`
155			`{`
156			`struct pollfd fds[2];`
157			`int n = 0;`
158
159			`fds[n].fd = server_fd;`
160			`fds[n].events = POLLIN;`
161			`fds[n++].revents = 0;`
162			`if(gdb_fd)`
163			`{`
164			`fds[n].fd = gdb_fd;`
165			`fds[n].events = POLLIN;`
166			`fds[n++].revents = 0;`
167			`}`
168			`poll(fds,n,-1);`
169			`}`
170
171			`void HandleServerSocket(Boolean block)`
172			`{`
173			`struct pollfd fds[3];`
174			`int n = 0;`
175			`int timeout = block ? -1 : 0;`
176			`int server_index = -1;`
177			`int gdb_index = -1;`
178			`Boolean data_on_stdin = false;`
179			`int o_serv_fd = server_fd;`
180
181			`if(!o_serv_fd && !gdb_fd)`
182			`return;`
183
184			`if(o_serv_fd)`
185			`{`
186			`fds[n].fd = o_serv_fd;`
187			`fds[n].events = POLLIN;`
188			`fds[n++].revents = 0;`
189			`}`
190			`if(gdb_fd)`
191			`{`
192			`fds[n].fd = gdb_fd;`
193			`fds[n].events = POLLIN;`
194			`fds[n++].revents = 0;`
195			`}`
196			`if(block)`
197			`{`
198			`fds[n].fd = 0;`
199			`fds[n].events = POLLIN;`
200			`fds[n++].revents = 0;`
201			`}`
202
203			`while(!data_on_stdin)`
204			`{`
205			`switch(poll(fds,n,timeout))`
206			`{`
207			`case -1:`
208			`if(errno == EINTR)`
209			`continue;`
210			`perror("poll");`
211			`server_fd = 0;`
212			`break;`
213			`case 0: /* Nothing interesting going on */`
214			`data_on_stdin = true; /* Can only get here if nonblocking */`
215			`break;`
216			`default:`
217			`/* Make sure to handle the gdb port first! */`
218			`if((fds[0].revents && (gdb_fd && !o_serv_fd) \|\|`
219			`fds[1].revents && (server_fd && gdb_fd)))`
220			`{`
221			`int revents = o_serv_fd ? fds[1].revents : fds[0].revents;`
222
223			`if(revents & POLLIN)`
224			`GDBRequest();`
225			`else /* Error Occurred */`
226			`{`
227			`fprintf(stderr,"Received flags 0x%08x on gdb socket. Shutting down.\n",revents);`
228			`close(gdb_fd);`
229			`gdb_fd = 0;`
230			`}`
231			`}`
232			`if(fds[0].revents && o_serv_fd)`
233			`{`
234			`if(fds[0].revents & POLLIN)`
235			`JTAGRequest();`
236			`else /* Error Occurred */`
237			`{`
238			`fprintf(stderr,"Received flags 0x%08x on server. Shutting down.\n",fds[0].revents);`
239			`close(o_serv_fd);`
240			`server_fd = 0;`
241			`serverPort = 0;`
242			`serverIP = 0;`
243			`}`
244			`}`
245			`if(fds[2].revents \|\| (fds[1].revents && !gdb_fd))`
246			`data_on_stdin = true;`
247			`break;`
248			`} /* End of switch statement */`
249			`} /* End of while statement */`
250			`}`
251
252			`void JTAGRequest()`
253			`{`
254			`struct sockaddr_in sa;`
255			`struct sockaddr* addr = (struct sockaddr*)&sa;`
256			`int n = sizeof(struct sockaddr_in);`
257			`int fd = accept(server_fd,addr,&n);`
258			`int on_off = 0; /* Turn off Nagel's algorithm on the socket */`
259			`int flags;`
260			`char sTemp[256];`
261
262			`if(fd < 0)`
263			`{`
264			`/* This is valid, because a connection could have started,`
265			`and then terminated due to a protocol error or user`
266			`initiation before the accept could take place. */`
267			`if(errno != EWOULDBLOCK && errno != EAGAIN)`
268			`{`
269			`perror("accept");`
270			`close(server_fd);`
271			`server_fd = 0;`
272			`serverPort = 0;`
273			`serverIP = 0;`
274			`}`
275			`return;`
276			`}`
277
278			`if(gdb_fd)`
279			`{`
280			`close(fd);`
281			`return;`
282			`}`
283
284			`if(fcntl(fd,F_GETFL,&flags) < 0)`
285			`{`
286			`sprintf(sTemp,"Unable to get flags for gdb socket %d",fd);`
287			`perror(sTemp);`
288			`close(fd);`
289			`return;`
290			`}`
291
292			`if(fcntl(fd,F_SETFL, flags \| O_NONBLOCK) < 0)`
293			`{`
294			`sprintf(sTemp,"Unable to set flags for gdb socket %d to value 0x%08x",`
295			`fd,flags \| O_NONBLOCK);`
296			`perror(sTemp);`
297			`close(fd);`
298			`return;`
299			`}`
300
301			`if(setsockopt(fd,tcp_level,TCP_NODELAY,&on_off,sizeof(int)) < 0)`
302			`{`
303			`sprintf(sTemp,"Unable to disable Nagel's algorithm for socket %d.\nsetsockopt",fd);`
304			`perror(sTemp);`
305			`close(fd);`
306			`return;`
307			`}`
308
309			`gdb_fd = fd;`
310			`}`
311
312			`void GDBRequest()`
313			`{`
314			`JTAGProxyWriteMessage msg_write;`
315			`JTAGProxyReadMessage msg_read;`
316			`JTAGProxyChainMessage msg_chain;`
317			`JTAGProxyWriteResponse resp_write;`
318			`JTAGProxyReadResponse resp_read;`
319			`JTAGProxyChainResponse resp_chain;`
320			`JTAGProxyBlockWriteMessage *msg_bwrite;`
321			`JTAGProxyBlockReadMessage msg_bread;`
322			`JTAGProxyBlockWriteResponse resp_bwrite;`
323			`JTAGProxyBlockReadResponse *resp_bread;`
324			`char *buf;`
325			`unsigned long long data;`
326			`int err = 0;`
327			`uint32_t command,length;`
328			`int len,i;`
329
330			`/* First, we must read the incomming command */`
331			`if(gdb_read(&command,sizeof(uint32_t)) < 0)`
332			`{`
333			`if(gdb_fd)`
334			`{`
335			`perror("gdb socket - 1");`
336			`close(gdb_fd);`
337			`gdb_fd = 0;`
338			`}`
339			`return;`
340			`}`
341			`if(gdb_read(&length,sizeof(uint32_t)) < 0)`
342			`{`
343			`if(gdb_fd)`
344			`{`
345			`perror("gdb socket - 2");`
346			`close(gdb_fd);`
347			`gdb_fd = 0;`
348			`}`
349			`return;`
350			`}`
351			`length = ntohl(length);`
352
353			`/* Now, verify the protocol and implement the command */`
354			`switch(ntohl(command))`
355			`{`
356			`case JTAG_COMMAND_WRITE:`
357			`if(length != sizeof(msg_write) - 8)`
358			`{`
359			`ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);`
360			`return;`
361			`}`
362			`buf = (char*)&msg_write;`
363			`if(gdb_read(&buf[8],length) < 0)`
364			`{`
365			`if(gdb_fd)`
366			`{`
367			`perror("gdb socket - 3");`
368			`close(gdb_fd);`
369			`gdb_fd = 0;`
370			`}`
371			`return;`
372			`}`
373			`msg_write.address = ntohl(msg_write.address);`
374			`msg_write.data_H = ntohl(msg_write.data_H);`
375			`msg_write.data_L = ntohl(msg_write.data_L);`
376			`err = DebugSetRegister(msg_write.address,msg_write.data_L);`
377			`resp_write.status = htonl(err);`
378			`if(gdb_write(&resp_write,sizeof(resp_write)) < 0)`
379			`{`
380			`if(gdb_fd)`
381			`{`
382			`perror("gdb socket - 4");`
383			`close(gdb_fd);`
384			`gdb_fd = 0;`
385			`}`
386			`return;`
387			`}`
388			`break;`
389			`case JTAG_COMMAND_READ:`
390			`if(length != sizeof(msg_read) - 8)`
391			`{`
392			`ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);`
393			`return;`
394			`}`
395			`buf = (char*)&msg_read;`
396			`if(gdb_read(&buf[8],length) < 0)`
397			`{`
398			`if(gdb_fd)`
399			`{`
400			`perror("gdb socket - 5");`
401			`close(gdb_fd);`
402			`gdb_fd = 0;`
403			`}`
404			`return;`
405			`}`
406			`msg_read.address = ntohl(msg_read.address);`
407			`err = DebugGetRegister(msg_read.address,&resp_read.data_L);`
408			`resp_read.status = htonl(err);`
409			`resp_read.data_H = 0;`
410			`resp_read.data_L = htonl(resp_read.data_L);`
411			`if(gdb_write(&resp_read,sizeof(resp_read)) < 0)`
412			`{`
413			`if(gdb_fd)`
414			`{`
415			`perror("gdb socket - 6");`
416			`close(gdb_fd);`
417			`gdb_fd = 0;`
418			`}`
419			`return;`
420			`}`
421			`break;`
422			`case JTAG_COMMAND_BLOCK_WRITE:`
423			`if(length < sizeof(JTAGProxyBlockWriteMessage)-8)`
424			`{`
425			`ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);`
426			`return;`
427			`}`
428			`if(!(buf = (char*)malloc(8+length)))`
429			`{`
430			`ProtocolClean(length,JTAG_PROXY_OUT_OF_MEMORY);`
431			`return;`
432			`}`
433			`msg_bwrite = (JTAGProxyBlockWriteMessage*)buf;`
434			`if(gdb_read(&buf[8],length) < 0)`
435			`{`
436			`if(gdb_fd)`
437			`{`
438			`perror("gdb socket - 5");`
439			`close(gdb_fd);`
440			`gdb_fd = 0;`
441			`}`
442			`free(buf);`
443			`return;`
444			`}`
445			`msg_bwrite->address = ntohl(msg_bwrite->address);`
446			`msg_bwrite->nRegisters = ntohl(msg_bwrite->nRegisters);`
447			`for(i=0;i<msg_bwrite->nRegisters;i++)`
448			`{`
449			`int t_err = 0;`
450
451			`msg_bwrite->data[i] = ntohl(msg_bwrite->data[i]);`
452			`t_err = DebugSetRegister(msg_bwrite->address+i,msg_bwrite->data[i]);`
453			`err = err ? err : t_err;`
454			`}`
455			`resp_bwrite.status = htonl(err);`
456			`free(buf);`
457			`buf = NULL;`
458			`msg_bwrite = NULL;`
459			`if(gdb_write(&resp_bwrite,sizeof(resp_bwrite)) < 0)`
460			`{`
461			`if(gdb_fd)`
462			`{`
463			`perror("gdb socket - 4");`
464			`close(gdb_fd);`
465			`gdb_fd = 0;`
466			`}`
467			`return;`
468			`}`
469			`break;`
470			`case JTAG_COMMAND_BLOCK_READ:`
471			`if(length != sizeof(msg_bread) - 8)`
472			`{`
473			`ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);`
474			`return;`
475			`}`
476			`buf = (char*)&msg_bread;`
477			`if(gdb_read(&buf[8],length) < 0)`
478			`{`
479			`if(gdb_fd)`
480			`{`
481			`perror("gdb socket - 5");`
482			`close(gdb_fd);`
483			`gdb_fd = 0;`
484			`}`
485			`return;`
486			`}`
487			`msg_bread.address = ntohl(msg_bread.address);`
488			`msg_bread.nRegisters = ntohl(msg_bread.nRegisters);`
489			`len = sizeof(JTAGProxyBlockReadResponse) + 4*(msg_bread.nRegisters-1);`
490			`if(!(buf = (char*)malloc(len)))`
491			`{`
492			`ProtocolClean(0,JTAG_PROXY_OUT_OF_MEMORY);`
493			`return;`
494			`}`
495			`resp_bread = (JTAGProxyBlockReadResponse*)buf;`
496			`for(i=0;i<msg_bread.nRegisters;i++)`
497			`{`
498			`int t_err;`
499
500			`t_err = DebugGetRegister(msg_bread.address+i,&resp_bread->data[i]);`
501			`resp_bread->data[i] = htonl(resp_bread->data[i]);`
502			`err = err ? err : t_err;`
503			`}`
504			`resp_bread->status = htonl(err);`
505			`resp_bread->nRegisters = htonl(msg_bread.nRegisters);`
506			`if(gdb_write(resp_bread,len) < 0)`
507			`{`
508			`if(gdb_fd)`
509			`{`
510			`perror("gdb socket - 6");`
511			`close(gdb_fd);`
512			`gdb_fd = 0;`
513			`}`
514			`free(buf);`
515			`return;`
516			`}`
517			`free(buf);`
518			`buf = NULL;`
519			`resp_bread = NULL;`
520			`break;`
521			`case JTAG_COMMAND_CHAIN:`
522			`if(length != sizeof(msg_chain) - 8)`
523			`{`
524			`ProtocolClean(length,JTAG_PROXY_PROTOCOL_ERROR);`
525			`return;`
526			`}`
527			`buf = (char*)&msg_chain;`
528			`if(gdb_read(&buf[8],sizeof(msg_chain)-8) < 0)`
529			`{`
530			`if(gdb_fd)`
531			`{`
532			`perror("gdb socket - 7");`
533			`close(gdb_fd);`
534			`gdb_fd = 0;`
535			`}`
536			`return;`
537			`}`
538			`msg_chain.chain = htonl(msg_chain.chain);`
539			`err = DebugSetChain(msg_chain.chain);`
540			`resp_chain.status = htonl(err);`
541			`if(gdb_write(&resp_chain,sizeof(resp_chain)) < 0)`
542			`{`
543			`if(gdb_fd)`
544			`{`
545			`perror("gdb socket - 8");`
546			`close(gdb_fd);`
547			`gdb_fd = 0;`
548			`}`
549			`return;`
550			`}`
551			`break;`
552			`default:`
553			`ProtocolClean(length,JTAG_PROXY_COMMAND_NOT_IMPLEMENTED);`
554			`break;`
555			`}`
556			`}`
557
558			`void ProtocolClean(int length,int32_t err)`
559			`{`
560			`char buf[4096];`
561
562			`err = htonl(err);`
563			`if((gdb_read(buf,length) < 0) \|\|`
564			`(gdb_write(&err,sizeof(err)) < 0) && gdb_fd)`
565			`{`
566			`perror("gdb socket - 9");`
567			`close(gdb_fd);`
568			`gdb_fd = 0;`
569			`}`
570			`}`
571
572	439	erez	`static int gdb_write(const void* buf,int len)`
573	321	markom	`{`
574	439	erez	`int n, log_n = 0;`
575			`const char* w_buf = (const char*)buf;`
576			`const uint32_t* log_buf = (const uint32_t*)buf;`
577	321	markom	`struct pollfd block;`
578
579	439	erez	`while(len) {`
580			`if((n = write(gdb_fd,w_buf,len)) < 0) {`
581			`switch(errno) {`
582			`case EWOULDBLOCK: /* or EAGAIN */`
583			`/* We've been called on a descriptor marked`
584			`for nonblocking I/O. We better simulate`
585			`blocking behavior. */`
586			`block.fd = gdb_fd;`
587			`block.events = POLLOUT;`
588			`block.revents = 0;`
589			`poll(&block,1,-1);`
590			`continue;`
591			`case EINTR:`
592			`continue;`
593			`case EPIPE:`
594			`close(gdb_fd);`
595			`gdb_fd = 0;`
596			`return -1;`
597			`default:`
598			`return -1;`
599			`}`
600	321	markom	`}`
601	439	erez	`else {`
602			`len -= n;`
603			`w_buf += n;`
604			`if ( config.debug.vapi_id )`
605			`for ( log_n += n; log_n >= 4; log_n -= 4, ++ log_buf )`
606			`vapi_write_log_file( VAPI_COMMAND_SEND, config.debug.vapi_id, ntohl(*log_buf) );`
607			`}`
608			`}`
609	321	markom	`return 0;`
610			`}`
611
612			`static int gdb_read(void* buf,int len)`
613			`{`
614	439	erez	`int n, log_n = 0;`
615	321	markom	`char* r_buf = (char*)buf;`
616	439	erez	`uint32_t* log_buf = (uint32_t*)buf;`
617	321	markom	`struct pollfd block;`
618
619	439	erez	`while(len) {`
620			`if((n = read(gdb_fd,r_buf,len)) < 0) {`
621			`switch(errno) {`
622			`case EWOULDBLOCK: /* or EAGAIN */`
623			`/* We've been called on a descriptor marked`
624			`for nonblocking I/O. We better simulate`
625			`blocking behavior. */`
626			`block.fd = gdb_fd;`
627			`block.events = POLLIN;`
628			`block.revents = 0;`
629			`poll(&block,1,-1);`
630			`continue;`
631			`case EINTR:`
632			`continue;`
633			`default:`
634			`return -1;`
635			`}`
636	321	markom	`}`
637	439	erez	`else if(n == 0) {`
638			`close(gdb_fd);`
639			`gdb_fd = 0;`
640			`return -1;`
641			`}`
642			`else {`
643			`len -= n;`
644			`r_buf += n;`
645			`if ( config.debug.vapi_id )`
646			`for ( log_n += n; log_n >= 4; log_n -= 4, ++ log_buf )`
647			`vapi_write_log_file( VAPI_COMMAND_REQUEST, config.debug.vapi_id, ntohl(*log_buf) );`
648			`}`
649			`}`
650	321	markom	`return 0;`
651			`}`
652
653			`int gdbcomm_init ()`
654			`{`
655			`serverPort = config.debug.server_port;`
656			`if(server_fd = GetServerSocket("or1ksim","tcp",serverPort))`
657			`printf("JTAG Proxy server started on port %d\n",serverPort);`
658			`}`