Subversion Repositories adv_debug_sys

/* hwp_server.c -- Server for hardware watchpoint handling

   Copyright(C) 2010 Nathan Yawn <nyawn@opencores.org>

   This file is part the advanced debug unit / bridge.  GDB does not

   have support for the OR1200's advanced hardware watchpoints.  This

   acts as a server for a client program that can read and set them.

   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 <netinet/in.h>

#include <unistd.h>

#include <netdb.h>

#include <string.h>

#include <pthread.h>

#include <errno.h>

#include "spr-defs.h"

#include "dbg_api.h"

#include "hardware_monitor.h"

#include "errcodes.h"

#define debug(...) // fprintf(stderr, __VA_ARGS__ )

#define HWP_BUF_MAX 256

/*! Data structure for RSP buffers. Can't be null terminated, since it may

  include zero bytes */

struct rsp_buf

{

  char  data[HWP_BUF_MAX];

  int   len;

};

int hwp_server_fd = -1;

int hwp_client_fd = -1;

int hwp_pipe_fds[2];

/* Some convenient net address info to have around */

char hwp_ipstr[INET6_ADDRSTRLEN];

char *hwp_ipver;

int hwp_portnum;

char hwp_hostname[256];

/* Other local data */

int hwp_server_running = 0;

int hwp_target_is_running = 0;

int use_cached_dmr2 = 0;

uint32_t cached_dmr2 = 0;

/*! String to map hex digits to chars */

static const char hexchars[]="0123456789abcdef";

pthread_t hwp_server_thread;

void *hwp_server(void *arg);

void hwp_server_close(void);

void hwp_client_close(void);

void hwp_client_request(void);

struct rsp_buf *hwp_get_packet(void);

void put_hwp_rsp_char(int fd, char c);

int get_hwp_rsp_char(int fd);

void hwp_read_reg(struct rsp_buf *buf);

void hwp_write_reg (struct rsp_buf *buf);

void hwp_put_str_packet (int fd, const char *str);

void hwp_put_packet (int fd, struct rsp_buf *buf);

unsigned long int hwp_hex2reg (char *buf);

void hwp_reg2hex (unsigned long int val, char *buf);

int hwp_hex(int c);

void hwp_report_run(void);

void hwp_report_stop(void);

/*----------------------------------------------------------*/

/* Public API                                               */

/*----------------------------------------------------------*/

void hwp_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("HWP Server initializing\n");

  hwp_server_fd      = -1;

  hwp_client_fd      = -1;

  hwp_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);

    hwp_ipver = "IPv4";

  } else { // IPv6

    struct sockaddr_in6 *ipv6 = (struct sockaddr_in6 *)servinfo->ai_addr;

    addr = &(ipv6->sin6_addr);

    hwp_ipver = "IPv6";

  }

  /* convert the IP to a string */

  inet_ntop(servinfo->ai_family, addr, hwp_ipstr, sizeof(hwp_ipstr));

  /* Find out what our name is, save for convenience */

  if (gethostname (hwp_hostname, sizeof(hwp_hostname)) < 0)

    {

      fprintf (stderr, "Warning: Unable to get hostname for HWP server: %s\n", strerror(errno));

      hwp_hostname[0] = '\0';  /* This is not a fatal error. */

    }

  /* Create the socket */

  hwp_server_fd = socket (servinfo->ai_family, servinfo->ai_socktype, servinfo->ai_protocol);

  if (hwp_server_fd < 0)

    {

      fprintf (stderr, "Error: HWP could not create server socket: %s\n", strerror(errno));

      return;

    }

  /* Set this socket to reuse its address. */

  optval = 1;

  if (setsockopt(hwp_server_fd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof (optval)) == -1)

    {

      fprintf (stderr, "Cannot set SO_REUSEADDR option on server socket %d: %s\n", hwp_server_fd, strerror(errno));

      hwp_server_close();

      return;

    }

  /* Bind the socket to the local address */

  if (bind (hwp_server_fd, servinfo->ai_addr, servinfo->ai_addrlen) < 0)

    {

      fprintf (stderr, "Error: Unable to bind HWP server socket %d to port %d: %s\n", hwp_server_fd, portNum, strerror(errno));

      hwp_server_close();

      return;

    }

  /* Set us up as a server, with a maximum backlog of 1 connection */

  if (listen (hwp_server_fd, 1) < 0)

    {

      fprintf (stderr, "Warning: Unable to set HWP backlog on server socket %d to %d: %s\n", hwp_server_fd, 1, strerror(errno));

      hwp_server_close();

      return;

    }

  fprintf(stderr, "HWP server listening on host %s (%s), port %i, address family %s\n",

          hwp_hostname, hwp_ipstr, hwp_portnum, hwp_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(hwp_pipe_fds)) {  // pipe_fds[0] is for writing to monitor, [1] is to read from it

    fprintf(stderr, "HWP server failed to register with monitor thread, exiting");

    hwp_server_close();

    return;

  }

}

