kopia lustrzana https://gitlab.com/eliggett/wfview
994 wiersze
34 KiB
C++
994 wiersze
34 KiB
C++
// Copyright 2021 Phil Taylor M0VSE
|
|
// This code is heavily based on "Kappanhang" by HA2NON, ES1AKOS and W6EL!
|
|
|
|
#include "udphandler.h"
|
|
|
|
udpHandler::udpHandler(QString ip, quint16 controlPort, quint16 civPort, quint16 audioPort, QString username, QString password,
|
|
quint16 buffer, quint16 rxsample, quint8 rxcodec, quint16 txsample, quint8 txcodec) :
|
|
controlPort(controlPort),
|
|
civPort(civPort),
|
|
audioPort(audioPort)
|
|
{
|
|
|
|
this->port = this->controlPort;
|
|
this->username = username;
|
|
this->password = password;
|
|
this->rxBufferSize = buffer;
|
|
this->rxSampleRate = rxsample;
|
|
this->txSampleRate = txsample;
|
|
this->rxCodec = rxcodec;
|
|
this->txCodec = txcodec;
|
|
|
|
qDebug() << "Starting udpHandler user:" << username << " buffer:" << buffer << " rx sample rate: " << rxsample <<
|
|
" rx codec: " << rxcodec << " tx sample rate: " << txsample << " tx codec: " << txcodec;
|
|
|
|
// Try to set the IP address, if it is a hostname then perform a DNS lookup.
|
|
if (!radioIP.setAddress(ip))
|
|
{
|
|
QHostInfo remote = QHostInfo::fromName(ip);
|
|
foreach(QHostAddress addr, remote.addresses())
|
|
{
|
|
if (addr.protocol() == QAbstractSocket::IPv4Protocol) {
|
|
radioIP = addr;
|
|
qDebug() << "Got IP Address :" << ip << ": " << addr.toString();
|
|
break;
|
|
}
|
|
}
|
|
if (radioIP.isNull())
|
|
{
|
|
qDebug() << "Error obtaining IP Address for :" << ip << ": " << remote.errorString();
|
|
return;
|
|
}
|
|
}
|
|
|
|
// Convoluted way to find the external IP address, there must be a better way????
|
|
QString localhostname = QHostInfo::localHostName();
|
|
QList<QHostAddress> hostList = QHostInfo::fromName(localhostname).addresses();
|
|
foreach(const QHostAddress & address, hostList)
|
|
{
|
|
if (address.protocol() == QAbstractSocket::IPv4Protocol && address.isLoopback() == false)
|
|
{
|
|
localIP = QHostAddress(address.toString());
|
|
}
|
|
}
|
|
|
|
udpBase::init(); // Perform UDP socket initialization.
|
|
|
|
// Connect socket to my dataReceived function.
|
|
QUdpSocket::connect(udp, &QUdpSocket::readyRead, this, &udpHandler::dataReceived);
|
|
|
|
/*
|
|
Connect various timers
|
|
*/
|
|
connect(&tokenTimer, &QTimer::timeout, this, std::bind(&udpHandler::sendToken, this, 0x05));
|
|
connect(&areYouThereTimer, &QTimer::timeout, this, QOverload<>::of(&udpHandler::sendAreYouThere));
|
|
connect(&pingTimer, &QTimer::timeout, this, &udpBase::sendPing);
|
|
connect(&idleTimer, &QTimer::timeout, this, std::bind(&udpBase::sendControl, this, true, 0, 0));
|
|
|
|
// Start sending are you there packets - will be stopped once "I am here" received
|
|
areYouThereTimer.start(AREYOUTHERE_PERIOD);
|
|
|
|
// Set my computer name. Should this be configurable?
|
|
compName = QString("wfview").toUtf8();
|
|
|
|
}
|
|
|
|
udpHandler::~udpHandler()
|
|
{
|
|
if (isAuthenticated) {
|
|
if (audio != Q_NULLPTR) {
|
|
delete audio;
|
|
}
|
|
|
|
if (civ != Q_NULLPTR) {
|
|
delete civ;
|
|
}
|
|
|
|
qDebug() << "Sending token removal packet";
|
|
sendToken(0x01);
|
|
}
|
|
}
|
|
|
|
void udpHandler::changeBufferSize(quint16 value)
|
|
{
|
|
emit haveChangeBufferSize(value);
|
|
}
|
|
|
|
void udpHandler::receiveFromCivStream(QByteArray data)
|
|
{
|
|
emit haveDataFromPort(data);
|
|
}
|
|
|
|
void udpHandler::receiveDataFromUserToRig(QByteArray data)
|
|
{
|
|
if (civ != Q_NULLPTR)
|
|
{
|
|
civ->send(data);
|
|
}
|
|
}
|
|
|
|
void udpHandler::dataReceived()
|
|
{
|
|
while (udp->hasPendingDatagrams()) {
|
|
lastReceived = time(0);
|
|
QNetworkDatagram datagram = udp->receiveDatagram();
|
|
//qDebug() << "Received: " << datagram.data();
|
|
QByteArray r = datagram.data();
|
|
|
|
switch (r.length())
|
|
{
|
|
case (16):
|
|
if (r.mid(0, 8) == QByteArrayLiteral("\x10\x00\x00\x00\x04\x00\x00\x00")) {
|
|
// If timer is active, stop it as they are obviously there!
|
|
if (areYouThereTimer.isActive()) {
|
|
areYouThereTimer.stop();
|
|
// send ping packets every second
|
|
pingTimer.start(PING_PERIOD);
|
|
idleTimer.start(IDLE_PERIOD);
|
|
}
|
|
}
|
|
// This is "I am ready" in response to "Are you ready" so send login.
|
|
else if (r.mid(0, 8) == QByteArrayLiteral("\x10\x00\x00\x00\x06\x00\x01\x00"))
|
|
{
|
|
qDebug() << this->metaObject()->className() << ": Received I am ready";
|
|
remoteId = qFromLittleEndian<quint32>(r.mid(8, 4));
|
|
sendLogin(); // second login packet
|
|
}
|
|
break;
|
|
case (21): // pkt7,
|
|
if (r.mid(1, 5) == QByteArrayLiteral("\x00\x00\x00\x07\x00") && r[16] == (char)0x01 && serialAndAudioOpened)
|
|
{
|
|
// This is a response to our ping request so measure latency
|
|
latency += lastPingSentTime.msecsTo(QDateTime::currentDateTime());
|
|
latency /= 2;
|
|
quint32 totalsent = packetsSent;
|
|
quint32 totallost = packetsLost/2;
|
|
if (audio != Q_NULLPTR) {
|
|
totalsent = totalsent + audio->packetsSent;
|
|
totallost = totallost + audio->packetsLost/2;
|
|
}
|
|
if (civ != Q_NULLPTR) {
|
|
totalsent = totalsent + civ->packetsSent;
|
|
totallost = totallost + civ->packetsLost/2;
|
|
}
|
|
//double perclost = 1.0 * totallost / totalsent * 100.0 ;
|
|
|
|
emit haveNetworkStatus(" rtt: " + QString::number(latency) + " ms, loss: (" +QString::number(packetsLost)+ "/"+ QString::number(packetsSent) +")");
|
|
}
|
|
break;
|
|
case (64): // Response to Auth packet?
|
|
if (r.mid(0, 6) == QByteArrayLiteral("\x40\x00\x00\x00\x00\x00") && r[21] == (char)0x05) {
|
|
if (r.mid(0x30, 4) == QByteArrayLiteral("\x00\x00\x00\x00")) {
|
|
qDebug() << this->metaObject()->className() << ": Token renewal successful";
|
|
tokenTimer.start(TOKEN_RENEWAL);
|
|
gotAuthOK = true;
|
|
if (!serialAndAudioOpened)
|
|
{
|
|
sendRequestSerialAndAudio();
|
|
}
|
|
|
|
}
|
|
else if (r.mid(0x30, 4) == QByteArrayLiteral("\xff\xff\xff\xff"))
|
|
{
|
|
qWarning() << this->metaObject()->className() << ": Radio rejected token renewal, performing login";
|
|
remoteId = qFromLittleEndian<quint32>(r.mid(8, 4));
|
|
isAuthenticated = false;
|
|
sendLogin(); // Try sending login packet (didn't seem to work?)
|
|
}
|
|
else
|
|
{
|
|
qWarning() << this->metaObject()->className() << ": Unknown response to token renewal? " << r.mid(0x30,4);
|
|
}
|
|
}
|
|
break;
|
|
case (80): // Status packet
|
|
if (r.mid(0, 6) == QByteArrayLiteral("\x50\x00\x00\x00\x00\x00"))
|
|
{
|
|
if (r.mid(48, 3) == QByteArrayLiteral("\xff\xff\xff"))
|
|
{
|
|
if (!serialAndAudioOpened)
|
|
{
|
|
emit haveNetworkError(radioIP.toString(), "Auth failed, try rebooting the radio.");
|
|
qDebug() << this->metaObject()->className() << ": Auth failed, try rebooting the radio.";
|
|
}
|
|
}
|
|
if (r.mid(48, 3) == QByteArrayLiteral("\x00\x00\x00") && r[64] == (char)0x01)
|
|
{
|
|
emit haveNetworkError(radioIP.toString(), "Got radio disconnected.");
|
|
qDebug() << this->metaObject()->className() << ": Got radio disconnected.";
|
|
}
|
|
}
|
|
break;
|
|
|
|
case(96): // Response to Login packet.
|
|
if (r.mid(0, 6) == QByteArrayLiteral("\x60\x00\x00\x00\x00\x00"))
|
|
{
|
|
if (r.mid(48, 4) == QByteArrayLiteral("\xff\xff\xff\xfe"))
|
|
{
|
|
emit haveNetworkStatus("Invalid Username/Password");
|
|
qDebug() << this->metaObject()->className() << ": Invalid Username/Password";
|
|
|
|
}
|
|
else if (!isAuthenticated)
|
|
{
|
|
emit haveNetworkStatus("Radio Login OK!");
|
|
qDebug() << this->metaObject()->className() << ": Received Login OK";
|
|
|
|
authId = r.mid(0x1a, 6);
|
|
sendToken(0x02);
|
|
tokenTimer.start(TOKEN_RENEWAL); // Start token request timer
|
|
|
|
isAuthenticated = true;
|
|
}
|
|
|
|
if (r.mid(0x40, 4) == QByteArrayLiteral("FTTH"))
|
|
{
|
|
highBandwidthConnection = true;
|
|
}
|
|
qDebug() << this->metaObject()->className() << ": Detected connection speed " << QString::fromUtf8(parseNullTerminatedString(r, 0x40));
|
|
|
|
}
|
|
break;
|
|
case (144):
|
|
if (!serialAndAudioOpened && r.mid(0, 6) == QByteArrayLiteral("\x90\x00\x00\x00\x00\x00") && r[0x60] == (char)0x00)
|
|
{
|
|
devName = parseNullTerminatedString(r, 0x40);
|
|
QHostAddress ip = QHostAddress(qFromBigEndian<quint32>(r.mid(0x84, 4)));
|
|
if (!ip.isEqual(QHostAddress("0.0.0.0")) && parseNullTerminatedString(r, 0x64) != compName) // || ip != localIP ) // TODO: More testing of IP address detection code!
|
|
{
|
|
emit haveNetworkStatus(QString::fromUtf8(devName) + "in use by: " + QString::fromUtf8(parseNullTerminatedString(r, 0x64)) + " (" + ip.toString() + ")");
|
|
}
|
|
else {
|
|
emit haveNetworkStatus(QString::fromUtf8(devName) + " available");
|
|
sendRequestSerialAndAudio();
|
|
}
|
|
}
|
|
else if (!serialAndAudioOpened && r.mid(0, 6) == QByteArrayLiteral("\x90\x00\x00\x00\x00\x00") && r[0x60] == (char)0x01)
|
|
{
|
|
devName = parseNullTerminatedString(r, 0x40);
|
|
|
|
civ = new udpCivData(localIP, radioIP, civPort);
|
|
audio = new udpAudio(localIP, radioIP, audioPort,rxBufferSize,rxSampleRate, rxCodec,txSampleRate,txCodec);
|
|
|
|
QObject::connect(civ, SIGNAL(receive(QByteArray)), this, SLOT(receiveFromCivStream(QByteArray)));
|
|
QObject::connect(this, SIGNAL(haveChangeBufferSize(quint16)), audio, SLOT(changeBufferSize(quint16)));
|
|
|
|
serialAndAudioOpened = true;
|
|
|
|
emit haveNetworkStatus(QString::fromUtf8(devName));
|
|
|
|
qDebug() << this->metaObject()->className() << "Got serial and audio request success, device name: " << QString::fromUtf8(devName);
|
|
|
|
// Stuff can change in the meantime because of a previous login...
|
|
remoteId = qFromLittleEndian<quint32>(r.mid(0x08, 4));
|
|
myId = qFromLittleEndian<quint32>(r.mid(0x0c, 4));
|
|
authId = r.mid(0x1a, 6);
|
|
// Is there already somebody connected to the radio?
|
|
}
|
|
break;
|
|
|
|
case (168):
|
|
audioType = parseNullTerminatedString(r, 0x72);
|
|
devName = parseNullTerminatedString(r, 0x52);
|
|
replyId = r.mid(0x42, 16);
|
|
qDebug() << this->metaObject()->className() << "Received radio capabilities, Name:" <<
|
|
QString::fromUtf8(devName) << " Audio:" <<
|
|
QString::fromUtf8(audioType);
|
|
|
|
break;
|
|
}
|
|
udpBase::dataReceived(r); // Call parent function to process the rest.
|
|
r.clear();
|
|
datagram.clear();
|
|
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
#define SERIALAUDIO_SIZE 0x90
|
|
void udpHandler::sendRequestSerialAndAudio()
|
|
{
|
|
QByteArray usernameEncoded;
|
|
passcode(username, usernameEncoded);
|
|
int txSeqBufLengthMs = 50;
|
|
|
|
quint8 p[SERIALAUDIO_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian(sizeof(p), p + 0x00);
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
memcpy(p + 0x13, QByteArrayLiteral("\x80\x01\x03").constData(), 3);
|
|
qToLittleEndian(authInnerSendSeq, p + 0x16);
|
|
memcpy(p + 0x1a, authId.constData(), authId.length());
|
|
memcpy(p + 0x20, replyId.constData(), replyId.length());
|
|
memcpy(p + 0x40, devName.constData(), devName.length());
|
|
memcpy(p + 0x60, usernameEncoded.constData(), usernameEncoded.length());
|
|
memcpy(p + 0x70, QByteArrayLiteral("\x01\x01").constData(), 2);
|
|
p[0x72] = rxCodec;
|
|
p[0x73] = txCodec;
|
|
qToBigEndian(rxSampleRate, p + 0x76);
|
|
qToBigEndian(txSampleRate, p + 0x7a);
|
|
qToBigEndian(civPort, p + 0x7e);
|
|
qToBigEndian(audioPort, p + 0x82);
|
|
qToBigEndian(txSeqBufLengthMs, p + 0x86);
|
|
p[0x88] = 0x01;
|
|
|
|
authInnerSendSeq++;
|
|
|
|
sendTrackedPacket(QByteArray::fromRawData((const char*)p, sizeof(p)));
|
|
return;
|
|
}
|
|
|
|
void udpHandler::sendAreYouThere()
|
|
{
|
|
if (areYouThereCounter == 20)
|
|
{
|
|
qDebug() << this->metaObject()->className() << ": Radio not responding.";
|
|
emit haveNetworkStatus("Radio not responding!");
|
|
}
|
|
qDebug() << this->metaObject()->className() << ": Sending Are You There...";
|
|
|
|
areYouThereCounter++;
|
|
udpBase::sendControl(false,0x03,0x00);
|
|
}
|
|
|
|
#define LOGIN_SIZE 0x80
|
|
void udpHandler::sendLogin() // Only used on control stream.
|
|
{
|
|
|
|
qDebug() << this->metaObject()->className() << ": Sending login packet";
|
|
|
|
uint16_t authStartID = rand() | rand() << 8;
|
|
//uint16_t authStartID = 0x7866;
|
|
QByteArray usernameEncoded;
|
|
QByteArray passwordEncoded;
|
|
passcode(username, usernameEncoded);
|
|
passcode(password, passwordEncoded);
|
|
|
|
quint8 p[LOGIN_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian(sizeof(p), p + 0x00);
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
memcpy(p + 0x13, QByteArrayLiteral("\x70\x01").constData(), 2);
|
|
qToLittleEndian(authInnerSendSeq, p + 0x17);
|
|
qToLittleEndian(authStartID, p + 0x20);
|
|
memcpy(p + 0x40, usernameEncoded.constData(), usernameEncoded.length());
|
|
memcpy(p + 0x50, passwordEncoded.constData(), passwordEncoded.length());
|
|
memcpy(p + 0x60, compName.constData(), compName.length());
|
|
|
|
authInnerSendSeq++;
|
|
sendTrackedPacket(QByteArray::fromRawData((const char*)p, sizeof(p)));
|
|
return;
|
|
}
|
|
|
|
#define TOKEN_SIZE 0x40
|
|
void udpHandler::sendToken(uint8_t magic)
|
|
{
|
|
qDebug() << this->metaObject()->className() << "Sending Token request: " << magic;
|
|
|
|
quint8 p[TOKEN_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian(sizeof(p), p + 0x00);
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
memcpy(p + 0x13, QByteArrayLiteral("\x30\x01").constData(), 2);
|
|
qToLittleEndian(magic, p + 0x15);
|
|
qToLittleEndian(authInnerSendSeq, p + 0x17);
|
|
memcpy(p + 0x1a, authId.constData(), authId.length());
|
|
|
|
authInnerSendSeq++;
|
|
sendTrackedPacket(QByteArray::fromRawData((const char *)p, sizeof(p)));
|
|
tokenTimer.start(100); // Set 100ms timer for retry (this will be cancelled if a response is received)
|
|
return;
|
|
}
|
|
|
|
|
|
// Class that manages all Civ Data to/from the rig
|
|
udpCivData::udpCivData(QHostAddress local, QHostAddress ip, quint16 civPort)
|
|
{
|
|
qDebug() << "Starting udpCivData";
|
|
localIP = local;
|
|
port = civPort;
|
|
radioIP = ip;
|
|
|
|
udpBase::init(); // Perform connection
|
|
|
|
QUdpSocket::connect(udp, &QUdpSocket::readyRead, this, &udpCivData::dataReceived);
|
|
|
|
sendControl(false, 0x03, 0x00); // First connect packet
|
|
|
|
/*
|
|
Connect various timers
|
|
*/
|
|
connect(&pingTimer, &QTimer::timeout, this, &udpBase::sendPing);
|
|
connect(&idleTimer, &QTimer::timeout, this, std::bind(&udpBase::sendControl, this, true, 0, 0));
|
|
|
|
// send ping packets every 100 ms (maybe change to less frequent?)
|
|
pingTimer.start(PING_PERIOD);
|
|
// Send idle packets every 100ms, this timer will be reset everytime a non-idle packet is sent.
|
|
idleTimer.start(IDLE_PERIOD);
|
|
}
|
|
|
|
udpCivData::~udpCivData() {
|
|
sendOpenClose(true);
|
|
}
|
|
|
|
#define CIV_SIZE 0x15
|
|
void udpCivData::send(QByteArray d)
|
|
{
|
|
// qDebug() << "Sending: (" << d.length() << ") " << d;
|
|
|
|
quint8 p[CIV_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian((quint16)sizeof(p) + d.length(), p + 0x00);
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
p[0x10] = 0xc1;
|
|
qToLittleEndian((quint16)d.length(), p + 0x11);
|
|
qToBigEndian(sendSeqB, p + 0x13); // THIS IS BIG ENDIAN!
|
|
|
|
QByteArray t = QByteArray::fromRawData((const char*)p, sizeof(p));
|
|
t.append(d);
|
|
sendTrackedPacket(t);
|
|
sendSeqB++;
|
|
return;
|
|
}
|
|
|
|
|
|
#define OPENCLOSE_SIZE 0x16
|
|
void udpCivData::sendOpenClose(bool close)
|
|
{
|
|
uint8_t magic = 0x05;
|
|
|
|
if (close)
|
|
{
|
|
magic = 0x00;
|
|
}
|
|
|
|
quint8 p[OPENCLOSE_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian((quint16)sizeof(p), p + 0x00);
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
memcpy(p + 0x10, QByteArrayLiteral("\xc0\x01").constData(), 2);
|
|
qToLittleEndian(sendSeqB, p + 0x13);
|
|
p[0x15] = magic;
|
|
|
|
sendSeqB++;
|
|
|
|
sendTrackedPacket(QByteArray::fromRawData((const char*)p, sizeof(p)));
|
|
return;
|
|
}
|
|
|
|
|
|
|
|
void udpCivData::dataReceived()
|
|
{
|
|
while (udp->hasPendingDatagrams()) {
|
|
QNetworkDatagram datagram = udp->receiveDatagram();
|
|
//qDebug() << "Received: " << datagram.data();
|
|
QByteArray r = datagram.data();
|
|
|
|
switch (r.length())
|
|
{
|
|
case (16): // Response to pkt0
|
|
if (r.mid(0, 8) == QByteArrayLiteral("\x10\x00\x00\x00\x06\x00\x01\x00"))
|
|
{
|
|
// Update remoteId
|
|
remoteId = qFromLittleEndian<quint32>(r.mid(8, 4));
|
|
sendOpenClose(false);
|
|
}
|
|
break;
|
|
default:
|
|
if (r.length() > 21) {
|
|
// First check if we are missing any packets?
|
|
uint16_t gotSeq = qFromLittleEndian<quint16>(r.mid(6, 2));
|
|
if (lastReceivedSeq == 0 || lastReceivedSeq > gotSeq) {
|
|
lastReceivedSeq = gotSeq;
|
|
}
|
|
|
|
for (uint16_t f = lastReceivedSeq + 1; f < gotSeq; f++) {
|
|
// Do we need to request a retransmit?
|
|
qDebug() << this->metaObject()->className() << ": Missing Sequence: (" << r.length() << ") " << f;
|
|
}
|
|
|
|
|
|
lastReceivedSeq = gotSeq;
|
|
|
|
quint8 temp = r[0] - 0x15;
|
|
if ((quint8)r[16] == 0xc1 && (quint8)r[17] == temp)
|
|
{
|
|
emit receive(r.mid(0x15));
|
|
}
|
|
}
|
|
break;
|
|
|
|
}
|
|
udpBase::dataReceived(r); // Call parent function to process the rest.
|
|
r.clear();
|
|
datagram.clear();
|
|
|
|
}
|
|
}
|
|
|
|
|
|
// Audio stream
|
|
udpAudio::udpAudio(QHostAddress local, QHostAddress ip, quint16 audioPort, quint16 buffer, quint16 rxsample, quint8 rxcodec, quint16 txsample, quint8 txcodec)
|
|
{
|
|
qDebug() << "Starting udpAudio";
|
|
this->localIP = local;
|
|
this->port = audioPort;
|
|
this->radioIP = ip;
|
|
this->bufferSize = buffer;
|
|
this->rxSampleRate = rxsample;
|
|
this->txSampleRate = txsample;
|
|
this->rxCodec = rxcodec;
|
|
this->txCodec = txcodec;
|
|
|
|
init(); // Perform connection
|
|
|
|
QUdpSocket::connect(udp, &QUdpSocket::readyRead, this, &udpAudio::dataReceived);
|
|
|
|
/*
|
|
0x72 is RX audio codec
|
|
0x73 is TX audio codec (only single channel options)
|
|
0x01 uLaw 1ch 8bit
|
|
0x02 PCM 1ch 8bit
|
|
0x04 PCM 1ch 16bit
|
|
0x08 PCM 2ch 8bit
|
|
0x10 PCM 2ch 16bit
|
|
0x20 uLaw 2ch 8bit
|
|
*/
|
|
|
|
if (rxCodec == 0x01 || rxCodec == 0x20) {
|
|
rxIsUlawCodec = true;
|
|
}
|
|
if (rxCodec == 0x08 || rxCodec == 0x10 || rxCodec == 0x20) {
|
|
rxChannelCount = 2;
|
|
}
|
|
if (rxCodec == 0x04 || rxCodec == 0x10) {
|
|
rxNumSamples = 16;
|
|
}
|
|
|
|
rxaudio = new audioHandler();
|
|
rxAudioThread = new QThread(this);
|
|
|
|
rxaudio->moveToThread(rxAudioThread);
|
|
|
|
connect(this, SIGNAL(setupRxAudio(quint8, quint8, quint16, quint16, bool, bool)), rxaudio, SLOT(init(quint8, quint8, quint16, quint16, bool, bool)));
|
|
//connect(this, SIGNAL(haveAudioData(QByteArray)), rxaudio, SLOT(incomingAudio(QByteArray)));
|
|
connect(this, SIGNAL(haveChangeBufferSize(quint16)), rxaudio, SLOT(changeBufferSize(quint16)));
|
|
connect(rxAudioThread, SIGNAL(finished()), rxaudio, SLOT(deleteLater()));
|
|
|
|
if (txCodec == 0x01)
|
|
txIsUlawCodec = true;
|
|
else if (txCodec == 0x04)
|
|
txNumSamples = 16;
|
|
|
|
txChannelCount = 1; // Only 1 channel is supported.
|
|
|
|
txaudio = new audioHandler();
|
|
txAudioThread = new QThread(this);
|
|
|
|
txaudio->moveToThread(txAudioThread);
|
|
|
|
connect(this, SIGNAL(setupTxAudio(quint8, quint8, quint16, quint16, bool, bool)), txaudio, SLOT(init(quint8, quint8, quint16, quint16, bool, bool)));
|
|
//connect(txaudio, SIGNAL(haveAudioData(QByteArray)), this, SLOT(sendTxAudio(QByteArray)));
|
|
connect(txAudioThread, SIGNAL(finished()), txaudio, SLOT(deleteLater()));
|
|
|
|
rxAudioThread->start();
|
|
|
|
txAudioThread->start();
|
|
|
|
sendControl(false, 0x03, 0x00); // First connect packet
|
|
|
|
connect(&pingTimer, &QTimer::timeout, this, &udpBase::sendPing);
|
|
pingTimer.start(PING_PERIOD); // send ping packets every 100ms
|
|
|
|
connect(&txAudioTimer, &QTimer::timeout, this, &udpAudio::sendTxAudio);
|
|
txAudioTimer.start(TXAUDIO_PERIOD);
|
|
|
|
emit setupTxAudio(txNumSamples, txChannelCount, txSampleRate, bufferSize, txIsUlawCodec, true);
|
|
emit setupRxAudio(rxNumSamples, rxChannelCount, rxSampleRate, bufferSize, rxIsUlawCodec, false);
|
|
|
|
|
|
}
|
|
|
|
udpAudio::~udpAudio()
|
|
{
|
|
if (txAudioTimer.isActive())
|
|
{
|
|
txAudioTimer.stop();
|
|
}
|
|
|
|
if (rxAudioThread) {
|
|
rxAudioThread->quit();
|
|
rxAudioThread->wait();
|
|
}
|
|
|
|
if (txAudioThread) {
|
|
txAudioThread->quit();
|
|
txAudioThread->wait();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
#define TXAUDIO_SIZE 0x18
|
|
void udpAudio::sendTxAudio()
|
|
{
|
|
quint8 p[TXAUDIO_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
p[0x10] = 0x80;
|
|
|
|
if (txaudio->chunkAvailable) {
|
|
QByteArray audio;
|
|
txaudio->getNextAudioChunk(audio);
|
|
int counter = 0;
|
|
while (counter < audio.length()) {
|
|
QByteArray tx = QByteArray::fromRawData((const char*)p, sizeof(p));
|
|
QByteArray partial = audio.mid(counter, 1364);
|
|
tx.append(partial);
|
|
tx[0] = static_cast<quint8>(tx.length() & 0xff);
|
|
tx[1] = static_cast<quint8>(tx.length() >> 8 & 0xff);
|
|
tx[18] = static_cast<quint8>(sendAudioSeq >> 8 & 0xff);
|
|
tx[19] = static_cast<quint8>(sendAudioSeq & 0xff);
|
|
tx[22] = static_cast<quint8>(partial.length() >> 8 & 0xff);
|
|
tx[23] = static_cast<quint8>(partial.length() & 0xff);
|
|
counter = counter + partial.length();
|
|
//qDebug() << "Sending audio packet length: " << tx.length();
|
|
sendTrackedPacket(tx);
|
|
sendAudioSeq++;
|
|
}
|
|
}
|
|
}
|
|
|
|
void udpAudio::changeBufferSize(quint16 value)
|
|
{
|
|
emit haveChangeBufferSize(value);
|
|
}
|
|
|
|
|
|
|
|
void udpAudio::dataReceived()
|
|
{
|
|
while (udp->hasPendingDatagrams()) {
|
|
QNetworkDatagram datagram = udp->receiveDatagram();
|
|
//qDebug() << "Received: " << datagram.data();
|
|
QByteArray r = datagram.data();
|
|
|
|
switch (r.length())
|
|
{
|
|
case (16): // Response to idle packet handled in udpBase
|
|
break;
|
|
|
|
default:
|
|
/* Audio packets start as follows:
|
|
PCM 16bit and PCM8/uLAW stereo: 0x44,0x02 for first packet and 0x6c,0x05 for second.
|
|
uLAW 8bit/PCM 8bit 0xd8,0x03 for all packets
|
|
PCM 16bit stereo 0x6c,0x05 first & second 0x70,0x04 third
|
|
|
|
|
|
*/
|
|
if (r.mid(0, 2) == QByteArrayLiteral("\x6c\x05") ||
|
|
r.mid(0, 2) == QByteArrayLiteral("\x44\x02") ||
|
|
r.mid(0, 2) == QByteArrayLiteral("\xd8\x03") ||
|
|
r.mid(0, 2) == QByteArrayLiteral("\x70\x04"))
|
|
{
|
|
// First check if we are missing any packets as seq should be sequential.
|
|
uint16_t gotSeq = qFromLittleEndian<quint16>(r.mid(6, 2));
|
|
if (lastReceivedSeq == 0 || lastReceivedSeq > gotSeq) {
|
|
lastReceivedSeq = gotSeq;
|
|
}
|
|
|
|
for (uint16_t f = lastReceivedSeq+1 ; f < gotSeq; f++) {
|
|
// Do we need to request a retransmit?
|
|
qDebug() << this->metaObject()->className() << ": Missing Sequence: (" << r.length() << ") " << f;
|
|
}
|
|
|
|
lastReceivedSeq = gotSeq;
|
|
|
|
rxaudio->incomingAudio(r.mid(24));
|
|
}
|
|
break;
|
|
}
|
|
|
|
udpBase::dataReceived(r); // Call parent function to process the rest.
|
|
r.clear();
|
|
datagram.clear();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
void udpBase::init()
|
|
{
|
|
udp = new QUdpSocket(this);
|
|
udp->bind(); // Bind to random port.
|
|
localPort = udp->localPort();
|
|
qDebug() << "UDP Stream bound to local port:" << localPort << " remote port:" << port;
|
|
uint32_t addr = localIP.toIPv4Address();
|
|
myId = (addr >> 8 & 0xff) << 24 | (addr & 0xff) << 16 | (localPort & 0xffff);
|
|
}
|
|
|
|
udpBase::~udpBase()
|
|
{
|
|
qDebug() << "Closing UDP stream :" << radioIP.toString() << ":" << port;
|
|
if (udp != Q_NULLPTR) {
|
|
sendControl(false, 0x05, 0x00); // Send disconnect
|
|
udp->close();
|
|
delete udp;
|
|
}
|
|
if (pingTimer.isActive())
|
|
{
|
|
pingTimer.stop();
|
|
}
|
|
if (idleTimer.isActive())
|
|
{
|
|
idleTimer.stop();
|
|
}
|
|
}
|
|
|
|
// Base class!
|
|
|
|
void udpBase::dataReceived(QByteArray r)
|
|
{
|
|
switch (r.length())
|
|
{
|
|
case (16): // Empty response used for simple comms and retransmit requests.
|
|
if (r.mid(0, 8) == QByteArrayLiteral("\x10\x00\x00\x00\x04\x00\x00\x00")) {
|
|
// If timer is active, stop it as they are obviously there!
|
|
qDebug() << this->metaObject()->className() << ": Received I am here";
|
|
areYouThereCounter = 0;
|
|
// I don't think that we will ever receive an "I am here" other than in response to "Are you there?"
|
|
remoteId = qFromLittleEndian<quint32>(r.mid(8, 4));
|
|
sendControl(false,0x06,0x01); // Send Are you ready - untracked.
|
|
}
|
|
else if (r.mid(0, 6) == QByteArrayLiteral("\x10\x00\x00\x00\x00\x00")) {
|
|
// Just get the seqnum and ignore the rest.
|
|
lastReceivedSeq = qFromLittleEndian<quint16>(r.mid(6, 2));
|
|
}
|
|
else if (r.mid(0, 6) == QByteArrayLiteral("\x10\x00\x00\x00\x01\x00")) {
|
|
// retransmit request
|
|
// Send an idle with the requested seqnum if not found.
|
|
uint16_t gotSeq = qFromLittleEndian<quint16>(r.mid(6, 2));
|
|
bool found=false;
|
|
for (int f = txSeqBuf.length() - 1; f >= 0; f--)
|
|
{
|
|
packetsLost++;
|
|
if (txSeqBuf[f].seqNum == gotSeq) {
|
|
//qDebug() << this->metaObject()->className() << ": retransmitting packet :" << gotSeq << " (len=" << txSeqBuf[f].data.length() << ")";
|
|
QMutexLocker locker(&mutex);
|
|
udp->writeDatagram(txSeqBuf[f].data, radioIP, port);
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found)
|
|
{
|
|
// Packet was not found in buffer
|
|
//qDebug() << this->metaObject()->className() << ": Could not find requested packet " << gotSeq << ", sending idle.";
|
|
sendControl(false, 0, gotSeq);
|
|
}
|
|
}
|
|
else if (r.mid(0, 6) == QByteArrayLiteral("\x18\x00\x00\x00\x01\x00"))
|
|
{ // retransmit range request, can contain multiple ranges.
|
|
for (int f = 16; f < r.length() - 4; f = f + 4)
|
|
{
|
|
quint16 start = qFromLittleEndian<quint16>(r.mid(f, 2));
|
|
quint16 end = qFromLittleEndian<quint16>(r.mid(f + 2, 2));
|
|
packetsLost=packetsLost + (end-start);
|
|
qDebug() << this->metaObject()->className() << ": Retransmit range request for:" << start << " to " << end;
|
|
for (quint16 gotSeq = start; gotSeq <= end; gotSeq++)
|
|
{
|
|
bool found=false;
|
|
for (int h = txSeqBuf.length() - 1; h >= 0; h--)
|
|
if (txSeqBuf[h].seqNum == gotSeq) {
|
|
//qDebug() << this->metaObject()->className() << ": retransmitting packet :" << gotSeq << " (len=" << txSeqBuf[f].data.length() << ")";
|
|
QMutexLocker locker(&mutex);
|
|
udp->writeDatagram(txSeqBuf[h].data, radioIP, port);
|
|
found = true;
|
|
break;
|
|
}
|
|
if (!found)
|
|
{
|
|
//qDebug() << this->metaObject()->className() << ": Could not find requested packet " << gotSeq << ", sending idle.";
|
|
sendControl(false, 0, gotSeq);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case (21):
|
|
if (r.mid(1, 5) == QByteArrayLiteral("\x00\x00\x00\x07\x00"))
|
|
{
|
|
// It is a ping request/response
|
|
uint16_t gotSeq = qFromLittleEndian<quint16>(r.mid(6, 2));
|
|
|
|
if (r[0x10] == (char)0x00)
|
|
{
|
|
quint8 p[0x15];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian(sizeof(p), p + 0x00);
|
|
p[0x04] = 0x07;
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
p[0x10] = (char)0x01;
|
|
p[0x11] = (char)r[0x11];
|
|
p[0x12] = (char)r[0x12];
|
|
p[0x13] = (char)r[0x13];
|
|
p[0x14] = (char)r[0x14];
|
|
|
|
QMutexLocker locker(&mutex);
|
|
udp->writeDatagram(QByteArray::fromRawData((const char*)p, sizeof(p)), radioIP, port);
|
|
}
|
|
else if (r[0x10] == (char)0x01) {
|
|
if (gotSeq == pingSendSeq)
|
|
{
|
|
// This is response to OUR request so increment counter
|
|
pingSendSeq++;
|
|
}
|
|
else {
|
|
// Not sure what to do here, need to spend more time with the protocol but try sending ping with same seq next time?
|
|
//qDebug() << "Received out-of-sequence ping response. Sent:" << pingSendSeq << " received " << gotSeq;
|
|
}
|
|
|
|
}
|
|
else {
|
|
qDebug() << "Unhandled response to ping. I have never seen this! 0x10=" << r[16];
|
|
}
|
|
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#define CONTROL_SIZE 0x10
|
|
// Used to send idle and other "control" style messages
|
|
void udpBase::sendControl(bool tracked=true, quint8 id=0, quint16 seq=0)
|
|
{
|
|
quint8 p[CONTROL_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian((quint16)sizeof(p), p + 0x00);
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
p[0x04] = id;
|
|
|
|
lastControlPacketSentTime = QDateTime::currentDateTime(); // Is this used?
|
|
if (!tracked) {
|
|
qToLittleEndian(seq, p + 0x06);
|
|
QMutexLocker locker(&mutex);
|
|
udp->writeDatagram(QByteArray::fromRawData((const char*)p, sizeof(p)), radioIP, port);
|
|
}
|
|
else {
|
|
sendTrackedPacket(QByteArray::fromRawData((const char*)p, sizeof(p)));
|
|
}
|
|
if (idleTimer.isActive()) {
|
|
idleTimer.start(IDLE_PERIOD); // Reset idle counter if it's running
|
|
}
|
|
return;
|
|
}
|
|
|
|
#define PING_SIZE 0x15
|
|
// Send periodic ping packets
|
|
void udpBase::sendPing()
|
|
{
|
|
quint32 pingSeq = (quint32)((quint8)(rand() & 0xff)) | (quint16)innerSendSeq << 8 | (quint8)0x06 << 24;
|
|
|
|
quint8 p[PING_SIZE];
|
|
memset(p, 0x0, sizeof(p));
|
|
qToLittleEndian((quint16)sizeof(p), p + 0x00);
|
|
p[0x04] = 0x07;
|
|
qToLittleEndian(myId, p + 0x08);
|
|
qToLittleEndian(remoteId, p + 0x0c);
|
|
qToLittleEndian(pingSeq, p + 0x11);
|
|
lastPingSentTime = QDateTime::currentDateTime();
|
|
QMutexLocker locker(&mutex);
|
|
udp->writeDatagram(QByteArray::fromRawData((const char*)p, sizeof(p)), radioIP, port);
|
|
innerSendSeq++;
|
|
return;
|
|
}
|
|
|
|
|
|
void udpBase::sendTrackedPacket(QByteArray d)
|
|
{
|
|
// As the radio can request retransmission of these packets, store them in a buffer (eventually!)
|
|
d[6] = sendSeq & 0xff;
|
|
d[7] = (sendSeq >> 8) & 0xff;
|
|
SEQBUFENTRY s;
|
|
s.seqNum = sendSeq;
|
|
s.timeSent = time(NULL);
|
|
s.data = (d);
|
|
txSeqBuf.append(s);
|
|
purgeOldEntries(); // Delete entries older than PURGE_SECONDS seconds (currently 5)
|
|
sendSeq++;
|
|
|
|
QMutexLocker locker(&mutex);
|
|
udp->writeDatagram(d, radioIP, port);
|
|
if (idleTimer.isActive()) {
|
|
idleTimer.start(IDLE_PERIOD); // Reset idle counter if it's running
|
|
}
|
|
packetsSent++;
|
|
return;
|
|
}
|
|
|
|
void udpBase::purgeOldEntries()
|
|
{
|
|
for (int f = txSeqBuf.length() - 1; f >= 0; f--)
|
|
{
|
|
if (difftime(time(NULL), txSeqBuf[f].timeSent) > PURGE_SECONDS)
|
|
{
|
|
txSeqBuf.removeAt(f);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// passcode function used to generate secure (ish) code
|
|
/// </summary>
|
|
/// <param name="str"></param>
|
|
/// <returns>pointer to encoded username or password</returns>
|
|
void passcode(QString in, QByteArray& out)
|
|
{
|
|
const quint8 sequence[] =
|
|
{
|
|
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
|
0x47,0x5d,0x4c,0x42,0x66,0x20,0x23,0x46,0x4e,0x57,0x45,0x3d,0x67,0x76,0x60,0x41,0x62,0x39,0x59,0x2d,0x68,0x7e,
|
|
0x7c,0x65,0x7d,0x49,0x29,0x72,0x73,0x78,0x21,0x6e,0x5a,0x5e,0x4a,0x3e,0x71,0x2c,0x2a,0x54,0x3c,0x3a,0x63,0x4f,
|
|
0x43,0x75,0x27,0x79,0x5b,0x35,0x70,0x48,0x6b,0x56,0x6f,0x34,0x32,0x6c,0x30,0x61,0x6d,0x7b,0x2f,0x4b,0x64,0x38,
|
|
0x2b,0x2e,0x50,0x40,0x3f,0x55,0x33,0x37,0x25,0x77,0x24,0x26,0x74,0x6a,0x28,0x53,0x4d,0x69,0x22,0x5c,0x44,0x31,
|
|
0x36,0x58,0x3b,0x7a,0x51,0x5f,0x52,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
|
|
|
|
};
|
|
|
|
quint8* res = new quint8[16];
|
|
memset(res, 0, 16); // Make sure res buffer is empty!
|
|
QByteArray ba = in.toLocal8Bit();
|
|
uchar* ascii = (uchar*)ba.constData();
|
|
for (int i = 0; i < in.length() && i < 16; i++)
|
|
{
|
|
int p = ascii[i] + i;
|
|
if (p > 126)
|
|
{
|
|
p = 32 + p % 127;
|
|
}
|
|
out.append(sequence[p]);
|
|
}
|
|
return;
|
|
}
|
|
|
|
/// <summary>
|
|
/// returns a QByteArray of a null terminated string
|
|
/// </summary>
|
|
/// <param name="c"></param>
|
|
/// <param name="s"></param>
|
|
/// <returns></returns>
|
|
QByteArray parseNullTerminatedString(QByteArray c, int s)
|
|
{
|
|
//QString res = "";
|
|
QByteArray res;
|
|
for (int i = s; i < c.length(); i++)
|
|
{
|
|
if (c[i] != '\0')
|
|
{
|
|
res.append(c[i]);
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|