UDPClient.cc

#include "UDPClient.hh"

#ifdef WIN32
#  include <winsock.h>
#  include <io.h>
#else
#  include <unistd.h>
#  include <sys/socket.h>
#  include <sys/types.h>
#  include <arpa/inet.h>
#  include <netdb.h>
#endif

#include <cstdlib>
#include <string>
#include <cstring>
#include <iostream>
#include <cstdio>
#include "UDPServerClientDefinitions.hh"
#include "UDPDataConsumer.hh"
#include <pthread.h>
#include <Base/Debug/Error.hh>
#include <sys/types.h>
#include <Base/Common/W32Compat.hh>
#include <Base/Common/BIASpragma.hh>

using namespace std;
using namespace BIAS;

UDPClient::UDPClient(){
  TCPinsteadOfUDP_ = false;
  message_counter = 0;
  isMulticast_ = false;
  // Set up the local address
  local_socket_address_.sin_family = AF_INET;  // host byte order
  local_socket_address_.sin_port = htons(4000);// short, network byte order
  local_socket_address_.sin_addr.s_addr = INADDR_ANY;
  memset(&(local_socket_address_.sin_zero),0, 8);// zero the rest of the struct
  // The Client is not running yet
  is_active_ = false;
  has_ended_ = true;
  ServerAddr_ = NULL;
}


void UDPClient::Init(int port ,const char* bind_to_ip){
  TCPinsteadOfUDP_ = false;
  message_counter = 0;
  isMulticast_ = false;
  // Set up the local address
  local_socket_address_.sin_family = AF_INET;  // host byte order
  local_socket_address_.sin_port = htons(port);// short, network byte order
  if(bind_to_ip !=NULL)
    local_socket_address_.sin_addr.s_addr = inet_addr(bind_to_ip);
  else
    local_socket_address_.sin_addr.s_addr = INADDR_ANY;
  memset(&(local_socket_address_.sin_zero),0, 8);// zero the rest of the struct
  // The Client is not running yet
  is_active_ = false;
  has_ended_ = true;
  ServerAddr_ = NULL;
}


void UDPClient::create_buffer_structure(unsigned int numbuffers) 
{

  message_frame_buffer buf;
  buf.is_free = true;
  buf.number = -1;
  buf.frame_number = -1;
  buf.frame_size = -1;
  buf.frames_total = -1;

  for(unsigned int i=0; i < numbuffers; i++) {
    buf.frame_data = new char[UDP_PACKAGE_SIZE];
    Buffers_.push_back(buf);
  }
  // get one array of pointer to assemble completed message
  completed_message = new char*[numbuffers];

} // create_buffer_structure



void UDPClient::destroy_buffer_structure() 
{
  for(unsigned int i=0; i < Buffers_.size(); i++) 
    delete[] Buffers_[i].frame_data;
  Buffers_.clear();

  delete[] completed_message;
} // destroy_buffer_structure


void UDPClient::check_for_complete_message(int last_message_number_inserted, 
                                           int frames_total) \
{
  bool droppedMessage = false;

  int message_frames_so_far = 0;
  for (unsigned int i = 0; i < Buffers_.size(); i++) {
    if (Buffers_[i].number == last_message_number_inserted) 
      message_frames_so_far++;
    else if (!Buffers_[i].is_free){
      // if a non-free buffers has a message number other then the
      // expected one, it must be a frame of an older message which
      // has not been identified as completed earlier in this method.
      cerr<<"UDPClient: Dropped message ! found frame of message:"
          <<Buffers_[i].number<<" while expecting "
          <<last_message_number_inserted<<endl;
      droppedMessage = true;
    }
  }
  
  // remove frames of deprecated messages
  if (droppedMessage)
    for (unsigned int i = 0; i < Buffers_.size(); i++) 
      if (Buffers_[i].number != last_message_number_inserted) 
        Buffers_[i].is_free = true;

  // Is it enough for a complete message?
  if (message_frames_so_far == frames_total) {
    //     cout <<"  Message is complete"<<endl; 
    // We have a complete message!

    // Look for the buffers containing the data belonging to the actual
    // message number and put the addresses of the frame data into 
    // the array completed_message, which will be consumed by the 
    // UDP data consumer.

    int size_of_last_message_frame = -1;
    for (unsigned int i = 0; i < Buffers_.size(); i++) {
      if (Buffers_[i].number == last_message_number_inserted) {
        completed_message[Buffers_[i].frame_number] = (Buffers_[i].frame_data) + sizeof(message_frame_header);
        Buffers_[i].is_free = true;
        if (Buffers_[i].frame_number == frames_total-1)
          size_of_last_message_frame = Buffers_[i].frame_size;
      }
    }

    message_counter++;
    if (message_counter == 1000000000)
      message_counter = 0;
    //    cout << "*** Message " << message_counter << " received in " << frames_total << " frames.   \r";
    cout.flush();

    // Let the UDP data consumer proceed
    udc->consume_complete_message(frames_total, 
                                  completed_message,
                                  UDP_PACKAGE_SIZE-sizeof(message_frame_header),
                                  size_of_last_message_frame);
  }
  //   else cout <<"  Message is NOT complete: "<<message_frames_so_far<<"/"<<frames_total<<endl;
} // check_for_complete_message


