jsp_server.c

/* jsp_server.c -- Server for the JTAG serial port
   Copyright(C) 2010 Nathan Yawn <nyawn@opencores.org>

   This file is part the advanced debug unit / bridge.  It acts as a
   telnet server, to send and receive data for the JTAG Serial Port
   (JSP)

   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 <stdio.h>
#include <sys/unistd.h>
#include <sys/types.h>
#include <sys/fcntl.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <string.h>
#include <pthread.h>
#include <errno.h>
#include <netinet/in.h>
#include <unistd.h>

#include "dbg_api.h"
#include "hardware_monitor.h"
#include "errcodes.h"


#define debug(...) if (getenv("ADV_DEBUG")) fprintf(stderr, __VA_ARGS__ )

int jsp_server_fd = -1;
int jsp_client_fd = -1;
int jsp_pipe_fds[2];

/* Some convenient net address info to have around */
char jsp_ipstr[INET6_ADDRSTRLEN];
char *jsp_ipver;
int jsp_portnum;
char jsp_hostname[256];

/* Buffers for network data. Simple, static char arrays. */
#define JSP_BUFFER_SIZE 256
char jsp_tohw_buf[JSP_BUFFER_SIZE];
int jsp_tohw_rd_idx = 0;
int jsp_tohw_wr_idx = 0;
int jsp_tohw_count = 0;

char jsp_fromhw_buf[JSP_BUFFER_SIZE];
int jsp_fromhw_rd_idx = 0;
int jsp_fromhw_wr_idx = 0;
int jsp_fromhw_count = 0;

/* Other local data */
int jsp_server_running = 0;
int jsp_target_is_running = 0;

pthread_t jsp_server_thread;
void *jsp_server(void *arg);

void jsp_server_close(void);
void jsp_client_close(void);

void jsp_print_welcome(int fd);
void jsp_queue_data_from_client(int fd);
void jsp_send_data_to_client(int fd);
void jsp_hardware_transact(void);
void jsp_send_all(int fd, char *buf, int len);

/*----------------------------------------------------------*/
/* Public API                                               */
/*----------------------------------------------------------*/

void jsp_init(int portNum)
{
  int status;
  struct addrinfo hints;
  struct addrinfo *servinfo;  // will point to the results of getaddrinfo
  int optval; /* Socket options */
  char portnum[6];  /* portNum as a string */
  void *addr;

  debug("JSP Server initializing\n");

  jsp_server_fd      = -1;
  jsp_client_fd      = -1;
  jsp_portnum = portNum;

  memset(portnum, '\0', sizeof(portnum));
  snprintf(portnum, 5, "%i", portNum);

  /* Get the address info for the local host */
  memset(&hints, 0, sizeof hints); // make sure the struct is empty
  hints.ai_family = AF_UNSPEC;     // don't care IPv4 or IPv6
  hints.ai_socktype = SOCK_STREAM; // TCP stream sockets
  hints.ai_flags = AI_PASSIVE;     // fill in my IP for me
  
  if ((status = getaddrinfo(NULL, portnum, &hints, &servinfo)) != 0) {
    fprintf(stderr, "getaddrinfo error: %s\n", gai_strerror(status));
    return;
  }


  /* *** TODO: Select the appropriate servinfo in the linked list
   * For now, we just use the first entry in servinfo.
     struct addrinfo *servinfo, *p;
     for(p = servinfo;p != NULL; p = p->ai_next) {
     if (p->ai_family == AF_INET) { // IPv4
     } else { // IPv6
     }
     }
  */


  /* Save the IP address, for convenience (different fields in IPv4 and IPv6) */
  if (servinfo->ai_family == AF_INET) { // IPv4
    struct sockaddr_in *ipv4 = (struct sockaddr_in *)servinfo->ai_addr;
    addr = &(ipv4->sin_addr);
    jsp_ipver = "IPv4";
  } else { // IPv6
    struct sockaddr_in6 *ipv6 = (struct sockaddr_in6 *)servinfo->ai_addr;
    addr = &(ipv6->sin6_addr);
    jsp_ipver = "IPv6";
  }

  /* convert the IP to a string */
  inet_ntop(servinfo->ai_family, addr, jsp_ipstr, sizeof(jsp_ipstr));

  /* Find out what our name is, save for convenience */
  if (gethostname (jsp_hostname, sizeof(jsp_hostname)) < 0)
    {
      fprintf (stderr, "Warning: Unable to get hostname for JSP server: %s\n", strerror(errno));
      jsp_hostname[0] = '\0';  /* This is not a fatal error. */
    }

  /* Create the socket */
  jsp_server_fd = socket (servinfo->ai_family, servinfo->ai_socktype, servinfo->ai_protocol);
  if (jsp_server_fd < 0)
    {
      fprintf (stderr, "Error: JSP could not create server socket: %s\n", strerror(errno));
      return;
    }

  /* Set this socket to reuse its address. */
  optval = 1;
  if (setsockopt(jsp_server_fd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof (optval)) == -1)
    {
      fprintf (stderr, "Cannot set SO_REUSEADDR option on server socket %d: %s\n", jsp_server_fd, strerror(errno));
      jsp_server_close();
      return;
    }

  /* Bind the socket to the local address */
  if (bind (jsp_server_fd, servinfo->ai_addr, servinfo->ai_addrlen) < 0)
    {
      fprintf (stderr, "Error: Unable to bind JSP server socket %d to port %d: %s\n", jsp_server_fd, portNum, strerror(errno));
      jsp_server_close();
      return;
    }

  /* Set us up as a server, with a maximum backlog of 1 connection */
  if (listen (jsp_server_fd, 1) < 0)
    {
      fprintf (stderr, "Warning: Unable to set JSP backlog on server socket %d to %d: %s\n", jsp_server_fd, 1, strerror(errno));
      jsp_server_close();
      return;
    }

  fprintf(stderr, "JSP server listening on host %s (%s), port %i, address family %s\n", 
	  jsp_hostname, jsp_ipstr, jsp_portnum, jsp_ipver);

  /* Register for stall/unstall events from the target monitor thread. Also creates pipe
   * for sending stall/unstall command to the target monitor, unused by us. */
  if(0 > register_with_monitor_thread(jsp_pipe_fds)) {  // pipe_fds[0] is for writing to monitor, [1] is to read from it
    fprintf(stderr, "JSP server failed to register with monitor thread, exiting");
    jsp_server_close();
    return;
  }

}


