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

Subversion Repositories or1k

[/] [or1k/] [trunk/] [jtag/] [gdb.c] - Blame information for rev 1778

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


#include <stdio.h>
#include <ctype.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdarg.h>
 
/* Libraries for JTAG proxy server.  */
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/poll.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/tcp.h>
#include <inttypes.h>
#include <errno.h>
 
#include "gdb.h" /* partially copied from gdb/config/or1k */
#include "jp2.h"
 
 
/* connection to jp2 routines */
int gdb_chain = -1;
 
int gdb_read_reg(unsigned long adr, unsigned long *data) {
  switch (gdb_chain) {
    SC_RISC_DEBUG: return dbg_cpu_read32(adr, data) ? ERR_CRC : ERR_NONE;
    SC_REGISTER:   return dbg_cpu_read_reg(adr, data) ? ERR_CRC : ERR_NONE;
    SC_WISHBONE:   return dbg_wb_read32(adr, data) ? ERR_CRC : ERR_NONE;
    default:       return JTAG_PROXY_INVALID_CHAIN;
  }
}
 
int gdb_write_reg(unsigned long adr, unsigned long data) {
  switch (gdb_chain) {
    SC_RISC_DEBUG: return dbg_cpu_write32(adr, data) ? ERR_CRC : ERR_NONE;
    SC_REGISTER:   return dbg_cpu_write_reg(adr, data) ? ERR_CRC : ERR_NONE;
    SC_WISHBONE:   return dbg_wb_write32(adr, data) ? ERR_CRC : ERR_NONE;
    default:       return JTAG_PROXY_INVALID_CHAIN;
  }
}
 
int gdb_read_block(unsigned long adr, unsigned long *data, int len) {
  switch (gdb_chain) {
    SC_WISHBONE:   return dbg_wb_read_block32(adr, data, len) ? ERR_CRC : ERR_NONE;
    default:       return JTAG_PROXY_INVALID_CHAIN;
  }
}
 
int gdb_write_block(unsigned long adr, unsigned long *data, int len) {
  switch (gdb_chain) {
    SC_WISHBONE:   return dbg_wb_write_block32(adr, data, len) ? ERR_CRC : ERR_NONE;
    default:       return JTAG_PROXY_INVALID_CHAIN;
  }
}
 
int gdb_set_chain(int chain) {
  switch (gdb_chain) {
    SC_RISC_DEBUG:
    SC_REGISTER:
    SC_WISHBONE:   gdb_chain = chain;
                   return ERR_NONE;
    default:       return JTAG_PROXY_INVALID_CHAIN;
  }
}
 
 
 
/************************
   JTAG Server Routines
 ************************/
 
unsigned int serverIP = 0;
unsigned int serverPort = 0;
unsigned int server_fd = 0;
unsigned int gdb_fd = 0;
static int gdb_read(void*, int);
static int gdb_write(void*, int);
static void ProtocolClean(int, int32_t);
 
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 && (service = getservbyname(name, protocol->p_name)))
    port = ntohs(service->s_port);
 
  /* Create the socket using the TCP protocol */
  if ((sockfd = socket(PF_INET, SOCK_STREAM, protocol->p_proto)) < 0) {
    perror("Unable to create socket");
    return 0;
  }
 
  flags = 1;
  if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (const char*)&flags, sizeof(int)) < 0) {
    sprintf(sTemp, "Can not set SO_REUSEADDR option on socket %d", sockfd);
    perror(sTemp);
    close(sockfd);
    return 0;
  }
 
  /* The server should also be non blocking. Get the current flags. */
  if((flags = fcntl(sockfd, F_GETFL, 0)) < 0) {
    sprintf(sTemp, "Unable to get flags for socket %d", sockfd);
    perror(sTemp);
    close(sockfd);
    return 0;
  }
 
  /* Set the nonblocking flag */
  if(fcntl(sockfd, F_SETFL, flags | O_NONBLOCK) < 0) {
    sprintf(sTemp, "Unable to set flags for socket %d to value 0x%08x",
                    sockfd, flags | O_NONBLOCK);
    perror(sTemp);
    close(sockfd);
    return 0;
  }
 
  /* Find out what our address is */
  memset(&sa, 0, sizeof(struct sockaddr_in));
  gethostname(myname, sizeof(myname));
  if(!(hp = gethostbyname(myname))) {
    perror("Unable to read hostname");
    close(sockfd);
    return 0;
  }
 
  /* Bind our socket to the appropriate address */
  sa.sin_family = hp->h_addrtype;
  sa.sin_port = htons(port);
  if(bind(sockfd, (struct sockaddr*)&sa, sizeof(struct sockaddr_in)) < 0) {
    sprintf(sTemp, "Unable to bind socket %d to port %d", sockfd, port);
    perror(sTemp);
    close(sockfd);
    return 0;
  }
  serverIP = sa.sin_addr.s_addr;
  flags = sizeof(struct sockaddr_in);
  if(getsockname(sockfd, (struct sockaddr*)&sa, &flags) < 0) {
    sprintf(sTemp, "Unable to get socket information for socket %d", sockfd);
    perror(sTemp);
    close(sockfd);
    return 0;
  }
  serverPort = ntohs(sa.sin_port);
 
  /* Set the backlog to 1 connections */
  if(listen(sockfd, 1) < 0) {
    sprintf(sTemp, "Unable to set backlog on socket %d to %d", sockfd, 1);
    perror(sTemp);
    close(sockfd);
    return 0;
  }
 
  return sockfd;
}
 
void HandleServerSocket(Boolean block) {
  struct pollfd fds[2];
  int n;
 
rebuild:
  n = 0;
  if(!server_fd && !gdb_fd) return;
 
  if(server_fd) {
    fds[n].fd = server_fd;
    fds[n].events = POLLIN;
    fds[n++].revents = 0;
  }
  if(gdb_fd) {
    fds[n].fd = gdb_fd;
    fds[n].events = POLLIN;
    fds[n++].revents = 0;
  }
 
  while(1) {
  switch(poll(fds, n, -1)) {
    case 0:
    case -1:
      if(errno == EINTR) continue;
      perror("poll");
      server_fd = 0;
      return;
    default:
      /* Make sure to handle the gdb port first! */
      if (gdb_fd && (fds[0].revents && !server_fd || fds[1].revents && server_fd)) {
        int revents = server_fd ? fds[1].revents : fds[0].revents;
        if (revents & POLLIN) GDBRequest();
        else {/* Error Occurred */
          fprintf(stderr, "Received flags 0x%08x on gdb socket. Shutting down.\n", revents);
          close(gdb_fd);
          gdb_fd = 0;
        }
      }
      if(fds[0].revents && server_fd) {
        if(fds[0].revents & POLLIN) {
          JTAGRequest();
          goto rebuild;
        } else { /* Error Occurred */
            fprintf(stderr, "Received flags 0x%08x on server. Shutting down.\n", fds[0].revents);
            close(server_fd);
            server_fd = 0;
            serverPort = 0;
            serverIP = 0;
            return;
          }
        }
      break;
    } /* End of switch statement */
  } /* End of while statement */
}
 
