diff --git a/YSFGateway/WiresX.cpp b/YSFGateway/WiresX.cpp index 34e04a5..2cdecd0 100644 --- a/YSFGateway/WiresX.cpp +++ b/YSFGateway/WiresX.cpp @@ -18,6 +18,10 @@ #include "WiresX.h" #include "YSFPayload.h" +#include "YSFFICH.h" +#include "Sync.h" +#include "CRC.h" +#include "Log.h" #include #include @@ -26,35 +30,69 @@ const unsigned char CALL_DX[] = {0x5DU, 0x71U}; const unsigned char CALL_CONNECT[] = {0x5DU, 0x41U}; const unsigned char CALL_ALL[] = {0x5DU, 0x66U}; +const unsigned char DEFAULT_FICH[] = {0x20U, 0x00U, 0x15U, 0x00U}; + +const unsigned char NET_HEADER[] = "YSFDGATEWAY ALL "; + CWiresX::CWiresX(CNetwork* network) : m_network(network), -m_reflector() +m_reflector(), +m_timer(1000U, 0U, 100U + 750U), +m_csd1(NULL) { assert(network != NULL); + + m_csd1 = new unsigned char[20U]; } CWiresX::~CWiresX() { + delete[] m_csd1; } WX_STATUS CWiresX::process(const unsigned char* data, unsigned char fi, unsigned char dt, unsigned char fn) { - if (fi != YSF_FI_COMMUNICATIONS || dt != YSF_DT_DATA_FR_MODE || fn != 1U) - return WXS_NONE; + assert(data != NULL); - unsigned char buffer[20U]; + if (dt != YSF_DT_DATA_FR_MODE) + return WXS_NONE; CYSFPayload payload; - bool valid = payload.readDataFRModeData2(data, buffer); - if (!valid) + + if (fi == YSF_FI_HEADER) { + payload.readDataFRModeData1(data, m_csd1); return WXS_NONE; + } - if (::memcmp(buffer + 1U, CALL_DX, 2U) == 0) - processDX(); - else if (::memcmp(buffer + 1U, CALL_ALL, 2U) == 0) - processAll(); - else if (::memcmp(buffer + 1U, CALL_CONNECT, 2U) == 0) - return processConnect(); + if (fi == YSF_FI_COMMUNICATIONS && fn == 0U) { + if (::memcmp(m_csd1, " ", 20U) == 0) + payload.readDataFRModeData1(data, m_csd1); + return WXS_NONE; + } + + if (fi == YSF_FI_TERMINATOR) { + if (::memcmp(m_csd1, " ", 20U) == 0) + payload.readDataFRModeData1(data, m_csd1); + return WXS_NONE; + } + + if (fi == YSF_FI_COMMUNICATIONS && fn == 1U) { + unsigned char buffer[20U]; + bool valid = payload.readDataFRModeData2(data, buffer); + if (!valid) { + ::memset(m_csd1, ' ', 20U); + return WXS_NONE; + } + + if (::memcmp(buffer + 1U, CALL_DX, 2U) == 0) + processDX(); + else if (::memcmp(buffer + 1U, CALL_ALL, 2U) == 0) + processAll(); + else if (::memcmp(buffer + 1U, CALL_CONNECT, 2U) == 0) + return processConnect(); + else + ::memset(m_csd1, ' ', 20U); + } return WXS_NONE; } @@ -66,7 +104,9 @@ std::string CWiresX::getReflector() const void CWiresX::processDX() { + ::LogDebug("Received DX from %10.10s", m_csd1 + 10U); + m_timer.start(); } void CWiresX::processAll() @@ -81,5 +121,110 @@ WX_STATUS CWiresX::processConnect() void CWiresX::clock(unsigned int ms) { + m_timer.clock(ms); + if (m_timer.isRunning() && m_timer.hasExpired()) { + LogDebug("Send reply"); + + unsigned char data[150U]; + ::memset(data, 0x00U, 150U); + ::memset(data, ' ', 128U); + + data[0U] = 0x03U; // XXX Serial no + data[1U] = 0x5DU; + data[2U] = 0x51U; + data[3U] = 0x5FU; // Start of data marker + data[4U] = 0x26U; // Repeater type + data[127U] = 0x03U; // End of data marker + data[128U] = CCRC::addCRC(data, 128U); + + createReply(data, 140U); + + m_timer.stop(); + } +} + +void CWiresX::createReply(const unsigned char* data, unsigned int length) +{ + assert(data != NULL); + assert(length > 0U); + + // Write the header + unsigned char buffer[200U]; + ::memcpy(buffer, NET_HEADER, 34U); + buffer[34U] = 0x00U; + + CSync::add(buffer + 35U); + + CYSFFICH fich; + fich.load(DEFAULT_FICH); + fich.setFI(YSF_FI_HEADER); + fich.encode(buffer + 35U); + + CYSFPayload payload; + payload.writeDataFRModeData1(m_csd1, buffer + 35U); + // payload.writeDataFRModeData2(" ", buffer + 35U); + + m_network->write(buffer); + + unsigned char bt = length / 260U; + fich.setBT(bt); + + unsigned char fn = 0U; + unsigned char bn = 0U; + + unsigned int offset = 0U; + while (offset < length) { + switch (fn) { + case 0U: { + unsigned int len = length - offset; + if (len > 220U) fich.setFT(7U); + else if (len > 180U) fich.setFT(6U); + else if (len > 140U) fich.setFT(5U); + else if (len > 100U) fich.setFT(4U); + else if (len > 60U) fich.setFT(3U); + else if (len > 20U) fich.setFT(2U); + else fich.setFT(1U); + payload.writeDataFRModeData1(m_csd1, buffer + 35U); + // payload.writeDataFRModeData2(" ", buffer + 35U); + } + break; + case 1U: + // payload.writeDataFRModeData1(" ", buffer + 35U); + payload.writeDataFRModeData2(data + offset, buffer + 35U); + offset += 20U; + break; + default: + payload.writeDataFRModeData1(data + offset, buffer + 35U); + offset += 20U; + payload.writeDataFRModeData2(data + offset, buffer + 35U); + offset += 20U; + break; + } + + fich.setFN(fn); + fich.setBN(bn); + fich.setFI(YSF_FI_COMMUNICATIONS); + fich.encode(buffer + 35U); + + m_network->write(buffer); + + fn++; + if (fn >= 8U) { + fn = 0U; + bn++; + } + } + + // Write the trailer + buffer[34U] = 0x01U; + + fich.load(DEFAULT_FICH); + fich.setFI(YSF_FI_TERMINATOR); + fich.encode(buffer + 35U); + + payload.writeDataFRModeData1(m_csd1, buffer + 35U); + // payload.writeDataFRModeData2(" ", buffer + 35U); + + m_network->write(buffer); } diff --git a/YSFGateway/WiresX.h b/YSFGateway/WiresX.h index d707f85..9910aa2 100644 --- a/YSFGateway/WiresX.h +++ b/YSFGateway/WiresX.h @@ -20,6 +20,7 @@ #define WIRESX_H #include "Network.h" +#include "Timer.h" #include @@ -41,12 +42,16 @@ public: void clock(unsigned int ms); private: - CNetwork* m_network; - std::string m_reflector; + CNetwork* m_network; + std::string m_reflector; + CTimer m_timer; + unsigned char* m_csd1; WX_STATUS processConnect(); void processDX(); void processAll(); + + void createReply(const unsigned char* data, unsigned int length); }; #endif diff --git a/YSFGateway/YSFFICH.cpp b/YSFGateway/YSFFICH.cpp index 55c6a42..2b3ffaf 100644 --- a/YSFGateway/YSFFICH.cpp +++ b/YSFGateway/YSFFICH.cpp @@ -190,3 +190,33 @@ void CYSFFICH::load(const unsigned char* fich) ::memcpy(m_fich, fich, 4U); } + +void CYSFFICH::setFI(unsigned char fi) +{ + m_fich[0U] &= 0x3FU; + m_fich[0U] |= (fi << 6) & 0xC0U; +} + +void CYSFFICH::setBN(unsigned char bn) +{ + m_fich[0U] &= 0xFCU; + m_fich[0U] |= bn & 0x03U; +} + +void CYSFFICH::setBT(unsigned char bt) +{ + m_fich[1U] &= 0x3FU; + m_fich[1U] |= (bt << 6) & 0xC0U; +} + +void CYSFFICH::setFN(unsigned char fn) +{ + m_fich[0U] &= 0xC7U; + m_fich[0U] |= (fn << 3) & 0x38U; +} + +void CYSFFICH::setFT(unsigned char ft) +{ + m_fich[1U] &= 0xF8U; + m_fich[1U] |= ft & 0x07U; +} diff --git a/YSFGateway/YSFFICH.h b/YSFGateway/YSFFICH.h index d75b496..8dc8211 100644 --- a/YSFGateway/YSFFICH.h +++ b/YSFGateway/YSFFICH.h @@ -34,6 +34,12 @@ public: unsigned char getFT() const; unsigned char getDT() const; + void setFI(unsigned char fi); + void setBN(unsigned char bn); + void setBT(unsigned char bt); + void setFN(unsigned char fn); + void setFT(unsigned char ft); + void load(const unsigned char* fich); private: diff --git a/YSFGateway/YSFGateway.cpp b/YSFGateway/YSFGateway.cpp index e5bdf35..a061c25 100644 --- a/YSFGateway/YSFGateway.cpp +++ b/YSFGateway/YSFGateway.cpp @@ -226,12 +226,12 @@ int CYSFGateway::run() if (m_gps != NULL) m_gps->data(buffer + 14U, buffer + 35U, fi, dt, fn); + } - if (fi == YSF_FI_TERMINATOR) { - if (m_gps != NULL) - m_gps->reset(); - watchdogTimer.stop(); - } + if (buffer[34U] == 0x01U) { + if (m_gps != NULL) + m_gps->reset(); + watchdogTimer.stop(); } if (networkEnabled && m_linked) diff --git a/YSFGateway/YSFPayload.cpp b/YSFGateway/YSFPayload.cpp index f79d53b..729903b 100644 --- a/YSFGateway/YSFPayload.cpp +++ b/YSFGateway/YSFPayload.cpp @@ -186,6 +186,8 @@ bool CYSFPayload::readDataFRModeData1(const unsigned char* data, unsigned char* assert(data != NULL); assert(dt != NULL); + ::memset(dt, ' ', 20U); + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; unsigned char dch[45U]; @@ -231,6 +233,8 @@ bool CYSFPayload::readDataFRModeData2(const unsigned char* data, unsigned char* assert(data != NULL); assert(dt != NULL); + ::memset(dt, ' ', 20U); + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; unsigned char dch[45U];