int jsp_server_start(void)
{

  jsp_server_running = 1;
 
  debug("Starting JSP server\n");

  // Create the JSP server thread
  if(pthread_create(&jsp_server_thread, NULL, jsp_server, NULL))
    {
      fprintf(stderr, "Failed to create JSP server thread!\n");
      return 0;
    }

  return 1;
}


int jsp_server_stop(void)
{
  /*** NOTE: Since we currently don't use select() in front of the accept()
   *** in the server thread, this won't actually work unless/until a client
   *** is connected.  Otherwise, the server thread will be blocked on the
   *** accept() (though closing the server socket may break it out.)
   ***/

  jsp_server_running = 0;
  jsp_server_close();
  return 1;
}

/*--------------------------------------------------------------------*/
/* Main server thread                                                 */
/*--------------------------------------------------------------------*/


void *jsp_server(void *arg)
{
  struct sockaddr_storage their_addr;
  struct timeval tv, *tvp;
  fd_set  readset;
  socklen_t addr_size;
  int nfds, flags;
  int ret;
  char cmd;

  fprintf(stderr, "JSP server thread running!\n");

  while(jsp_server_running)
    {
      /* Listen for an incoming connection */
      addr_size = sizeof their_addr;
      jsp_client_fd = accept(jsp_server_fd, (struct sockaddr *)&their_addr, &addr_size);

      if(jsp_client_fd == -1)
	{
	  perror("Error in accept() in JSP server thread");
	}
      else
	{
	  debug("JSP server got connection!\n");

	  // Clear the in/out buffers
	  jsp_tohw_rd_idx = 0;
	  jsp_tohw_wr_idx = 0;
	  jsp_tohw_count = 0;
	  jsp_fromhw_rd_idx = 0;
	  jsp_fromhw_wr_idx = 0;
	  jsp_fromhw_count = 0;

	  /* New client should be non-blocking. */
	  flags = fcntl (jsp_client_fd, F_GETFL);
	  if (flags < 0)
	    {
	      fprintf (stderr, "Warning: Unable to get flags for JSP client socket %d: %s\n", jsp_client_fd, strerror(errno));
	      // Not really fatal.
	    }
	  else {
	    flags |= O_NONBLOCK;
	    if (fcntl (jsp_client_fd, F_SETFL, flags) < 0)
	      {
		fprintf (stderr, "Warning: Unable to set flags for JSP client socket %d to 0x%08x: %s\n", jsp_client_fd, flags, strerror(errno));
		// Also not really fatal.
	      }
	  }

	  jsp_print_welcome(jsp_client_fd);
	}

      /* Send/receive data on the new connection for as long as it's valid */
      while(jsp_server_running && (jsp_client_fd != -1))
	{
	  /* if target not running, block on data from client or monitor thread */
	  /* if target running, just poll (don't block */
	  if(jsp_target_is_running) {
	    tv.tv_sec = 0;  // Set this each loop, it may be changed by the select() call
	    tv.tv_usec = 0;  // instant timeout when polling
	    tvp = &tv;
	  } else {
	    tvp = NULL;
	  }

	  FD_ZERO(&readset);
	  FD_SET(jsp_client_fd, &readset);
	  FD_SET(jsp_pipe_fds[1], &readset);
	  nfds = jsp_client_fd;
	  if(jsp_pipe_fds[1] > nfds) nfds = jsp_pipe_fds[1];
	  nfds++;

	  ret = select(nfds, &readset, NULL, NULL, tvp);

	  if(ret == -1)  // error
	    {
		perror("select()");
	    }
	  else if(ret != 0)  // fd ready (ret == 0 on timeout)
	    {
	      debug("JSP thread got data\n");

	      if(FD_ISSET(jsp_pipe_fds[1], &readset))
		{
		  ret = read(jsp_pipe_fds[1], &cmd, 1);
		  debug("JSP server got monitor status \'%c\' (0x%X)\n", cmd, cmd);
		  if(ret == 1)
		    {
		      if(cmd == 'H')  
			{
			  jsp_target_is_running = 0;
			}
		      else if(cmd == 'R')  
			{
			  jsp_target_is_running = 1;
			}
		      else
			{
			  fprintf(stderr, "JSP server got unknown monitor status \'%c\' (0x%X)\n", cmd, cmd);
			}
		    }
		  else
		    {
		      fprintf(stderr, "JSP server failed to read from ready monitor pipe!\n");
		    }
		}  // if FD_ISSET(jsp_pipe_fds[1])

	      if(FD_ISSET(jsp_client_fd, &readset))
		{
		  jsp_queue_data_from_client(jsp_client_fd);
		}
	    }   // else if (ret != 0)


	  /* Send any buffered output data to the client */
	  jsp_send_data_to_client(jsp_client_fd);

	  /* If target running, transact with the JSP to send/receive buffered data */
	  if(jsp_target_is_running)
	    {
	      jsp_hardware_transact();
	    }

	}  /* while client connection is valid */

    } /* while(jsp_server_running) */

  jsp_client_close();

  return arg;  // unused
}