int hwp_server_start(void)

{

  hwp_server_running = 1;

  debug("Starting HWP server\n");

  // Create the HWP server thread

  if(pthread_create(&hwp_server_thread, NULL, hwp_server, NULL))

    {

      fprintf(stderr, "Failed to create HWP server thread!\n");

      return 0;

    }

  return 1;

}

int hwp_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.)

   ***/

  hwp_server_running = 0;

  hwp_server_close();

  return 1;

}

/*--------------------------------------------------------------------*/

/* Main server thread                                                 */

/*--------------------------------------------------------------------*/

void *hwp_server(void *arg)

{

  struct sockaddr_storage their_addr;

  struct timeval *tvp;

  fd_set  readset;

  socklen_t addr_size;

  int nfds, flags;

  int ret;

  char cmd;

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

  while(hwp_server_running)

    {

      /* Listen for an incoming connection */

      addr_size = sizeof their_addr;

      hwp_client_fd = accept(hwp_server_fd, (struct sockaddr *)&their_addr, &addr_size);

      if(hwp_client_fd == -1)

        {

          perror("Error in accept() in HWP server thread");

        }

      else

        {

          debug("HWP server got connection!\n");

          /* Do new client init stuff here */

          use_cached_dmr2 = 0;

          /* New client should be non-blocking. */

          flags = fcntl (hwp_client_fd, F_GETFL);

          if (flags < 0)

            {

              fprintf (stderr, "Warning: Unable to get flags for HWP client socket %d: %s\n", hwp_client_fd, strerror(errno));

              // Not really fatal.

            }

          else {

            flags |= O_NONBLOCK;

            if (fcntl (hwp_client_fd, F_SETFL, flags) < 0)

              {

                fprintf (stderr, "Warning: Unable to set flags for HWP client socket %d to 0x%08x: %s\n", hwp_client_fd, flags, strerror(errno));

                // Also not really fatal.

              }

          }

        }

      /* Send/receive data on the new connection for as long as it's valid */

      while(hwp_server_running && (hwp_client_fd != -1))

        {

          /* if target not running, block on data from client or monitor thread */

          /* if target running, just poll (don't block) */

          // if(hwp_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(hwp_client_fd, &readset);

          FD_SET(hwp_pipe_fds[1], &readset);

          nfds = hwp_client_fd;

          if(hwp_pipe_fds[1] > nfds) nfds = hwp_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("HWP thread got data\n");

              if(FD_ISSET(hwp_pipe_fds[1], &readset))

                {

                  ret = read(hwp_pipe_fds[1], &cmd, 1);

                  debug("HWP server got monitor status \'%c\' (0x%X)\n", cmd, cmd);

                  if(ret == 1)

                    {

                      if(cmd == 'H')

                        {

                          // Save state so we can tell client on request

                          hwp_target_is_running = 0;

                        }

                      else if(cmd == 'R')

                        {

                          // Save state so we can tell client on request

                          hwp_target_is_running = 1;

                          use_cached_dmr2 = 0;

                        }

                      else

                        {

                          fprintf(stderr, "HWP server got unknown monitor status \'%c\' (0x%X)\n", cmd, cmd);

                        }

                    }

                  else

                    {

                      fprintf(stderr, "HWP server failed to read from ready monitor pipe!\n");

                    }

                }  // if FD_ISSET(hwp_pipe_fds[1])

              if(FD_ISSET(hwp_client_fd, &readset))

                {

                  hwp_client_request();

                }

            }   // else if (ret != 0)

        }  /* while client connection is valid */

    } /* while(hwp_server_running) */

  hwp_client_close();

  return arg;  // unused

}

/*--------------------------------------------------------------------*/

/* Helper functions                                                   */

/*--------------------------------------------------------------------*/

void hwp_server_close(void)

{

  if (hwp_server_fd != -1)

    {

      close(hwp_server_fd);

      hwp_server_fd = -1;

    }

}

