source: flair-src/trunk/lib/FlairCore/src/ui_com.cpp@ 441

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2011/05/01
//  filename:   ui_com.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    classe permettant la lecture et l'ecriture sur socket UDT
//
//
/*********************************************************************/

#include "ui_com.h"
#include <cstdlib>
#include <string.h>
#include <unistd.h>
#include "Mutex.h"
#include "SendData.h"
#include "communication.h"
#include "FrameworkManager.h"
#include "zlib.h"
#include <assert.h>
#include "config.h"

#ifdef __XENO__
#include <pthread.h>
#include <native/task.h>
#include <native/task.h>
#include <fcntl.h>
#endif

using std::string;
using namespace flair::core;
using namespace flair::gui;

ui_com::ui_com(const Object *parent, UDTSOCKET sock)
    : Thread(parent, "send", 2,16384*2) {
  // buffer envoi
  send_buffer = (char *)malloc(3);
  send_size = 3; // id(1)+period(2)

  // mutex
  send_mutex = NULL;
  send_mutex = new Mutex(this, ObjectName());

  socket_fd = sock;
  connection_lost = false;

#ifdef __XENO__
  int status;
  string tmp_name;

  is_running = true;

  // pipe
  tmp_name = getFrameworkManager()->ObjectName() + "-" + ObjectName() + "-pipe";
  // xenomai limitation
  if (tmp_name.size() > 31)
    Err("rt_pipe_create error (%s is too long)\n", tmp_name.c_str());
#ifdef RT_PIPE_SIZE
  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
#else
  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, 0);
#endif

  if (status != 0) {
                char errorMsg[256];
    Err("rt_pipe_create error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
    // return -1;
  }

// start user side thread
#ifdef NRT_STACK_SIZE
  // Initialize thread creation attributes
  pthread_attr_t attr;
  if (pthread_attr_init(&attr) != 0) {
    Err("pthread_attr_init error\n");
  }

  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
    Err("pthread_attr_setstacksize error\n");
  }

  if (pthread_create(&thread, &attr, user_thread, (void *)this) < 0)
#else  // NRT_STACK_SIZE
  if (pthread_create(&thread, NULL, user_thread, (void *)this) < 0)
#endif // NRT_STACK_SIZE
  {
    Err("pthread_create error\n");
    // return -1;
  }
#ifdef NRT_STACK_SIZE
  if (pthread_attr_destroy(&attr) != 0) {
    Err("pthread_attr_destroy error\n");
  }
#endif

#endif //__XENO__
  int timeout = 100;
  if (UDT::setsockopt(socket_fd, 0, UDT_RCVTIMEO, &timeout, sizeof(int)) != 0)
    Err("UDT::setsockopt error (UDT_RCVTIMEO)\n");
    /*
  timeout=-1;
  if (UDT::setsockopt(socket_fd, 0, UDT_SNDTIMEO, &timeout, sizeof(int)) != 0)
    Err("UDT::setsockopt error (UDT_SNDTIMEO)\n");
*/
  bool blocking = true;
  if (UDT::setsockopt(socket_fd, 0, UDT_SNDSYN, &blocking, sizeof(bool)) != 0)
    Err("UDT::setsockopt error (UDT_SNDSYN)\n");

  if (UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
    Err("UDT::setsockopt error (UDT_RCVSYN)\n");
  //#endif //__XENO__

  Start();
}

