From 5e6b6a4061aee642394672df7e2b9cc7cfdfb00e Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Mon, 13 Jun 2016 20:54:39 +0100 Subject: [PATCH] Fix the Wires-X data collector and other fixes. --- YSFGateway/WiresX.cpp | 101 ++++++++++++++++++++------------------ YSFGateway/WiresX.h | 11 ++--- YSFGateway/YSFGateway.cpp | 3 +- YSFParrot/Network.cpp | 2 +- 4 files changed, 62 insertions(+), 55 deletions(-) diff --git a/YSFGateway/WiresX.cpp b/YSFGateway/WiresX.cpp index b1b0aa2..7764508 100644 --- a/YSFGateway/WiresX.cpp +++ b/YSFGateway/WiresX.cpp @@ -56,7 +56,6 @@ m_rxFrequency(0U), m_timer(1000U, 2U), m_seqNo(0U), m_header(NULL), -m_source(NULL), m_csd1(NULL), m_csd2(NULL), m_csd3(NULL), @@ -79,7 +78,6 @@ m_search(NULL) m_command = new unsigned char[300U]; m_header = new unsigned char[34U]; - m_source = new unsigned char[20U]; m_csd1 = new unsigned char[20U]; m_csd2 = new unsigned char[20U]; m_csd3 = new unsigned char[20U]; @@ -90,7 +88,6 @@ CWiresX::~CWiresX() delete[] m_csd3; delete[] m_csd2; delete[] m_csd1; - delete[] m_source; delete[] m_header; delete[] m_command; } @@ -153,56 +150,66 @@ bool CWiresX::start() return m_reflectors.load(); } -WX_STATUS CWiresX::process(const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn) +WX_STATUS CWiresX::process(const unsigned char* data, const unsigned char* source, unsigned char fi, unsigned char dt, unsigned char fn, unsigned char ft) { assert(data != NULL); + assert(source != NULL); if (dt != YSF_DT_DATA_FR_MODE) return WXS_NONE; + if (fi != YSF_FI_COMMUNICATIONS) + return WXS_NONE; + CYSFPayload payload; - if (fi == YSF_FI_HEADER) { - payload.readDataFRModeData1(data, m_source); - ::memset(m_command, 0x00U, 300U); + if (fn == 0U) return WXS_NONE; - } - if (fi == YSF_FI_COMMUNICATIONS && fn == 0U) { - if (::memcmp(m_source, " ", 20U) == 0) - payload.readDataFRModeData1(data, m_source); - ::memset(m_command, 0x00U, 300U); - return WXS_NONE; + if (fn == 1U) { + bool valid = payload.readDataFRModeData2(data, m_command + 0U); + if (!valid) + return WXS_NONE; + } else { + bool valid = payload.readDataFRModeData1(data, m_command + (fn - 1U) * 20U + 0U); + if (!valid) + return WXS_NONE; + + valid = payload.readDataFRModeData2(data, m_command + (fn - 1U) * 20U + 20U); + if (!valid) + return WXS_NONE; } - if (fi == YSF_FI_COMMUNICATIONS) { - if (fn == 1U) - ::memset(m_command, 0x00U, 300U); + if (fn == ft) { + bool valid = false; - bool valid = payload.readDataFRModeData2(data, m_command + (fn - 1U) * 20U); - if (!valid) { - ::memset(m_command, 0x00U, 300U); - ::memset(m_source, ' ', 20U); - return WXS_NONE; + // Find the end marker + for (unsigned int i = fn * 20U; i > 0U; i--) { + if (m_command[i] == 0x03U) { + unsigned char crc = CCRC::addCRC(m_command, i + 1U); + if (crc == m_command[i + 1U]) + valid = true; + break; + } } - if (fi == fn) { - if (::memcmp(m_command + 1U, DX_REQ, 3U) == 0) { - processDX(); - return WXS_NONE; - } else if (::memcmp(m_command + 1U, ALL_REQ, 3U) == 0) { - processAll(m_command + 5U); - return WXS_NONE; - } else if (::memcmp(m_command + 1U, CONN_REQ, 3U) == 0) { - return processConnect(m_command + 5U); - } else if (::memcmp(m_command + 1U, DISC_REQ, 3U) == 0) { - processDisconnect(); - return WXS_DISCONNECT; - } else { - CUtils::dump("Unknown Wires-X command", m_command, fn * 20U); - ::memset(m_source, ' ', 20U); - return WXS_NONE; - } + if (!valid) + return WXS_NONE; + + if (::memcmp(m_command + 1U, DX_REQ, 3U) == 0) { + processDX(source); + return WXS_NONE; + } else if (::memcmp(m_command + 1U, ALL_REQ, 3U) == 0) { + processAll(source, m_command + 5U); + return WXS_NONE; + } else if (::memcmp(m_command + 1U, CONN_REQ, 3U) == 0) { + return processConnect(source, m_command + 5U); + } else if (::memcmp(m_command + 1U, DISC_REQ, 3U) == 0) { + processDisconnect(source); + return WXS_DISCONNECT; + } else { + CUtils::dump("Unknown Wires-X command", m_command, fn * 20U); + return WXS_NONE; } } @@ -214,18 +221,18 @@ CYSFReflector* CWiresX::getReflector() const return m_reflector; } -void CWiresX::processDX() +void CWiresX::processDX(const unsigned char* source) { - ::LogDebug("Received DX from %10.10s", m_source + 10U); + ::LogDebug("Received DX from %10.10s", source); m_status = WXSI_DX; m_timer.start(); } -void CWiresX::processAll(const unsigned char* data) +void CWiresX::processAll(const unsigned char* source, const unsigned char* data) { if (data[0U] == '0' && data[1] == '1') { - ::LogDebug("Received ALL for \"%3.3s\" from %10.10s", data + 2U, m_source + 10U); + ::LogDebug("Received ALL for \"%3.3s\" from %10.10s", data + 2U, source); char buffer[4U]; ::memcpy(buffer, data + 2U, 3U); @@ -239,7 +246,7 @@ void CWiresX::processAll(const unsigned char* data) m_timer.start(); } else if (data[0U] == '1' && data[1U] == '1') { - ::LogDebug("Received SEARCH for \"%16.16s\" from %10.10s", data + 5U, m_source + 10U); + ::LogDebug("Received SEARCH for \"%16.16s\" from %10.10s", data + 5U, source); std::string search = std::string((char*)(data + 5U), 16U); @@ -251,9 +258,9 @@ void CWiresX::processAll(const unsigned char* data) } } -WX_STATUS CWiresX::processConnect(const unsigned char* data) +WX_STATUS CWiresX::processConnect(const unsigned char* source, const unsigned char* data) { - ::LogDebug("Received Connect to %5.5s from %10.10s", data, m_source + 10U); + ::LogDebug("Received Connect to %5.5s from %10.10s", data, source); std::string id = std::string((char*)data, 5U); @@ -267,9 +274,9 @@ WX_STATUS CWiresX::processConnect(const unsigned char* data) return WXS_CONNECT; } -void CWiresX::processDisconnect() +void CWiresX::processDisconnect(const unsigned char* source) { - ::LogDebug("Received Disconect from %10.10s", m_source + 10U); + ::LogDebug("Received Disconect from %10.10s", source); m_status = WXSI_DISCONNECT; m_timer.start(); diff --git a/YSFGateway/WiresX.h b/YSFGateway/WiresX.h index e5b890f..b2f4629 100644 --- a/YSFGateway/WiresX.h +++ b/YSFGateway/WiresX.h @@ -50,7 +50,7 @@ public: bool start(); - WX_STATUS process(const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn); + WX_STATUS process(const unsigned char* data, const unsigned char* source, unsigned char fi, unsigned char dt, unsigned char fn, unsigned char ft); CYSFReflector* getReflector() const; @@ -70,7 +70,6 @@ private: CTimer m_timer; unsigned char m_seqNo; unsigned char* m_header; - unsigned char* m_source; unsigned char* m_csd1; unsigned char* m_csd2; unsigned char* m_csd3; @@ -78,10 +77,10 @@ private: unsigned int m_start; CYSFReflector* m_search; - WX_STATUS processConnect(const unsigned char* data); - void processDisconnect(); - void processDX(); - void processAll(const unsigned char* data); + WX_STATUS processConnect(const unsigned char* source, const unsigned char* data); + void processDisconnect(const unsigned char* source); + void processDX(const unsigned char* source); + void processAll(const unsigned char* source, const unsigned char* data); void sendDXReply(); void sendConnectReply(); diff --git a/YSFGateway/YSFGateway.cpp b/YSFGateway/YSFGateway.cpp index 3882e93..d0f58de 100644 --- a/YSFGateway/YSFGateway.cpp +++ b/YSFGateway/YSFGateway.cpp @@ -225,12 +225,13 @@ int CYSFGateway::run() unsigned char fi = fich.getFI(); unsigned char dt = fich.getDT(); unsigned char fn = fich.getFN(); + unsigned char ft = fich.getFT(); // Don't send out control data m_exclude = (dt == YSF_DT_DATA_FR_MODE); if (m_wiresX != NULL) { - WX_STATUS status = m_wiresX->process(buffer + 35U, fi, dt, fn); + WX_STATUS status = m_wiresX->process(buffer + 35U, buffer + 14U, fi, dt, fn, ft); switch (status) { case WXS_CONNECT: { CYSFReflector* reflector = m_wiresX->getReflector(); diff --git a/YSFParrot/Network.cpp b/YSFParrot/Network.cpp index ab954fc..5fbd2cc 100644 --- a/YSFParrot/Network.cpp +++ b/YSFParrot/Network.cpp @@ -114,7 +114,7 @@ void CNetwork::clock(unsigned int ms) // Handle the status command if (::memcmp(buffer, "YSFS", 4U) == 0) { unsigned char status[50U]; - ::sprintf((char*)status, "YSFS%05u%16.16s%14.14s%03u", 1U, "Parrot", "Parrot", 0U); + ::sprintf((char*)status, "YSFS%05u%-16.16s%-14.14s%03u", 1U, "Parrot", "Parrot", 0U); m_socket.write(status, 42U, address, port); return; }