jp1.c

嵌入式开发板连接程序源码

    }

  /* 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;
                  }
              }
           if (fds[1].revents && !gdb_fd) {
                   /* gdb_fd was cleared because connection was closed,
                    * but fd still in poll set */
                   fds[1].fd = -1;
                   n = 1;
           }
            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;
  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);
      jtag_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);
      jtag_read_reg(msg_read.address); /* Data not ready at this time, repeat. */
      resp_read.data_L = jtag_read_reg(msg_read.address);
      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]);
          jtag_write_reg(msg_bwrite->address + i * 4,msg_bwrite->data[i]);    
        }
      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;
      jtag_read_reg(msg_bread.address); /* Prepare for reading. */
      for(i=0;i<msg_bread.nRegisters;i++)
        {
          /* Read previous, address next one. */
          resp_bread->data[i] = jtag_read_reg(msg_bread.address + (i + 1) * 4);
          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);
      jtag_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;
    }
}

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;
}