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

Last change on this file since 243 was 243, checked in by Sanahuja Guillaume, 4 years ago

resolve some bugs when closing connection with gcs

File size: 15.5 KB
RevLine 
[2]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[2]4// %flair:license}
5//  created:    2011/05/01
6//  filename:   ui_com.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    classe permettant la lecture et l'ecriture sur socket UDT
14//
15//
16/*********************************************************************/
17
18#include "ui_com.h"
19#include <cstdlib>
20#include <string.h>
21#include <unistd.h>
22#include "Mutex.h"
23#include "SendData.h"
24#include "communication.h"
25#include "FrameworkManager.h"
26#include "zlib.h"
27#include <assert.h>
28#include "config.h"
29
30#ifdef __XENO__
31#include <pthread.h>
32#include <native/task.h>
33#include <native/task.h>
34#include <fcntl.h>
35#endif
36
37using std::string;
38using namespace flair::core;
39using namespace flair::gui;
40
[15]41ui_com::ui_com(const Object *parent, UDTSOCKET sock)
[213]42    : Thread(parent, "send", 2,16384*2) {
[15]43  // buffer envoi
44  send_buffer = (char *)malloc(3);
45  send_size = 3; // id(1)+period(2)
[2]46
[15]47  // mutex
48  send_mutex = NULL;
49  send_mutex = new Mutex(this, ObjectName());
[2]50
[15]51  socket_fd = sock;
52  connection_lost = false;
[2]53
54#ifdef __XENO__
[15]55  int status;
56  string tmp_name;
[2]57
[15]58  is_running = true;
[2]59
[15]60  // pipe
61  tmp_name = getFrameworkManager()->ObjectName() + "-" + ObjectName() + "-pipe";
62  // xenomai limitation
63  if (tmp_name.size() > 31)
64    Err("rt_pipe_create error (%s is too long)\n", tmp_name.c_str());
[2]65#ifdef RT_PIPE_SIZE
[15]66  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
[2]67#else
[15]68  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, 0);
[2]69#endif
70
[15]71  if (status != 0) {
[133]72                char errorMsg[256];
73    Err("rt_pipe_create error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
[15]74    // return -1;
75  }
[2]76
[15]77// start user side thread
[2]78#ifdef NRT_STACK_SIZE
[15]79  // Initialize thread creation attributes
80  pthread_attr_t attr;
81  if (pthread_attr_init(&attr) != 0) {
82    Err("pthread_attr_init error\n");
83  }
[2]84
[15]85  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
86    Err("pthread_attr_setstacksize error\n");
87  }
[2]88
[15]89  if (pthread_create(&thread, &attr, user_thread, (void *)this) < 0)
90#else  // NRT_STACK_SIZE
91  if (pthread_create(&thread, NULL, user_thread, (void *)this) < 0)
92#endif // NRT_STACK_SIZE
93  {
94    Err("pthread_create error\n");
95    // return -1;
96  }
[2]97#ifdef NRT_STACK_SIZE
[15]98  if (pthread_attr_destroy(&attr) != 0) {
99    Err("pthread_attr_destroy error\n");
100  }
[2]101#endif
102
[15]103#endif //__XENO__
104  int timeout = 100;
105  if (UDT::setsockopt(socket_fd, 0, UDT_RCVTIMEO, &timeout, sizeof(int)) != 0)
106    Err("UDT::setsockopt error (UDT_RCVTIMEO)\n");
[2]107
[15]108  bool blocking = true;
109  if (UDT::setsockopt(socket_fd, 0, UDT_SNDSYN, &blocking, sizeof(bool)) != 0)
110    Err("UDT::setsockopt error (UDT_SNDSYN)\n");
[2]111
[15]112  if (UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
113    Err("UDT::setsockopt error (UDT_RCVSYN)\n");
114  //#endif //__XENO__
[2]115
[15]116  Start();
[2]117}
118
[15]119ui_com::~ui_com() {
[243]120//Printf("destruction ui_com\n");
[2]121
122#ifdef __XENO__
[15]123  is_running = false;
[2]124
[15]125  pthread_join(thread, NULL);
[2]126
[15]127  int status = rt_pipe_delete(&pipe);
[133]128  if (status != 0) {
129                char errorMsg[256];
130    Err("rt_pipe_delete error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
131        }
[2]132#endif
133
[15]134  SafeStop();
[2]135
[15]136  if (IsSuspended() == true)
137    Resume();
[2]138
[15]139  Join();
[2]140
[15]141  if (send_buffer != NULL)
142    free(send_buffer);
143  send_buffer = NULL;
[2]144
[243]145  //Printf("destruction ui_com ok\n");
[2]146}
147
[15]148void ui_com::Send(char *buf, ssize_t size) {
149  if (connection_lost == true)
150    return;
[2]151
[15]152  char *tosend = buf;
153  ssize_t nb_write;
[2]154
[15]155  if (buf[0] == XML_HEADER) {
156    // cut xml header
157    tosend = strstr(buf, "<root");
158    size -= tosend - buf;
159  }
[2]160
161#ifdef __XENO__
[15]162  nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
[2]163
[15]164  if (nb_write < 0) {
[133]165                char errorMsg[256];
166    Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
[15]167  } else if (nb_write != size) {
168    Err("rt_pipe_write error %i/%i\n", nb_write, size);
169  }
[2]170#else //__XENO__
171
172#ifdef COMPRESS_FRAMES
[15]173  bool sendItCompressed = false;
174  if (buf[0] == XML_HEADER) {
175    sendItCompressed = true;
176  }
[2]177
[15]178  if (sendItCompressed) {
179    char *out;
180    ssize_t out_size;
181    if (compressBuffer(tosend, size, &out, &out_size, 9) == Z_OK) {
182      size = out_size;
183      nb_write = UDT::sendmsg(socket_fd, out, size, -1, true);
184      free(out);
185    } else {
186      Warn("Compress error, sending it uncompressed\n");
187      sendItCompressed = false;
[2]188    }
[15]189  }
[2]190
[15]191  if (!sendItCompressed) {
192    nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
193  }
[2]194
[15]195#else // COMPRESS_FRAMES
196  nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
197#endif // COMPRESS_FRAMES
198  // Printf("write %i %i\n",nb_write,size);
199  if (nb_write < 0) {
200    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
201    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
202        UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
203      connection_lost = true;
[2]204    }
[15]205  } else if (nb_write != size) {
206    Err("%s, code %i (%ld/%ld)\n", UDT::getlasterror().getErrorMessage(),
207        UDT::getlasterror().getErrorCode(), nb_write, size);
208  }
[2]209#endif //__XENO__
210}
211
[15]212ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
213  ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);
[2]214
[15]215  if (bytesRead < 0) {
216    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST) {
217      Err("UDT::recvmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
218      connection_lost = true;
[2]219    }
[15]220  }
[2]221
[15]222  return bytesRead;
[2]223}
224
[234]225void ui_com::CheckConnection(void) {
226  int32_t val,len;
227 
228  int result=UDT::getsockopt(socket_fd,0,UDT_STATE,&val,&len); 
229   if (result < 0) {
230    Printf("UDT::getsockopt error (%s)\n", UDT::getlasterror().getErrorMessage());
231   }
232 // printf("opt: %i %i %i\n",result,val,len);
233}
234
[15]235void ui_com::Run(void) {
236  // check endianness
237  char header;
238  if (IsBigEndian()) {
239    header = DATAS_BIG_ENDIAN;
240    Printf("System is big endian\n");
241  } else {
242    header = DATAS_LITTLE_ENDIAN;
243    Printf("System is little endian\n");
244  }
[2]245
246#ifdef __XENO__
[15]247  WarnUponSwitches(true);
248  printf("\n"); // a revoir pourquoi??
249// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
[2]250#endif
[15]251  // on attend d'avoir des choses à faire
252  Suspend();
[2]253
[15]254  while (!ToBeStopped()) {
255    size_t resume_id;
256    Time min = 0xffffffffffffffffULL;
[2]257
[15]258    // on recpuere l'id de la prochaine execution
259    send_mutex->GetMutex();
260    resume_id = resume_time.size();
261    for (size_t i = 0; i < resume_time.size(); i++) {
262      if (resume_time.at(i) < min && data_to_send.at(i)->IsEnabled() == true) {
263        min = resume_time.at(i);
264        resume_id = i;
265      }
266    }
[2]267
[15]268    // attente
269    if (resume_id < resume_time.size()) {
270      Time time = resume_time.at(resume_id);
271      uint16_t resume_period = data_to_send.at(resume_id)->SendPeriod();
272      send_mutex->ReleaseMutex();
273      // on dort jusqu'a la prochaine execution
274      SleepUntil(time);
[2]275
[15]276      // envoi des donnees
277      int offset = 3;
278      send_buffer[0] = header;
279      send_mutex->GetMutex();
[2]280
[15]281      for (size_t i = 0; i < data_to_send.size(); i++) {
282        if (data_to_send.at(i)->SendPeriod() == resume_period &&
283            data_to_send.at(i)->IsEnabled() == true) {
284          data_to_send.at(i)->CopyDatas(send_buffer + offset);
285          offset += data_to_send.at(i)->SendSize();
286        }
287      }
288      if (offset != 3) {
289        memcpy(&send_buffer[1], &resume_period, sizeof(uint16_t));
[157]290        // printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
[15]291        // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
292        // printf("\n");
293        Send(send_buffer, offset);
294      }
[2]295
[15]296      // on planifie la prochaine execution
297      for (size_t i = 0; i < data_to_send.size(); i++) {
298        if (data_to_send.at(i)->SendPeriod() == resume_period) {
299          resume_time.at(i) += data_to_send.at(i)->SendPeriod() * 1000000;
[2]300        }
[15]301      }
302      send_mutex->ReleaseMutex();
[157]303      //Printf("%i %lld\n",resume_period,GetTime()/1000000);
[15]304    } else {
305      send_mutex->ReleaseMutex();
306      // rien a faire, suspend
[157]307      //Printf("rien a faire suspend\n");
[15]308      Suspend();
[157]309      //Printf("wake\n");
[15]310      // on planifie la prochaine execution
311      Time time = GetTime();
312      send_mutex->GetMutex();
313      for (size_t i = 0; i < data_to_send.size(); i++) {
314        resume_time.at(i) =
315            time + (Time)data_to_send.at(i)->SendPeriod() * 1000000;
316      }
317      send_mutex->ReleaseMutex();
[2]318    }
[15]319  }
[2]320}
321#ifdef __XENO__
[15]322void *ui_com::user_thread(void *arg) {
323  int pipe_fd = -1;
324  string devname;
325  char *buf = NULL;
326  ui_com *caller = (ui_com *)arg;
327  int rv;
328  fd_set set;
329  struct timeval timeout;
330  ssize_t nb_read, nb_write;
[2]331
[15]332  buf = (char *)malloc(NRT_PIPE_SIZE);
333  if (buf == NULL) {
334    caller->Err("malloc error\n");
335  }
[2]336
[15]337  devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
338            caller->ObjectName() + "-pipe";
339  while (pipe_fd < 0) {
340    pipe_fd = open(devname.c_str(), O_RDWR);
[133]341    if (pipe_fd < 0 && errno != ENOENT) {
342                        char errorMsg[256];
343      caller->Err("open pipe_fd error (%s)\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
344                }
[15]345    usleep(1000);
346  }
[2]347
[15]348  while (1) {
349    FD_ZERO(&set);         // clear the set
350    FD_SET(pipe_fd, &set); // add our file descriptor to the set
[2]351
[15]352    timeout.tv_sec = 0;
353    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
[2]354
[15]355    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
[2]356
[15]357    if (rv == -1) {
358      if (caller->is_running == false &&
359          UDT::getlasterror().getErrorCode() == CUDTException::ETIMEOUT)
360        break; // timeout
361      if (UDT::getlasterror().getErrorCode() != CUDTException::ETIMEOUT)
362        caller->Err("epoll_wait, %s, code %i\n",
363                    UDT::getlasterror().getErrorMessage(),
364                    UDT::getlasterror().getErrorCode());
365    } else if (rv == 0) {
366      // printf("timeout\n"); // a timeout occured
367      if (caller->is_running == false)
368        break;
[2]369
[15]370    } else {
371      nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
372      buf[nb_read] = 0;
373// printf("envoi\n%s\n",buf);
[2]374
375#ifdef COMPRESS_FRAMES
[15]376      char *out;
377      ssize_t out_size;
[2]378
[15]379      if (buf[0] == XML_HEADER) {
380        if (compressBuffer(buf, nb_read, &out, &out_size, 9) == Z_OK) {
381          nb_read = out_size;
382          nb_write = UDT::sendmsg(caller->socket_fd, out, out_size, -1, true);
383          free(out);
384        } else {
385          caller->Warn("Compress error, sending it uncompressed\n");
386          nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
387        }
388      } else {
389        nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
390      }
[2]391#else
[15]392      nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
[2]393#endif
394
[15]395      if (nb_write < 0) {
396        caller->Err("UDT::sendmsg error (%s)\n",
397                    UDT::getlasterror().getErrorMessage());
398        if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
399            UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
400          caller->connection_lost = true;
[2]401        }
[15]402      } else if (nb_write != nb_read) {
403        caller->Err("UDT::sendmsg error %i/%i\n", nb_write, nb_read);
404      }
[2]405    }
[15]406  }
[2]407
[15]408  close(pipe_fd);
409  if (buf != NULL)
410    free(buf);
411  pthread_exit(0);
[2]412}
413#endif
414
[15]415int ui_com::compressBuffer(char *in, ssize_t in_size, char **out,
416                           ssize_t *out_size, int level) {
417  int ret, flush;
418  unsigned have;
419  z_stream strm;
[2]420
[15]421  /* allocate deflate state */
422  strm.zalloc = Z_NULL;
423  strm.zfree = Z_NULL;
424  strm.opaque = Z_NULL;
425  ret = deflateInit(&strm, level);
426  if (ret != Z_OK)
427    return ret;
[2]428
[15]429  *out = (char *)malloc(COMPRESS_CHUNK);
430  if (!(*out))
431    return Z_BUF_ERROR;
[2]432
[15]433  strm.next_in = (unsigned char *)in;
434  strm.avail_out = COMPRESS_CHUNK;
435  strm.next_out = (unsigned char *)*out;
436  strm.avail_in = in_size;
437  flush = Z_FINISH;
[2]438
[15]439  ret = deflate(&strm, flush); /* no bad return value */
440  if (ret == Z_STREAM_ERROR) {
441    free(*out);
442    return ret;
443  }
[2]444
[15]445  have = COMPRESS_CHUNK - strm.avail_out;
446  *out_size = have;
447  // printf("%i -> %i\n",in_size,have);
448  /* clean up and return */
449  (void)deflateEnd(&strm);
[2]450
[15]451  if (strm.avail_out != 0) {
452    return Z_OK;
453  } else {
454    return Z_STREAM_ERROR;
455  }
[2]456}
457
[15]458int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,
459                             unsigned char **out, ssize_t *out_size) {
460  int ret;
461  // unsigned have;
462  z_stream strm;
[2]463
[15]464  /* allocate inflate state */
465  strm.zalloc = Z_NULL;
466  strm.zfree = Z_NULL;
467  strm.opaque = Z_NULL;
468  strm.avail_in = 0;
469  strm.next_in = Z_NULL;
470  ret = inflateInit(&strm);
471  if (ret != Z_OK)
472    return ret;
[2]473
[15]474  *out = (unsigned char *)malloc(COMPRESS_CHUNK);
475  if (!(*out))
476    return Z_BUF_ERROR;
[2]477
[15]478  strm.avail_in = in_size;
479  strm.next_in = in;
480  strm.avail_out = COMPRESS_CHUNK;
481  strm.next_out = *out;
[2]482
[15]483  ret = inflate(&strm, Z_NO_FLUSH);
484  assert(ret != Z_STREAM_ERROR); /* state not clobbered */
485  switch (ret) {
486  case Z_NEED_DICT:
487    ret = Z_DATA_ERROR; /* and fall through */
488  case Z_DATA_ERROR:
489  case Z_MEM_ERROR:
[2]490    (void)inflateEnd(&strm);
[15]491    return ret;
492  }
493  // have = COMPRESS_CHUNK - strm.avail_out;
[2]494
[15]495  /* clean up and return */
496  (void)inflateEnd(&strm);
497  return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
[2]498}
499
[15]500bool ui_com::ConnectionLost() { return connection_lost; }
[2]501
[15]502void ui_com::AddSendData(const SendData *obj) {
503  send_mutex->GetMutex();
[2]504
[15]505  resume_time.push_back(GetTime() + (Time)obj->SendPeriod() * 1000000);
506
507  // on resume en meme temps tout ceux qui ont la meme periode
508  for (size_t i = 0; i < data_to_send.size(); i++) {
509    if (data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
510      resume_time.at(resume_time.size() - 1) = resume_time.at(i);
511      break;
[2]512    }
[15]513  }
[2]514
[15]515  data_to_send.push_back(obj);
[2]516
[15]517  send_mutex->ReleaseMutex();
[2]518}
519
[15]520void ui_com::UpdateSendData(const SendData *obj) {
521  // le mutex est deja pris par l'appellant
522  size_t id, i;
[2]523
[15]524  // on recupere l'id
525  for (i = 0; i < data_to_send.size(); i++) {
526    if (data_to_send.at(i) == obj) {
527      id = i;
528      break;
[2]529    }
[15]530  }
[2]531
[15]532  // on resume en meme temps tout ceux qui ont la meme periode
533  for (i = 0; i < data_to_send.size(); i++) {
534    if (i == id)
535      continue;
536    if (data_to_send.at(i)->IsEnabled() == true &&
537        data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
538      resume_time.at(id) = resume_time.at(i);
539      break;
[2]540    }
[15]541  }
[2]542
[15]543  // si aucun match, on planifie l'execution
544  if (i == data_to_send.size())
545    resume_time.at(id) = GetTime() + (Time)obj->SendPeriod() * 1000000;
[2]546
[15]547  if (IsSuspended() == true)
548    Resume();
[2]549
[15]550  return;
[2]551}
552
[15]553void ui_com::UpdateDataToSendSize(void) {
554  send_mutex->GetMutex();
555  send_size = 3; // id(1)+period(2)
556  for (size_t i = 0; i < data_to_send.size(); i++) {
557    if (data_to_send[i] != NULL)
558      send_size += data_to_send[i]->SendSize();
559  }
[2]560
[15]561  // send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
562  if (send_buffer != NULL)
563    free(send_buffer);
564  send_buffer = (char *)malloc(send_size * sizeof(char));
565  send_mutex->ReleaseMutex();
[2]566}
567
[15]568void ui_com::RemoveSendData(const SendData *obj) {
569  // printf("remove_data_to_send %i\n",data_to_send.size());
[2]570
[15]571  send_mutex->GetMutex();
572  // on recupere l'id
573  for (size_t i = 0; i < data_to_send.size(); i++) {
574    if (data_to_send.at(i) == obj) {
575      data_to_send.erase(data_to_send.begin() + i);
576      resume_time.erase(resume_time.begin() + i);
577      // printf("remove_data_to_send %i ok\n",data_to_send.size());
578      break;
[2]579    }
[15]580  }
581  send_mutex->ReleaseMutex();
[2]582
[15]583  return;
[2]584}
585
[15]586void ui_com::Block(void) { send_mutex->GetMutex(); }
[2]587
[15]588void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }
Note: See TracBrowser for help on using the repository browser.