/* Buffer management 

   Buffers_ is a vector of mesage_frame_buffer each with a pointer to
   data.  A message is split up into frames. Each frame is prepended
   with a header (message_frame_header). which contains a message-ID,
   the number of frames of this message, and the framenumber.

   The receiver call get_a_free_buffer(), places
   the next received frame in it and marks that buffer as !free.
   After each reception, the method check_for_complete_message() is
   called, which, of course, scans the buffer for a completed message.
   If on message is complete,  udp_data_comsumer() is called to
   assemble the message. All buffers from this mesage are freed by
   check_for _complete_message() regardless the implementation of
   udp_data_consumer.

   Thus, get_a_free_buffer() _never_ has to free a buffer. it only
   hast to search one that is marked as free. If no free buffer is
   available, this indicates that the amount of receive buffers is to
   small. So more buffers should be allocated and the user should be
   informed about that.

*/


int UDPClient::get_a_free_buffer() 
{  
  int return_value = -1;
  // Look for next free buffer
  for(unsigned int i=0; i < Buffers_.size(); i++) {
    // Is this buffer free?
    if ( Buffers_[i].is_free ) {
      return_value = i;
      break;
    }
  }
  
  // If we haven't found a free buffer
  if (return_value == -1) {
    const unsigned int newbuffers = 10;
    // get the index of the first new element
    return_value = (int)Buffers_.size();
    BIASERR("Ran out of buffers, increasing number of buffers by "<<newbuffers
            <<"."<<endl
            <<"\tAvoid this by specifying an appropriate messagesize to "
            <<endl<<"\tUDPClient::start()");
    message_frame_buffer buf;
    buf.is_free = true;
    buf.number = -1;
    buf.frame_number = -1;
    buf.frame_size = -1;
    buf.frames_total = -1;
    for(unsigned int i=0; i < newbuffers; i++) {
      buf.frame_data = new char[UDP_PACKAGE_SIZE];
      Buffers_.push_back(buf);
    }

  }
  
  return return_value;
}



void* receive_loop_starter(void* udpclient) {
  UDPClient* c = (UDPClient*) udpclient;
  c->receive_loop();
  return NULL;
} // receive_loop_starter


void UDPClient::receive_loop() {

  //   cout << "UDPClient starts to listen for incoming packages." << endl;

  // The server socket address, to store the
  // address of the server from which a
  // package just was received
  struct sockaddr_in server_socket_address_;
  
  // The length of the socket address;
#if WIN32
  int address_length_ = sizeof(server_socket_address_);
#else
  socklen_t address_length_ = sizeof(server_socket_address_);
#endif
  int received_bytes = -1;
  int free_buffer_number = -1;

  has_ended_ = false;

  while (is_active_ ) {

    free_buffer_number = get_a_free_buffer();
    received_bytes = recvfrom(socket_fd_, 
                              Buffers_[free_buffer_number].frame_data,
                              UDP_PACKAGE_SIZE,
                              0,
                              (struct sockaddr *) &server_socket_address_,
                              &address_length_);

    if (ServerAddr_ ==NULL) {
      //   cout <<"addr len: "<<address_length_<<endl;
      //  cout <<"port: "<<ntohs(server_socket_address_.sin_port)<<endl;
      char *tmp = inet_ntoa(server_socket_address_.sin_addr);
      size_t len = strlen(tmp)+1;
      ServerAddr_ = new char[len];
      strncpy(ServerAddr_, tmp, len);
      //  cout <<"addr: "<<ServerAddr_<<" len: "<<len<<endl;
    }
    if (received_bytes < 0) {
      if (is_active_) { 
        syscallerror("Error receiving bytes. Aborting!");
        is_active_ = false;
      }
    } else {
      // obviously we received some data

      // extract the information from the message frame header
      message_frame_header* mfh = (message_frame_header*) Buffers_[free_buffer_number].frame_data;
      
      Buffers_[free_buffer_number].number = mfh->message_number;
      Buffers_[free_buffer_number].frame_number = mfh->message_frame_number;
      Buffers_[free_buffer_number].frame_size = received_bytes - sizeof(message_frame_header);
      Buffers_[free_buffer_number].frames_total = mfh->message_frames_total;


      //       cerr << "=======================" << endl;
      //       cerr << "package received! Size:" << received_bytes << endl;
      //       cerr << "message number: "        << mfh->message_number << endl;
      //       cerr << "message frame number: "  << mfh->message_frame_number << endl;
      //       cerr << "message frames total: "  << mfh->message_frames_total << endl;
      //       cerr << "=======================" << endl;


      // Did we receive enough bytes?
      if (mfh->message_frame_number < (mfh->message_frames_total-1) && received_bytes < UDP_PACKAGE_SIZE) {

        // It is not not the last message frame of a message, so it should
        // contain UDP_PACKAGE_SIZE bytes.  Doing nothing.
        cerr << "Warning: Received a truncated UDP frame, which has been discarded." << endl;
      } else {

        //
        // WARNING! CHECK FOR DUPLICATED UPD PACKAGES!!!
        //

        // Now we can analyse, wether we have enough frames for a complete message
        Buffers_[free_buffer_number].is_free = false;
        check_for_complete_message(mfh->message_number, mfh->message_frames_total);

        //  cerr << "package received! Size:" << received_bytes << endl;
        //  cerr << "message number: " << mfh->message_number << endl;
        //  cerr << "message frame number: " << mfh->message_frame_number << endl;
        //  cerr << "message frames total: " << mfh->message_frames_total << endl;
      }
    }
    
  } // while (is_active)

  has_ended_ = true;

} // receive_loop

