ArSocket.cpp

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArSocket.h"
#include "ArLog.h"


void ArSocket::internalInit(void)
{
  myCloseFunctor = NULL;
  myStringAutoEcho = true;
  myStringEcho = false;
  myStringPosLast = 0;
  myStringPos = 0;
  myStringGotComplete = false;
  myStringBufEmpty[0] = '\0';
  myStringGotEscapeChars = false;
  myStringHaveEchoed = false;
  sprintf(myIPString, "none");
  resetTracking();
}

AREXPORT int ArSocket::sendTo(const void *msg, int len)
{
  int ret;
  ret = ::sendto(myFD, (char*)msg, len, 0, (struct sockaddr*)&mySin,
          sizeof(mySin));
  if (ret > 0)
  {
    mySends++;
    myBytesSent += ret;
  }
  return ret;
}

AREXPORT int ArSocket::sendTo(const void *msg, int len, 
                  struct sockaddr_in *sin)
{ 
  int ret;
  ret = ::sendto(myFD, (char*)msg, len, 0, (struct sockaddr*)sin,
          sizeof(struct sockaddr_in));
  if (ret > 0)
  {
    mySends++;
    myBytesSent += ret;
  }
  return ret;
}


AREXPORT int ArSocket::recvFrom(void *msg, int len, sockaddr_in *sin)
{

#ifdef WIN32
  int i=sizeof(sockaddr_in);
#else
  socklen_t i=sizeof(sockaddr_in);
#endif
  int ret;
  ret = ::recvfrom(myFD, (char*)msg, len, 0, (struct sockaddr*)sin, &i);
  if (ret > 0)
  {
    myRecvs++;
    myBytesRecvd += ret;
  }
  return ret;
}

AREXPORT int ArSocket::write(const void *buff, size_t len)
{

  if (myFD < 0)
  {
    ArLog::log(ArLog::Terse, "ArSocket::write: called after socket closed");
    return 0;
  }

  struct timeval tval;
  fd_set fdSet;
  tval.tv_sec = 0;
  tval.tv_usec = 0;
  FD_ZERO(&fdSet);
  FD_SET(myFD, &fdSet);
  

  if (select(myFD + 1, NULL, &fdSet, NULL, &tval) <= 0)
    return 0;
  
  int ret;
#ifdef WIN32
  ret = ::send(myFD, (char*)buff, len, 0);
#else
  ret = ::write(myFD, (char*)buff, len);
#endif

  if (ret > 0)
  {
    mySends++;
    myBytesSent += ret;
  }
  return ret;
}

AREXPORT int ArSocket::read(void *buff, size_t len, unsigned int msWait)
{
  if (myFD < 0)
  {
    ArLog::log(ArLog::Terse, "ArSocket::read: called after socket closed");
    return 0;
  }

  int ret;
  if (msWait != 0)
  {
    struct timeval tval;
    fd_set fdSet;
    tval.tv_sec = msWait / 1000;
    tval.tv_usec = (msWait % 1000) * 1000;
    FD_ZERO(&fdSet);
    FD_SET(myFD, &fdSet);
    if (select(myFD + 1, &fdSet, NULL, NULL, &tval) <= 0)
      return 0;
  }
  ret = ::recv(myFD, (char*)buff, len, 0);
  if (ret > 0)
  {
    myRecvs++;
    myBytesRecvd += ret;
  }
  return ret;
}



/*
   This cannot write more than 512 number of bytes
   @param str the string to write to the socket
   @return number of bytes written
**/
AREXPORT int ArSocket::writeString(const char *str, ...)
{
  char buf[1200];
  int len;
  int ret;
  myWriteStringMutex.lock();
  va_list ptr;
  va_start(ptr, str);
  vsnprintf(buf, sizeof(buf), str, ptr);
  va_end(ptr);
  if (myLogWriteStrings)
    ArLog::log(ArLog::Normal, "Sent to %s: %s", getIPString(), buf);
  len = strlen(buf);
  buf[len] = '\n';
  len++;
  buf[len] = '\r';
  len++;
  ret = write(buf, len);
  myWriteStringMutex.unlock();
  return ret;
}

void ArSocket::setIPString(void)
{
  unsigned char *bytes;
  bytes = (unsigned char *)inAddr();
  if (bytes != NULL)
    sprintf(myIPString, "%d.%d.%d.%d", bytes[0], bytes[1], bytes[2], bytes[3]);
}


AREXPORT char *ArSocket::readString(void)
{
  size_t i;
  int n;

  myReadStringMutex.lock();
  myStringBufEmpty[0] = '\0';
  
  // read one byte at a time
  for (i = myStringPos; i < sizeof(myStringBuf); i++)
  {
    
    n = read(&myStringBuf[i], 1);
    if (n > 0)
    {
      if (i == 0 && myStringBuf[i] < 0)
      {
    myStringGotEscapeChars = true;
      }
      if (myStringBuf[i] == '\n' || myStringBuf[i] == '\r')
      {
    if (i != 0)
      myStringGotComplete = true;
    myStringBuf[i] = '\0';
    myStringPos = 0;
    myStringPosLast = 0;
    // if we have leading escape characters get rid of them
    if (myStringBuf[0] < 0)
    {
      int ei;
      myStringGotEscapeChars = true;
      // increment out the escape chars
      for (ei = 0; 
           myStringBuf[ei] < 0 || (ei > 0 && myStringBuf[ei - 1] < 0); 
           ei++);
      // okay now return the good stuff
      doStringEcho();
      myReadStringMutex.unlock();
      return &myStringBuf[ei];
    }
    // if we don't return what we got
    doStringEcho();
    myReadStringMutex.unlock();
    return myStringBuf;
      }
      // if its not an ending character but was good keep going
      else
    continue;
    }
    // failed
    else if (n == 0)
    {
      myReadStringMutex.unlock();
      return NULL;
    }
    else // which means (n < 0)
    {
#ifdef WIN32
      if (WSAGetLastError() == WSAEWOULDBLOCK)
      {
    myStringPos = i;
    doStringEcho();
    myReadStringMutex.unlock();
    return myStringBufEmpty;
      }
#endif
#ifndef WIN32
      if (errno == EAGAIN)
      {
    myStringPos = i;
    doStringEcho();
    myReadStringMutex.unlock();
    return myStringBufEmpty;
      }
#endif
      perror("Error in reading from network");
      myReadStringMutex.unlock();
      return NULL;
    }
  }
  // if they want a 0 length string
  ArLog::log(ArLog::Normal, "Some trouble in ArSocket::readString to %s", getIPString());
  writeString("String too long");
  myReadStringMutex.unlock();
  return NULL;
}

void ArSocket::doStringEcho(void)
{
  size_t to;

  if (!myStringAutoEcho && !myStringEcho)
    return;

  // if we're echoing complete thel ines
  if (myStringHaveEchoed && myStringGotComplete)
  {
    write("\n\r", 2);
    myStringGotComplete = false;
  }

  // if there's nothing to send we don't need to send it
  if (myStringPosLast == myStringPos)
    return;
  
  // we probably don't need it if its doing escape chars
  if (myStringAutoEcho && myStringGotEscapeChars)
    return;

  myStringHaveEchoed = true;
  to = strchr(myStringBuf, '\0') - myStringBuf;
  write(&myStringBuf[myStringPosLast], myStringPos - myStringPosLast);
  myStringPosLast = myStringPos;
}

---