diff --git a/DGIdGateway/IMRSNetwork.cpp b/DGIdGateway/IMRSNetwork.cpp index 08ad667..f7fbc9c 100644 --- a/DGIdGateway/IMRSNetwork.cpp +++ b/DGIdGateway/IMRSNetwork.cpp @@ -17,6 +17,7 @@ */ #include "IMRSNetwork.h" +#include "YSFPayload.h" #include "YSFDefines.h" #include "YSFFICH.h" #include "Utils.h" @@ -43,6 +44,7 @@ void CIMRSNetwork::addDGId(unsigned int dgId, const std::string& name, const std IMRSDGId* f = new IMRSDGId; f->m_dgId = dgId; f->m_name = name; + f->m_seqNo = 0U; f->m_destinations = destinations; f->m_debug = debug; @@ -86,8 +88,6 @@ void CIMRSNetwork::write(unsigned int dgId, const unsigned char* data) { assert(data != NULL); - return; // XXX Disable IMRS transmit - IMRSDGId* ptr = find(dgId); if (ptr == NULL) return; @@ -95,63 +95,128 @@ void CIMRSNetwork::write(unsigned int dgId, const unsigned char* data) CYSFFICH fich; fich.decode(data + 35U); + switch (fich.getFI()) { + case YSF_FI_HEADER: + case YSF_FI_TERMINATOR: + writeHeaderTrailer(ptr, fich, data); + break; + case YSF_FI_COMMUNICATIONS: + writeData(ptr, fich, data); + break; + default: + break; + } +} + +bool CIMRSNetwork::writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data) +{ + assert(ptr != NULL); + assert(data != NULL); + + unsigned char buffer[200U]; + unsigned int length = 0U; + + if (fich.getFI() == YSF_FI_HEADER) { + buffer[0U] = 0x11U; + ptr->m_seqNo = 0U; + } else { + buffer[0U] = 0x33U; + } + + // Copy the sequence number (2 bytes) + buffer[1U] = (ptr->m_seqNo << 8) & 0xFFU; + buffer[2U] = (ptr->m_seqNo << 0) & 0xFFU; + + for (std::vector::const_iterator it = ptr->m_destinations.begin(); it != ptr->m_destinations.end(); ++it) { + // Set the correct DG-ID for this destination + fich.setDGId((*it)->m_dgId); + // Copy the raw FICH + fich.getRaw(buffer + 3U); + + if (ptr->m_debug) + CUtils::dump(1U, "IMRS Network Data Sent", buffer, length); + + m_socket.write(buffer, length, (*it)->m_addr, (*it)->m_addrLen); + } + + return true; +} + +bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data) +{ + assert(ptr != NULL); + assert(data != NULL); + unsigned char buffer[200U]; + unsigned int length = 0U; + + buffer[0U] = 0x22U; + + // Copy the sequence number (2 bytes) + buffer[1U] = (ptr->m_seqNo << 8) & 0xFFU; + buffer[2U] = (ptr->m_seqNo << 0) & 0xFFU; + + unsigned char dt = fich.getDT(); + + CYSFPayload payload; // Create the header - switch (fich.getDT()) { + switch (dt) { case YSF_DT_VD_MODE1: - buffer[0U] = '0'; - buffer[1U] = '0'; - buffer[2U] = '0'; + // Copy the DCH (10 bytes) + payload.readVDMode1Data(data + 35U, buffer + 7U); // Copy the audio - ::memcpy(buffer + 35U + 0U, data + 35U + 9U, 9U); - ::memcpy(buffer + 35U + 9U, data + 35U + 27U, 9U); - ::memcpy(buffer + 35U + 18U, data + 35U + 45U, 9U); - ::memcpy(buffer + 35U + 27U, data + 35U + 63U, 9U); - ::memcpy(buffer + 35U + 36U, data + 35U + 81U, 9U); + ::memcpy(buffer + 17U + 0U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 9U, 9U); + ::memcpy(buffer + 17U + 9U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 27U, 9U); + ::memcpy(buffer + 17U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, 9U); + ::memcpy(buffer + 17U + 27U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 63U, 9U); + ::memcpy(buffer + 17U + 36U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 81U, 9U); + length = 62U; break; case YSF_DT_DATA_FR_MODE: - buffer[0U] = '1'; - buffer[1U] = '1'; - buffer[2U] = '1'; - // Copy the audio - ::memcpy(buffer + 35U, data + 45U, 130U); + // Copy the data + ::memcpy(buffer + 17U + 0U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U, 90U); + length = 107U; break; case YSF_DT_VD_MODE2: - buffer[0U] = '2'; - buffer[1U] = '2'; - buffer[2U] = '2'; + // Copy the DCH (10 bytes) + payload.readVDMode2Data(data + 35U, buffer + 7U); // Copy the audio - ::memcpy(buffer + 35U, data + 45U, 130U); + ::memcpy(buffer + 17U + 0U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 5U, 13U); + ::memcpy(buffer + 17U + 13U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 23U, 13U); + ::memcpy(buffer + 17U + 26U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 41U, 13U); + ::memcpy(buffer + 17U + 39U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 59U, 13U); + ::memcpy(buffer + 17U + 52U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 77U, 13U); + length = 82U; break; case YSF_DT_VOICE_FR_MODE: - buffer[0U] = '3'; - buffer[1U] = '3'; - buffer[2U] = '3'; // Copy the audio - ::memcpy(buffer + 35U + 0U, data + 35U + 9U, 18U); - ::memcpy(buffer + 35U + 18U, data + 35U + 27U, 18U); - ::memcpy(buffer + 35U + 36U, data + 35U + 45U, 18U); - ::memcpy(buffer + 35U + 54U, data + 35U + 63U, 18U); - ::memcpy(buffer + 35U + 72U, data + 35U + 81U, 18U); + ::memcpy(buffer + 17U + 0U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U, 18U); + ::memcpy(buffer + 17U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 18U, 18U); + ::memcpy(buffer + 17U + 36U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 36U, 18U); + ::memcpy(buffer + 17U + 54U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, 18U); + ::memcpy(buffer + 17U + 72U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, 18U); + length = 107U; break; default: - return; + return false; } - // Copy the raw DCH - for (std::vector::const_iterator it = ptr->m_destinations.begin(); it != ptr->m_destinations.end(); ++it) { // Set the correct DG-ID for this destination fich.setDGId((*it)->m_dgId); // Copy the raw FICH - fich.getRaw(buffer + 7U); + fich.getRaw(buffer + 3U); if (ptr->m_debug) - CUtils::dump(1U, "IMRS Network Data Sent", buffer, 165U); + CUtils::dump(1U, "IMRS Network Data Sent", buffer, length); - m_socket.write(buffer, 165U, (*it)->m_addr, (*it)->m_addrLen); + m_socket.write(buffer, length, (*it)->m_addr, (*it)->m_addrLen); } + + ptr->m_seqNo++; + + return true; } void CIMRSNetwork::link() @@ -172,11 +237,10 @@ void CIMRSNetwork::clock(unsigned int ms) if (length <= 0) return; - CUtils::dump(1U, "IMRS Network Data Received", buffer, length); - return; + if (addrLen == 0U) + return; - // if (port != IMRS_PORT) - // return; + CUtils::dump(1U, "IMRS Network Data Received", buffer, length); IMRSDGId* ptr = find(addr); if (ptr == NULL) diff --git a/DGIdGateway/IMRSNetwork.h b/DGIdGateway/IMRSNetwork.h index e7be4f5..5591d59 100644 --- a/DGIdGateway/IMRSNetwork.h +++ b/DGIdGateway/IMRSNetwork.h @@ -23,7 +23,9 @@ #include "YSFDefines.h" #include "UDPSocket.h" #include "RingBuffer.h" +#include "YSFFICH.h" +#include #include #include @@ -38,6 +40,7 @@ public: IMRSDGId() : m_dgId(0U), m_name(), + m_seqNo(0U), m_destinations(), m_debug(false), m_buffer(1000U, "IMRS Buffer") @@ -45,6 +48,7 @@ public: unsigned int m_dgId; std::string m_name; + uint16_t m_seqNo; std::vector m_destinations; bool m_debug; CRingBuffer m_buffer; @@ -84,6 +88,9 @@ private: IMRSDGId* find(const sockaddr_storage& address) const; IMRSDGId* find(unsigned int dgId) const; + + bool writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data); + bool writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data); }; #endif diff --git a/DGIdGateway/YSFFICH.cpp b/DGIdGateway/YSFFICH.cpp index 9975404..8547e54 100644 --- a/DGIdGateway/YSFFICH.cpp +++ b/DGIdGateway/YSFFICH.cpp @@ -177,8 +177,6 @@ void CYSFFICH::setRaw(const unsigned char* bytes) void CYSFFICH::getRaw(unsigned char* bytes) const { ::memcpy(bytes, m_fich, 6U); - - CCRC::addCCITT16(bytes, 6U); } unsigned char CYSFFICH::getFI() const