/////////////////////////////////////////////////////////////////////
int UDPClient::start(int max_message_size, int number_of_message_buffers, 
                     UDPDataConsumer* uc, bool UseTCP)
{ 
  TCPinsteadOfUDP_ = UseTCP;
  udc = uc;
  
  // One message must be bufferd at least
  if (number_of_message_buffers < 1) 
    number_of_message_buffers = 1;

  // So how many UDP packages must be buffered?
  int temp = (int) (max_message_size / UDP_PACKAGE_SIZE + 1);
  if (temp <1) 
    temp = 1;

  create_buffer_structure(number_of_message_buffers * temp);


  // Create a UDP socket
#ifdef WIN32
  WORD wVersionRequested;
  WSADATA wsaData;
  int err;
  wVersionRequested = MAKEWORD( 1, 1 );
  err = WSAStartup( wVersionRequested, &wsaData );
  if ( err != 0 ) {
    cout <<"Error on WSAStrtup(): "<<err<<endl;
  }   
  if (TCPinsteadOfUDP_)
    socket_fd_ = socket(PF_INET, SOCK_STREAM, 0);
  else
    socket_fd_ = socket(PF_INET, SOCK_DGRAM, 0);
  if (socket_fd_ == INVALID_SOCKET) WSAGetLastError();
#else
  if (TCPinsteadOfUDP_)
    socket_fd_ = socket(AF_INET, SOCK_STREAM, 0);
  else
    socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
#endif
  if ( socket_fd_ < 0 ) {
    BIASERR("Could not create a socket. Aborting!");
    return -1;
  }
  
  // Bind the socket to socket address
  if (bind(socket_fd_,
           (struct sockaddr *) &local_socket_address_,
           sizeof(struct sockaddr)) < 0 )
    {
    BIASERR("Could not bind to socket. Aborting!");
    return -1;
  }
  
  
  //
  // Is the data transmitted via multicast?
  //
  if (isMulticast_) {
    
    cout << "Joining Multicast group - to be implemented" << endl;
    
    //       struct  ip_mreq mreq;      
    //       mreq.imr_multiaddr.s_addr=inet_addr(ipaddress_);
    //       mreq.imr_interface.s_addr=htonl(INADDR_ANY);
    //       if (setsockopt(socket_fd_, IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq)) < 0) {
    //  syscallerror("Could not join multicast group. Aborting!");
    //  return -1;
    //       }    
  }
  
  
  //
  // Now, the socket is ready and the buffer structure is built
  // up, so we can create a thread
  //
  if (TCPinsteadOfUDP_) {
    int res = listen(socket_fd_,5);
    if (res<0) {
      perror("Failed to listen");
      return -1;
    }
    else {
      struct sockaddr tmpaddr;
#ifdef WIN32
      int len=sizeof(tmpaddr);
#else
      socklen_t len=sizeof(tmpaddr);
#endif
      accept(socket_fd_,&tmpaddr,&len);
    }
  }

  is_active_ = true;
  pthread_t receive_thread;
  if (pthread_create(&receive_thread, NULL, receive_loop_starter, (void*) this) !=0 ) {
    syscallerror("Could not create a thread.");   
    return -1;
    is_active_ = false;
  } else {
    return 0;
  }

} // start



void UDPClient::stop() {

  // Leave threaded receive loop
  is_active_ = false;

  // Close socket to unblock recvfrom
#ifdef WIN32
  closesocket(socket_fd_);
#else
  close(socket_fd_);
#endif 

  // Wait until thread has ended
  int i=0;
  while (!has_ended_ && i<1000) {
#ifdef WIN32
    Sleep(1);
#else
    usleep(1000);
#endif
    i++;
  }

#ifdef WIN32
  WSACleanup();
#endif 

  destroy_buffer_structure();
} // stop


UDPClient::~UDPClient() {
  is_active_ = false;
}