ui_com::~ui_com() {
//Printf("destruction ui_com\n");

#ifdef __XENO__
  is_running = false;

  pthread_join(thread, NULL);

  int status = rt_pipe_delete(&pipe);
  if (status != 0) {
                char errorMsg[256];
    Err("rt_pipe_delete error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
        }
#endif

  SafeStop();

  if (IsSuspended() == true)
    Resume();

  Join();
  
  char buf=CLOSING_CONNECTION;
  Send(&buf,1);
 
  if (send_buffer != NULL)
    free(send_buffer);
  send_buffer = NULL;

  //Printf("destruction ui_com ok\n");
}

//send, public part; dispatch to SendNRT (nrt) or rt_pipe (rt)
//rt_pipe is read by user thread which calls SendNRT
void ui_com::Send(char *buf, ssize_t size,int ttl) {
  if (connection_lost == true)
    return;

  char *tosend = buf;
  if (buf[0] == XML_HEADER) {
    // cut xml header
    tosend = strstr(buf, "<root");
    size -= tosend - buf;
  }

#ifdef __XENO__
  //data buf
  ssize_t nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
  if (nb_write < 0) {
                char errorMsg[256];
    Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
  } else if (nb_write != size) {
    Err("rt_pipe_write error %i/%i\n", nb_write, size);
  }
  //ttl
  nb_write = rt_pipe_write(&pipe, &ttl, sizeof(ttl), P_NORMAL);
  if (nb_write < 0) {
                char errorMsg[256];
    Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
  } else if (nb_write != sizeof(ttl)) {
    Err("rt_pipe_write error %i/%i\n", nb_write, size);
  }
#else //__XENO__
  SendNRT(tosend,size,ttl);
#endif //__XENO__
}

//send, private part; called to effectively send to udt
//this is always in nrt
void ui_com::SendNRT(char *buf, ssize_t size,int ttl) {
  ssize_t nb_write;
  bool sendItCompressed = false;
 
#ifdef COMPRESS_FRAMES
  if (buf[0] == XML_HEADER) {
    sendItCompressed = true;
  }
#endif // COMPRESS_FRAMES

  if (sendItCompressed) {
    char *out;
    ssize_t out_size;
    if (compressBuffer(buf, size, &out, &out_size, 9) == Z_OK) {
      size = out_size;
      nb_write = UDT::sendmsg(socket_fd, out, size,ttl, true);
      free(out);
    } else {
      Warn("Compress error, sending it uncompressed\n");
      sendItCompressed = false;
    }
  }

  if (!sendItCompressed) {
    nb_write = UDT::sendmsg(socket_fd, buf, size, ttl, true);
  }

   //Printf("write %i %i\n",nb_write,size);
  if (nb_write < 0) {
    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
        UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
      connection_lost = true;
    }
  } else if (nb_write != size) {
    Err("%s, code %i (%ld/%ld)\n", UDT::getlasterror().getErrorMessage(),
        UDT::getlasterror().getErrorCode(), nb_write, size);
  }
}

ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
  ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);

  if (bytesRead < 0) {
    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST) {
      Err("UDT::recvmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
      connection_lost = true;
    }
  }

  return bytesRead;
}

void ui_com::CheckConnection(void) {
  int32_t val,len;
  
  int result=UDT::getsockopt(socket_fd,0,UDT_STATE,&val,&len); 
   if (result < 0) {
    Printf("UDT::getsockopt error (%s)\n", UDT::getlasterror().getErrorMessage());
   }
 // printf("opt: %i %i %i\n",result,val,len);
}

//todo: le run (rt) push les données une par une 
//le nrt recupere et decide quand envoyer :
// -quand toutes les données d'une meme periode sont recues
// -quand on a atteind le nb buffering
// -le nrt alloue dynamiquement le buffer d'envoi
//-> enelver l'alloc du buffer au startup (UpdateDataToSendSize)
void ui_com::Run(void) {
  // check endianness
  char header;
  if (IsBigEndian()) {
    header = DATA_BIG_ENDIAN;
    Printf("System is big endian\n");
  } else {
    header = DATA_LITTLE_ENDIAN;
    Printf("System is little endian\n");
  }

#ifdef __XENO__
  WarnUponSwitches(true);
  printf("\n"); // a revoir pourquoi??
// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
#endif
  // on attend d'avoir des choses à faire
  Suspend();

  while (!ToBeStopped()) {
    size_t resume_id;
    Time min = 0xffffffffffffffffULL;

    // on recpuere l'id de la prochaine execution
    send_mutex->GetMutex();
    resume_id = resumeTimes.size();
    for (size_t i = 0; i < resumeTimes.size(); i++) {
      if (resumeTimes.at(i) < min && datasToSend.at(i)->IsEnabled() == true) {
        min = resumeTimes.at(i);
        resume_id = i;
      }
    }

    // attente
    if (resume_id < resumeTimes.size()) {
      Time time = resumeTimes.at(resume_id);
      uint16_t resume_period = datasToSend.at(resume_id)->SendPeriod();
      send_mutex->ReleaseMutex();
      // on dort jusqu'a la prochaine execution
      SleepUntil(time);

      // envoi des donnees
      int offset = 3;
      send_buffer[0] = header;
      send_mutex->GetMutex();

      for (size_t i = 0; i < datasToSend.size(); i++) {
        if (datasToSend.at(i)->SendPeriod() == resume_period && datasToSend.at(i)->IsEnabled() == true) {
          datasToSend.at(i)->CopyDatas(send_buffer + offset);
          offset += datasToSend.at(i)->SendSize();
        }
      }
      if (offset != 3) {
        memcpy(&send_buffer[1], &resume_period, sizeof(uint16_t));
        //printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
        // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
        // printf("\n");
        Send(send_buffer, offset,resume_period);
      }

      //test to push datas
      for (size_t i = 0; i < datasToSend.size(); i++) {
        if (datasToSend.at(i)->SendPeriod() == resume_period && datasToSend.at(i)->IsEnabled() == true) {
          PushDatasToSend(datasToSend.at(i));
        }
      }
      //end test
      
      // on planifie la prochaine execution
      for (size_t i = 0; i < datasToSend.size(); i++) {
        if (datasToSend.at(i)->SendPeriod() == resume_period) {
          resumeTimes.at(i) += datasToSend.at(i)->SendPeriod() * 1000000;
        }
      }
      send_mutex->ReleaseMutex();
      //Printf("%i %lld\n",resume_period,GetTime()/1000000);
    } else {
      send_mutex->ReleaseMutex();
      // rien a faire, suspend
      //Printf("rien a faire suspend\n");
      Suspend();
      //Printf("wake\n");
      // on planifie la prochaine execution
      Time time = GetTime();
      send_mutex->GetMutex();
      for (size_t i = 0; i < datasToSend.size(); i++) {
        resumeTimes.at(i) =
            time + (Time)datasToSend.at(i)->SendPeriod() * 1000000;
      }
      send_mutex->ReleaseMutex();
    }
  }
}


