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

Subversion Repositories or1k

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

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 "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(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;
}
 
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 321

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