void RemoteInterfaceListener::run() {
// Init socket
stopRequested = false;
QUdpSocket socket;
if(!socket.bind(QHostAddress(server->ip), server->port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint)) {
// Show error
Debug::print("[RemoteInterfaceListener] could not bind");
stopRequested = true;
}
// Enter processing loop...
while (stopRequested == false) {
// Process all pending datagrams
while(socket.hasPendingDatagrams() && !stopRequested) {
// Get the data from socket
QByteArray data;
data.resize(socket.pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
socket.readDatagram(data.data(), data.size(), &sender, &senderPort);
// Extract message and keep track of the pointer in the server
RemoteInterfaceMessage *message = new RemoteInterfaceMessage(&data);
message->sourceIp = sender.toString();
message->sourcePort = senderPort;
message->destinationIp = server->ip;
message->destinationPort = server->port;
server->messages->append(message);
// Show reception and pass on for processing...
Debug::print("[RemoteInterfaceListener] received %1 message from %2:%3", message->type, sender.toString(), senderPort);
server->processMessage(message);
}
// Sleep wait
QThread::msleep(core->intSetting("RemoteInterface_ListenerInterval"));
}
}
void Connection::slReceive() {
QUdpSocket* sock = (QUdpSocket*) QObject::sender();
const bool main = (sock->localPort() == sets.portMain) ? true : false;
const bool aux = (sock->localPort() == sets.portAux) ? true : false;
if (sock->hasPendingDatagrams()) {
if (main) emit sgRxMain();
if (aux) emit sgRxAux();
}
int size = 0;
bool enabled = false;
if (main) enabled = sets.main;
if (aux) enabled = sets.aux;
while (sock->hasPendingDatagrams() && run && enabled) {
size = sock->pendingDatagramSize();
convertor->setBufSize(size);
sock->readDatagram(convertor->getBuf(), size);
if (convertor->convert()) emit sgWr();
else emit sgNoWr();
if (main) timerRxMain->start();
if (aux) timerRxAux->start();
}
}
void tst_QUdpSocket::broadcasting()
{
QFETCH_GLOBAL(bool, setProxy);
if (setProxy) {
#ifdef TEST_QNETWORK_PROXY
QFETCH_GLOBAL(int, proxyType);
if (proxyType == QNetworkProxy::Socks5Proxy) {
QSKIP("With socks5 Broadcast is not supported.", SkipAll);
}
#endif
}
#ifdef Q_OS_AIX
QSKIP("Broadcast does not work on darko", SkipAll);
#endif
const char *message[] = {"Yo mista", "", "Yo", "Wassap"};
for (int i = 0; i < 4; ++i) {
QUdpSocket serverSocket;
QVERIFY2(serverSocket.bind(QHostAddress::Any, 5000), serverSocket.errorString().toLatin1().constData());
QCOMPARE(serverSocket.state(), QUdpSocket::BoundState);
connect(&serverSocket, SIGNAL(readyRead()), SLOT(empty_readyReadSlot()));
QUdpSocket broadcastSocket;
for (int j = 0; j < 100; ++j) {
broadcastSocket.writeDatagram(message[i], strlen(message[i]),
QHostAddress::Broadcast, 5000);
QTestEventLoop::instance().enterLoop(15);
if (QTestEventLoop::instance().timeout()) {
#if defined(Q_OS_FREEBSD)
QEXPECT_FAIL("",
"Broadcasting to 255.255.255.255 does not work on FreeBSD",
Abort);
QVERIFY(false); // seems that QFAIL() doesn't respect the QEXPECT_FAIL() :/
#endif
QFAIL("Network operation timed out");
}
QVERIFY(serverSocket.hasPendingDatagrams());
do {
QByteArray arr;
arr.resize(serverSocket.pendingDatagramSize() + 1);
QHostAddress host;
quint16 port;
QCOMPARE((int) serverSocket.readDatagram(arr.data(), arr.size() - 1, &host, &port),
(int) strlen(message[i]));
arr.resize(strlen(message[i]));
QCOMPARE(arr, QByteArray(message[i]));
} while (serverSocket.hasPendingDatagrams());
}
}
}
void StunClient::onStunReadyRead()
{
QUdpSocket *sock = (QUdpSocket*)(sender());
qDebug()<<""<<sock;
u08bits rbuf[STUN_BUFFER_SIZE];
while (sock->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(sock->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
sock->readDatagram(datagram.data(), datagram.size(),
&sender, &senderPort);
{
m_sending_udp = false;
m_sending_timer->stop();
}
qDebug()<<"read: "<<sender<<senderPort<<datagram.length()<<datagram.toHex().left(20)<<"...";
for (int i = 0; i < datagram.length(); i ++) {
char c = datagram.at(i);
fprintf(stderr, "%c", isprint(c) ? c : '.');
if (i > 375) break;
}
fprintf(stderr, " ...]\n");
this->processResponse(datagram, QString("%1:%2").arg(sender.toString()).arg(senderPort));
// processTheDatagram(datagram);
}
}
void QDmxE131Controler::readDatagram()
{
if(sender())
{
QUdpSocket* socket = qobject_cast<QUdpSocket*>(sender());
if(socket)
{
while(socket->hasPendingDatagrams())
{
QNetworkDatagram datagram = socket->receiveDatagram();
if(datagram.senderAddress().isInSubnet(_address, _entry.prefixLength()))
{
QByteArray rawData = datagram.data();
if(_packetizer->checkPacket(rawData))
{
QByteArray dmx;
quint32 u;
_packetizer->fillDMXdata(rawData, dmx, u);
u--;
if(_listenedUniverse.contains(u))
emit hasDmx(u, dmx);
}
}
}
}
}
}
void MainWindow::controlMessageReceived() {
qDebug() << "message received";
QUdpSocket* udpSocket = qobject_cast<QUdpSocket*>(sender());
while (udpSocket->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(udpSocket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
udpSocket->readDatagram(datagram.data(), datagram.size(),
&sender, &senderPort);
if (datagram=="left") {
drone->moveLeft();
} else if (datagram== "right") {
drone->moveRight();
} else if (datagram=="stayx" && datagram=="stayy"){
drone->stay();
} else if (datagram=="up") {
drone->higher();
} else if (datagram=="down") {
drone->lower();
} else {
qDebug() << "invalid command received";
}
}
}
// Called whenever there is data available on the device.
void SignalChunker::next(QIODevice* device)
{
// Get a pointer to the UDP socket.
QUdpSocket* udpSocket = static_cast<QUdpSocket*>(device);
_bytesRead = 0;
// Get writable buffer space for the chunk.
WritableData writableData = getDataStorage(_chunkSize);
if (writableData.isValid()) {
// Get pointer to start of writable memory.
char* ptr = (char*) (writableData.ptr());
// Read datagrams for chunk from the UDP socket.
while (isActive() && _bytesRead < _chunkSize) {
// Read the datagram, but avoid using pendingDatagramSize().
if (!udpSocket->hasPendingDatagrams()) {
// MUST WAIT for the next datagram.
udpSocket->waitForReadyRead(100);
continue;
}
qint64 maxlen = _chunkSize - _bytesRead;
qint64 length = udpSocket->readDatagram(ptr + _bytesRead, maxlen);
if (length > 0)
_bytesRead += length;
}
}
// Must discard the datagram if there is no available space.
else {
udpSocket->readDatagram(0, 0);
}
}
void OceanSocket::processIncomingWaves()
{
QUdpSocket* socket = dynamic_cast<QUdpSocket*>(QObject::sender());
while (socket->hasPendingDatagrams())
{
QByteArray wave;
QHostAddress host;
wave.resize(socket->pendingDatagramSize());
socket->readDatagram(wave.data(), wave.size(), &host);
for (int i = 0; i < boundAddresses.count(); i++) {
if (host == boundAddresses.at(i))
// skip processing since this is our own packet
return;
}
if (wave.startsWith(NET_OCEAN_HEARTBEAT))
emit shipHeartbeat(host, wave.mid(1).data());
else if (wave.startsWith(NET_OCEAN_COURSE_CHANGE))
emit shipChangedCourse(host, wave.mid(1).data());
else if (wave.startsWith(NET_OCEAN_RETURN_TO_PORT))
emit shipDocked(host, wave.mid(1).data());
else if (wave.startsWith(NET_OCEAN_ADMIN_LOCK_REQ))
emit adminLockRequested(host, wave.mid(1).data());
else if (wave.startsWith(NET_OCEAN_ADMIN_LOCK_RESP))
emit adminLockRequestReponse(host, (atoi(wave.mid(1,1).data()) == NET_OCEAN_ACKNOWLEDGE), wave.mid(2).data());
else if (wave.startsWith(NET_OCEAN_MESSAGE))
this->processMessage(host, wave.mid(1).data());
else if (wave.startsWith(NET_OCEAN_SAIL))
emit shipSailed(host, wave.mid(1).data());
}
}
void AudioReceiver::readPendingAudioRcvrData() {
QUdpSocket *socket = qobject_cast<QUdpSocket *>(sender());
while (socket->hasPendingDatagrams()) {
m_datagram.resize(socket->pendingDatagramSize());
if (socket->readDatagram(m_datagram.data(), m_datagram.size()) < 0) {
AUDIO_RECEIVER << "read client" << m_client << "socket failed.";
if (io->rcveIQ_toggle) { // toggles the rcveIQ signal
emit rcveIQEvent(this, 2);
io->rcveIQ_toggle = false;
}
}
else {
io->au_queue.enqueue(m_datagram);
if (!io->rcveIQ_toggle) { // toggles the rcveIQ signal
emit rcveIQEvent(this, 1);
io->rcveIQ_toggle = true;
}
}
}
}
void SocketTestWindow::readyReadMessage()
{
if(m_tcpEnable)
{
QByteArray data = m_socketClient->readAll();
QString rd = data;
// char logbuf[BUFSIZ] = {0};
// sprintf(logbuf,"receive data from %s:%d %s",)
// QLogProvider::getLog()->log(logbuf);
ui->m_teRecvData->setText(rd);
}
else
{
QUdpSocket* udpSocket = dynamic_cast<QUdpSocket*>(m_socketClient);
while (udpSocket->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(udpSocket->pendingDatagramSize());
udpSocket->readDatagram(datagram.data(), datagram.size(),
&m_remoteHost, &m_remotePort);
char logbuf[BUFSIZ] = {0};
sprintf(logbuf,"receive datagram from %s:%d",m_remoteHost.toString().toStdString().c_str(),m_remotePort);
ui->m_teRecvData->setText(datagram);
QLogProvider::getLog()->log(logbuf);
}
}
}
void tst_QUdpSocket::performance()
{
QUdpSocket server;
QVERIFY2(server.bind(), server.errorString().toLatin1().constData());
QHostAddress serverAddress = QHostAddress::LocalHost;
if (!(server.localAddress() == QHostAddress::Any))
serverAddress = server.localAddress();
QUdpSocket client;
client.connectToHost(serverAddress, server.localPort());
QByteArray arr(8192, '@');
QTime stopWatch;
stopWatch.start();
qint64 nbytes = 0;
while (stopWatch.elapsed() < 5000) {
for (int i = 0; i < 100; ++i) {
if (client.write(arr.data(), arr.size()) > 0) {
do {
nbytes += server.readDatagram(arr.data(), arr.size());
} while (server.hasPendingDatagrams());
}
}
}
float secs = stopWatch.elapsed() / 1000.0;
qDebug("\t%.2fMB/%.2fs: %.2fMB/s", float(nbytes / (1024.0*1024.0)),
secs, float(nbytes / (1024.0*1024.0)) / secs);
}
void ICServer::processPendingDatagrams()
{
QByteArray datagram;
QUdpSocket *socket = isRunning ? udpSocket : testUdpSocket;
do {
QHostAddress *address = new QHostAddress;
datagram.resize(socket->pendingDatagramSize());
socket->readDatagram(datagram.data(), datagram.size(), address);
ICMessageHandler *messageHandler = new ICMessageHandler(this);
connect(messageHandler, SIGNAL(connectionOffer(QString)), this, SLOT(processSidCollision(QString)));
connect(messageHandler, SIGNAL(serverDiscovery(QHostAddress,QString)), this, SLOT(processDiscovery(QHostAddress,QString)));
connect(messageHandler, SIGNAL(questionRequest(QHostAddress)), this, SLOT(processRequest(QHostAddress)));
connect(messageHandler, SIGNAL(answerReady(ICAnswer,QString)), this, SLOT(processAnswer(ICAnswer,QString)));
ICMessageProcessor *processor = new ICMessageProcessor(messageHandler, *address, datagram);
QThreadPool::globalInstance()->start(processor);
qDebug()<< "[1] Received from " << *address << datagram;
delete address;
} while (socket->hasPendingDatagrams());
}
void tst_QUdpSocket::pendingDatagramSize()
{
QUdpSocket server;
QVERIFY2(server.bind(), server.errorString().toLatin1().constData());
QHostAddress serverAddress = QHostAddress::LocalHost;
if (!(server.localAddress() == QHostAddress::Any))
serverAddress = server.localAddress();
QUdpSocket client;
QVERIFY(client.writeDatagram("this is", 7, serverAddress, server.localPort()) == 7);
QVERIFY(client.writeDatagram(0, 0, serverAddress, server.localPort()) == 0);
QVERIFY(client.writeDatagram("3 messages", 10, serverAddress, server.localPort()) == 10);
char c = 0;
QVERIFY(server.waitForReadyRead());
if (server.hasPendingDatagrams()) {
#if defined Q_OS_HPUX && defined __ia64
QEXPECT_FAIL("", "HP-UX 11i v2 can't determine the datagram size correctly.", Abort);
#endif
QCOMPARE(server.pendingDatagramSize(), qint64(7));
c = '\0';
QCOMPARE(server.readDatagram(&c, 1), qint64(1));
QCOMPARE(c, 't');
c = '\0';
} else {
QSKIP("does not have the 1st datagram", SkipSingle);
}
if (server.hasPendingDatagrams()) {
QCOMPARE(server.pendingDatagramSize(), qint64(0));
QCOMPARE(server.readDatagram(&c, 1), qint64(0));
QCOMPARE(c, '\0'); // untouched
c = '\0';
} else {
QSKIP("does not have the 2nd datagram", SkipSingle);
}
if (server.hasPendingDatagrams()) {
QCOMPARE(server.pendingDatagramSize(), qint64(10));
QCOMPARE(server.readDatagram(&c, 1), qint64(1));
QCOMPARE(c, '3');
} else {
QSKIP("does not have the 3rd datagram", SkipSingle);
}
}
void CNetwork::DataMayRead( )
{
QByteArray byData;
QUdpSocket* udpServer = qobject_cast< QUdpSocket* > ( sender( ) );
while ( udpServer->hasPendingDatagrams( ) ) {
byData.resize( udpServer->pendingDatagramSize( ) );
udpServer->readDatagram( byData.data( ), byData.size( ) );
ProcessData( byData );
}
}
void tst_QUdpSocket::zeroLengthDatagram()
{
QFETCH_GLOBAL(bool, setProxy);
if (setProxy)
return;
QUdpSocket receiver;
QVERIFY(receiver.bind());
QVERIFY(!receiver.waitForReadyRead(100));
QVERIFY(!receiver.hasPendingDatagrams());
QUdpSocket sender;
QCOMPARE(sender.writeDatagram(QByteArray(), QHostAddress::LocalHost, receiver.localPort()), qint64(0));
QVERIFY(receiver.waitForReadyRead(1000));
QVERIFY(receiver.hasPendingDatagrams());
char buf;
QCOMPARE(receiver.readDatagram(&buf, 1), qint64(0));
}
void ServerPool::newUdpDatagram(void)
{
QUdpSocket *socket = dynamic_cast<QUdpSocket*>(sender());
while (socket->state() == QAbstractSocket::BoundState &&
socket->hasPendingDatagrams())
{
QByteArray buffer;
buffer.resize(socket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
socket->readDatagram(buffer.data(), buffer.size(),
&sender, &senderPort);
emit newDatagram(buffer, sender, senderPort);
}
}
void tst_QUdpSocket::writeToNonExistingPeer()
{
QSKIP("Connected-mode UDP sockets and their behaviour are erratic", SkipAll);
QFETCH(QHostAddress, peerAddress);
quint16 peerPort = 34534;
qRegisterMetaType<QAbstractSocket::SocketError>("QAbstractSocket::SocketError");
QUdpSocket sConnected;
QSignalSpy sConnectedReadyReadSpy(&sConnected, SIGNAL(readyRead()));
QSignalSpy sConnectedErrorSpy(&sConnected, SIGNAL(error(QAbstractSocket::SocketError)));
sConnected.connectToHost(peerAddress, peerPort, QIODevice::ReadWrite);
// the first write succeeds...
QCOMPARE(sConnected.write("", 1), qint64(1));
// the second one should fail!
QTest::qSleep(1000); // do not process events
QCOMPARE(sConnected.write("", 1), qint64(-1));
QCOMPARE(int(sConnected.error()), int(QUdpSocket::ConnectionRefusedError));
// the third one will succeed...
QCOMPARE(sConnected.write("", 1), qint64(1));
QTestEventLoop::instance().enterLoop(1);
QCOMPARE(sConnectedReadyReadSpy.count(), 0);
QCOMPARE(sConnectedErrorSpy.count(), 1);
QCOMPARE(int(sConnected.error()), int(QUdpSocket::ConnectionRefusedError));
// we should now get a read error
QCOMPARE(sConnected.write("", 1), qint64(1));
QTest::qSleep(1000); // do not process events
char buf[2];
QVERIFY(!sConnected.hasPendingDatagrams());
QCOMPARE(sConnected.bytesAvailable(), Q_INT64_C(0));
QCOMPARE(sConnected.pendingDatagramSize(), Q_INT64_C(-1));
QCOMPARE(sConnected.readDatagram(buf, 2), Q_INT64_C(-1));
QCOMPARE(int(sConnected.error()), int(QUdpSocket::ConnectionRefusedError));
QCOMPARE(sConnected.write("", 1), qint64(1));
QTest::qSleep(1000); // do not process events
QCOMPARE(sConnected.read(buf, 2), Q_INT64_C(0));
QCOMPARE(int(sConnected.error()), int(QUdpSocket::ConnectionRefusedError));
// we should still be connected
QCOMPARE(int(sConnected.state()), int(QUdpSocket::ConnectedState));
}
void ExitTunnel::UdpReadFromProxy()
{
if(!_running) {
qDebug("SOCKS UDP read but not running");
return;
}
QUdpSocket* socket = qobject_cast<QUdpSocket*>(sender());
if(!socket) {
qWarning("SOCKS Illegal call to UDP ReadFromClient()");
return;
}
if(!CheckSession()) {
qDebug("SOCKS read but no session");
return;
}
// Restart the timeout timer
_timers[socket]->start();
QHostAddress peer;
quint16 peer_port;
QByteArray datagram;
while(socket->hasPendingDatagrams()) {
datagram.resize(socket->pendingDatagramSize());
quint64 bytes = socket->readDatagram(datagram.data(), datagram.size(), &peer, &peer_port);
qDebug() << "SOCKS UDP read bytes:" << bytes;
if(bytes != static_cast<quint64>(datagram.size())) {
qWarning() << "SOCKS UDP invalid dgram read. Got:" << bytes << "Expected:" << datagram.size();
continue;
}
SendReply(UdpResponsePacket(_table.IdForConnection(socket),
SocksHostAddress(peer, peer_port), datagram).ToByteArray());
}
qDebug() << "MEM active" << _table.Count();
}
void StreamDataChunker::next(QIODevice* device)
{
qDebug() << "StreamDataChunker::next() [" << chunkCounter_++ << "]";
QUdpSocket* socket = static_cast<QUdpSocket*>(device);
bytesRead_ = 0;
// Get writable buffer space for chunk.
pelican::WritableData chunk = getDataStorage(chunkSize_);
if (chunk.isValid())
{
// Get pointer to start of chunk memory.
char* ptr = (char*)chunk.ptr();
// Read datagrams for chunk from the socket.
while (isActive() && bytesRead_ < chunkSize_)
{
// Read the datagram, but avoid using pendingDatagramSize()
if (!socket->hasPendingDatagrams())
{
// MUST wait for the next datagram
socket->waitForReadyRead(100);
continue;
}
qint64 maxlength = chunkSize_ - bytesRead_;
qint64 length = socket->readDatagram(ptr + bytesRead_, maxlength);
if (length > 0)
bytesRead_ += length;
}
}
// Discard the datagram if there is no space.
else
{
socket->readDatagram(0, 0);
}
}
void Discover::readDatagrams ()
{
QUdpSocket* socket = qobject_cast<QUdpSocket*>(QObject::sender());
if (socket)
while (socket->hasPendingDatagrams())
{
// Read it to a buffer
QByteArray datagram;
datagram.resize(socket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
socket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
// Check the prefix
bool respond = false;
bool removeThem = false;
if (datagram.startsWith("DSDA")) respond = false; // Announcement, do not respond
else if (datagram.startsWith("DSDR")) respond = true; // Request, do respond
else if (datagram.startsWith("DSDD")) removeThem = true; // This is a departure, remove records following
else
{
// This datagram isn't written for Discover, pass it on
QList<Record> matchingRecords;
for (Record record : foundRecords.keys())
{
if (record.has("Address") and record["Address"]==sender.toString())
if (record.has("Port") and record["Port"]==QString::number(senderPort))
matchingRecords.append(record);
}
emit gotDatagram(datagram, sender, senderPort, matchingRecords);
continue;
}
datagram.remove(0, 4);
// Determine the scope of the sender
// See if it's this machine
QString scope("Global");
if (sender.isLoopback())
{
scope = "Loopback";
}
else if (sender.protocol() == QAbstractSocket::IPv4Protocol)
{
if (addressIsLocal(sender)) scope = "Local";
}
// Process the records locally
acceptRecords(Record::listFromBytes(datagram), removeThem, scope, sender.toString(), QString::number(senderPort));
// Normal: Respond to only Local requests with only owned Records
if (respond and not mServerMode and (scope=="Local" or scope=="Loopback"))
{
globalSocket->writeDatagram(QByteArray("DSDA")+makeDatagram(ownedRecords, true), sender, senderPort);
}
// Server: Respond to only global requests with owned and found Records
else if (respond and mServerMode and scope=="Global")
{
globalSocket->writeDatagram(QByteArray("DSDA")+makeDatagram(ownedRecords + foundRecords.keys(), false), sender, senderPort);
}
}
}
int Discoverer::findHPSDRDevices() {
int devicesFound = 0;
m_findDatagram.resize(63);
m_findDatagram[0] = (char)0xEF;
m_findDatagram[1] = (char)0xFE;
m_findDatagram[2] = (char)0x02;
for (int i = 3; i < 63; i++)
m_findDatagram[i] = (char)0x00;
QUdpSocket socket;
CHECKED_CONNECT(
&socket,
SIGNAL(error(QAbstractSocket::SocketError)),
this,
SLOT(displayDiscoverySocketError(QAbstractSocket::SocketError)));
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "using " << qPrintable(QHostAddress(set->getHPSDRDeviceLocalAddr()).toString()) << " for discovery.";
io->networkIOMutex.unlock();
// clear comboBox entries in the network dialogue
set->clearNetworkIOComboBoxEntry();
#if defined(Q_OS_WIN32)
if (socket.bind(
QHostAddress(set->getHPSDRDeviceLocalAddr()), 0,
QUdpSocket::ReuseAddressHint | QUdpSocket::ShareAddress))
//QUdpSocket::ReuseAddressHint))
{
set->setMetisPort(this, socket.localPort());
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "discovery_socket bound successfully to port " << socket.localPort();
io->networkIOMutex.unlock();
}
else {
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "discovery_socket bind failed.";
io->networkIOMutex.unlock();
socket.close();
return 0;
}
#elif defined(Q_OS_LINUX)
if (socket.bind(
QHostAddress(set->getHPSDRDeviceLocalAddr()),
QUdpSocket::DefaultForPlatform))
{
CHECKED_CONNECT(
&socket,
SIGNAL(error(QAbstractSocket::SocketError)),
this,
SLOT(displayDiscoverySocketError(QAbstractSocket::SocketError)));
set->setMetisPort(this, socket.localPort());
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "discovery_socket bound successfully to port " << socket.localPort();
io->networkIOMutex.unlock();
}
else {
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "discovery_socket bind failed.";
io->networkIOMutex.unlock();
socket.close();
return 0;
}
#endif
if (socket.writeDatagram(m_findDatagram, QHostAddress::Broadcast, DEVICE_PORT) == 63) {
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "discovery data sent.";
io->networkIOMutex.unlock();
}
else {
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "discovery data not sent.";
io->networkIOMutex.unlock();
}
// wait a little
//SleeperThread::msleep(30);
SleeperThread::msleep(500);
while (socket.hasPendingDatagrams()) {
TNetworkDevicecard mc;
quint16 port;
m_deviceDatagram.resize(socket.pendingDatagramSize());
socket.readDatagram(m_deviceDatagram.data(), m_deviceDatagram.size(), &mc.ip_address, &port);
if (m_deviceDatagram[0] == (char)0xEF && m_deviceDatagram[1] == (char)0xFE) {
if (m_deviceDatagram[2] == (char)0x02) {
sprintf(mc.mac_address, "%02X:%02X:%02X:%02X:%02X:%02X",
m_deviceDatagram[3] & 0xFF, m_deviceDatagram[4] & 0xFF, m_deviceDatagram[5] & 0xFF,
m_deviceDatagram[6] & 0xFF, m_deviceDatagram[7] & 0xFF, m_deviceDatagram[8] & 0xFF);
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "Device found at " << qPrintable(mc.ip_address.toString()) << ":" << port << "; Mac addr: [" << mc.mac_address << "]";
DISCOVERER_DEBUG << "Device code version: " << qPrintable(QString::number(m_deviceDatagram.at(9), 16));
io->networkIOMutex.unlock();
int no = m_deviceDatagram.at(10);
QString str;
if (no == 0)
str = "Metis";
else if (no == 1)
str = "Hermes";
else if (no == 2)
str = "Griffin";
else if (no == 4)
str = "Angelia";
mc.boardID = no;
mc.boardName = str;
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "Device board ID: " << no;
DISCOVERER_DEBUG << "Device is: " << qPrintable(str);
io->networkIOMutex.unlock();
m_deviceCards.append(mc);
str += " (";
str += mc.ip_address.toString();
str += ")";
set->addNetworkIOComboBoxEntry(str);
devicesFound++;
}
else if (m_deviceDatagram[2] == (char)0x03) {
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "Device already sending data!";
io->networkIOMutex.unlock();
}
}
}
set->setMetisCardList(m_deviceCards);
if (devicesFound == 1) {
set->setCurrentHPSDRDevice(m_deviceCards.at(0));
io->networkIOMutex.lock();
DISCOVERER_DEBUG << "Device selected: " << qPrintable(m_deviceCards.at(0).ip_address.toString());
io->networkIOMutex.unlock();
}
socket.close();
return devicesFound;
}
int main(int argc, char *argv[])
{
int debugMode = DEBUGMODE;
if(debugMode)
{
} else {
qInstallMessageHandler(myMessageOutputDisable);
}
QDEBUG() << "number of arguments:" << argc;
QStringList args;
QDEBUGVAR(RAND_MAX);
if(argc > 1) {
QCoreApplication a(argc, argv);
args = a.arguments();
QDEBUGVAR(args);
qRegisterMetaType<Packet>();
QDEBUG() << "Running command line mode.";
Packet sendPacket;
sendPacket.init();
QString outBuilder;
QTextStream o(&outBuilder);
QTextStream out(stdout);
QDate vDate = QDate::fromString(QString(__DATE__).simplified(), "MMM d yyyy");
QCoreApplication::setApplicationName("Packet Sender");
QCoreApplication::setApplicationVersion("version " + vDate.toString("yyyy-MM-dd"));
QCommandLineParser parser;
parser.setApplicationDescription("Packet Sender is a Network TCP and UDP Test Utility by Dan Nagle\nSee http://PacketSender.com/ for more information.");
parser.addHelpOption();
parser.addVersionOption();
// A boolean option with a single name (-p)
QCommandLineOption quietOption(QStringList() << "q" << "quiet", "Quiet mode. Only output received data.");
parser.addOption(quietOption);
QCommandLineOption hexOption(QStringList() << "x" << "hex", "Parse data as hex (default).");
parser.addOption(hexOption);
QCommandLineOption asciiOption(QStringList() << "a" << "ascii", "Parse data as mixed-ascii (like the GUI).");
parser.addOption(asciiOption);
QCommandLineOption pureAsciiOption(QStringList() << "A" << "ASCII", "Parse data as pure ascii (no \\xx translation).");
parser.addOption(pureAsciiOption);
// An option with a value
QCommandLineOption waitOption(QStringList() << "w" << "wait",
"Wait up to <milliseconds> for a response after sending. Zero means do not wait (Default).",
"milliseconds");
parser.addOption(waitOption);
// An option with a value
QCommandLineOption fileOption(QStringList() << "f" << "file",
"Send contents of specified path. Max 1024 for UDP, 10 MiB for TCP.",
"path");
parser.addOption(fileOption);
// An option with a value
QCommandLineOption bindPortOption(QStringList() << "b" << "bind",
"Bind port. Default is 0 (dynamic).",
"port");
parser.addOption(bindPortOption);
QCommandLineOption tcpOption(QStringList() << "t" << "tcp", "Send TCP (default).");
parser.addOption(tcpOption);
// A boolean option with multiple names (-f, --force)
QCommandLineOption udpOption(QStringList() << "u" << "udp", "Send UDP.");
parser.addOption(udpOption);
// An option with a value
QCommandLineOption nameOption(QStringList() << "n" << "name",
"Send previously saved packet named <name>. Other options overrides saved packet parameters.",
"name");
parser.addOption(nameOption);
parser.addPositionalArgument("address", "Destination address. Optional for saved packet.");
parser.addPositionalArgument("port", "Destination port. Optional for saved packet.");
parser.addPositionalArgument("data", "Data to send. Optional for saved packet.");
// Process the actual command line arguments given by the user
parser.process(a);
const QStringList args = parser.positionalArguments();
bool quiet = parser.isSet(quietOption);
bool hex = parser.isSet(hexOption);
bool mixedascii = parser.isSet(asciiOption);
bool ascii = parser.isSet(pureAsciiOption);
unsigned int wait = parser.value(waitOption).toUInt();
unsigned int bind = parser.value(bindPortOption).toUInt();
bool tcp = parser.isSet(tcpOption);
bool udp = parser.isSet(udpOption);
bool ipv6 = false;
QString name = parser.value(nameOption);
QString filePath = parser.value(fileOption);
QString address = "";
unsigned int port = 0;
int argssize = args.size();
QString data, dataString;
data.clear();
dataString.clear();
if(argssize >= 1) {
address = args[0];
}
if(argssize >= 2) {
port = args[1].toUInt();
}
if(argssize >= 3) {
data = (args[2]);
}
//check for invalid options..
if(argssize > 3) {
OUTIF() << "Warning: Extra parameters detected. Try surrounding your data with quotes.";
}
if(hex && mixedascii) {
OUTIF() << "Warning: both hex and pure ascii set. Defaulting to hex.";
mixedascii = false;
}
if(hex && ascii) {
OUTIF() << "Warning: both hex and pure ascii set. Defaulting to hex.";
ascii = false;
}
if(mixedascii && ascii) {
OUTIF() << "Warning: both mixed ascii and pure ascii set. Defaulting to pure ascii.";
mixedascii = false;
}
if(tcp && udp) {
OUTIF() << "Warning: both TCP and UDP set. Defaulting to TCP.";
udp = false;
}
if(!filePath.isEmpty() && !QFile::exists(filePath)) {
OUTIF() << "Error: specified path "<< filePath <<" does not exist.";
filePath.clear();
OUTPUT();
return -1;
}
//bind is now default 0
if(!bind && parser.isSet(bindPortOption)) {
OUTIF() << "Warning: Binding to port zero is dynamic.";
}
if(!port && name.isEmpty()) {
OUTIF() << "Warning: Sending to port zero.";
}
//set default choices
if(!hex && !ascii && !mixedascii) {
hex = true;
}
if(!tcp && !udp) {
tcp = true;
}
//Create the packet to send.
if(!name.isEmpty()) {
sendPacket = Packet::fetchFromDB(name);
if(sendPacket.name.isEmpty()) {
OUTIF() << "Error: Saved packet \""<< name <<"\" not found.";
OUTPUT();
return -1;
} else {
if(data.isEmpty()) {
data = sendPacket.hexString;
hex = true;
ascii = false;
mixedascii = false;
}
if(!port) {
port = sendPacket.port;
}
if(address.isEmpty()) {
address = sendPacket.toIP;
}
if(!parser.isSet(tcpOption) && !parser.isSet(udpOption)) {
if(sendPacket.tcpOrUdp.toUpper() == "TCP") {
tcp=true;
udp = false;
} else {
tcp=false;
udp = true;
}
}
}
}
if(!parser.isSet(bindPortOption)) {
bind = 0;
}
if(!filePath.isEmpty() && QFile::exists(filePath)) {
QFile dataFile(filePath);
if(dataFile.open(QFile::ReadOnly)) {
if(tcp) {
QByteArray dataArray = dataFile.read(1024*1024*10);;
dataString = Packet::byteArrayToHex(dataArray);
} else {
QByteArray dataArray = dataFile.read(1024);
dataString = Packet::byteArrayToHex(dataArray);
}
//data format is raw.
ascii = 0;
hex = 0;
mixedascii = 0;
}
}
QDEBUGVAR(argssize);
QDEBUGVAR(quiet);
QDEBUGVAR(hex);
QDEBUGVAR(mixedascii);
QDEBUGVAR(ascii);
QDEBUGVAR(address);
QDEBUGVAR(port);
QDEBUGVAR(wait);
QDEBUGVAR(bind);
QDEBUGVAR(tcp);
QDEBUGVAR(udp);
QDEBUGVAR(name);
QDEBUGVAR(data);
QDEBUGVAR(filePath);
//NOW LETS DO THIS!
if(ascii) { //pure ascii
dataString = Packet::byteArrayToHex(data.toLatin1());
}
if(hex) { //hex
dataString = Packet::byteArrayToHex(Packet::HEXtoByteArray(data));
}
if(mixedascii) { //mixed ascii
dataString = Packet::ASCIITohex(data);
}
if(dataString.isEmpty()) {
OUTIF() << "Warning: No data to send. Is your formatting correct?";
}
QHostAddress addy;
if(!addy.setAddress(address)) {
QHostInfo info = QHostInfo::fromName(address);
if (info.error() != QHostInfo::NoError)
{
OUTIF() << "Error: Could not resolve address:" + address;
OUTPUT();
return -1;
} else {
addy = info.addresses().at(0);
address = addy.toString();
}
}
QHostAddress theAddress(address);
if (QAbstractSocket::IPv6Protocol == theAddress.protocol())
{
QDEBUG() << "Valid IPv6 address.";
ipv6 = true;
}
QByteArray sendData = sendPacket.HEXtoByteArray(dataString);
QByteArray recvData;
recvData.clear();
int bytesWriten = 0;
int bytesRead = 0;
if(tcp) {
QTcpSocket sock;
if(ipv6) {
sock.bind(QHostAddress::AnyIPv6, bind);
} else {
sock.bind(QHostAddress::AnyIPv4, bind);
}
sock.connectToHost(addy, port);
sock.waitForConnected(1000);
if(sock.state() == QAbstractSocket::ConnectedState)
{
OUTIF() << "TCP (" <<sock.localPort() <<")://" << address << ":" << port << " " << dataString;
bytesWriten = sock.write(sendData);
sock.waitForBytesWritten(1000);
//OUTIF() << "Sent:" << Packet::byteArrayToHex(sendData);
if(wait) {
sock.waitForReadyRead(wait);
recvData = sock.readAll();
bytesRead = recvData.size();
QString hexString = Packet::byteArrayToHex(recvData);
if(quiet) {
o << "\n" << hexString;
} else {
o << "\nResponse Time:" << QDateTime::currentDateTime().toString(DATETIMEFORMAT);
o << "\nResponse HEX:" << hexString;
o << "\nResponse ASCII:" << Packet::hexToASCII(hexString);
}
}
sock.disconnectFromHost();
sock.waitForDisconnected(1000);
sock.close();
OUTPUT();
return bytesWriten;
} else {
OUTIF() << "Error: Failed to connect to " << address;
OUTPUT();
return -1;
}
} else {
QUdpSocket sock;
if(ipv6) {
if(!sock.bind(QHostAddress::AnyIPv6, bind)) {
OUTIF() << "Error: Could not bind to " << bind;
OUTPUT();
return -1;
}
} else {
if(!sock.bind(QHostAddress::AnyIPv4, bind)) {
OUTIF() << "Error: Could not bind to " << bind;
OUTPUT();
return -1;
}
}
OUTIF() << "UDP (" <<sock.localPort() <<")://" << address << ":" << port << " " << dataString;
bytesWriten = sock.writeDatagram(sendData, addy, port);
//OUTIF() << "Wrote " << bytesWriten << " bytes";
sock.waitForBytesWritten(1000);
if(wait) {
sock.waitForReadyRead(wait);
if(sock.hasPendingDatagrams()) {
QHostAddress sender;
quint16 senderPort;
recvData.resize(sock.pendingDatagramSize());
sock.readDatagram(recvData.data(), recvData.size(),
&sender, &senderPort);
QString hexString = Packet::byteArrayToHex(recvData);
if(quiet) {
o << "\n" << hexString;
} else {
o << "\nResponse Time:" << QDateTime::currentDateTime().toString(DATETIMEFORMAT);
o << "\nResponse HEX:" << hexString;
o << "\nResponse ASCII:" << Packet::hexToASCII(hexString);
}
}
}
sock.close();
OUTPUT();
return bytesWriten;
}
OUTPUT();
} else {
QApplication a(argc, argv);
QDEBUGVAR(args);
qRegisterMetaType<Packet>();
QFile file(":/packetsender.css");
if(file.open(QFile::ReadOnly)) {
QString StyleSheet = QLatin1String(file.readAll());
// qDebug() << "stylesheet: " << StyleSheet;
a.setStyleSheet(StyleSheet);
}
MainWindow w;
w.show();
return a.exec();
}
return 0;
}
// Gets the next chunk of data from the UDP socket (if it exists).
void K7Chunker::next(QIODevice* device)
{
unsigned int i;
unsigned int lostPackets, difference;
unsigned int offsetStream;
unsigned int lostPacketCounter;
unsigned long int previousTimestamp;
unsigned int previousAccumulation;
unsigned long int timestamp;
unsigned int accumulation;
unsigned int rate;
QUdpSocket* udpSocket = static_cast<QUdpSocket*>(device);
K7Packet currentPacket;
K7Packet outputPacket;
K7Packet _emptyPacket;
difference = 0;
offsetStream = 0;
timestamp = 0;
accumulation = 0;
rate = 0;
previousTimestamp = _startTimestamp;
previousAccumulation = _startAccumulation;
// Get writable buffer space for the chunk.
WritableData writableData;
if ( ! _writable.isValid() )
{
writableData = getDataStorage(_nPackets * _packetSizeStream, chunkTypes().at(0));
}
else
{
writableData = _writable;
}
if (writableData.isValid())
{
// Loop over the number of UDP packets to put in a chunk.
for (i = 0; i < _nPackets; ++i)
{
// Chunker sanity check.
if (!isActive())
return;
// Wait for datagram to be available.
while ( !udpSocket->hasPendingDatagrams() )
{
_writable = writableData;
continue;
}
// Read the current UDP packet from the socket.
if (udpSocket->readDatagram(reinterpret_cast<char*>(¤tPacket), _packetSize) <= 0)
{
std::cerr << "K7Chunker::next(): Error while receiving UDP Packet!" << std::endl;
i--;
continue;
}
// Get the UDP packet header.
timestamp = currentPacket.header.UTCtimestamp;
accumulation = currentPacket.header.accumulationNumber;
rate = currentPacket.header.accumulationRate;
//std::cout << "K7Chunker::next(): timestamp " << timestamp << " accumulation " << accumulation << " rate " << rate << std::endl;
// First time next() has been run, initialise _startTimestamp and _startAccumulation.
if (i == 0 && _startTimestamp == 0)
{
previousTimestamp = _startTimestamp = _startTimestamp == 0 ? timestamp : _startTimestamp;
previousAccumulation = _startAccumulation = _startAccumulation == 0 ? accumulation : _startAccumulation;
//std::cout << "K7Chunker::next(): timestamp " << timestamp << " accumulation " << accumulation << " rate " << rate << std::endl;
}
// Sanity check in UTCtimestamp. If the seconds counter is 0xFFFFFFFFFFFFFFFF, the data cannot be trusted (ignore).
if (timestamp == ~0UL || previousTimestamp + 10 < timestamp)
{
std::cerr << "K7Chunker::next(): Data cannot be trusted! Timestamp is " << timestamp << " or previousTimestamp is " << previousTimestamp << std::endl;
exit(-1);
}
// Check that the packets are contiguous. accumulationNumber increments by
// accumulationRate which is defined in the header of UDP packet.
// accumulationNumber is reset every interval (although it might not start from 0
// as the previous frame might contain data from this one).
lostPackets = 0;
difference = (accumulation >= previousAccumulation) ? (accumulation - previousAccumulation) : (accumulation + _packetsPerSecond - previousAccumulation);
#if 0
// Duplicated packets. Need to address this. ICBF does not duplicate packets though.
if (difference < rate)
{
std::cout << "Duplicated packets, difference " << difference << std::endl;
++_packetsRejected;
i -= 1;
continue;
}
else
#endif
// Missing packets.
if (difference > rate)
{
// -1 since it includes the received packet as well.
lostPackets = (difference / rate) - 1;
}
if (lostPackets > 0)
{
printf("K7Chunker::next(): generate %u empty packets, previousTimestamp: %lu, new timestamp: %lu, prevAccumulation: %u, newAccumulation: %u\n",
lostPackets, previousTimestamp, timestamp, previousAccumulation, accumulation);
}
// Generate lostPackets (empty packets) if needed.
lostPacketCounter = 0;
for (lostPacketCounter = 0; lostPacketCounter < lostPackets && i + lostPacketCounter < _nPackets; ++lostPacketCounter)
{
// Generate empty packet with correct timestamp and accumulation number.
previousTimestamp = (previousAccumulation < _packetsPerSecond) ? previousTimestamp : previousTimestamp + 1;
previousAccumulation = previousAccumulation % _packetsPerSecond;
updateEmptyPacket(_emptyPacket, previousTimestamp, previousAccumulation, rate);
offsetStream = writePacket(&writableData, _emptyPacket, _packetSizeStream, offsetStream);
}
i += lostPacketCounter;
// Write received packet to stream after updating header and data.
if (i != _nPackets)
{
++_packetsAccepted;
// Generate stream packet.
outputPacket.header = currentPacket.header;
memcpy((void*)outputPacket.data, ¤tPacket.data[_byte1OfStream], _bytesStream);
offsetStream = writePacket(&writableData, outputPacket, _packetSizeStream, offsetStream);
previousTimestamp = timestamp;
previousAccumulation = accumulation;
}
}
_chunksProcessed++;
_chunkerCounter++;
// Clear any data locks.
_writable = WritableData();
if (_chunkerCounter % 100 == 0)
{
std::cout << "K7Chunker::next(): " << _chunksProcessed << " chunks processed. " << "UTC timestamp " << timestamp << " accumulationNumber " << accumulation << " accumulationRate " << rate << std::endl;
}
}
else
{
// Must discard the datagram if there is no available space.
if (!isActive()) return;
udpSocket->readDatagram(0, 0);
std::cout << "K7Chunker::next(): Writable data not valid, discarding packets." << std::endl;
}
// Update _startTime.
_startTimestamp = previousTimestamp;
_startAccumulation = previousAccumulation;
}
/**
* @details
* Gets the next chunk of data from the UDP socket (if it exists).
*/
void LofarDataSplittingChunker::next(QIODevice* device)
{
QUdpSocket* socket = static_cast<QUdpSocket*>(device);
unsigned offsetStream1 = 0;
unsigned offsetStream2 = 0;
unsigned prevSeqid = _startTime;
unsigned prevBlockid = _startBlockid;
UDPPacket currPacket;
UDPPacket outputPacket1;
UDPPacket outputPacket2;
UDPPacket _emptyPacket1;
UDPPacket _emptyPacket2;
WritableData writableData1 = getDataStorage(_nPackets * _packetSizeStream1,
chunkTypes().at(0));
WritableData writableData2 = getDataStorage(_nPackets * _packetSizeStream2,
chunkTypes().at(1));
unsigned seqid, blockid;
unsigned totBlocks, lostPackets, diff;
unsigned packetCounter;
if (writableData1.isValid() && writableData2.isValid())
{
// Loop over the number of UDP packets to put in a chunk.
for (unsigned i = 0; i < _nPackets; ++i)
{
// Chunker sanity check.
if (!isActive()) return;
// Wait for datagram to be available.
while (!socket->hasPendingDatagrams())
socket->waitForReadyRead(100);
// Read the current packet from the socket.
if (socket->readDatagram(reinterpret_cast<char*>(&currPacket), _packetSize) <= 0)
{
cerr << "LofarDataSplittingChunker::next(): "
"Error while receiving UDP Packet!" << endl;
i--;
continue;
}
// Check for endianness (Packet data is in little endian format).
#if Q_BYTE_ORDER == Q_BIG_ENDIAN
// TODO: Convert from little endian to big endian.
throw QString("LofarDataSplittingChunker: Endianness not supported.");
seqid = currPacket.header.timestamp;
std::cout << seqid << std::endl;
blockid = currPacket.header.blockSequenceNumber;
#elif Q_BYTE_ORDER == Q_LITTLE_ENDIAN
seqid = currPacket.header.timestamp;
blockid = currPacket.header.blockSequenceNumber;
#endif
// First time next has been run, initialise startTime and startBlockId.
if (i == 0 && _startTime == 0) {
prevSeqid = _startTime = _startTime == 0 ? seqid : _startTime;
prevBlockid = _startBlockid = _startBlockid == 0 ? blockid : _startBlockid;
}
// Sanity check in seqid. If the seconds counter is 0xFFFFFFFF,
// the data cannot be trusted (ignore).
if (seqid == ~0U || prevSeqid + 10 < seqid)
{
_packetsRejected++;
i--;
continue;
}
// Check that the packets are contiguous.
// Block id increments by nrblocks which is defined in the header.
// Blockid is reset every interval (although it might not start
// from 0 as the previous frame might contain data from this one).
totBlocks = (_clock == 160) ?
156250 : (prevSeqid % 2 == 0 ? 195313 : 195312);
lostPackets = 0;
diff = (blockid >= prevBlockid) ?
(blockid - prevBlockid) : (blockid + totBlocks - prevBlockid);
// Duplicated packets... ignore
if (diff < _nSamples)
{
++_packetsRejected;
i -= 1;
continue;
}
// Missing packets
else if (diff > _nSamples)
{
// -1 since it includes this includes the received packet as well
lostPackets = (diff / _nSamples) - 1;
}
if (lostPackets > 0)
{
printf("Generate %u empty packets, prevSeq: %u, new Seq: %u, prevBlock: %u, newBlock: %u\n",
lostPackets, prevSeqid, seqid, prevBlockid, blockid);
}
// TODO
// BELOW HERE WRITE INTO WRITABLE DATA 1 and 2 correctly.
// = Missing packets -> write pair of empty data packets.
// = Full packets -> split the packet into the two buffers.
// =================================================================
// Generate lostPackets (empty packets) if needed.
packetCounter = 0;
for (packetCounter = 0; packetCounter < lostPackets && i + packetCounter < _nPackets; ++packetCounter)
{
// Generate empty packet with correct seqid and blockid
prevSeqid = (prevBlockid + _nSamples < totBlocks) ?
prevSeqid : prevSeqid + 1;
prevBlockid = (prevBlockid + _nSamples) % totBlocks;
// TODO: probably a cost saving here.
updateEmptyPacket(_emptyPacket1, prevSeqid, prevBlockid);
updateEmptyPacket(_emptyPacket2, prevSeqid, prevBlockid);
offsetStream1 = writePacket(&writableData1, _emptyPacket1, _packetSizeStream1, offsetStream1);
offsetStream2 = writePacket(&writableData2, _emptyPacket2, _packetSizeStream2, offsetStream2);
// Check if the number of required packets is reached
// TODO writePacket(&writableData2, emptyPacket, offset);
}
i += packetCounter;
// Write received packet to 2 streams after updating header and data
if (i != _nPackets)
{
++_packetsAccepted;
// Generate Stream 1 packet
outputPacket1.header = currPacket.header;
outputPacket1.header.nrBeamlets = _stream1Subbands;
memcpy((void*)outputPacket1.data, &currPacket.data[_byte1OfStream1], _bytesStream1);
offsetStream1 = writePacket(&writableData1, outputPacket1, _packetSizeStream1, offsetStream1);
// Generate Stream 2 packet
//--------------------------------------------------------------
outputPacket2.header = currPacket.header;
outputPacket2.header.nrBeamlets = _stream2Subbands;
memcpy((void*)outputPacket2.data, &currPacket.data[_byte1OfStream2], _bytesStream2);
offsetStream2 = writePacket(&writableData2, outputPacket2, _packetSizeStream2, offsetStream2);
prevSeqid = seqid;
prevBlockid = blockid;
}
}
}
else {
// Must discard the datagram if there is no available space.
if (!isActive()) return;
socket->readDatagram(0, 0);
cout << "LofarDataSplittingChunker::next(): "
"Writable data not valid, discarding packets." << endl;
}
// Update _startTime
_startTime = prevSeqid;
_startBlockid = prevBlockid;
}
int main(int argc, char *argv[])
{
int framePeriod = 1000000 / 60;
QString logFile;
// Determine log file name
if (argc == 2)
{
logFile = argv[1];
}
if (logFile.isNull())
{
if (!QDir("logs").exists())
{
printf("No logs directory and no log file specified\n");
return 1;
}
logFile = QString("logs/") + QDateTime::currentDateTime().toString("yyyyMMdd-hhmmss.log");
}
// Create vision socket
QUdpSocket visionSocket;
if (!visionSocket.bind(SharedVisionPort, QUdpSocket::ShareAddress))
{
printf("Can't bind to shared vision port");
return 1;
}
multicast_add(&visionSocket, SharedVisionAddress);
// Create referee socket
QUdpSocket refereeSocket;
if (!refereeSocket.bind(LegacyRefereePort, QUdpSocket::ShareAddress))
{
printf("Can't bind to referee port");
return 1;
}
multicast_add(&refereeSocket, RefereeAddress);
// Create log file
int fd = creat(logFile.toAscii(), 0666);
if (fd < 0)
{
printf("Can't create %s: %m\n", (const char *)logFile.toAscii());
return 1;
}
printf("Writing to %s\n", (const char *)logFile.toAscii());
// Main loop
LogFrame logFrame;
bool first = true;
while (true)
{
uint64_t startTime = timestamp();
logFrame.Clear();
logFrame.set_command_time(startTime);
// Check for user input (to exit)
struct pollfd pfd;
pfd.fd = 0;
pfd.events = POLLIN;
if (poll(&pfd, 1, 0) > 0)
{
// Enter pressed
break;
}
// Read vision data
while (visionSocket.hasPendingDatagrams())
{
string buf;
unsigned int n = visionSocket.pendingDatagramSize();
buf.resize(n);
visionSocket.readDatagram(&buf[0], n);
SSL_WrapperPacket *packet = logFrame.add_raw_vision();
if (!packet->ParseFromString(buf))
{
printf("Bad vision packet of %d bytes\n", n);
continue;
}
}
// Read referee data
while (refereeSocket.hasPendingDatagrams())
{
unsigned int n = refereeSocket.pendingDatagramSize();
string str(6, 0);
refereeSocket.readDatagram(&str[0], str.size());
// Check the size after receiving to discard bad packets
if (n != str.size())
{
printf("Bad referee packet of %d bytes\n", n);
continue;
}
logFrame.add_raw_referee(str);
}
if (first)
{
first = false;
LogConfig *logConfig = logFrame.mutable_log_config();
logConfig->set_generator("simple_logger");
logConfig->set_git_version_hash(git_version_hash);
logConfig->set_git_version_dirty(git_version_dirty);
}
uint32_t size = logFrame.ByteSize();
if (write(fd, &size, sizeof(size)) != sizeof(size))
{
printf("Failed to write size: %m\n");
break;
} else if (!logFrame.SerializeToFileDescriptor(fd))
{
printf("Failed to write frame: %m\n");
break;
}
uint64_t endTime = timestamp();
int lastFrameTime = endTime - startTime;
if (lastFrameTime < framePeriod)
{
usleep(framePeriod - lastFrameTime);
} else {
printf("Processor took too long: %d us\n", lastFrameTime);
}
}
// Discard input on stdin
tcflush(0, TCIFLUSH);
printf("Done.\n");
return 0;
}
void MainWindow::processData()
{
int cnt = 0;
while (m_udp.hasPendingDatagrams())
{
QByteArray datagram;
datagram.resize(m_udp.pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
m_udp.readDatagram(datagram.data(), datagram.size(), &sender, &senderPort);
quint32 size = *(quint32*)datagram.data();
QByteArray data = qUncompress(datagram);
char *udpData = (char*)data.data();
switch (udpData[0])
{
case TYPE_SENSORS:
processSensorsData((TUdpDataSENSORS*)udpData);
break;
case TYPE_PLAYER:
{
TUdpDataPLAYER *d = (TUdpDataPLAYER*)udpData;
for (int i = 0; i < 8; i++)
{
float x = d->frustumPoints[i][0];
float y = d->frustumPoints[i][1];
float z = d->frustumPoints[i][2];
m_pdaData.frustum[i] = QVector3D(x, y, z);
}
QByteArray b(udpData + sizeof(TUdpDataPLAYER), data.size());
QDataStream ds(b);
ds.setByteOrder(QDataStream::LittleEndian);
ds.setFloatingPointPrecision(QDataStream::SinglePrecision);
m_pdaData.playerPoints.clear();
for (int i = 0; i < d->playerSize; i++)
{
int16_t latU, lonU;
ds >> latU >> lonU;
m_pdaData.playerPoints.append(latlonUtoVector(latU, lonU));
}
ui->lbInfo->setText(QString("Points: %1\nLives: %2").arg(d->points).arg(d->lives));
ui->lbInfo_2->setText(QString("Time left: %1:%2").arg(d->timeLeft / 60).arg((int)(d->timeLeft % 60), 2, 10, QChar('0')));
break;
}
case TYPE_STAB:
m_pdaData.sphereEnabled = udpData[1];
break;
case TYPE_ENEMY:
{
TUdpDataENEMY *d = (TUdpDataENEMY*)udpData;
QByteArray b(udpData + sizeof(TUdpDataENEMY), data.size());
QDataStream ds(b);
ds.setByteOrder(QDataStream::LittleEndian);
ds.setFloatingPointPrecision(QDataStream::SinglePrecision);
while (m_pdaData.enemies.size() < d->idx + 1)
{
m_pdaData.enemies.append(Enemy());
}
m_pdaData.enemies[d->idx].points.clear();
for (int i = 0; i < d->enemiesSize; i++)
{
int16_t latU, lonU;
ds >> latU >> lonU;
m_pdaData.enemies[d->idx].points.push_back(latlonUtoVector(latU, lonU));
}
break;
}
case TYPE_FOOD:
{
TUdpDataFOOD *d = (TUdpDataFOOD*)udpData;
QByteArray b(udpData + sizeof(TUdpDataFOOD), data.size());
QDataStream ds(b);
ds.setByteOrder(QDataStream::LittleEndian);
ds.setFloatingPointPrecision(QDataStream::SinglePrecision);
m_pdaData.foodPoints.clear();
for (int i = 0; i < d->foodSize; i++)
{
int16_t latU, lonU;
ds >> latU >> lonU;
m_pdaData.foodPoints.append(latlonUtoVector(latU, lonU));
}
break;
}
}
cnt++;
}
}