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, 6 years ago

resolve some bugs when closing connection with gcs
File size: 15.5 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 bool blocking = true;
109 if (UDT::setsockopt(socket_fd, 0, UDT_SNDSYN, &blocking, sizeof(bool)) != 0)
110 Err("UDT::setsockopt error (UDT_SNDSYN)\n");
111
112 if (UDT::setsockopt(socket_fd, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
113 Err("UDT::setsockopt error (UDT_RCVSYN)\n");
114 //#endif //__XENO__
115
116 Start();
117}
118
119ui_com::~ui_com() {
120//Printf("destruction ui_com\n");
121
122#ifdef __XENO__
123 is_running = false;
124
125 pthread_join(thread, NULL);
126
127 int status = rt_pipe_delete(&pipe);
128 if (status != 0) {
129 char errorMsg[256];
130 Err("rt_pipe_delete error (%s)\n", strerror_r(-status, errorMsg, sizeof(errorMsg)));
131 }
132#endif
133
134 SafeStop();
135
136 if (IsSuspended() == true)
137 Resume();
138
139 Join();
140
141 if (send_buffer != NULL)
142 free(send_buffer);
143 send_buffer = NULL;
144
145 //Printf("destruction ui_com ok\n");
146}
147
148void ui_com::Send(char *buf, ssize_t size) {
149 if (connection_lost == true)
150 return;
151
152 char *tosend = buf;
153 ssize_t nb_write;
154
155 if (buf[0] == XML_HEADER) {
156 // cut xml header
157 tosend = strstr(buf, "<root");
158 size -= tosend - buf;
159 }
160
161#ifdef __XENO__
162 nb_write = rt_pipe_write(&pipe, tosend, size, P_NORMAL);
163
164 if (nb_write < 0) {
165 char errorMsg[256];
166 Err("rt_pipe_write error (%s)\n", strerror_r(-nb_write, errorMsg, sizeof(errorMsg)));
167 } else if (nb_write != size) {
168 Err("rt_pipe_write error %i/%i\n", nb_write, size);
169 }
170#else //__XENO__
171
172#ifdef COMPRESS_FRAMES
173 bool sendItCompressed = false;
174 if (buf[0] == XML_HEADER) {
175 sendItCompressed = true;
176 }
177
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;
188 }
189 }
190
191 if (!sendItCompressed) {
192 nb_write = UDT::sendmsg(socket_fd, tosend, size, -1, true);
193 }
194
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;
204 }
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 }
209#endif //__XENO__
210}
211
212ssize_t ui_com::Receive(char *buf, ssize_t buf_size) {
213 ssize_t bytesRead = UDT::recvmsg(socket_fd, buf, buf_size);
214
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;
219 }
220 }
221
222 return bytesRead;
223}
224
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
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 }
245
246#ifdef __XENO__
247 WarnUponSwitches(true);
248 printf("\n"); // a revoir pourquoi??
249// sans ce printf, dans le simu_roll, le suspend ne fonctionne pas...
250#endif
251 // on attend d'avoir des choses à faire
252 Suspend();
253
254 while (!ToBeStopped()) {
255 size_t resume_id;
256 Time min = 0xffffffffffffffffULL;
257
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 }
267
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);
275
276 // envoi des donnees
277 int offset = 3;
278 send_buffer[0] = header;
279 send_mutex->GetMutex();
280
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));
290 // printf("send %i %i %i %x %x\n",resume_period,offset,sizeof(uint16_t),send_buffer,&send_buffer[1]);
291 // for(int i=0;i<offset;i++) printf("%x ",send_buffer[i]);
292 // printf("\n");
293 Send(send_buffer, offset);
294 }
295
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;
300 }
301 }
302 send_mutex->ReleaseMutex();
303 //Printf("%i %lld\n",resume_period,GetTime()/1000000);
304 } else {
305 send_mutex->ReleaseMutex();
306 // rien a faire, suspend
307 //Printf("rien a faire suspend\n");
308 Suspend();
309 //Printf("wake\n");
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();
318 }
319 }
320}
321#ifdef __XENO__
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;
331
332 buf = (char *)malloc(NRT_PIPE_SIZE);
333 if (buf == NULL) {
334 caller->Err("malloc error\n");
335 }
336
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);
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 }
345 usleep(1000);
346 }
347
348 while (1) {
349 FD_ZERO(&set); // clear the set
350 FD_SET(pipe_fd, &set); // add our file descriptor to the set
351
352 timeout.tv_sec = 0;
353 timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
354
355 rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
356
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;
369
370 } else {
371 nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
372 buf[nb_read] = 0;
373// printf("envoi\n%s\n",buf);
374
375#ifdef COMPRESS_FRAMES
376 char *out;
377 ssize_t out_size;
378
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 }
391#else
392 nb_write = UDT::sendmsg(caller->socket_fd, buf, nb_read, -1, true);
393#endif
394
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;
401 }
402 } else if (nb_write != nb_read) {
403 caller->Err("UDT::sendmsg error %i/%i\n", nb_write, nb_read);
404 }
405 }
406 }
407
408 close(pipe_fd);
409 if (buf != NULL)
410 free(buf);
411 pthread_exit(0);
412}
413#endif
414
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;
420
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;
428
429 *out = (char *)malloc(COMPRESS_CHUNK);
430 if (!(*out))
431 return Z_BUF_ERROR;
432
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;
438
439 ret = deflate(&strm, flush); /* no bad return value */
440 if (ret == Z_STREAM_ERROR) {
441 free(*out);
442 return ret;
443 }
444
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);
450
451 if (strm.avail_out != 0) {
452 return Z_OK;
453 } else {
454 return Z_STREAM_ERROR;
455 }
456}
457
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;
463
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;
473
474 *out = (unsigned char *)malloc(COMPRESS_CHUNK);
475 if (!(*out))
476 return Z_BUF_ERROR;
477
478 strm.avail_in = in_size;
479 strm.next_in = in;
480 strm.avail_out = COMPRESS_CHUNK;
481 strm.next_out = *out;
482
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:
490 (void)inflateEnd(&strm);
491 return ret;
492 }
493 // have = COMPRESS_CHUNK - strm.avail_out;
494
495 /* clean up and return */
496 (void)inflateEnd(&strm);
497 return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
498}
499
500bool ui_com::ConnectionLost() { return connection_lost; }
501
502void ui_com::AddSendData(const SendData *obj) {
503 send_mutex->GetMutex();
504
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;
512 }
513 }
514
515 data_to_send.push_back(obj);
516
517 send_mutex->ReleaseMutex();
518}
519
520void ui_com::UpdateSendData(const SendData *obj) {
521 // le mutex est deja pris par l'appellant
522 size_t id, i;
523
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;
529 }
530 }
531
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;
540 }
541 }
542
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;
546
547 if (IsSuspended() == true)
548 Resume();
549
550 return;
551}
552
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 }
560
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();
566}
567
568void ui_com::RemoveSendData(const SendData *obj) {
569 // printf("remove_data_to_send %i\n",data_to_send.size());
570
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;
579 }
580 }
581 send_mutex->ReleaseMutex();
582
583 return;
584}
585
586void ui_com::Block(void) { send_mutex->GetMutex(); }
587
588void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }
Note: See TracBrowser for help on using the repository browser.