void hwp_client_close(void)

{

  if (hwp_client_fd != -1)

    {

      close (hwp_client_fd);

      hwp_client_fd = -1;

    }

}       /* hwp_client_close () */

void hwp_client_request(void)

{

  struct rsp_buf *buf = hwp_get_packet ();      /* Message sent to us */

  // Null packet means we hit EOF or the link was closed for some other

  // reason. Close the client and return

  if (NULL == buf)

    {

      hwp_client_close ();

      return;

    }

  debug("HWP Packet received %s: %d chars\n", buf->data, buf->len );

  switch (buf->data[0])

    {

    case '?':

      // Different meaning than RSP: in RSP, this always returns an 'S' packet.

      // here, we want to know running / stopped.

      if(hwp_target_is_running) {

        hwp_report_run();

      } else {

        hwp_report_stop();

      }

      return;

      /*

        case 'g':

        rsp_read_all_regs ();

        return;

        case 'G':

        rsp_write_all_regs (buf);

        return;

      */

    case 'p':

      /* Read a register */

      hwp_read_reg(buf);

      return;

    case 'P':

      /* Write a register */

      hwp_write_reg(buf);

      return;

      /*

        case 'q':

        // Any one of a number of query packets

        rsp_query (buf);

        return;

        case 'Q':

        // Any one of a number of set packets

        rsp_set (buf);

        return;

      */

    default:

      /* Unknown commands are ignored */

      fprintf (stderr, "Warning: Unknown HWP request %s\n", buf->data);

      return;

    }

}       /* hwp_client_request () */

/*---------------------------------------------------------------------------*/

/*!Get a packet from the GDB client

   Modeled on the stub version supplied with GDB. The data is in a static

   buffer. The data should be copied elsewhere if it is to be preserved across

   a subsequent call to get_packet().

   Unlike the reference implementation, we don't deal with sequence

   numbers. GDB has never used them, and this implementation is only intended

   for use with GDB 6.8 or later. Sequence numbers were removed from the RSP

   standard at GDB 5.0.

   @return  A pointer to the static buffer containing the data                */

/*---------------------------------------------------------------------------*/

struct rsp_buf *hwp_get_packet(void)

{

  static struct rsp_buf  buf;           /* Survives the return */

  /* Keep getting packets, until one is found with a valid checksum */

  while (1)

    {

      unsigned char  checksum;          /* The checksum we have computed */

      int            count;             /* Index into the buffer */

      int            ch;                /* Current character */

      /* Wait around for the start character ('$'). Ignore all other

         characters */

      ch = get_hwp_rsp_char(hwp_client_fd);

      while (ch != '$')

        {

          if (-1 == ch)

            {

              return  NULL;             /* Connection failed */

            }

          ch = get_hwp_rsp_char(hwp_client_fd);

        }

      /* Read until a '#' or end of buffer is found */

      checksum =  0;

      count    =  0;

      while (count < HWP_BUF_MAX - 1)

        {

          ch = get_hwp_rsp_char(hwp_client_fd);

          /* Check for connection failure */

          if (-1 == ch)

            {

              return  NULL;

            }

          /* If we hit a start of line char begin all over again */

          if ('$' == ch)

            {

              checksum =  0;

              count    =  0;

              continue;

            }

          /* Break out if we get the end of line char */

          if ('#' == ch)

            {

              break;

            }

          /* Update the checksum and add the char to the buffer */

          checksum        = checksum + (unsigned char)ch;

          buf.data[count] = (char)ch;

          count           = count + 1;

        }

      /* Mark the end of the buffer with EOS - it's convenient for non-binary

         data to be valid strings. */

      buf.data[count] = 0;

      buf.len         = count;

      /* If we have a valid end of packet char, validate the checksum */

      if ('#' == ch)

        {

          unsigned char  xmitcsum;      /* The checksum in the packet */

          ch = get_hwp_rsp_char(hwp_client_fd);

          if (-1 == ch)

            {

              return  NULL;             /* Connection failed */

            }

          xmitcsum = hwp_hex(ch) << 4;

          ch = get_hwp_rsp_char(hwp_client_fd);

          if (-1 == ch)

            {

              return  NULL;             /* Connection failed */

            }

          xmitcsum += hwp_hex(ch);

          /* If the checksums don't match print a warning, and put the

             negative ack back to the client. Otherwise put a positive ack. */

          if (checksum != xmitcsum)

            {

              fprintf (stderr, "Warning: Bad HWP RSP checksum: Computed "

                       "0x%02x, received 0x%02x\n", checksum, xmitcsum);

              put_hwp_rsp_char (hwp_client_fd, '-');    /* Failed checksum */

            }

          else

            {

              put_hwp_rsp_char (hwp_client_fd, '+');    /* successful transfer */

              break;

            }

        }

      else

        {

          fprintf (stderr, "Warning: HWP RSP packet overran buffer\n");

        }

    }

