521 lines
14 KiB
C++
521 lines
14 KiB
C++
/**
|
|
* @file cusdr_protocol2_io.cpp
|
|
* @brief openHPSDR Protocol 2 transport helper
|
|
*/
|
|
|
|
#define LOG_DATAIO
|
|
|
|
#include "cusdr_protocol2_io.h"
|
|
|
|
#ifdef LOG_DATAIO
|
|
# define PROTOCOL2_DEBUG qDebug().nospace() << "Protocol2::\t"
|
|
#else
|
|
# define PROTOCOL2_DEBUG nullDebug()
|
|
#endif
|
|
|
|
|
|
namespace {
|
|
|
|
static const quint16 kProtocol2DdcSpecificPort = 1025;
|
|
static const quint16 kProtocol2HighPriorityToPcPort = 1025;
|
|
static const quint16 kProtocol2DdcAudioPort = 1028;
|
|
static const quint16 kProtocol2DucIqPort = 1029;
|
|
static const quint16 kProtocol2DdcBasePort = 1035;
|
|
static const int kProtocol2GeneralPacketSize = 60;
|
|
static const int kProtocol2HighPriorityPacketSize = 1444;
|
|
static const int kProtocol2StatusPacketMinSize = 31;
|
|
|
|
quint32 alexFilterWordForFrequency(Settings *set, long frequency, int currentBand) {
|
|
|
|
if (!set || !set->getAlexPresence())
|
|
return 0;
|
|
|
|
quint32 alexWord = 0;
|
|
const quint16 alexConfig = set->getAlexConfig();
|
|
const QList<long> hpfLo = set->getHPFLoFrequencies();
|
|
const QList<long> hpfHi = set->getHPFHiFrequencies();
|
|
const QList<long> lpfLo = set->getLPFLoFrequencies();
|
|
const QList<long> lpfHi = set->getLPFHiFrequencies();
|
|
const QList<int> alexStates = set->getAlexStates();
|
|
|
|
if (currentBand >= 0 && currentBand < alexStates.size()) {
|
|
|
|
const int rxAntenna = alexStates.at(currentBand) & 0x03;
|
|
if (rxAntenna >= 1 && rxAntenna <= 3)
|
|
alexWord |= (1u << (23 + rxAntenna));
|
|
}
|
|
|
|
if ((alexConfig & 0x02) != 0) {
|
|
alexWord |= (1u << 12);
|
|
}
|
|
else if (hpfLo.size() >= 6 && hpfHi.size() >= 6) {
|
|
|
|
if (frequency >= hpfLo.at(0) && frequency <= hpfHi.at(0))
|
|
alexWord |= (1u << 6);
|
|
else if (frequency >= hpfLo.at(1) && frequency <= hpfHi.at(1))
|
|
alexWord |= (1u << 5);
|
|
else if (frequency >= hpfLo.at(2) && frequency <= hpfHi.at(2))
|
|
alexWord |= (1u << 4);
|
|
else if (frequency >= hpfLo.at(3) && frequency <= hpfHi.at(3))
|
|
alexWord |= (1u << 1);
|
|
else if (frequency >= hpfLo.at(4) && frequency <= hpfHi.at(4))
|
|
alexWord |= (1u << 2);
|
|
else if (frequency >= hpfLo.at(5) && frequency <= hpfHi.at(5)) {
|
|
|
|
if ((alexConfig & 0x04) != 0)
|
|
alexWord |= (1u << 3);
|
|
else
|
|
alexWord |= (1u << 12);
|
|
}
|
|
else {
|
|
alexWord |= (1u << 12);
|
|
}
|
|
}
|
|
|
|
if (lpfLo.size() >= 7 && lpfHi.size() >= 7) {
|
|
|
|
if (frequency >= lpfLo.at(0) && frequency <= lpfHi.at(0))
|
|
alexWord |= (1u << 23);
|
|
else if (frequency >= lpfLo.at(1) && frequency <= lpfHi.at(1))
|
|
alexWord |= (1u << 22);
|
|
else if (frequency >= lpfLo.at(2) && frequency <= lpfHi.at(2))
|
|
alexWord |= (1u << 21);
|
|
else if (frequency >= lpfLo.at(3) && frequency <= lpfHi.at(3))
|
|
alexWord |= (1u << 20);
|
|
else if (frequency >= lpfLo.at(4) && frequency <= lpfHi.at(4))
|
|
alexWord |= (1u << 31);
|
|
else if (frequency >= lpfLo.at(5) && frequency <= lpfHi.at(5))
|
|
alexWord |= (1u << 30);
|
|
else
|
|
alexWord |= (1u << 29);
|
|
}
|
|
|
|
return alexWord;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
Protocol2DataPath::Protocol2DataPath(Settings *settings, THPSDRParameter *ioData, QObject *parent)
|
|
: QObject(parent)
|
|
, set(settings)
|
|
, io(ioData)
|
|
, m_commandSocket(0)
|
|
, m_statusSocket(0)
|
|
, m_heartbeatTimer(0)
|
|
, m_running(false)
|
|
, m_generalSequence(0)
|
|
, m_ddcSequence(0)
|
|
, m_highPrioritySequence(0)
|
|
{
|
|
m_legacyControlBytes = QByteArray(5, 0x00);
|
|
}
|
|
|
|
Protocol2DataPath::~Protocol2DataPath() {
|
|
stop();
|
|
}
|
|
|
|
bool Protocol2DataPath::isActive() const {
|
|
return set->getCurrentMetisCard().protocolVersion >= 2;
|
|
}
|
|
|
|
bool Protocol2DataPath::initSockets() {
|
|
|
|
if (!isActive())
|
|
return false;
|
|
|
|
closeSockets();
|
|
|
|
const QHostAddress localAddress(set->getHPSDRDeviceLocalAddr());
|
|
const quint16 discoveryPort = set->getMetisPort();
|
|
const int receivers = qMax(1, set->getNumberOfReceivers());
|
|
|
|
m_commandSocket = new QUdpSocket(this);
|
|
if (!m_commandSocket->bind(localAddress, discoveryPort, QUdpSocket::DontShareAddress)) {
|
|
PROTOCOL2_DEBUG << "command socket bind failed on port " << discoveryPort;
|
|
closeSockets();
|
|
return false;
|
|
}
|
|
|
|
m_statusSocket = new QUdpSocket(this);
|
|
if (!m_statusSocket->bind(localAddress, kProtocol2HighPriorityToPcPort, QUdpSocket::DontShareAddress)) {
|
|
PROTOCOL2_DEBUG << "status socket bind failed on port " << kProtocol2HighPriorityToPcPort;
|
|
closeSockets();
|
|
return false;
|
|
}
|
|
CHECKED_CONNECT(
|
|
m_statusSocket,
|
|
SIGNAL(readyRead()),
|
|
this,
|
|
SLOT(readStatusData()));
|
|
|
|
m_ddcSampleBuffers.clear();
|
|
m_ddcSockets.clear();
|
|
for (int rx = 0; rx < receivers; ++rx) {
|
|
|
|
QUdpSocket *socket = new QUdpSocket(this);
|
|
const quint16 port = kProtocol2DdcBasePort + rx;
|
|
if (!socket->bind(localAddress, port, QUdpSocket::DontShareAddress)) {
|
|
PROTOCOL2_DEBUG << "DDC socket bind failed on port " << port;
|
|
delete socket;
|
|
closeSockets();
|
|
return false;
|
|
}
|
|
|
|
CHECKED_CONNECT(
|
|
socket,
|
|
SIGNAL(readyRead()),
|
|
this,
|
|
SLOT(readDdcData()));
|
|
|
|
m_ddcSockets.append(socket);
|
|
m_ddcSampleBuffers.append(QList<Protocol2IQSample>());
|
|
}
|
|
|
|
m_heartbeatTimer = new QTimer(this);
|
|
m_heartbeatTimer->setInterval(100);
|
|
CHECKED_CONNECT(
|
|
m_heartbeatTimer,
|
|
SIGNAL(timeout()),
|
|
this,
|
|
SLOT(sendHeartbeat()));
|
|
|
|
m_partialLegacyFrame.clear();
|
|
m_partialLegacyFrame.reserve(BUFFER_SIZE);
|
|
m_legacyControlBytes.fill(0x00, 5);
|
|
m_running = false;
|
|
|
|
PROTOCOL2_DEBUG << "sockets initialized for " << receivers << " receiver(s).";
|
|
return true;
|
|
}
|
|
|
|
void Protocol2DataPath::stop() {
|
|
|
|
m_running = false;
|
|
|
|
if (m_heartbeatTimer)
|
|
m_heartbeatTimer->stop();
|
|
|
|
if (m_commandSocket)
|
|
sendHighPriorityPacket(false);
|
|
|
|
closeSockets();
|
|
}
|
|
|
|
void Protocol2DataPath::sendInitFramesToNetworkDevice(int rx) {
|
|
Q_UNUSED(rx)
|
|
}
|
|
|
|
void Protocol2DataPath::networkDeviceStartStop(char value) {
|
|
|
|
if (!m_commandSocket)
|
|
return;
|
|
|
|
if (value == 0) {
|
|
m_running = false;
|
|
if (m_heartbeatTimer)
|
|
m_heartbeatTimer->stop();
|
|
sendHighPriorityPacket(false);
|
|
return;
|
|
}
|
|
|
|
sendGeneralPacket();
|
|
sendDdcSpecificPacket();
|
|
sendHighPriorityPacket(true);
|
|
|
|
m_running = true;
|
|
if (m_heartbeatTimer)
|
|
m_heartbeatTimer->start();
|
|
}
|
|
|
|
void Protocol2DataPath::readStatusData() {
|
|
|
|
while (m_statusSocket && m_statusSocket->hasPendingDatagrams()) {
|
|
|
|
m_statusDatagram.resize(m_statusSocket->pendingDatagramSize());
|
|
m_statusSocket->readDatagram(m_statusDatagram.data(), m_statusDatagram.size());
|
|
|
|
if (m_statusDatagram.size() >= kProtocol2StatusPacketMinSize)
|
|
updateLegacyControlBytes(m_statusDatagram);
|
|
}
|
|
}
|
|
|
|
void Protocol2DataPath::readDdcData() {
|
|
|
|
QUdpSocket *socket = qobject_cast<QUdpSocket *>(sender());
|
|
const int rx = m_ddcSockets.indexOf(socket);
|
|
|
|
if (rx < 0)
|
|
return;
|
|
|
|
while (socket->hasPendingDatagrams()) {
|
|
|
|
QByteArray datagram;
|
|
datagram.resize(socket->pendingDatagramSize());
|
|
socket->readDatagram(datagram.data(), datagram.size());
|
|
|
|
if (datagram.size() < 16)
|
|
continue;
|
|
|
|
const int bitsPerSample = ((quint8)datagram.at(12) << 8) | (quint8)datagram.at(13);
|
|
const int sampleCount = ((quint8)datagram.at(14) << 8) | (quint8)datagram.at(15);
|
|
const int payloadBytes = 16 + sampleCount * 6;
|
|
|
|
if (bitsPerSample != 24 || datagram.size() < payloadBytes)
|
|
continue;
|
|
|
|
for (int i = 0; i < sampleCount; ++i) {
|
|
|
|
const char *sample = datagram.constData() + 16 + i * 6;
|
|
Protocol2IQSample iqSample;
|
|
iqSample.i = read24BitSample(sample);
|
|
iqSample.q = read24BitSample(sample + 3);
|
|
m_ddcSampleBuffers[rx].append(iqSample);
|
|
}
|
|
}
|
|
|
|
tryProduceLegacyFrames();
|
|
}
|
|
|
|
void Protocol2DataPath::sendHeartbeat() {
|
|
|
|
if (!m_running)
|
|
return;
|
|
|
|
sendHighPriorityPacket(true);
|
|
}
|
|
|
|
void Protocol2DataPath::closeSockets() {
|
|
|
|
if (m_heartbeatTimer) {
|
|
delete m_heartbeatTimer;
|
|
m_heartbeatTimer = 0;
|
|
}
|
|
|
|
foreach (QUdpSocket *socket, m_ddcSockets) {
|
|
socket->close();
|
|
delete socket;
|
|
}
|
|
m_ddcSockets.clear();
|
|
m_ddcSampleBuffers.clear();
|
|
|
|
if (m_statusSocket) {
|
|
m_statusSocket->close();
|
|
delete m_statusSocket;
|
|
m_statusSocket = 0;
|
|
}
|
|
|
|
if (m_commandSocket) {
|
|
m_commandSocket->close();
|
|
delete m_commandSocket;
|
|
m_commandSocket = 0;
|
|
}
|
|
|
|
m_partialLegacyFrame.clear();
|
|
}
|
|
|
|
void Protocol2DataPath::sendGeneralPacket() {
|
|
|
|
QByteArray datagram(kProtocol2GeneralPacketSize, 0x00);
|
|
|
|
datagram[0] = (m_generalSequence >> 24) & 0xFF;
|
|
datagram[1] = (m_generalSequence >> 16) & 0xFF;
|
|
datagram[2] = (m_generalSequence >> 8) & 0xFF;
|
|
datagram[3] = m_generalSequence & 0xFF;
|
|
datagram[4] = 0x00;
|
|
datagram[5] = (kProtocol2DdcSpecificPort >> 8) & 0xFF;
|
|
datagram[6] = kProtocol2DdcSpecificPort & 0xFF;
|
|
datagram[7] = (kProtocol2DucIqPort >> 8) & 0xFF;
|
|
datagram[8] = kProtocol2DucIqPort & 0xFF;
|
|
datagram[9] = (HIGH_PRIORITY_FROM_HOST_PORT >> 8) & 0xFF;
|
|
datagram[10] = HIGH_PRIORITY_FROM_HOST_PORT & 0xFF;
|
|
datagram[11] = (kProtocol2HighPriorityToPcPort >> 8) & 0xFF;
|
|
datagram[12] = kProtocol2HighPriorityToPcPort & 0xFF;
|
|
datagram[13] = (kProtocol2DdcAudioPort >> 8) & 0xFF;
|
|
datagram[14] = kProtocol2DdcAudioPort & 0xFF;
|
|
datagram[15] = (kProtocol2DucIqPort >> 8) & 0xFF;
|
|
datagram[16] = kProtocol2DucIqPort & 0xFF;
|
|
datagram[17] = (kProtocol2DdcBasePort >> 8) & 0xFF;
|
|
datagram[18] = kProtocol2DdcBasePort & 0xFF;
|
|
datagram[21] = (HIGH_PRIORITY_FROM_HOST_PORT >> 8) & 0xFF;
|
|
datagram[22] = HIGH_PRIORITY_FROM_HOST_PORT & 0xFF;
|
|
datagram[24] = 0x02;
|
|
datagram[26] = 0x10;
|
|
datagram[27] = 20;
|
|
datagram[28] = 32;
|
|
datagram[59] = set->getAlexPresence() ? 0x01 : 0x00;
|
|
|
|
if (m_commandSocket->writeDatagram(datagram, set->getCurrentMetisCard().ip_address, DEVICE_PORT) >= 0)
|
|
++m_generalSequence;
|
|
else
|
|
PROTOCOL2_DEBUG << "general packet write failed.";
|
|
}
|
|
|
|
void Protocol2DataPath::sendDdcSpecificPacket() {
|
|
|
|
const int receivers = qMax(1, set->getNumberOfReceivers());
|
|
QByteArray datagram(17 + receivers * 6, 0x00);
|
|
|
|
datagram[0] = (m_ddcSequence >> 24) & 0xFF;
|
|
datagram[1] = (m_ddcSequence >> 16) & 0xFF;
|
|
datagram[2] = (m_ddcSequence >> 8) & 0xFF;
|
|
datagram[3] = m_ddcSequence & 0xFF;
|
|
datagram[4] = 0x01;
|
|
datagram[5] = io->ccTx.dither ? 0x01 : 0x00;
|
|
datagram[6] = io->ccTx.random ? 0x01 : 0x00;
|
|
|
|
for (int rx = 0; rx < receivers; ++rx) {
|
|
|
|
datagram[7 + rx / 8] = datagram.at(7 + rx / 8) | (1 << (rx % 8));
|
|
|
|
const int offset = 17 + rx * 6;
|
|
datagram[offset + 0] = 0x00;
|
|
datagram[offset + 1] = (io->samplerate >> 8) & 0xFF;
|
|
datagram[offset + 2] = io->samplerate & 0xFF;
|
|
datagram[offset + 3] = 0x00;
|
|
datagram[offset + 4] = 0x00;
|
|
datagram[offset + 5] = 24;
|
|
}
|
|
|
|
if (m_commandSocket->writeDatagram(datagram, set->getCurrentMetisCard().ip_address, kProtocol2DdcSpecificPort) >= 0)
|
|
++m_ddcSequence;
|
|
else
|
|
PROTOCOL2_DEBUG << "DDC specific packet write failed.";
|
|
}
|
|
|
|
void Protocol2DataPath::sendHighPriorityPacket(bool run) {
|
|
|
|
QByteArray datagram(kProtocol2HighPriorityPacketSize, 0x00);
|
|
const QList<long> frequencies = set->getCtrFrequencies();
|
|
const int receivers = qMax(1, set->getNumberOfReceivers());
|
|
const int currentReceiver = set->getCurrentReceiver();
|
|
const int currentBand = (int)set->getCurrentHamBand(currentReceiver);
|
|
const long currentVfoFrequency = set->getVfoFrequency(currentReceiver);
|
|
const quint32 alexWord = alexFilterWordForFrequency(set, currentVfoFrequency, currentBand);
|
|
|
|
datagram[0] = (m_highPrioritySequence >> 24) & 0xFF;
|
|
datagram[1] = (m_highPrioritySequence >> 16) & 0xFF;
|
|
datagram[2] = (m_highPrioritySequence >> 8) & 0xFF;
|
|
datagram[3] = m_highPrioritySequence & 0xFF;
|
|
datagram[4] = run ? 0x01 : 0x00;
|
|
|
|
for (int rx = 0; rx < receivers; ++rx) {
|
|
|
|
const quint32 encodedFrequency = encodeFrequency(frequencies.value(rx, frequencies.value(0, 0)));
|
|
const int offset = 9 + rx * 4;
|
|
|
|
datagram[offset + 0] = (encodedFrequency >> 24) & 0xFF;
|
|
datagram[offset + 1] = (encodedFrequency >> 16) & 0xFF;
|
|
datagram[offset + 2] = (encodedFrequency >> 8) & 0xFF;
|
|
datagram[offset + 3] = encodedFrequency & 0xFF;
|
|
}
|
|
|
|
if (set->getAlexPresence()) {
|
|
|
|
datagram[1432] = (alexWord >> 24) & 0xFF;
|
|
datagram[1433] = (alexWord >> 16) & 0xFF;
|
|
datagram[1434] = (alexWord >> 8) & 0xFF;
|
|
datagram[1435] = alexWord & 0xFF;
|
|
}
|
|
|
|
if (m_commandSocket->writeDatagram(datagram, set->getCurrentMetisCard().ip_address, HIGH_PRIORITY_FROM_HOST_PORT) >= 0)
|
|
++m_highPrioritySequence;
|
|
else
|
|
PROTOCOL2_DEBUG << "high priority packet write failed.";
|
|
}
|
|
|
|
void Protocol2DataPath::updateLegacyControlBytes(const QByteArray &statusDatagram) {
|
|
|
|
const uchar statusBits = (uchar)statusDatagram.at(4);
|
|
const uchar overloadBits = (uchar)statusDatagram.at(5);
|
|
|
|
char control0 = 0x00;
|
|
if (statusBits & 0x01) control0 |= 0x01;
|
|
if (statusBits & 0x04) control0 |= 0x02;
|
|
if (statusBits & 0x02) control0 |= 0x04;
|
|
|
|
m_legacyControlBytes[0] = control0;
|
|
m_legacyControlBytes[1] = (overloadBits & 0x01) ? 0x01 : 0x00;
|
|
m_legacyControlBytes[2] = 0x00;
|
|
m_legacyControlBytes[3] = 0x00;
|
|
m_legacyControlBytes[4] = (char)(set->getCurrentMetisCard().firmwareVersion & 0xFF);
|
|
}
|
|
|
|
void Protocol2DataPath::tryProduceLegacyFrames() {
|
|
|
|
const int receivers = qMax(1, set->getNumberOfReceivers());
|
|
const int halfFrameSamples = samplesPerLegacyHalfFrame();
|
|
|
|
if (halfFrameSamples <= 0)
|
|
return;
|
|
|
|
while (true) {
|
|
|
|
bool hasEnoughData = true;
|
|
for (int rx = 0; rx < receivers; ++rx) {
|
|
if (rx >= m_ddcSampleBuffers.size() || m_ddcSampleBuffers[rx].size() < halfFrameSamples) {
|
|
hasEnoughData = false;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!hasEnoughData)
|
|
break;
|
|
|
|
QByteArray halfFrame(BUFFER_SIZE / 2, 0x00);
|
|
int pos = 0;
|
|
halfFrame[pos++] = SYNC;
|
|
halfFrame[pos++] = SYNC;
|
|
halfFrame[pos++] = SYNC;
|
|
for (int i = 0; i < m_legacyControlBytes.size(); ++i)
|
|
halfFrame[pos++] = m_legacyControlBytes.at(i);
|
|
|
|
for (int sampleIndex = 0; sampleIndex < halfFrameSamples; ++sampleIndex) {
|
|
for (int rx = 0; rx < receivers; ++rx) {
|
|
|
|
const Protocol2IQSample sample = m_ddcSampleBuffers[rx].takeFirst();
|
|
halfFrame[pos++] = (sample.i >> 16) & 0xFF;
|
|
halfFrame[pos++] = (sample.i >> 8) & 0xFF;
|
|
halfFrame[pos++] = sample.i & 0xFF;
|
|
halfFrame[pos++] = (sample.q >> 16) & 0xFF;
|
|
halfFrame[pos++] = (sample.q >> 8) & 0xFF;
|
|
halfFrame[pos++] = sample.q & 0xFF;
|
|
}
|
|
|
|
halfFrame[pos++] = 0x00;
|
|
halfFrame[pos++] = 0x00;
|
|
}
|
|
|
|
m_partialLegacyFrame += halfFrame;
|
|
if (m_partialLegacyFrame.size() == BUFFER_SIZE) {
|
|
if (!io->iq_queue.isFull())
|
|
io->iq_queue.enqueue(m_partialLegacyFrame);
|
|
m_partialLegacyFrame.clear();
|
|
}
|
|
}
|
|
}
|
|
|
|
int Protocol2DataPath::samplesPerLegacyHalfFrame() const {
|
|
|
|
const int receivers = qMax(1, set->getNumberOfReceivers());
|
|
const int bytesPerSample = receivers * 6 + 2;
|
|
|
|
return (BUFFER_SIZE / 2 - 8) / bytesPerSample;
|
|
}
|
|
|
|
quint32 Protocol2DataPath::encodeFrequency(long frequency) const {
|
|
return (quint32)frequency;
|
|
}
|
|
|
|
qint32 Protocol2DataPath::read24BitSample(const char *data) const {
|
|
|
|
qint32 value = ((qint32)(quint8)data[0] << 16) |
|
|
((qint32)(quint8)data[1] << 8) |
|
|
(qint32)(quint8)data[2];
|
|
|
|
if (value & 0x00800000)
|
|
value |= 0xFF000000;
|
|
|
|
return value;
|
|
}
|