/*--------------------------------------------------------------------*/
/* Helper functions                                                   */
/*--------------------------------------------------------------------*/

void jsp_server_close(void)
{
  if (jsp_server_fd != -1)
    {
      close(jsp_server_fd);
      jsp_server_fd = -1;
    }
}


void jsp_client_close(void)
{  
  if (jsp_client_fd != -1)
    {
      close (jsp_client_fd);
      jsp_client_fd = -1;
    }
}	/* jsp_client_close () */


void jsp_print_welcome(int fd)
{
  char msg[] = "Advanced Debug System JTAG Serial Port Server\n\r";
  char msg2[] = " (";
  char msg3[] = "), port ";
  char portnum[24];

  jsp_send_all(fd, msg, sizeof(msg));
  jsp_send_all(fd, jsp_hostname, strlen(jsp_hostname));
  jsp_send_all(fd, msg2, sizeof(msg2));
  jsp_send_all(fd, jsp_ipstr, strlen(jsp_ipstr));
  jsp_send_all(fd, msg3, sizeof(msg3));
  
  memset(portnum, '\0', sizeof(portnum));
  snprintf(portnum, 23, "%i\n\n\r", jsp_portnum);
  jsp_send_all(fd, portnum, strlen(portnum));
}


void jsp_queue_data_from_client(int fd)
{
  int space_available;
  int bytes_received;

  debug("JSP queueing data from client; Tohw count now %i\n", jsp_tohw_count);

  // First, try to fill from the write index to the end of the array, or the read index, whichever is less
  // This keeps the buffer that recv() writes to linear
  space_available = JSP_BUFFER_SIZE - jsp_tohw_wr_idx;
  if(space_available > (JSP_BUFFER_SIZE - jsp_tohw_count))
    space_available = JSP_BUFFER_SIZE - jsp_tohw_count;

  bytes_received = recv(fd, &jsp_tohw_buf[jsp_tohw_wr_idx], space_available, 0);
  if(bytes_received < 0)
    {
      perror("JSP client socket read failed");
      return;
    }
  else if(bytes_received > 0)
    {
      jsp_tohw_wr_idx = (jsp_tohw_wr_idx + bytes_received) % JSP_BUFFER_SIZE;  // modulo will only happen if wrapping to 0
      jsp_tohw_count += bytes_received;
    }

  // Now, do the same thing again, potentially filling the buffer from index 0 to the read index
  space_available = JSP_BUFFER_SIZE - jsp_tohw_wr_idx;
  if(space_available > (JSP_BUFFER_SIZE - jsp_tohw_count))
    space_available = JSP_BUFFER_SIZE - jsp_tohw_count;

  bytes_received = recv(fd, &jsp_tohw_buf[jsp_tohw_wr_idx], space_available, 0);
  if(bytes_received < 0)
    {
      if(errno != EAGAIN) {
	perror("JSP client socket read failed");
      } else {
	debug("Second JSP client socket read got EAGAIN.\n");
      }
      return;
    }
  else if(bytes_received > 0)
    {
      jsp_tohw_wr_idx = (jsp_tohw_wr_idx + bytes_received) % JSP_BUFFER_SIZE;  // modulo will only happen if wrapping to 0
      jsp_tohw_count += bytes_received;
    }

  debug("JSP queued data from client; Tohw count now %i\n", jsp_tohw_count);
}