//called with send_mutex locked
//in nrt
//TODO: RT part has to put datas in pipe, then user thread call this
void ui_com::PushDatasToSend(const SendData *dataToSend) {
  PushedData_t* ptr=NULL;
  //check if we already have one buffer for this couple period/nb_buffering
  for (size_t i = 0; i < pushedDatas.size(); i++) {
      if (pushedDatas.at(i).period==dataToSend->SendPeriod() && pushedDatas.at(i).nb_buffering==dataToSend->NbBuffering()) {
          ptr=&pushedDatas.at(i);
        }
  }
  if(ptr==NULL) {
      Warn("no match in buffers\n");
      return;
  }
  
  dataToSend->CopyDatas(ptr->buf+ptr->actual_size);
  ptr->actual_size+=dataToSend->SendSize();
  Printf("nb buffered %i\n",ptr->actual_size);
  Printf("pushed size %i period %i nb buffering %i\n",dataToSend->SendSize(),dataToSend->SendPeriod(),dataToSend->NbBuffering());
  if(ptr->actual_size==ptr->final_size) {
      Printf("ready to send\n");
      //clean
      ptr->actual_size=0;
  }
}


#ifdef __XENO__
void *ui_com::user_thread(void *arg) {
  int pipe_fd = -1;
  string devname;
  char *buf = NULL;
  ui_com *caller = (ui_com *)arg;
  int rv;
  fd_set set;
  struct timeval timeout;
  ssize_t nb_read;
  int ttl;

  buf = (char *)malloc(NRT_PIPE_SIZE);
  if (buf == NULL) {
    caller->Err("malloc error\n");
  }

  devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
            caller->ObjectName() + "-pipe";
  while (pipe_fd < 0) {
    pipe_fd = open(devname.c_str(), O_RDWR);
    if (pipe_fd < 0 && errno != ENOENT) {
                        char errorMsg[256];
      caller->Err("open pipe_fd error (%s)\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
                }
    usleep(1000);
  }

  while (1) {
    FD_ZERO(&set);         // clear the set
    FD_SET(pipe_fd, &set); // add our file descriptor to the set

    timeout.tv_sec = 0;
    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;

    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);

    if (rv == -1) {
      if (caller->is_running == false && UDT::getlasterror().getErrorCode() == CUDTException::ETIMEOUT)
        break; // timeout
      if (UDT::getlasterror().getErrorCode() != CUDTException::ETIMEOUT)
        caller->Err("epoll_wait, %s, code %i\n",
                    UDT::getlasterror().getErrorMessage(), UDT::getlasterror().getErrorCode());
    } else if (rv == 0) {
      // printf("timeout\n"); // a timeout occured
      if (caller->is_running == false)
        break;

    } else {
      //read data buf and ttl
      //RT part has made 2 rt_pipe_write; they should be available in nrt
      //(select ensure at least one data is ready)
      nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
      buf[nb_read] = 0;
      read(pipe_fd, &ttl, sizeof(ttl));
// printf("envoi\n%s\n",buf);

      caller->SendNRT(buf, nb_read,ttl);
    }
  }

  close(pipe_fd);
  if (buf != NULL)
    free(buf);
  pthread_exit(0);
}
#endif