  return &buf;                          /* Success */

}       /* hwp_get_packet () */

/*---------------------------------------------------------------------------*/

/*Single character get/set routines                                          */

/*---------------------------------------------------------------------------*/

void put_hwp_rsp_char(int fd, char  c)

{

  if (-1 == fd)

    {

      fprintf (stderr, "Warning: Attempt to write '%c' to unopened HWP RSP client: Ignored\n", c);

      return;

    }

  /* Write until successful (we retry after interrupts) or catastrophic

     failure. */

  while (1)

    {

      switch (write(fd, &c, sizeof (c)))

        {

        case -1:

          /* Error: only allow interrupts or would block */

          if ((EAGAIN != errno) && (EINTR != errno))

            {

              fprintf (stderr, "Warning: Failed to write to HWP RSP client: Closing client connection: %s\n",

                       strerror (errno));

              hwp_client_close();

              return;

            }

          break;

        case 0:

          break;                /* Nothing written! Try again */

        default:

          return;               /* Success, we can return */

        }

    }

}       /* put_hwp_rsp_char() */

int get_hwp_rsp_char(int fd)

{

  unsigned char  c;             /* The character read */

  if (-1 == fd)

    {

      fprintf (stderr, "Warning: Attempt to read from unopened HWP RSP client: Ignored\n");

      return  -1;

    }

  /* Read until successful (we retry after interrupts) or catastrophic

     failure. */

  while (1)

    {

      switch (read (fd, &c, sizeof (c)))

        {

        case -1:

          /* Error: only allow interrupts or would block */

          if ((EAGAIN != errno) && (EINTR != errno))

            {

              fprintf (stderr, "Warning: Failed to read from HWP RSP client: Closing client connection: %s\n",

                       strerror (errno));

              hwp_client_close();

              return  -1;

            }

          break;

        case 0:

          // EOF

          hwp_client_close();

          return  -1;

        default:

          return  c & 0xff;     /* Success, we can return (no sign extend!) */

        }

    }

}       /* get_hwp_rsp_char() */

/*---------------------------------------------------------------------------*/

/*!Read a single register

   The registers follow the GDB sequence for OR1K: GPR0 through GPR31, PC

   (i.e. SPR NPC) and SR (i.e. SPR SR). The register is returned as a

   sequence of bytes in target endian order.

   Each byte is packed as a pair of hex digits.

   @param[in] buf  The original packet request. Reused for the reply.        */

/*---------------------------------------------------------------------------*/

void hwp_read_reg(struct rsp_buf *buf)

{

  unsigned int  regnum;

  uint32_t tmp;

  unsigned int errcode = APP_ERR_NONE;

  /* Break out the fields from the data */

  if (1 != sscanf (buf->data, "p%x", &regnum))

    {

      fprintf (stderr, "Warning: Failed to recognize HWP RSP read register command: \'%s\'\n", buf->data);

      hwp_put_str_packet (hwp_client_fd, "E01");

      return;

    }

  if((regnum == SPR_DMR2) && use_cached_dmr2)  // Should we use the cached DMR2 value?

    {

      tmp = cached_dmr2;

      errcode = APP_ERR_NONE;

      fprintf(stderr, "Using cached DMR2 value 0x%X\n", tmp);

    }

  else

    {

      /* Get the relevant register.  We assume the client is not GDB,

       * and that no register number translation is needed.

       */

      errcode = dbg_cpu0_read(regnum, &tmp);

    }

  if(errcode == APP_ERR_NONE) {

    hwp_reg2hex(tmp, buf->data);

    buf->len = strlen (buf->data);

    debug("Read reg 0x%x, got %s (0x%X), len %i\n", regnum, buf->data, tmp, buf->len);

    hwp_put_packet (hwp_client_fd, buf);

  }

  else {

    fprintf(stderr, "Error reading HWP register: %s\n", get_err_string(errcode));