void jsp_send_data_to_client(int fd)
{
  int bytes_written;
  int bytes_available;

  // *** TODO: use sendvec()
  debug("JSP will send data to client. Fromhw count now %i\n", jsp_fromhw_count);

  if(jsp_fromhw_count > 0)
    {
      bytes_available = jsp_fromhw_count;
      if(bytes_available > (JSP_BUFFER_SIZE - jsp_fromhw_rd_idx))
	bytes_available = JSP_BUFFER_SIZE - jsp_fromhw_rd_idx;
      
      bytes_written = send(fd, &jsp_fromhw_buf[jsp_fromhw_rd_idx], bytes_available, 0); 
      if(bytes_written < 0)
	{
	  perror("JSP server failed client socket write");
	}
      else
	{
	  jsp_fromhw_count -= bytes_written;
	  jsp_fromhw_rd_idx = (jsp_fromhw_rd_idx + bytes_written) % JSP_BUFFER_SIZE;
	}
    }

  // Now do it again, in case of buffer wraparound
  if(jsp_fromhw_count > 0)
    {
      bytes_available = jsp_fromhw_count;
      if(bytes_available > (JSP_BUFFER_SIZE - jsp_fromhw_rd_idx))
	bytes_available = JSP_BUFFER_SIZE - jsp_fromhw_rd_idx;
      
      bytes_written = send(fd, &jsp_fromhw_buf[jsp_fromhw_rd_idx], bytes_available, 0); 
      if(bytes_written < 0)
	{
	  perror("JSP server failed client socket write");
	}
      else
	{
	  jsp_fromhw_count -= bytes_written;
	  jsp_fromhw_rd_idx = (jsp_fromhw_rd_idx + bytes_written) % JSP_BUFFER_SIZE;
	}
    }

  debug("JSP sent data to client. Fromhw count now %i\n", jsp_fromhw_count);
}


void jsp_hardware_transact(void)
{
  unsigned int bytes_to_send;
  unsigned int bytes_received = 8;  // can receive up to 8 bytes
  unsigned char sendbuf[8];
  unsigned char rcvbuf[8];
  int i,j;
  int ret;

  debug("JSP about to transact; Tohw buf size now %i, fromhw buf size %i\n", jsp_tohw_count, jsp_fromhw_count);

  // Get data to send, if any
  bytes_to_send = jsp_tohw_count;
  if(bytes_to_send > 8) bytes_to_send = 8;

  j = jsp_tohw_rd_idx;
  for(i = 0; i < bytes_to_send; i++)
    {
      sendbuf[i] = jsp_tohw_buf[j];
      j = (j+1) % JSP_BUFFER_SIZE;
    }

  // Do the transaction
  ret = dbg_serial_sndrcv(&bytes_to_send, sendbuf, &bytes_received, rcvbuf);
  if(ret != APP_ERR_NONE)
    {
      fprintf(stderr, "Error in JSP transaction: %s\n", get_err_string(ret));
    }
  else
    {
      debug("Transacted, bytes sent = %i, received = %i\n", bytes_to_send, bytes_received);

      // Adjust send buffer pointers as necessary - we may not have sent all 8 bytes
      jsp_tohw_count -= bytes_to_send;
      jsp_tohw_rd_idx = (jsp_tohw_rd_idx + bytes_to_send) % JSP_BUFFER_SIZE;
      
      // Queue data received, if any, and adjust the pointers
      for(i = 0; i < bytes_received; i++)
	{
	  jsp_fromhw_buf[jsp_fromhw_wr_idx] = rcvbuf[i];
	  jsp_fromhw_wr_idx = (jsp_fromhw_wr_idx + 1) % JSP_BUFFER_SIZE;
	  jsp_fromhw_count++;
	}
      
      debug("JSP transacted; Tohw buf size now %i, fromhw buf size %i\n", jsp_tohw_count, jsp_fromhw_count);
    }
}

void jsp_send_all(int fd, char *buf, int len)
{
  int total_sent = 0;
  int bytes_sent;

  while(total_sent < len)
    {
      bytes_sent = send(fd, buf, len, 0);
      if(bytes_sent < 0)
	{
	  perror("JSP server socket send failed");
	  break;
	}
      total_sent += bytes_sent;
    }
}