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

Last change on this file since 439 was 439, checked in by Sanahuja Guillaume, 17 months ago

refactor

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
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 // on planifie la prochaine execution
322 for (size_t i = 0; i < data_to_send.size(); i++) {
323 if (data_to_send.at(i)->SendPeriod() == resume_period) {
324 resume_time.at(i) += data_to_send.at(i)->SendPeriod() * 1000000;
325 }
326 }
327 send_mutex->ReleaseMutex();
328 //Printf("%i %lld\n",resume_period,GetTime()/1000000);
329 } else {
330 send_mutex->ReleaseMutex();
331 // rien a faire, suspend
332 //Printf("rien a faire suspend\n");
333 Suspend();
334 //Printf("wake\n");
335 // on planifie la prochaine execution
336 Time time = GetTime();
337 send_mutex->GetMutex();
338 for (size_t i = 0; i < data_to_send.size(); i++) {
339 resume_time.at(i) =
340 time + (Time)data_to_send.at(i)->SendPeriod() * 1000000;
341 }
342 send_mutex->ReleaseMutex();
343 }
344 }
345}
346#ifdef __XENO__
347void *ui_com::user_thread(void *arg) {
348 int pipe_fd = -1;
349 string devname;
350 char *buf = NULL;
351 ui_com *caller = (ui_com *)arg;
352 int rv;
353 fd_set set;
354 struct timeval timeout;
355 ssize_t nb_read;
356 int ttl;
357
358 buf = (char *)malloc(NRT_PIPE_SIZE);
359 if (buf == NULL) {
360 caller->Err("malloc error\n");
361 }
362
363 devname = NRT_PIPE_PATH + getFrameworkManager()->ObjectName() + "-" +
364 caller->ObjectName() + "-pipe";
365 while (pipe_fd < 0) {
366 pipe_fd = open(devname.c_str(), O_RDWR);
367 if (pipe_fd < 0 && errno != ENOENT) {
368 char errorMsg[256];
369 caller->Err("open pipe_fd error (%s)\n", strerror_r(errno, errorMsg, sizeof(errorMsg)));
370 }
371 usleep(1000);
372 }
373
374 while (1) {
375 FD_ZERO(&set); // clear the set
376 FD_SET(pipe_fd, &set); // add our file descriptor to the set
377
378 timeout.tv_sec = 0;
379 timeout.tv_usec = SELECT_TIMEOUT_MS * 1000;
380
381 rv = select(FD_SETSIZE, &set, NULL, NULL, &timeout);
382
383 if (rv == -1) {
384 if (caller->is_running == false && UDT::getlasterror().getErrorCode() == CUDTException::ETIMEOUT)
385 break; // timeout
386 if (UDT::getlasterror().getErrorCode() != CUDTException::ETIMEOUT)
387 caller->Err("epoll_wait, %s, code %i\n",
388 UDT::getlasterror().getErrorMessage(), UDT::getlasterror().getErrorCode());
389 } else if (rv == 0) {
390 // printf("timeout\n"); // a timeout occured
391 if (caller->is_running == false)
392 break;
393
394 } else {
395 //read data buf and ttl
396 //RT part has made 2 rt_pipe_write; they should be available in nrt
397 //(select ensure at least one data is ready)
398 nb_read = read(pipe_fd, buf, NRT_PIPE_SIZE);
399 buf[nb_read] = 0;
400 read(pipe_fd, &ttl, sizeof(ttl));
401// printf("envoi\n%s\n",buf);
402
403 caller->SendNRT(buf, nb_read,ttl);
404 }
405 }
406
407 close(pipe_fd);
408 if (buf != NULL)
409 free(buf);
410 pthread_exit(0);
411}
412#endif
413
414int ui_com::compressBuffer(char *in, ssize_t in_size, char **out,
415 ssize_t *out_size, int level) {
416 int ret, flush;
417 unsigned have;
418 z_stream strm;
419
420 /* allocate deflate state */
421 strm.zalloc = Z_NULL;
422 strm.zfree = Z_NULL;
423 strm.opaque = Z_NULL;
424 ret = deflateInit(&strm, level);
425 if (ret != Z_OK)
426 return ret;
427
428 *out = (char *)malloc(COMPRESS_CHUNK);
429 if (!(*out))
430 return Z_BUF_ERROR;
431
432 strm.next_in = (unsigned char *)in;
433 strm.avail_out = COMPRESS_CHUNK;
434 strm.next_out = (unsigned char *)*out;
435 strm.avail_in = in_size;
436 flush = Z_FINISH;
437
438 ret = deflate(&strm, flush); /* no bad return value */
439 if (ret == Z_STREAM_ERROR) {
440 free(*out);
441 return ret;
442 }
443
444 have = COMPRESS_CHUNK - strm.avail_out;
445 *out_size = have;
446 // printf("%i -> %i\n",in_size,have);
447 /* clean up and return */
448 (void)deflateEnd(&strm);
449
450 if (strm.avail_out != 0) {
451 return Z_OK;
452 } else {
453 return Z_STREAM_ERROR;
454 }
455}
456
457int ui_com::uncompressBuffer(unsigned char *in, ssize_t in_size,
458 unsigned char **out, ssize_t *out_size) {
459 int ret;
460 // unsigned have;
461 z_stream strm;
462
463 /* allocate inflate state */
464 strm.zalloc = Z_NULL;
465 strm.zfree = Z_NULL;
466 strm.opaque = Z_NULL;
467 strm.avail_in = 0;
468 strm.next_in = Z_NULL;
469 ret = inflateInit(&strm);
470 if (ret != Z_OK)
471 return ret;
472
473 *out = (unsigned char *)malloc(COMPRESS_CHUNK);
474 if (!(*out))
475 return Z_BUF_ERROR;
476
477 strm.avail_in = in_size;
478 strm.next_in = in;
479 strm.avail_out = COMPRESS_CHUNK;
480 strm.next_out = *out;
481
482 ret = inflate(&strm, Z_NO_FLUSH);
483 assert(ret != Z_STREAM_ERROR); /* state not clobbered */
484 switch (ret) {
485 case Z_NEED_DICT:
486 ret = Z_DATA_ERROR; /* and fall through */
487 case Z_DATA_ERROR:
488 case Z_MEM_ERROR:
489 (void)inflateEnd(&strm);
490 return ret;
491 }
492 // have = COMPRESS_CHUNK - strm.avail_out;
493
494 /* clean up and return */
495 (void)inflateEnd(&strm);
496 return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
497}
498
499bool ui_com::ConnectionLost() { return connection_lost; }
500
501void ui_com::AddSendData(const SendData *obj) {
502 send_mutex->GetMutex();
503
504 resume_time.push_back(GetTime() + (Time)obj->SendPeriod() * 1000000);
505
506 // on resume en meme temps tout ceux qui ont la meme periode
507 for (size_t i = 0; i < data_to_send.size(); i++) {
508 if (data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
509 resume_time.at(resume_time.size() - 1) = resume_time.at(i);
510 break;
511 }
512 }
513
514 data_to_send.push_back(obj);
515
516 send_mutex->ReleaseMutex();
517}
518
519void ui_com::UpdateSendData(const SendData *obj) {
520 // le mutex est deja pris par l'appellant
521 size_t id, i;
522
523 // on recupere l'id
524 for (i = 0; i < data_to_send.size(); i++) {
525 if (data_to_send.at(i) == obj) {
526 id = i;
527 break;
528 }
529 }
530
531 // on resume en meme temps tout ceux qui ont la meme periode
532 for (i = 0; i < data_to_send.size(); i++) {
533 if (i == id)
534 continue;
535 if (data_to_send.at(i)->IsEnabled() == true &&
536 data_to_send.at(i)->SendPeriod() == obj->SendPeriod()) {
537 resume_time.at(id) = resume_time.at(i);
538 break;
539 }
540 }
541
542 // si aucun match, on planifie l'execution
543 if (i == data_to_send.size())
544 resume_time.at(id) = GetTime() + (Time)obj->SendPeriod() * 1000000;
545
546 if (IsSuspended() == true)
547 Resume();
548
549 return;
550}
551
552//allocate a buffer at startup for the worst case
553//(every data send with same pedriod)
554//TODO: dynamic size depending on what gcs asks?
555void ui_com::UpdateDataToSendSize(void) {
556 send_mutex->GetMutex();
557 send_size = 3; // id(1)+period(2)
558 for (size_t i = 0; i < data_to_send.size(); i++) {
559 if (data_to_send[i] != NULL)
560 send_size += data_to_send[i]->SendSize();
561 }
562
563 // send_buffer=(char*)realloc((void*)send_buffer,send_size*sizeof(char));
564 if (send_buffer != NULL)
565 free(send_buffer);
566 send_buffer = (char *)malloc(send_size * sizeof(char));
567 send_mutex->ReleaseMutex();
568}
569
570void ui_com::RemoveSendData(const SendData *obj) {
571 // printf("remove_data_to_send %i\n",data_to_send.size());
572
573 send_mutex->GetMutex();
574 // on recupere l'id
575 for (size_t i = 0; i < data_to_send.size(); i++) {
576 if (data_to_send.at(i) == obj) {
577 data_to_send.erase(data_to_send.begin() + i);
578 resume_time.erase(resume_time.begin() + i);
579 // printf("remove_data_to_send %i ok\n",data_to_send.size());
580 break;
581 }
582 }
583 send_mutex->ReleaseMutex();
584
585 return;
586}
587
588void ui_com::Block(void) { send_mutex->GetMutex(); }
589
590void ui_com::UnBlock(void) { send_mutex->ReleaseMutex(); }
Note: See TracBrowser for help on using the repository browser.