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

Last change on this file since 247 was 247, checked in by Sanahuja Guillaume, 6 years ago

improve gcs disconnections

File size: 15.7 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");
[247]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*/
[15]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");
[2]115
[15]116 if (UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
117 Err("UDT::setsockopt error (UDT_RCVSYN)\n");
118 //#endif //__XENO__
[2]119
[15]120 Start();
[2]121}
122
[15]123ui_com::~ui_com() {
[243]124//Printf("destruction ui_com\n");
[2]125
126#ifdef __XENO__
[15]127 is_running = false;
[2]128
[15]129 pthread_join(thread, NULL);
[2]130
[15]131 int status = rt_pipe_delete(&pipe);
[133]132 if (status != 0) {
133 char errorMsg[256];
134 Err("rt_pipe_delete error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
135 }
[2]136#endif
137
[15]138 SafeStop();
[2]139
[15]140 if (IsSuspended() == true)
141 Resume();
[2]142
[15]143 Join();
[247]144
145 char buf=CLOSING_CONNECTION;
146 Send(&buf,1);
147
[15]148 if (send_buffer != NULL)
149 free(send_buffer);
150 send_buffer = NULL;
[2]151
[243]152 //Printf("destruction ui_com ok\n");
[2]153}
154
[15]155void ui_com::Send(char *buf, ssize_t size) {
156 if (connection_lost == true)
157 return;
[2]158
[15]159 char *tosend = buf;
160 ssize_t nb_write;
[2]161
[15]162 if (buf[0] == XML_HEADER) {
163 // cut xml header
164 tosend = strstr(buf, "<root");
165 size -= tosend - buf;
166 }
[2]167
168#ifdef __XENO__
[15]169 nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
[2]170
[15]171 if (nb_write < 0) {
[133]172 char errorMsg[256];
173 Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
[15]174 } else if (nb_write != size) {
175 Err("rt_pipe_write error %i/%i\n", nb_write, size);
176 }
[2]177#else //__XENO__
178
179#ifdef COMPRESS_FRAMES
[15]180 bool sendItCompressed = false;
181 if (buf[0] == XML_HEADER) {
182 sendItCompressed = true;
183 }
[2]184
[15]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, -1, true);
191 free(out);
192 } else {
193 Warn("Compress error, sending it uncompressed\n");
194 sendItCompressed = false;
[2]195 }
[15]196 }
[2]197
[15]198 if (!sendItCompressed) {
199 nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
200 }
[2]201
[15]202#else // COMPRESS_FRAMES
203 nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
204#endif // COMPRESS_FRAMES
[247]205 //Printf("write %i %i\n",nb_write,size);
[15]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;
[2]211 }
[15]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 }
[2]216#endif //__XENO__
217}
218
[15]219ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
220 ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);
[2]221
[15]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;
[2]226 }
[15]227 }
[2]228
[15]229 return bytesRead;
[2]230}
231
[234]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
[15]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 }
[2]252
253#ifdef __XENO__
[15]254 WarnUponSwitches(true);
255 printf("\n"); // a revoir pourquoi??
256// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
[2]257#endif
[15]258 // on attend d'avoir des choses à faire
259 Suspend();
[2]260
[15]261 while (!ToBeStopped()) {
262 size_t resume_id;
263 Time min = 0xffffffffffffffffULL;
[2]264
[15]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 }
[2]274
[15]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);
[2]282
[15]283 // envoi des donnees
284 int offset = 3;
285 send_buffer[0] = header;
286 send_mutex->GetMutex();
[2]287
[15]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));
[157]297 // printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
[15]298 // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
299 // printf("\n");
300 Send(send_buffer, offset);
301 }
[2]302
[15]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;
[2]307 }
[15]308 }
309 send_mutex->ReleaseMutex();
[157]310 //Printf("%i %lld\n",resume_period,GetTime()/1000000);
[15]311 } else {
312 send_mutex->ReleaseMutex();
313 // rien a faire, suspend
[157]314 //Printf("rien a faire suspend\n");
[15]315 Suspend();
[157]316 //Printf("wake\n");
[15]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();
[2]325 }
[15]326 }
[2]327}
328#ifdef __XENO__
[15]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;
[2]338
[15]339 buf = (char *)malloc(NRT_PIPE_SIZE);
340 if (buf == NULL) {
341 caller->Err("malloc error\n");
342 }
[2]343
[15]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);
[133]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 }
[15]352 usleep(1000);
353 }
[2]354
[15]355 while (1) {
356 FD_ZERO(&set); // clear the set
357 FD_SET(pipe_fd, &set); // add our file descriptor to the set
[2]358
[15]359 timeout.tv_sec = 0;
360 timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
[2]361
[15]362 rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
[2]363
[15]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;
[2]376
[15]377 } else {
378 nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
379 buf[nb_read] = 0;
380// printf("envoi\n%s\n",buf);
[2]381
382#ifdef COMPRESS_FRAMES
[15]383 char *out;
384 ssize_t out_size;
[2]385
[15]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 }
[2]398#else
[15]399 nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
[2]400#endif
401
[15]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;
[2]408 }
[15]409 } else if (nb_write != nb_read) {
410 caller->Err("UDT::sendmsg error %i/%i\n", nb_write, nb_read);
411 }
[2]412 }
[15]413 }
[2]414
[15]415 close(pipe_fd);
416 if (buf != NULL)
417 free(buf);
418 pthread_exit(0);
[2]419}
420#endif
421
[15]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;
[2]427
[15]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;
[2]435
[15]436 *out = (char *)malloc(COMPRESS_CHUNK);
437 if (!(*out))
438 return Z_BUF_ERROR;
[2]439
[15]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;
[2]445
[15]446 ret = deflate(&strm, flush); /* no bad return value */
447 if (ret == Z_STREAM_ERROR) {
448 free(*out);
449 return ret;
450 }
[2]451
[15]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);
[2]457
[15]458 if (strm.avail_out != 0) {
459 return Z_OK;
460 } else {
461 return Z_STREAM_ERROR;
462 }
[2]463}
464
[15]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;
[2]470
[15]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;
[2]480
[15]481 *out = (unsigned char *)malloc(COMPRESS_CHUNK);
482 if (!(*out))
483 return Z_BUF_ERROR;
[2]484
[15]485 strm.avail_in = in_size;
486 strm.next_in = in;
487 strm.avail_out = COMPRESS_CHUNK;
488 strm.next_out = *out;
[2]489
[15]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:
[2]497 (void)inflateEnd(&strm);
[15]498 return ret;
499 }
500 // have = COMPRESS_CHUNK - strm.avail_out;
[2]501
[15]502 /* clean up and return */
503 (void)inflateEnd(&strm);
504 return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
[2]505}
506
[15]507bool ui_com::ConnectionLost() { return connection_lost; }
[2]508
[15]509void ui_com::AddSendData(const SendData *obj) {
510 send_mutex->GetMutex();
[2]511
[15]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;
[2]519 }
[15]520 }
[2]521
[15]522 data_to_send.push_back(obj);
[2]523
[15]524 send_mutex->ReleaseMutex();
[2]525}
526
[15]527void ui_com::UpdateSendData(const SendData *obj) {
528 // le mutex est deja pris par l'appellant
529 size_t id, i;
[2]530
[15]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;
[2]536 }
[15]537 }
[2]538
[15]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;
[2]547 }
[15]548 }
[2]549
[15]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;
[2]553
[15]554 if (IsSuspended() == true)
555 Resume();
[2]556
[15]557 return;
[2]558}
559
[15]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 }
[2]567
[15]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();
[2]573}
574
[15]575void ui_com::RemoveSendData(const SendData *obj) {
576 // printf("remove_data_to_send %i\n",data_to_send.size());
[2]577
[15]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;
[2]586 }
[15]587 }
588 send_mutex->ReleaseMutex();
[2]589
[15]590 return;
[2]591}
592
[15]593void ui_com::Block(void) { send_mutex->GetMutex(); }
[2]594
[15]595void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }
Note: See TracBrowser for help on using the repository browser.