int ui_com::compressBuffer(char *in, ssize_t in_size, char **out,
                           ssize_t *out_size, int level) {
  int ret, flush;
  unsigned have;
  z_stream strm;

  /* allocate deflate state */
  strm.zalloc = Z_NULL;
  strm.zfree = Z_NULL;
  strm.opaque = Z_NULL;
  ret = deflateInit(&strm, level);
  if (ret != Z_OK)
    return ret;

  *out = (char *)malloc(COMPRESS_CHUNK);
  if (!(*out))
    return Z_BUF_ERROR;

  strm.next_in = (unsigned char *)in;
  strm.avail_out = COMPRESS_CHUNK;
  strm.next_out = (unsigned char *)*out;
  strm.avail_in = in_size;
  flush = Z_FINISH;

  ret = deflate(&strm, flush); /* no bad return value */
  if (ret == Z_STREAM_ERROR) {
    free(*out);
    return ret;
  }

  have = COMPRESS_CHUNK - strm.avail_out;
  *out_size = have;
  // printf("%i -> %i\n",in_size,have);
  /* clean up and return */
  (void)deflateEnd(&strm);

  if (strm.avail_out != 0) {
    return Z_OK;
  } else {
    return Z_STREAM_ERROR;
  }
}

int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,
                             unsigned char **out, ssize_t *out_size) {
  int ret;
  // unsigned have;
  z_stream strm;

  /* allocate inflate state */
  strm.zalloc = Z_NULL;
  strm.zfree = Z_NULL;
  strm.opaque = Z_NULL;
  strm.avail_in = 0;
  strm.next_in = Z_NULL;
  ret = inflateInit(&strm);
  if (ret != Z_OK)
    return ret;

  *out = (unsigned char *)malloc(COMPRESS_CHUNK);
  if (!(*out))
    return Z_BUF_ERROR;

  strm.avail_in = in_size;
  strm.next_in = in;
  strm.avail_out = COMPRESS_CHUNK;
  strm.next_out = *out;

  ret = inflate(&strm, Z_NO_FLUSH);
  assert(ret != Z_STREAM_ERROR); /* state not clobbered */
  switch (ret) {
  case Z_NEED_DICT:
    ret = Z_DATA_ERROR; /* and fall through */
  case Z_DATA_ERROR:
  case Z_MEM_ERROR:
    (void)inflateEnd(&strm);
    return ret;
  }
  // have = COMPRESS_CHUNK - strm.avail_out;

  /* clean up and return */
  (void)inflateEnd(&strm);
  return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
}

bool ui_com::ConnectionLost() { return connection_lost; }

void ui_com::AddSendData(const SendData *obj) {
  send_mutex->GetMutex();

  resumeTimes.push_back(GetTime() + (Time)obj->SendPeriod() * 1000000);

  // on resume en meme temps tout ceux qui ont la meme periode
  for (size_t i = 0; i < datasToSend.size(); i++) {
    if (datasToSend.at(i)->SendPeriod() == obj->SendPeriod()) {
      resumeTimes.at(resumeTimes.size() - 1) = resumeTimes.at(i);
      break;
    }
  }

  datasToSend.push_back(obj);

  send_mutex->ReleaseMutex();
}

