[10] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[10] | 4 | // %flair:license}
|
---|
[9] | 5 | #include "UdtSocket.h"
|
---|
[234] | 6 | #include "ConnectionLayout.h"
|
---|
[9] | 7 | #include <stdio.h>
|
---|
| 8 | #include <stdlib.h>
|
---|
| 9 | #include <QApplication>
|
---|
| 10 | #include <QtEndian>
|
---|
| 11 | #include <QDir>
|
---|
| 12 | #include <QDate>
|
---|
| 13 | #include "communication.h"
|
---|
[234] | 14 | #include <zlib.h>
|
---|
| 15 | #include <assert.h>
|
---|
| 16 | #include <QThread>
|
---|
[399] | 17 |
|
---|
[234] | 18 | #define COMPRESS_CHUNK 1024*100
|
---|
[9] | 19 |
|
---|
| 20 | #ifndef WIN32
|
---|
[15] | 21 | #include <arpa/inet.h>
|
---|
[9] | 22 | #else
|
---|
[15] | 23 | #include <winsock2.h>
|
---|
| 24 | #include <ws2tcpip.h>
|
---|
[9] | 25 | #endif
|
---|
| 26 |
|
---|
[234] | 27 | #define COMPUTE_UDT_STATS_TIMER 2000
|
---|
| 28 | #define HEARTBEAT_TIMER 200
|
---|
| 29 |
|
---|
[9] | 30 | using namespace std;
|
---|
| 31 |
|
---|
[234] | 32 | UdtSocket::UdtSocket(UDTSOCKET socket,QString name): QObject() {
|
---|
| 33 | this->socket = socket;
|
---|
| 34 | this->name=name;
|
---|
[15] | 35 | stop = false;
|
---|
[234] | 36 | destroySocket=true;
|
---|
| 37 | socketType=unknown;
|
---|
| 38 | total_received=0;
|
---|
[248] | 39 | pktSndLossTotal=0;
|
---|
[399] | 40 | lastpktRecvTotal=0;
|
---|
| 41 |
|
---|
[234] | 42 | bool blocking = false;
|
---|
| 43 | if (UDT::setsockopt(socket, 0, UDT_RCVSYN, &blocking, sizeof(bool)) != 0)
|
---|
[244] | 44 | fprintf(stderr,"UDT::setsockopt error (UDT_RCVSYN)\n");
|
---|
[234] | 45 |
|
---|
[15] | 46 | heartbeat_timer = new QTimer(this);
|
---|
| 47 | connect(heartbeat_timer, SIGNAL(timeout()), this, SLOT(heartbeat()));
|
---|
[234] | 48 | heartbeat_timer->start(HEARTBEAT_TIMER);
|
---|
[253] | 49 | //heartbeat();//send directly the first one, but conflicts if this socket is for logs... TODO: delay start of watchdog timer on uav side
|
---|
[234] | 50 | udtstats_timer = new QTimer(this);
|
---|
| 51 | connect(udtstats_timer, SIGNAL(timeout()), this, SLOT(getUTDStats()));
|
---|
| 52 | udtstats_timer->start(COMPUTE_UDT_STATS_TIMER);
|
---|
[9] | 53 | }
|
---|
| 54 |
|
---|
| 55 | UdtSocket::~UdtSocket() {
|
---|
[15] | 56 | heartbeat_timer->stop();
|
---|
[234] | 57 | udtstats_timer->stop();
|
---|
[247] | 58 | if(destroySocket) UDT::close(socket);
|
---|
[234] | 59 | }
|
---|
[9] | 60 |
|
---|
[234] | 61 | void UdtSocket::setName(QString name) {
|
---|
[244] | 62 | fprintf(stderr," %s is %s\n",this->name.toLocal8Bit().constData(),name.toLocal8Bit().constData());
|
---|
[234] | 63 | this->name=name;
|
---|
[9] | 64 | }
|
---|
| 65 |
|
---|
[15] | 66 | // send signal to uav, to check connectivity through a watchdog
|
---|
| 67 | // this is necessary because we use udt (udp based), and we cannot check
|
---|
| 68 | // disconnection of ground station
|
---|
[9] | 69 | void UdtSocket::heartbeat(void) {
|
---|
[269] | 70 | //printf("h %s %i.%i\n",name.toLocal8Bit().constData(),QTime::currentTime().second(),QTime::currentTime().msec());
|
---|
[15] | 71 | char data = WATCHDOG_HEADER;
|
---|
[254] | 72 | quint64 sent=write(&data, 1,HEARTBEAT_TIMER,true);
|
---|
[234] | 73 | if (sent != 1 && UDT::getlasterror().getErrorCode() == 2001) {
|
---|
| 74 | heartbeat_timer->stop();
|
---|
| 75 | }
|
---|
| 76 |
|
---|
[9] | 77 | }
|
---|
| 78 |
|
---|
[234] | 79 | void UdtSocket::getUTDStats(void) {
|
---|
| 80 | float rxRate=((float)total_received/(COMPUTE_UDT_STATS_TIMER/1000))/1000;//in Ko/s
|
---|
| 81 | total_received=0;
|
---|
| 82 |
|
---|
| 83 | UDT::TRACEINFO perf;
|
---|
| 84 | if (UDT::ERROR == UDT::perfmon(socket, &perf)) {
|
---|
[244] | 85 | fprintf(stderr,"perfmon: %s\n",UDT::getlasterror().getErrorMessage());
|
---|
[234] | 86 | }/* else {
|
---|
[244] | 87 | fprintf(stderr,"%s socket stats:\n",name.toLocal8Bit().constData());
|
---|
| 88 | fprintf(stderr,"total number of sent packets, including retransmissions: %i\n",perf.pktSentTotal);
|
---|
| 89 | fprintf(stderr,"total number of received packets: %i\n",perf.pktRecvTotal);
|
---|
| 90 | fprintf(stderr,"total number of lost packets, measured in the sending side: %i\n",perf.pktSndLossTotal);
|
---|
| 91 | fprintf(stderr,"total number of lost packets, measured in the receiving side: %i\n",perf.pktRcvLossTotal);
|
---|
| 92 | fprintf(stderr,"total number of retransmitted packets, measured in the sending side: %i\n",perf.pktRetransTotal);
|
---|
| 93 | fprintf(stderr,"total number of sent ACK packets: %i\n",perf.pktSentACKTotal);
|
---|
| 94 | fprintf(stderr,"total number of received ACK packets: %i\n",perf.pktRecvACKTotal);
|
---|
| 95 | fprintf(stderr,"total number of sent NAK packets: %i\n",perf.pktSentNAKTotal);
|
---|
| 96 | fprintf(stderr,"total number of received NAK packets: %i\n",perf.pktRecvNAKTotal);
|
---|
| 97 | fprintf(stderr,"round trip time: %fms\n",perf.msRTT);
|
---|
[234] | 98 | }*/
|
---|
[248] | 99 | bool loosingPackets=false;
|
---|
| 100 | if(perf.pktSndLossTotal>pktSndLossTotal) loosingPackets=true;
|
---|
| 101 | pktSndLossTotal=perf.pktSndLossTotal;
|
---|
[399] | 102 | unsigned int pktPerSec=(perf.pktRecvTotal-lastpktRecvTotal)*1000/COMPUTE_UDT_STATS_TIMER;
|
---|
| 103 | QString stylesheet="color: black";
|
---|
| 104 | if(pktPerSec>=100) {
|
---|
| 105 | stylesheet="color: red";
|
---|
| 106 | }
|
---|
| 107 | else if(pktPerSec>=50) {
|
---|
| 108 | stylesheet="color: orange";
|
---|
| 109 | }
|
---|
| 110 | stats=QString("rx rate %1kB/s, %2 rx packets/s, round trip %3ms, lost packets %4").arg(rxRate,0,'f',3).arg(pktPerSec).arg(perf.msRTT,0,'f',3).arg(perf.pktSndLossTotal);
|
---|
| 111 | lastpktRecvTotal=perf.pktRecvTotal;
|
---|
| 112 | UDTStats(stats,stylesheet,loosingPackets);
|
---|
[234] | 113 | }
|
---|
| 114 |
|
---|
| 115 | QString UdtSocket::getUDTStats() {
|
---|
| 116 | return stats;
|
---|
| 117 | }
|
---|
| 118 |
|
---|
[9] | 119 | void UdtSocket::kill(void) {
|
---|
[15] | 120 | stop = true;
|
---|
| 121 | deleteLater();
|
---|
[9] | 122 | }
|
---|
| 123 |
|
---|
[234] | 124 | void UdtSocket::receiveData(void) {
|
---|
| 125 | int buf_size;
|
---|
| 126 | int opt_size;
|
---|
| 127 | UDT::getsockopt(socket, 0, UDT_RCVBUF, &buf_size, &opt_size);
|
---|
| 128 | char *buf = (char *)malloc(buf_size);
|
---|
| 129 | if (!buf) {
|
---|
[244] | 130 | fprintf(stderr,"error malloc UdtSocket::receiveData buffer\n");
|
---|
[234] | 131 | return;
|
---|
| 132 | }
|
---|
| 133 | char *uncompressbuf=(char *)malloc(COMPRESS_CHUNK);
|
---|
| 134 | if (!uncompressbuf) {
|
---|
[244] | 135 | fprintf(stderr,"error malloc UdtSocket::receiveData uncompress buffer\n");
|
---|
[234] | 136 | free(buf);
|
---|
| 137 | return;
|
---|
| 138 | }
|
---|
[248] | 139 | //fprintf(stderr,stderr,"receiveData %x\n",thread());
|
---|
[234] | 140 |
|
---|
[15] | 141 | while (!stop) {
|
---|
[234] | 142 | QCoreApplication::processEvents();
|
---|
| 143 |
|
---|
| 144 | //we need to use epoll?
|
---|
| 145 | //tests are showing that UDT::recvmsg is waiting for the entire timeout before returning
|
---|
| 146 | //note that in flair::core the behaviour of UDT::recvmsg timeout is as expected
|
---|
| 147 | //is it a difference between client and server??
|
---|
| 148 | //tests also show that we need to recreate the eid every time, otherwise wait returns immediately
|
---|
[15] | 149 | int eid = UDT::epoll_create();
|
---|
| 150 | if (eid < 0) {
|
---|
[244] | 151 | fprintf(stderr,"%s: epoll_create error (%s)\n",name.toLocal8Bit().constData(),UDT::getlasterror().getErrorMessage());
|
---|
[15] | 152 | }
|
---|
[9] | 153 |
|
---|
[234] | 154 | if (UDT::epoll_add_usock(eid, socket) < 0) {
|
---|
[15] | 155 | if (UDT::getlasterror().getErrorCode() == 5004) {
|
---|
[244] | 156 | fprintf(stderr,"disconnected from %s\n",name.toLocal8Bit().constData());
|
---|
[234] | 157 | heartbeat_timer->stop();
|
---|
| 158 | deleteLater();
|
---|
| 159 | stop=true;;
|
---|
[15] | 160 | } else {
|
---|
[244] | 161 | fprintf(stderr,"%s: epoll_add_usock error (%s)\n",name.toLocal8Bit().constData(),UDT::getlasterror().getErrorMessage());
|
---|
[15] | 162 | }
|
---|
| 163 | }
|
---|
[234] | 164 |
|
---|
| 165 | int num = 1;
|
---|
| 166 | UDTSOCKET readfds;
|
---|
[247] | 167 |
|
---|
[234] | 168 | int rv = UDT::epoll_wait2(eid, &readfds, &num,NULL, NULL,100);
|
---|
[15] | 169 | if (rv == -1) {
|
---|
| 170 | if (UDT::getlasterror().getErrorCode() != 6003)
|
---|
[244] | 171 | fprintf(stderr,"prob %i\n", UDT::getlasterror().getErrorCode());
|
---|
[234] | 172 | } else if(readfds==socket && num==1 && rv==1) {
|
---|
[247] | 173 | int size;
|
---|
[269] | 174 | QTime initTime;
|
---|
| 175 | initTime.start();
|
---|
[247] | 176 | do {
|
---|
[269] | 177 | //we are in "infinite" loop, so let the heartbeat run if needed
|
---|
| 178 | if(initTime.elapsed()>HEARTBEAT_TIMER) {
|
---|
| 179 | initTime.start();
|
---|
| 180 | //do not use error check of heartbeat() method: (otherwise, closes com too early)
|
---|
| 181 | char data = WATCHDOG_HEADER;
|
---|
| 182 | UDT::sendmsg(socket, &data, 1, HEARTBEAT_TIMER, true);
|
---|
| 183 | }
|
---|
| 184 |
|
---|
[247] | 185 | size=UDT::recvmsg(socket, buf, buf_size);
|
---|
| 186 |
|
---|
[253] | 187 | //fprintf(stderr,"recu %i %x\n",size,buf[0]);
|
---|
[247] | 188 | if (size > 0) {
|
---|
| 189 | total_received+=size;
|
---|
| 190 |
|
---|
| 191 | switch ((unsigned char)buf[0]) {
|
---|
| 192 | case ZLIB_HEADER: {
|
---|
| 193 | ssize_t out_size;
|
---|
| 194 | uncompressBuffer(buf, size, uncompressbuf, &out_size);
|
---|
| 195 | if((unsigned char)uncompressbuf[0]==XML_HEADER && socketType==unknown) {
|
---|
| 196 | socketType=gui;
|
---|
| 197 | QString remoteName=ConnectionLayout::getDocRootName(uncompressbuf, out_size);
|
---|
| 198 | setName(remoteName);
|
---|
| 199 | emit newConnectionLayout(remoteName);//connection is Qt::BlockingQueuedConnection
|
---|
| 200 | }
|
---|
| 201 | emit dataReady(uncompressbuf, out_size);//connection is Qt::BlockingQueuedConnection, as we have only one buffer
|
---|
| 202 | break;
|
---|
[234] | 203 | }
|
---|
[247] | 204 | case START_SENDING_FILES: {
|
---|
[255] | 205 | //printf("log\n");
|
---|
[247] | 206 | if((unsigned char)uncompressbuf[0]==XML_HEADER && socketType==unknown) {
|
---|
| 207 | socketType=log;
|
---|
| 208 | }
|
---|
| 209 | setName("log files");
|
---|
| 210 | heartbeat_timer->stop();
|
---|
| 211 | emit newFileUI(socket);
|
---|
| 212 | deleteLater();
|
---|
| 213 | stop=true;
|
---|
[253] | 214 | size=-1;//exit from do while loop
|
---|
[247] | 215 | destroySocket=false;
|
---|
| 216 | break;
|
---|
[234] | 217 | }
|
---|
[247] | 218 | case XML_HEADER:
|
---|
| 219 | if(socketType==unknown) {
|
---|
| 220 | socketType=gui;
|
---|
| 221 | QString remoteName=ConnectionLayout::getDocRootName(buf, size );
|
---|
| 222 | setName(remoteName);
|
---|
| 223 | emit newConnectionLayout(remoteName);
|
---|
| 224 | }
|
---|
[253] | 225 | case DATA_BIG_ENDIAN:
|
---|
| 226 | case DATA_LITTLE_ENDIAN:
|
---|
[443] | 227 | case MULTIPLE_DATA_BIG_ENDIAN:
|
---|
| 228 | case MULTIPLE_DATA_LITTLE_ENDIAN:
|
---|
| 229 | emit dataReady(buf, size);
|
---|
[247] | 230 | break;
|
---|
| 231 | case CLOSING_CONNECTION:
|
---|
[255] | 232 | fprintf(stderr,"%s, connection closed by remote\n",name.toLocal8Bit().constData());
|
---|
[247] | 233 | emit dataReady(buf, size );
|
---|
| 234 | stop = true;
|
---|
| 235 | heartbeat_timer->stop();
|
---|
| 236 | deleteLater();
|
---|
| 237 | break;
|
---|
| 238 | default:
|
---|
[253] | 239 | fprintf(stderr,"udt trame non supportée %x\n", buf[0]);
|
---|
[234] | 240 | }
|
---|
[247] | 241 | } else {
|
---|
| 242 | //not necessary to check, watchdog (heartbeat_timer) can do it
|
---|
| 243 | //if(UDT::getlasterror().getErrorCode()!=6002 && !stop)
|
---|
| 244 | //fprintf(stderr,"udt socket: %s\n",UDT::getlasterror().getErrorMessage());
|
---|
| 245 | //UDT::close(socket);//si deconnecté
|
---|
| 246 | //free(buf);
|
---|
| 247 | //break;
|
---|
[234] | 248 | }
|
---|
[247] | 249 | } while(size>0);
|
---|
[15] | 250 | } else {
|
---|
[247] | 251 | //not necessary to check, watchdog (heartbeat_timer) can do it
|
---|
| 252 | //fprintf(stderr,"udt socket: %s\n",UDT::getlasterror().getErrorMessage());
|
---|
[15] | 253 | }
|
---|
[234] | 254 | UDT::epoll_remove_usock(eid, socket);
|
---|
[15] | 255 | UDT::epoll_release(eid);
|
---|
| 256 | }
|
---|
[247] | 257 |
|
---|
[234] | 258 | free(uncompressbuf);
|
---|
| 259 | free(buf);
|
---|
| 260 | thread()->quit();
|
---|
[9] | 261 | }
|
---|
| 262 |
|
---|
[234] | 263 | qint64 UdtSocket::write(const char *buf, qint64 size,int ttl,bool inOrder) {
|
---|
| 264 | qint64 sent = UDT::sendmsg(socket, buf, size, ttl, inOrder);
|
---|
| 265 | if (sent != size) {
|
---|
[258] | 266 | fprintf(stderr,"sent %lld/%lld %s\n",sent ,size,UDT::getlasterror().getErrorMessage());
|
---|
[244] | 267 | fprintf(stderr,"%s, error writting to udt (%s)\n",name.toLocal8Bit().constData(), UDT::getlasterror().getErrorMessage());
|
---|
[234] | 268 | if (UDT::getlasterror().getErrorCode() == 2001) {
|
---|
[244] | 269 | fprintf(stderr,"%s, closing connection\n",name.toLocal8Bit().constData());
|
---|
[234] | 270 | stop = true;
|
---|
| 271 | heartbeat_timer->stop();
|
---|
| 272 | deleteLater();
|
---|
[15] | 273 | }
|
---|
| 274 | }
|
---|
[234] | 275 | return sent;
|
---|
[9] | 276 | }
|
---|
| 277 |
|
---|
[234] | 278 | int UdtSocket::uncompressBuffer(char *in, ssize_t in_size, char *out,
|
---|
| 279 | ssize_t *out_size) {
|
---|
| 280 | int ret;
|
---|
| 281 | unsigned have;
|
---|
| 282 | z_stream strm;
|
---|
[9] | 283 |
|
---|
[234] | 284 | // allocate inflate state
|
---|
| 285 | strm.zalloc = Z_NULL;
|
---|
| 286 | strm.zfree = Z_NULL;
|
---|
| 287 | strm.opaque = Z_NULL;
|
---|
| 288 | strm.avail_in = 0;
|
---|
| 289 | strm.next_in = Z_NULL;
|
---|
| 290 | ret = inflateInit(&strm);
|
---|
| 291 | if (ret != Z_OK)
|
---|
| 292 | return ret;
|
---|
[9] | 293 |
|
---|
[234] | 294 | strm.avail_in = in_size;
|
---|
| 295 | strm.next_in = (unsigned char *)in;
|
---|
| 296 | strm.avail_out = COMPRESS_CHUNK;
|
---|
| 297 | strm.next_out = (unsigned char *)out;
|
---|
| 298 |
|
---|
| 299 | ret = inflate(&strm, Z_NO_FLUSH);
|
---|
| 300 | assert(ret != Z_STREAM_ERROR); // state not clobbered
|
---|
| 301 | switch (ret) {
|
---|
| 302 | case Z_NEED_DICT:
|
---|
| 303 | ret = Z_DATA_ERROR; // and fall through
|
---|
| 304 | case Z_DATA_ERROR:
|
---|
| 305 | case Z_MEM_ERROR:
|
---|
| 306 | (void)inflateEnd(&strm);
|
---|
| 307 | return ret;
|
---|
[15] | 308 | }
|
---|
[234] | 309 | have = COMPRESS_CHUNK - strm.avail_out;
|
---|
| 310 | *out_size = have;
|
---|
[9] | 311 |
|
---|
[234] | 312 | // clean up and return
|
---|
| 313 | (void)inflateEnd(&strm);
|
---|
| 314 | return ret == Z_STREAM_END ? Z_OK : Z_DATA_ERROR;
|
---|
[307] | 315 | }
|
---|