Subversion Repositories or1k

#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 "jp.h"  /* just for debug() */
#include "jp2.h"
 
 
/* connection to jp2 routines */
int gdb_chain = -1;
 
int gdb_read_reg(unsigned long adr, unsigned long *data) {
  debug("rr %d\n", gdb_chain);
  switch (gdb_chain) {
    case SC_RISC_DEBUG: return dbg_cpu0_read(adr, data) ? ERR_CRC : ERR_NONE;
    case SC_REGISTER:   return dbg_cpu0_read_ctrl(adr, (unsigned char*)data) ? ERR_CRC : ERR_NONE;
    case SC_WISHBONE:   return dbg_wb_read32(adr, data) ? ERR_CRC : ERR_NONE;
    case SC_TRACE:      *data = 0; return 0;
    default:            return JTAG_PROXY_INVALID_CHAIN;
  }
}
 
int gdb_write_reg(unsigned long adr, unsigned long data) {
  debug("wr %d\n", gdb_chain);
  switch (gdb_chain) { /* remap registers, to be compatible with jp1 */
    case SC_RISC_DEBUG: if (adr == JTAG_RISCOP) adr = 0x00;
                        return dbg_cpu0_write(adr, data) ? ERR_CRC : ERR_NONE;
    case SC_REGISTER:   return dbg_cpu0_write_ctrl(adr, data) ? ERR_CRC : ERR_NONE;
    case SC_WISHBONE:   return dbg_wb_write32(adr, data) ? ERR_CRC : ERR_NONE;
    case SC_TRACE:      return 0;
    default:            return JTAG_PROXY_INVALID_CHAIN;
  }
}
 
int gdb_read_block(unsigned long adr, unsigned long *data, int len) {
  debug("rb %d\n", gdb_chain);
  switch (gdb_chain) {
    case 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) {
  debug("wb %d\n", gdb_chain);
  switch (gdb_chain) {
    case 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) {
  debug("set_chain %d\n", chain);
  switch (chain) {
    case SC_RISC_DEBUG:
    case SC_REGISTER:
    case SC_TRACE:      
    case 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;
}