//TODO: check if it is RT compatible
void ui_com::UpdateSendData(const SendData *obj) {
  // le mutex est deja pris par l'appellant
  size_t id, i;

  // on recupere l'id
  for (i = 0; i < datasToSend.size(); i++) {
    if (datasToSend.at(i) == obj) {
      id = i;
      break;
    }
  }

  // on resume en meme temps tout ceux qui ont la meme periode
  for (i = 0; i < datasToSend.size(); i++) {
    if (i == id)
      continue;
    if (datasToSend.at(i)->IsEnabled() == true && datasToSend.at(i)->SendPeriod() == obj->SendPeriod()) {
      resumeTimes.at(id) = resumeTimes.at(i);
      break;
    }
  }

  // si aucun match, on planifie l'execution
  if (i == datasToSend.size())
    resumeTimes.at(id) = GetTime() + (Time)obj->SendPeriod() * 1000000;

  if (IsSuspended() == true) Resume();

//update all buffers if necessary, discard datas
    std::vector<PushedData_t>::iterator pushedDatasIterator = pushedDatas.begin();
    while(pushedDatasIterator != pushedDatas.end()) {
        size_t bufSize=0;
        for (size_t i = 0; i < datasToSend.size(); i++) {
            if (datasToSend.at(i)->IsEnabled() && datasToSend.at(i)->SendPeriod() == (*pushedDatasIterator).period && datasToSend.at(i)->NbBuffering()==(*pushedDatasIterator).nb_buffering) {
                bufSize+=datasToSend.at(i)->SendSize();
            }
        }
        bufSize*=(*pushedDatasIterator).nb_buffering;
        if(bufSize!=(*pushedDatasIterator).final_size && bufSize!=0) {
            Printf("change buf size %i->%i\n",(*pushedDatasIterator).final_size,bufSize);
            (*pushedDatasIterator).actual_size=0;
            (*pushedDatasIterator).final_size=bufSize;
            free((*pushedDatasIterator).buf);
            (*pushedDatasIterator).buf=(char*)malloc(bufSize);
        }
        if(bufSize==0) {
            Printf("delete buf\n");
            free((*pushedDatasIterator).buf);
            pushedDatasIterator = pushedDatas.erase(pushedDatasIterator);
        } else {
            ++pushedDatasIterator;
        }
    }
  
  //check if we need a new buffer for this couple period/nb_buffering
  if(obj->IsEnabled()) {
      bool match=false;
      for (size_t i = 0; i < pushedDatas.size(); i++) {
          if (pushedDatas.at(i).period==obj->SendPeriod() && pushedDatas.at(i).nb_buffering==obj->NbBuffering()) {
              Printf("match item %i, period %i nb buff %i\n",i,obj->SendPeriod(),obj->NbBuffering());
              match=true;
            }
      }
      //create new one
      if(!match) {
        printf("no match\n");
        PushedData_t tmp;
        tmp.period=obj->SendPeriod();
        tmp.nb_buffering=obj->NbBuffering();
        tmp.actual_size=0;
        tmp.final_size=0;
        for (size_t i = 0; i < datasToSend.size(); i++) {
            if (datasToSend.at(i)->IsEnabled() && datasToSend.at(i)->SendPeriod() == obj->SendPeriod() && datasToSend.at(i)->NbBuffering()==obj->NbBuffering()) {
              tmp.final_size+=datasToSend.at(i)->SendSize();
            }
        }
        tmp.final_size*=obj->NbBuffering();
        printf("final size %i\n",tmp.final_size);
        tmp.buf=(char*)malloc(tmp.final_size);
        if(tmp.buf!=NULL) {
            pushedDatas.push_back(tmp);
        } else {
            Warn("could not allocate buffer of size %i\n",tmp.final_size);
        }
      }
  }
  Printf("nb buf %i\n",pushedDatas.size());
  return;
}

//allocate a buffer at startup for the worst case
//(every data send with same pedriod)
//TODO: dynamic size depending on what gcs asks?
void ui_com::UpdateDataToSendSize(void) {
  send_mutex->GetMutex();
  send_size = 3; // id(1)+period(2)
  for (size_t i = 0; i < datasToSend.size(); i++) {
    if (datasToSend[i] != NULL)
      send_size += datasToSend[i]->SendSize();
  }

  // send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
  if (send_buffer != NULL)
    free(send_buffer);
  send_buffer = (char *)malloc(send_size * sizeof(char));
  send_mutex->ReleaseMutex();
}

void ui_com::RemoveSendData(const SendData *obj) {
  // printf("remove_data_to_send %i\n",data_to_send.size());

  send_mutex->GetMutex();
  // on recupere l'id
  for (size_t i = 0; i < datasToSend.size(); i++) {
    if (datasToSend.at(i) == obj) {
      datasToSend.erase(datasToSend.begin() + i);
      resumeTimes.erase(resumeTimes.begin() + i);
      // printf("remove_data_to_send %i ok\n",data_to_send.size());
      break;
    }
  }
  send_mutex->ReleaseMutex();

  return;
}

void ui_com::Block(void) { send_mutex->GetMutex(); }

void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }