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

Last change on this file since 252 was 252, checked in by Sanahuja Guillaume, 3 years ago

change io_data CopyDate? to RawRead?

File size: 15.7 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
155void ui_com::Send(char *buf, ssize_t size,int ttl) {
156  if (connection_lost == true)
157    return;
158
159  char *tosend = buf;
160  ssize_t nb_write;
161
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  nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
170
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#else //__XENO__
178
179#ifdef COMPRESS_FRAMES
180  bool sendItCompressed = false;
181  if (buf[0] == XML_HEADER) {
182    sendItCompressed = true;
183  }
184
185  if (sendItCompressed) {
186    char *out;
187    ssize_t out_size;
188    if (compressBuffer(tosend, size, &out, &out_size, 9) == Z_OK) {
189      size = out_size;
190      nb_write = UDT::sendmsg(socket_fd, out, size,ttl, true);
191      free(out);
192    } else {
193      Warn("Compress error, sending it uncompressed\n");
194      sendItCompressed = false;
195    }
196  }
197
198  if (!sendItCompressed) {
199    nb_write = UDT::sendmsg(socket_fd, tosend, size, ttl, true);
200  }
201
202#else // COMPRESS_FRAMES
203  nb_write = UDT::sendmsg(socket_fd, tosend, size, ttl, true);
204#endif // COMPRESS_FRAMES
205   //Printf("write %i %i\n",nb_write,size);
206  if (nb_write < 0) {
207    Err("UDT::sendmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
208    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
209        UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
210      connection_lost = true;
211    }
212  } else if (nb_write != size) {
213    Err("%s, code %i (%ld/%ld)\n", UDT::getlasterror().getErrorMessage(),
214        UDT::getlasterror().getErrorCode(), nb_write, size);
215  }
216#endif //__XENO__
217}
218
219ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
220  ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);
221
222  if (bytesRead < 0) {
223    if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST) {
224      Err("UDT::recvmsg error (%s)\n", UDT::getlasterror().getErrorMessage());
225      connection_lost = true;
226    }
227  }
228
229  return bytesRead;
230}
231
232void ui_com::CheckConnection(void) {
233  int32_t val,len;
234 
235  int result=UDT::getsockopt(socket_fd,0,UDT_STATE,&val,&len); 
236   if (result < 0) {
237    Printf("UDT::getsockopt error (%s)\n", UDT::getlasterror().getErrorMessage());
238   }
239 // printf("opt: %i %i %i\n",result,val,len);
240}
241
242void ui_com::Run(void) {
243  // check endianness
244  char header;
245  if (IsBigEndian()) {
246    header = DATAS_BIG_ENDIAN;
247    Printf("System is big endian\n");
248  } else {
249    header = DATAS_LITTLE_ENDIAN;
250    Printf("System is little endian\n");
251  }
252
253#ifdef __XENO__
254  WarnUponSwitches(true);
255  printf("\n"); // a revoir pourquoi??
256// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
257#endif
258  // on attend d'avoir des choses à faire
259  Suspend();
260
261  while (!ToBeStopped()) {
262    size_t resume_id;
263    Time min = 0xffffffffffffffffULL;
264
265    // on recpuere l'id de la prochaine execution
266    send_mutex->GetMutex();
267    resume_id = resume_time.size();
268    for (size_t i = 0; i < resume_time.size(); i++) {
269      if (resume_time.at(i) < min && data_to_send.at(i)->IsEnabled() == true) {
270        min = resume_time.at(i);
271        resume_id = i;
272      }
273    }
274
275    // attente
276    if (resume_id < resume_time.size()) {
277      Time time = resume_time.at(resume_id);
278      uint16_t resume_period = data_to_send.at(resume_id)->SendPeriod();
279      send_mutex->ReleaseMutex();
280      // on dort jusqu'a la prochaine execution
281      SleepUntil(time);
282
283      // envoi des donnees
284      int offset = 3;
285      send_buffer[0] = header;
286      send_mutex->GetMutex();
287
288      for (size_t i = 0; i < data_to_send.size(); i++) {
289        if (data_to_send.at(i)->SendPeriod() == resume_period &&
290            data_to_send.at(i)->IsEnabled() == true) {
291          data_to_send.at(i)->CopyDatas(send_buffer + offset);
292          offset += data_to_send.at(i)->SendSize();
293        }
294      }
295      if (offset != 3) {
296        memcpy(&send_buffer[1], &resume_period, sizeof(uint16_t));
297        //printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
298        // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
299        // printf("\n");
300        Send(send_buffer, offset,resume_period);
301      }
302
303      // on planifie la prochaine execution
304      for (size_t i = 0; i < data_to_send.size(); i++) {
305        if (data_to_send.at(i)->SendPeriod() == resume_period) {
306          resume_time.at(i) += data_to_send.at(i)->SendPeriod() * 1000000;
307        }
308      }
309      send_mutex->ReleaseMutex();
310      //Printf("%i %lld\n",resume_period,GetTime()/1000000);
311    } else {
312      send_mutex->ReleaseMutex();
313      // rien a faire, suspend
314      //Printf("rien a faire suspend\n");
315      Suspend();
316      //Printf("wake\n");
317      // on planifie la prochaine execution
318      Time time = GetTime();
319      send_mutex->GetMutex();
320      for (size_t i = 0; i < data_to_send.size(); i++) {
321        resume_time.at(i) =
322            time + (Time)data_to_send.at(i)->SendPeriod() * 1000000;
323      }
324      send_mutex->ReleaseMutex();
325    }
326  }
327}
328#ifdef __XENO__
329void *ui_com::user_thread(void *arg) {
330  int pipe_fd = -1;
331  string devname;
332  char *buf = NULL;
333  ui_com *caller = (ui_com *)arg;
334  int rv;
335  fd_set set;
336  struct timeval timeout;
337  ssize_t nb_read, nb_write;
338
339  buf = (char *)malloc(NRT_PIPE_SIZE);
340  if (buf == NULL) {
341    caller->Err("malloc error\n");
342  }
343
344  devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
345            caller->ObjectName() + "-pipe";
346  while (pipe_fd < 0) {
347    pipe_fd = open(devname.c_str(), O_RDWR);
348    if (pipe_fd < 0 && errno != ENOENT) {
349                        char errorMsg[256];
350      caller->Err("open pipe_fd error (%s)\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
351                }
352    usleep(1000);
353  }
354
355  while (1) {
356    FD_ZERO(&set);         // clear the set
357    FD_SET(pipe_fd, &set); // add our file descriptor to the set
358
359    timeout.tv_sec = 0;
360    timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
361
362    rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
363
364    if (rv == -1) {
365      if (caller->is_running == false &&
366          UDT::getlasterror().getErrorCode() == CUDTException::ETIMEOUT)
367        break; // timeout
368      if (UDT::getlasterror().getErrorCode() != CUDTException::ETIMEOUT)
369        caller->Err("epoll_wait, %s, code %i\n",
370                    UDT::getlasterror().getErrorMessage(),
371                    UDT::getlasterror().getErrorCode());
372    } else if (rv == 0) {
373      // printf("timeout\n"); // a timeout occured
374      if (caller->is_running == false)
375        break;
376
377    } else {
378      nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
379      buf[nb_read] = 0;
380// printf("envoi\n%s\n",buf);
381
382#ifdef COMPRESS_FRAMES
383      char *out;
384      ssize_t out_size;
385
386      if (buf[0] == XML_HEADER) {
387        if (compressBuffer(buf, nb_read, &out, &out_size, 9) == Z_OK) {
388          nb_read = out_size;
389          nb_write = UDT::sendmsg(caller->socket_fd, out, out_size, -1, true);
390          free(out);
391        } else {
392          caller->Warn("Compress error, sending it uncompressed\n");
393          nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
394        }
395      } else {
396        nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
397      }
398#else
399      nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
400#endif
401
402      if (nb_write < 0) {
403        caller->Err("UDT::sendmsg error (%s)\n",
404                    UDT::getlasterror().getErrorMessage());
405        if (UDT::getlasterror().getErrorCode() == CUDTException::ECONNLOST ||
406            UDT::getlasterror().getErrorCode() == CUDTException::EINVSOCK) {
407          caller->connection_lost = true;
408        }
409      } else if (nb_write != nb_read) {
410        caller->Err("UDT::sendmsg error %i/%i\n", nb_write, nb_read);
411      }
412    }
413  }
414
415  close(pipe_fd);
416  if (buf != NULL)
417    free(buf);
418  pthread_exit(0);
419}
420#endif
421
422int ui_com::compressBuffer(char *in, ssize_t in_size, char **out,
423                           ssize_t *out_size, int level) {
424  int ret, flush;
425  unsigned have;
426  z_stream strm;
427
428  /* allocate deflate state */
429  strm.zalloc = Z_NULL;
430  strm.zfree = Z_NULL;
431  strm.opaque = Z_NULL;
432  ret = deflateInit(&strm, level);
433  if (ret != Z_OK)
434    return ret;
435
436  *out = (char *)malloc(COMPRESS_CHUNK);
437  if (!(*out))
438    return Z_BUF_ERROR;
439
440  strm.next_in = (unsigned char *)in;
441  strm.avail_out = COMPRESS_CHUNK;
442  strm.next_out = (unsigned char *)*out;
443  strm.avail_in = in_size;
444  flush = Z_FINISH;
445
446  ret = deflate(&strm, flush); /* no bad return value */
447  if (ret == Z_STREAM_ERROR) {
448    free(*out);
449    return ret;
450  }
451
452  have = COMPRESS_CHUNK - strm.avail_out;
453  *out_size = have;
454  // printf("%i -> %i\n",in_size,have);
455  /* clean up and return */
456  (void)deflateEnd(&strm);
457
458  if (strm.avail_out != 0) {
459    return Z_OK;
460  } else {
461    return Z_STREAM_ERROR;
462  }
463}
464
465int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,
466                             unsigned char **out, ssize_t *out_size) {
467  int ret;
468  // unsigned have;
469  z_stream strm;
470
471  /* allocate inflate state */
472  strm.zalloc = Z_NULL;
473  strm.zfree = Z_NULL;
474  strm.opaque = Z_NULL;
475  strm.avail_in = 0;
476  strm.next_in = Z_NULL;
477  ret = inflateInit(&strm);
478  if (ret != Z_OK)
479    return ret;
480
481  *out = (unsigned char *)malloc(COMPRESS_CHUNK);
482  if (!(*out))
483    return Z_BUF_ERROR;
484
485  strm.avail_in = in_size;
486  strm.next_in = in;
487  strm.avail_out = COMPRESS_CHUNK;
488  strm.next_out = *out;
489
490  ret = inflate(&strm, Z_NO_FLUSH);
491  assert(ret != Z_STREAM_ERROR); /* state not clobbered */
492  switch (ret) {
493  case Z_NEED_DICT:
494    ret = Z_DATA_ERROR; /* and fall through */
495  case Z_DATA_ERROR:
496  case Z_MEM_ERROR:
497    (void)inflateEnd(&strm);
498    return ret;
499  }
500  // have = COMPRESS_CHUNK - strm.avail_out;
501
502  /* clean up and return */
503  (void)inflateEnd(&strm);
504  return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
505}
506
507bool ui_com::ConnectionLost() { return connection_lost; }
508
509void ui_com::AddSendData(const SendData *obj) {
510  send_mutex->GetMutex();
511
512  resume_time.push_back(GetTime() + (Time)obj->SendPeriod() * 1000000);
513
514  // on resume en meme temps tout ceux qui ont la meme periode
515  for (size_t i = 0; i < data_to_send.size(); i++) {
516    if (data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
517      resume_time.at(resume_time.size() - 1) = resume_time.at(i);
518      break;
519    }
520  }
521
522  data_to_send.push_back(obj);
523
524  send_mutex->ReleaseMutex();
525}
526
527void ui_com::UpdateSendData(const SendData *obj) {
528  // le mutex est deja pris par l'appellant
529  size_t id, i;
530
531  // on recupere l'id
532  for (i = 0; i < data_to_send.size(); i++) {
533    if (data_to_send.at(i) == obj) {
534      id = i;
535      break;
536    }
537  }
538
539  // on resume en meme temps tout ceux qui ont la meme periode
540  for (i = 0; i < data_to_send.size(); i++) {
541    if (i == id)
542      continue;
543    if (data_to_send.at(i)->IsEnabled() == true &&
544        data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
545      resume_time.at(id) = resume_time.at(i);
546      break;
547    }
548  }
549
550  // si aucun match, on planifie l'execution
551  if (i == data_to_send.size())
552    resume_time.at(id) = GetTime() + (Time)obj->SendPeriod() * 1000000;
553
554  if (IsSuspended() == true)
555    Resume();
556
557  return;
558}
559
560void ui_com::UpdateDataToSendSize(void) {
561  send_mutex->GetMutex();
562  send_size = 3; // id(1)+period(2)
563  for (size_t i = 0; i < data_to_send.size(); i++) {
564    if (data_to_send[i] != NULL)
565      send_size += data_to_send[i]->SendSize();
566  }
567
568  // send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
569  if (send_buffer != NULL)
570    free(send_buffer);
571  send_buffer = (char *)malloc(send_size * sizeof(char));
572  send_mutex->ReleaseMutex();
573}
574
575void ui_com::RemoveSendData(const SendData *obj) {
576  // printf("remove_data_to_send %i\n",data_to_send.size());
577
578  send_mutex->GetMutex();
579  // on recupere l'id
580  for (size_t i = 0; i < data_to_send.size(); i++) {
581    if (data_to_send.at(i) == obj) {
582      data_to_send.erase(data_to_send.begin() + i);
583      resume_time.erase(resume_time.begin() + i);
584      // printf("remove_data_to_send %i ok\n",data_to_send.size());
585      break;
586    }
587  }
588  send_mutex->ReleaseMutex();
589
590  return;
591}
592
593void ui_com::Block(void) { send_mutex->GetMutex(); }
594
595void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }
Note: See TracBrowser for help on using the repository browser.