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

Last change on this file since 440 was 440, checked in by Sanahuja Guillaume, 4 months ago

up

File size: 17.2 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
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
41ui_com::ui_com(const Object *parent, UDTSOCKET sock)
42    : Thread(parent, "send", 2,16384*2) {
43  // buffer envoi
44  send_buffer = (char *)malloc(3);
45  send_size = 3; // id(1)+period(2)
46
47  // mutex
48  send_mutex = NULL;
49  send_mutex = new Mutex(this, ObjectName());
50
51  socket_fd = sock;
52  connection_lost = false;
53
54#ifdef __XENO__
55  int status;
56  string tmp_name;
57
58  is_running = true;
59
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());
65#ifdef RT_PIPE_SIZE
66  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, RT_PIPE_SIZE);
67#else
68  status = rt_pipe_create(&pipe, tmp_name.c_str(), P_MINOR_AUTO, 0);
69#endif
70
71  if (status != 0) {
72                char errorMsg[256];
73    Err("rt_pipe_create error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
74    // return -1;
75  }
76
77// start user side thread
78#ifdef NRT_STACK_SIZE
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  }
84
85  if (pthread_attr_setstacksize(&attr, NRT_STACK_SIZE) != 0) {
86    Err("pthread_attr_setstacksize error\n");
87  }
88
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  }
97#ifdef NRT_STACK_SIZE
98  if (pthread_attr_destroy(&attr) != 0) {
99    Err("pthread_attr_destroy error\n");
100  }
101#endif
102
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");
107    /*
108  timeout=-1;
109  if (UDT::setsockopt(socket_fd, 0, UDT_SNDTIMEO, &timeout, sizeof(int)) != 0)
110    Err("UDT::setsockopt error (UDT_SNDTIMEO)\n");
111*/
112  bool blocking = true;
113  if (UDT::setsockopt(socket_fd, 0, UDT_SNDSYN, &blocking, sizeof(bool)) != 0)
114    Err("UDT::setsockopt error (UDT_SNDSYN)\n");
115
116  if (UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
117    Err("UDT::setsockopt error (UDT_RCVSYN)\n");
118  //#endif //__XENO__
119
120  Start();
121}
122
123ui_com::~ui_com() {
124//Printf("destruction ui_com\n");
125
126#ifdef __XENO__
127  is_running = false;
128
129  pthread_join(thread, NULL);
130
131  int status = rt_pipe_delete(&pipe);
132  if (status != 0) {
133                char errorMsg[256];
134    Err("rt_pipe_delete error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
135        }
136#endif
137
138  SafeStop();
139
140  if (IsSuspended() == true)
141    Resume();
142
143  Join();
144 
145  char buf=CLOSING_CONNECTION;
146  Send(&buf,1);
147 
148  if (send_buffer != NULL)
149    free(send_buffer);
150  send_buffer = NULL;
151
152  //Printf("destruction ui_com ok\n");
153}
154
155//send, public part; dispatch to SendNRT (nrt) or rt_pipe (rt)
156//rt_pipe is read by user thread which calls SendNRT
157void ui_com::Send(char *buf, ssize_t size,int ttl) {
158  if (connection_lost == true)
159    return;
160
161  char *tosend = buf;
162  if (buf[0] == XML_HEADER) {
163    // cut xml header
164    tosend = strstr(buf, "<root");
165    size -= tosend - buf;
166  }
167
168#ifdef __XENO__
169  //data buf
170  ssize_t nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
171  if (nb_write < 0) {
172                char errorMsg[256];
173    Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
174  } else if (nb_write != size) {
175    Err("rt_pipe_write error %i/%i\n", nb_write, size);
176  }
177  //ttl
178  nb_write = rt_pipe_write(&pipe, &ttl, sizeof(ttl), P_NORMAL);
179  if (nb_write < 0) {
180                char errorMsg[256];
181    Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
182  } else if (nb_write != sizeof(ttl)) {
183    Err("rt_pipe_write error %i/%i\n", nb_write, size);
184  }
185#else //__XENO__
186  SendNRT(tosend,size,ttl);
187#endif //__XENO__
188}
189
190//send, private part; called to effectively send to udt
191//this is always in nrt
192void ui_com::SendNRT(char *buf, ssize_t size,int ttl) {
193  ssize_t nb_write;
194  bool sendItCompressed = false;
195 
196#ifdef COMPRESS_FRAMES
197  if (buf[0] == XML_HEADER) {
198    sendItCompressed = true;
199  }
200#endif // COMPRESS_FRAMES
201
202  if (sendItCompressed) {
203    char *out;
204    ssize_t out_size;
205    if (compressBuffer(buf, size, &out, &out_size, 9) == Z_OK) {
206      size = out_size;
207      nb_write = UDT::sendmsg(socket_fd, out, size,ttl, true);
208      free(out);
209    } else {
210      Warn("Compress error, sending it uncompressed\n");
211      sendItCompressed = false;
212    }
213  }
214
215  if (!sendItCompressed) {
216    nb_write = UDT::sendmsg(socket_fd, buf, size, ttl, true);
217  }
218
219   //Printf("write %i %i\n",nb_write,size);
220  if (nb_write < 0) {
221    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
222    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
223        UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
224      connection_lost = true;
225    }
226  } else if (nb_write != size) {
227    Err("%s, code %i (%ld/%ld)\n", UDT::getlasterror().getErrorMessage(),
228        UDT::getlasterror().getErrorCode(), nb_write, size);
229  }
230}
231
232ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
233  ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);
234
235  if (bytesRead < 0) {
236    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST) {
237      Err("UDT::recvmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
238      connection_lost = true;
239    }
240  }
241
242  return bytesRead;
243}
244
245void ui_com::CheckConnection(void) {
246  int32_t val,len;
247 
248  int result=UDT::getsockopt(socket_fd,0,UDT_STATE,&val,&len); 
249   if (result < 0) {
250    Printf("UDT::getsockopt error (%s)\n", UDT::getlasterror().getErrorMessage());
251   }
252 // printf("opt: %i %i %i\n",result,val,len);
253}
254
255//todo: le run (rt) push les données une par une
256//le nrt recupere et decide quand envoyer :
257// -quand toutes les données d'une meme periode sont recues
258// -quand on a atteind le nb buffering
259// -le nrt alloue dynamiquement le buffer d'envoi
260//-> enelver l'alloc du buffer au startup (UpdateDataToSendSize)
261void ui_com::Run(void) {
262  // check endianness
263  char header;
264  if (IsBigEndian()) {
265    header = DATA_BIG_ENDIAN;
266    Printf("System is big endian\n");
267  } else {
268    header = DATA_LITTLE_ENDIAN;
269    Printf("System is little endian\n");
270  }
271
272#ifdef __XENO__
273  WarnUponSwitches(true);
274  printf("\n"); // a revoir pourquoi??
275// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
276#endif
277  // on attend d'avoir des choses à faire
278  Suspend();
279
280  while (!ToBeStopped()) {
281    size_t resume_id;
282    Time min = 0xffffffffffffffffULL;
283
284    // on recpuere l'id de la prochaine execution
285    send_mutex->GetMutex();
286    resume_id = resume_time.size();
287    for (size_t i = 0; i < resume_time.size(); i++) {
288      if (resume_time.at(i) < min && data_to_send.at(i)->IsEnabled() == true) {
289        min = resume_time.at(i);
290        resume_id = i;
291      }
292    }
293
294    // attente
295    if (resume_id < resume_time.size()) {
296      Time time = resume_time.at(resume_id);
297      uint16_t resume_period = data_to_send.at(resume_id)->SendPeriod();
298      send_mutex->ReleaseMutex();
299      // on dort jusqu'a la prochaine execution
300      SleepUntil(time);
301
302      // envoi des donnees
303      int offset = 3;
304      send_buffer[0] = header;
305      send_mutex->GetMutex();
306
307      for (size_t i = 0; i < data_to_send.size(); i++) {
308        if (data_to_send.at(i)->SendPeriod() == resume_period && data_to_send.at(i)->IsEnabled() == true) {
309          data_to_send.at(i)->CopyDatas(send_buffer + offset);
310          offset += data_to_send.at(i)->SendSize();
311        }
312      }
313      if (offset != 3) {
314        memcpy(&send_buffer[1], &resume_period, sizeof(uint16_t));
315        //printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
316        // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
317        // printf("\n");
318        Send(send_buffer, offset,resume_period);
319      }
320
321      //test to push datas
322      for (size_t i = 0; i < data_to_send.size(); i++) {
323        if (data_to_send.at(i)->SendPeriod() == resume_period && data_to_send.at(i)->IsEnabled() == true) {
324          PushDatasToSend(data_to_send.at(i));
325        }
326      }
327      //end test
328     
329      // on planifie la prochaine execution
330      for (size_t i = 0; i < data_to_send.size(); i++) {
331        if (data_to_send.at(i)->SendPeriod() == resume_period) {
332          resume_time.at(i) += data_to_send.at(i)->SendPeriod() * 1000000;
333        }
334      }
335      send_mutex->ReleaseMutex();
336      //Printf("%i %lld\n",resume_period,GetTime()/1000000);
337    } else {
338      send_mutex->ReleaseMutex();
339      // rien a faire, suspend
340      //Printf("rien a faire suspend\n");
341      Suspend();
342      //Printf("wake\n");
343      // on planifie la prochaine execution
344      Time time = GetTime();
345      send_mutex->GetMutex();
346      for (size_t i = 0; i < data_to_send.size(); i++) {
347        resume_time.at(i) =
348            time + (Time)data_to_send.at(i)->SendPeriod() * 1000000;
349      }
350      send_mutex->ReleaseMutex();
351    }
352  }
353}
354
355
356//called with send_mutex locked
357//in nrt
358//TODO: RT part has to put datas in pipe, then useer thread call this
359void ui_com::PushDatasToSend(const SendData *data_to_send) {
360  PushedData_t* ptr=NULL;
361  //check if we already have one buffer for this couple period/nb_buffering
362  for (size_t i = 0; i < PushedDatas.size(); i++) {
363      if (PushedDatas.at(i).period==data_to_send->SendPeriod() && PushedDatas.at(i).nb_buffering==data_to_send->NbBuffering()) {
364          Printf("match item %i, period %i nb buff %i\n",i,data_to_send->SendPeriod(),data_to_send->NbBuffering());
365          ptr=&PushedDatas.at(i);
366        }
367  }
368  //otherwire create new one
369  if(ptr==NULL) {
370    printf("no match\n");
371    PushedData_t tmp;
372    tmp.period=data_to_send->SendPeriod();
373    tmp.nb_buffering=data_to_send->NbBuffering();
374    tmp.size=0;
375    tmp.buf=NULL;
376    PushedDatas.push_back(tmp);
377    ptr=&PushedDatas.back();
378  }
379   
380  ptr->buf=(char*)realloc(ptr->buf,ptr->size+data_to_send->SendSize());
381 
382  data_to_send->CopyDatas(ptr->buf+ptr->size);
383  ptr->size+=data_to_send->SendSize();
384  Printf("nb buffered %i\n",ptr->size);
385//TODO: savoir quand a fait tous les nb buffers
386  Printf("pushed size %i period %i nb buffering %i\n",data_to_send->SendSize(),data_to_send->SendPeriod(),data_to_send->NbBuffering());
387}
388
389
390#ifdef __XENO__
391void *ui_com::user_thread(void *arg) {
392  int pipe_fd = -1;
393  string devname;
394  char *buf = NULL;
395  ui_com *caller = (ui_com *)arg;
396  int rv;
397  fd_set set;
398  struct timeval timeout;
399  ssize_t nb_read;
400  int ttl;
401
402  buf = (char *)malloc(NRT_PIPE_SIZE);
403  if (buf == NULL) {
404    caller->Err("malloc error\n");
405  }
406
407  devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
408            caller->ObjectName() + "-pipe";
409  while (pipe_fd < 0) {
410    pipe_fd = open(devname.c_str(), O_RDWR);
411    if (pipe_fd < 0 && errno != ENOENT) {
412                        char errorMsg[256];
413      caller->Err("open pipe_fd error (%s)\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
414                }
415    usleep(1000);
416  }
417
418  while (1) {
419    FD_ZERO(&set);         // clear the set
420    FD_SET(pipe_fd, &set); // add our file descriptor to the set
421
422    timeout.tv_sec = 0;
423    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
424
425    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
426
427    if (rv == -1) {
428      if (caller->is_running == false && UDT::getlasterror().getErrorCode() == CUDTException::ETIMEOUT)
429        break; // timeout
430      if (UDT::getlasterror().getErrorCode() != CUDTException::ETIMEOUT)
431        caller->Err("epoll_wait, %s, code %i\n",
432                    UDT::getlasterror().getErrorMessage(), UDT::getlasterror().getErrorCode());
433    } else if (rv == 0) {
434      // printf("timeout\n"); // a timeout occured
435      if (caller->is_running == false)
436        break;
437
438    } else {
439      //read data buf and ttl
440      //RT part has made 2 rt_pipe_write; they should be available in nrt
441      //(select ensure at least one data is ready)
442      nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
443      buf[nb_read] = 0;
444      read(pipe_fd, &ttl, sizeof(ttl));
445// printf("envoi\n%s\n",buf);
446
447      caller->SendNRT(buf, nb_read,ttl);
448    }
449  }
450
451  close(pipe_fd);
452  if (buf != NULL)
453    free(buf);
454  pthread_exit(0);
455}
456#endif
457
458int ui_com::compressBuffer(char *in, ssize_t in_size, char **out,
459                           ssize_t *out_size, int level) {
460  int ret, flush;
461  unsigned have;
462  z_stream strm;
463
464  /* allocate deflate state */
465  strm.zalloc = Z_NULL;
466  strm.zfree = Z_NULL;
467  strm.opaque = Z_NULL;
468  ret = deflateInit(&strm, level);
469  if (ret != Z_OK)
470    return ret;
471
472  *out = (char *)malloc(COMPRESS_CHUNK);
473  if (!(*out))
474    return Z_BUF_ERROR;
475
476  strm.next_in = (unsigned char *)in;
477  strm.avail_out = COMPRESS_CHUNK;
478  strm.next_out = (unsigned char *)*out;
479  strm.avail_in = in_size;
480  flush = Z_FINISH;
481
482  ret = deflate(&strm, flush); /* no bad return value */
483  if (ret == Z_STREAM_ERROR) {
484    free(*out);
485    return ret;
486  }
487
488  have = COMPRESS_CHUNK - strm.avail_out;
489  *out_size = have;
490  // printf("%i -> %i\n",in_size,have);
491  /* clean up and return */
492  (void)deflateEnd(&strm);
493
494  if (strm.avail_out != 0) {
495    return Z_OK;
496  } else {
497    return Z_STREAM_ERROR;
498  }
499}
500
501int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,
502                             unsigned char **out, ssize_t *out_size) {
503  int ret;
504  // unsigned have;
505  z_stream strm;
506
507  /* allocate inflate state */
508  strm.zalloc = Z_NULL;
509  strm.zfree = Z_NULL;
510  strm.opaque = Z_NULL;
511  strm.avail_in = 0;
512  strm.next_in = Z_NULL;
513  ret = inflateInit(&strm);
514  if (ret != Z_OK)
515    return ret;
516
517  *out = (unsigned char *)malloc(COMPRESS_CHUNK);
518  if (!(*out))
519    return Z_BUF_ERROR;
520
521  strm.avail_in = in_size;
522  strm.next_in = in;
523  strm.avail_out = COMPRESS_CHUNK;
524  strm.next_out = *out;
525
526  ret = inflate(&strm, Z_NO_FLUSH);
527  assert(ret != Z_STREAM_ERROR); /* state not clobbered */
528  switch (ret) {
529  case Z_NEED_DICT:
530    ret = Z_DATA_ERROR; /* and fall through */
531  case Z_DATA_ERROR:
532  case Z_MEM_ERROR:
533    (void)inflateEnd(&strm);
534    return ret;
535  }
536  // have = COMPRESS_CHUNK - strm.avail_out;
537
538  /* clean up and return */
539  (void)inflateEnd(&strm);
540  return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
541}
542
543bool ui_com::ConnectionLost() { return connection_lost; }
544
545void ui_com::AddSendData(const SendData *obj) {
546  send_mutex->GetMutex();
547
548  resume_time.push_back(GetTime() + (Time)obj->SendPeriod() * 1000000);
549
550  // on resume en meme temps tout ceux qui ont la meme periode
551  for (size_t i = 0; i < data_to_send.size(); i++) {
552    if (data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
553      resume_time.at(resume_time.size() - 1) = resume_time.at(i);
554      break;
555    }
556  }
557
558  data_to_send.push_back(obj);
559
560  send_mutex->ReleaseMutex();
561}
562
563void ui_com::UpdateSendData(const SendData *obj) {
564  // le mutex est deja pris par l'appellant
565  size_t id, i;
566
567  // on recupere l'id
568  for (i = 0; i < data_to_send.size(); i++) {
569    if (data_to_send.at(i) == obj) {
570      id = i;
571      break;
572    }
573  }
574
575  // on resume en meme temps tout ceux qui ont la meme periode
576  for (i = 0; i < data_to_send.size(); i++) {
577    if (i == id)
578      continue;
579    if (data_to_send.at(i)->IsEnabled() == true &&
580        data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
581      resume_time.at(id) = resume_time.at(i);
582      break;
583    }
584  }
585
586  // si aucun match, on planifie l'execution
587  if (i == data_to_send.size())
588    resume_time.at(id) = GetTime() + (Time)obj->SendPeriod() * 1000000;
589
590  if (IsSuspended() == true)
591    Resume();
592
593  return;
594}
595
596//allocate a buffer at startup for the worst case
597//(every data send with same pedriod)
598//TODO: dynamic size depending on what gcs asks?
599void ui_com::UpdateDataToSendSize(void) {
600  send_mutex->GetMutex();
601  send_size = 3; // id(1)+period(2)
602  for (size_t i = 0; i < data_to_send.size(); i++) {
603    if (data_to_send[i] != NULL)
604      send_size += data_to_send[i]->SendSize();
605  }
606
607  // send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
608  if (send_buffer != NULL)
609    free(send_buffer);
610  send_buffer = (char *)malloc(send_size * sizeof(char));
611  send_mutex->ReleaseMutex();
612}
613
614void ui_com::RemoveSendData(const SendData *obj) {
615  // printf("remove_data_to_send %i\n",data_to_send.size());
616
617  send_mutex->GetMutex();
618  // on recupere l'id
619  for (size_t i = 0; i < data_to_send.size(); i++) {
620    if (data_to_send.at(i) == obj) {
621      data_to_send.erase(data_to_send.begin() + i);
622      resume_time.erase(resume_time.begin() + i);
623      // printf("remove_data_to_send %i ok\n",data_to_send.size());
624      break;
625    }
626  }
627  send_mutex->ReleaseMutex();
628
629  return;
630}
631
632void ui_com::Block(void) { send_mutex->GetMutex(); }
633
634void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }
Note: See TracBrowser for help on using the repository browser.