void JTAGRequest() {
  struct sockaddr_in sa;
  struct sockaddr* addr = (struct sockaddr*)&sa;
  int n = sizeof(struct sockaddr_in);
  int fd = accept(server_fd, addr, &n);
  int on_off = 0; /* Turn off Nagel's algorithm on the socket */
  int flags;
  char sTemp[256];
 
  if(fd < 0) {
    /* This is valid, because a connection could have started,
       and then terminated due to a protocol error or user
       initiation before the accept could take place. */
    if(errno != EWOULDBLOCK && errno != EAGAIN) {
      perror("accept");
      close(server_fd);
      server_fd = 0;
      serverPort = 0;
      serverIP = 0;
    }
    return;
  }
 
  if(gdb_fd) {
    close(fd);
    return;
  }
 
  if((flags = fcntl(fd, F_GETFL,0)) < 0) {
    sprintf(sTemp, "Unable to get flags for gdb socket %d", fd);
    perror(sTemp);
    close(fd);
    return;
  }
 
  if(fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
    sprintf(sTemp, "Unable to set flags for gdb socket %d to value 0x%08x",
      fd, flags | O_NONBLOCK);
    perror(sTemp);
    close(fd);
    return;
  }
 
  if(setsockopt(fd, tcp_level, TCP_NODELAY, &on_off, sizeof(int)) < 0) {
    sprintf(sTemp, "Unable to disable Nagel's algorithm for socket %d.\nsetsockopt", fd);
    perror(sTemp);
    close(fd);
    return;
  }
 
  gdb_fd = fd;
}
 
void GDBRequest() {
  JTAGProxyWriteMessage msg_write;
  JTAGProxyReadMessage msg_read;
  JTAGProxyChainMessage msg_chain;
  JTAGProxyWriteResponse resp_write;
  JTAGProxyReadResponse resp_read;
  JTAGProxyChainResponse resp_chain;
  JTAGProxyBlockWriteMessage *msg_bwrite;
  JTAGProxyBlockReadMessage msg_bread;
  JTAGProxyBlockWriteResponse resp_bwrite;
  JTAGProxyBlockReadResponse *resp_bread;
  char *buf;
  unsigned long long data;
  uint32_t command, length;
  int len, i;
  int err = 0;
 
  /* First, we must read the incomming command */
  if(gdb_read(&command, sizeof(uint32_t)) < 0) {
    if(gdb_fd) {
      perror("gdb socket - 1");
      close(gdb_fd);
      gdb_fd = 0;
    }
    return;
  }
  if(gdb_read(&length, sizeof(uint32_t)) < 0) {
    if(gdb_fd) {
      perror("gdb socket - 2");
      close(gdb_fd);
      gdb_fd = 0;
    }
    return;
  }
  length = ntohl(length);
 
  /* Now, verify the protocol and implement the command */
  switch(ntohl(command)) {
    case JTAG_COMMAND_WRITE:
      if(length != sizeof(msg_write) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_write;
      if(gdb_read(&buf[8], length) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 3");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_write.address = ntohl(msg_write.address);
      msg_write.data_H = ntohl(msg_write.data_H);
      msg_write.data_L = ntohl(msg_write.data_L);
      err = gdb_write_reg(msg_write.address, msg_write.data_L);
      resp_write.status = htonl(err);
      if(gdb_write(&resp_write, sizeof(resp_write)) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 4");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      break;
    case JTAG_COMMAND_READ:
      if(length != sizeof(msg_read) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_read;
      if(gdb_read(&buf[8], length) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 5");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_read.address = ntohl(msg_read.address);
      err = gdb_read_reg(msg_read.address, (unsigned long *)&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++) {
        msg_bwrite->data[i] = ntohl(msg_bwrite->data[i]);
      }
      err = gdb_write_block(msg_bwrite->address, (unsigned long*)msg_bwrite->data, msg_bwrite->nRegisters * 4);
      resp_bwrite.status = htonl(err);
      free(buf);
      msg_bwrite = (JTAGProxyBlockWriteMessage *)NULL;
      buf = (char *)msg_bwrite;
      if(gdb_write(&resp_bwrite, sizeof(resp_bwrite)) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 4");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      break;
    case JTAG_COMMAND_BLOCK_READ:
      if(length != sizeof(msg_bread) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_bread;
      if(gdb_read(&buf[8], length) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 5");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_bread.address = ntohl(msg_bread.address);
      msg_bread.nRegisters = ntohl(msg_bread.nRegisters);
      len = sizeof(JTAGProxyBlockReadResponse) + 4*(msg_bread.nRegisters-1);
      if(!(buf = (char*)malloc(len))) {
        ProtocolClean(0, JTAG_PROXY_OUT_OF_MEMORY);
        return;
      }
      resp_bread = (JTAGProxyBlockReadResponse*)buf;
      err = gdb_read_block(msg_bread.address, (unsigned long*)resp_bread->data, msg_bread.nRegisters * 4);
      for(i=0;i<msg_bread.nRegisters;i++) {
        /* Read previous, address next one. */
        resp_bread->data[i] = htonl(resp_bread->data[i]);
      }
      resp_bread->status = htonl(err);
      resp_bread->nRegisters = htonl(msg_bread.nRegisters);
      if(gdb_write(resp_bread, len) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 6");
          close(gdb_fd);
          gdb_fd = 0;
        }
        free(buf);
        return;
      }
      free(buf);
      resp_bread = (JTAGProxyBlockReadResponse *)NULL;
      buf = (char *)resp_bread;
      break;
    case JTAG_COMMAND_CHAIN:
      if(length != sizeof(msg_chain) - 8) {
        ProtocolClean(length, JTAG_PROXY_PROTOCOL_ERROR);
        return;
      }
      buf = (char*)&msg_chain;
      if(gdb_read(&buf[8], sizeof(msg_chain)-8) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 7");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      msg_chain.chain = htonl(msg_chain.chain);
      err = gdb_set_chain(msg_chain.chain);
      resp_chain.status = htonl(err);
      if(gdb_write(&resp_chain, sizeof(resp_chain)) < 0) {
        if(gdb_fd) {
          perror("gdb socket - 8");
          close(gdb_fd);
          gdb_fd = 0;
        }
        return;
      }
      break;
    default:
      perror("Unknown JTAG command.");
      ProtocolClean(length, JTAG_PROXY_COMMAND_NOT_IMPLEMENTED);
      break;
  }
}
 
static void ProtocolClean(int length, int32_t err) {
  char buf[4096];
 
  err = htonl(err);
  if((gdb_read(buf, length) < 0) || (gdb_write(&err, sizeof(err)) < 0) && gdb_fd) {
    perror("gdb socket - 9");
    close(gdb_fd);
    gdb_fd = 0;
  }
}
 
static int gdb_write(void* buf, int len) {
  int n;
  char* w_buf = (char*)buf;
  struct pollfd block;
 
  while(len) {
    if((n = write(gdb_fd, w_buf, len)) < 0) {
      switch(errno) {
        case EWOULDBLOCK: /* or EAGAIN */
          /* We've been called on a descriptor marked
             for nonblocking I/O. We better simulate
             blocking behavior. */
          block.fd = gdb_fd;
          block.events = POLLOUT;
          block.revents = 0;
          poll(&block, 1, -1);
          continue;
        case EINTR:
          continue;
        case EPIPE:
          close(gdb_fd);
          gdb_fd = 0;
          return -1;
        default:
          return -1;
        }
      } else {
        len -= n;
        w_buf += n;
      }
  }
  return 0;
}
 
static int gdb_read(void* buf, int len) {
  int n;
  char* r_buf = (char*)buf;
  struct pollfd block;
 
  while(len) {
    if((n = read(gdb_fd, r_buf, len)) < 0) {
      switch(errno) {
        case EWOULDBLOCK: /* or EAGAIN */
          /* We've been called on a descriptor marked
       for nonblocking I/O. We better simulate
       blocking behavior. */
          block.fd = gdb_fd;
          block.events = POLLIN;
          block.revents = 0;
          poll(&block, 1, -1);
          continue;
        case EINTR:
          continue;
        default:
          return -1;
        }
    } else if(n == 0) {
      close(gdb_fd);
      gdb_fd = 0;
      return -1;
    } else {
      len -= n;
      r_buf += n;
    }
  }
  return 0;
}

Browse

Tools

Subversion Repositories or1k

[/] [or1k/] [trunk/] [jtag/] [gdb.c] - Blame information for rev 1778

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