From 5ac1bbe599969eff6d8f8cb70bdc3a9ca707c036 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Thu, 24 Sep 2020 11:47:14 +0100 Subject: [PATCH] More IMRS work. --- DGIdGateway/DGIdGateway.cpp | 3 + DGIdGateway/DGIdGateway.ini | 2 +- DGIdGateway/IMRSNetwork.cpp | 176 +++++++++++++++++++++++-- DGIdGateway/IMRSNetwork.h | 19 ++- DGIdGateway/Version.h | 2 +- DGIdGateway/YSFFICH.cpp | 4 +- DGIdGateway/YSFPayload.cpp | 254 +++++++++++++++++++++++++++++++++--- DGIdGateway/YSFPayload.h | 9 +- 8 files changed, 428 insertions(+), 41 deletions(-) diff --git a/DGIdGateway/DGIdGateway.cpp b/DGIdGateway/DGIdGateway.cpp index 463e7d3..b275a4b 100644 --- a/DGIdGateway/DGIdGateway.cpp +++ b/DGIdGateway/DGIdGateway.cpp @@ -225,6 +225,9 @@ int CDGIdGateway::run() std::vector dgIdData = m_conf.getDGIdData(); for (std::vector::const_iterator it = dgIdData.begin(); it != dgIdData.end(); ++it) { unsigned int dgid = (*it)->m_dgId; + if (dgid >= 100U) + continue; + std::string type = (*it)->m_type; bool statc = (*it)->m_static; unsigned int rfHangTime = (*it)->m_rfHangTime; diff --git a/DGIdGateway/DGIdGateway.ini b/DGIdGateway/DGIdGateway.ini index 0ea6e82..fe5d719 100644 --- a/DGIdGateway/DGIdGateway.ini +++ b/DGIdGateway/DGIdGateway.ini @@ -143,7 +143,7 @@ DGId=80 #NetHangTime=60 Debug=0 -[DGId=100] +[DGId=60] # Local IMRS System Fusion Network Type=IMRS Name=South Coast diff --git a/DGIdGateway/IMRSNetwork.cpp b/DGIdGateway/IMRSNetwork.cpp index f7fbc9c..34b7e54 100644 --- a/DGIdGateway/IMRSNetwork.cpp +++ b/DGIdGateway/IMRSNetwork.cpp @@ -92,6 +92,8 @@ void CIMRSNetwork::write(unsigned int dgId, const unsigned char* data) if (ptr == NULL) return; + CUtils::dump(1U, "YSF Data Received", data, 155U); + CYSFFICH fich; fich.decode(data + 35U); @@ -114,7 +116,6 @@ bool CIMRSNetwork::writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsig assert(data != NULL); unsigned char buffer[200U]; - unsigned int length = 0U; if (fich.getFI() == YSF_FI_HEADER) { buffer[0U] = 0x11U; @@ -127,6 +128,16 @@ bool CIMRSNetwork::writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsig buffer[1U] = (ptr->m_seqNo << 8) & 0xFFU; buffer[2U] = (ptr->m_seqNo << 0) & 0xFFU; + // Copy CSD1 and CSD2 (40 bytes) + CYSFPayload payload; + payload.readHeaderData(data + 35U, buffer + 7U); + + fich.getRaw(buffer + 3U); + CUtils::dump(1U, "IMRS Network Data Sent", buffer, 47U); + + readHeaderTrailer(ptr, fich, buffer); + +#ifdef notdef 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); @@ -134,10 +145,13 @@ bool CIMRSNetwork::writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsig fich.getRaw(buffer + 3U); if (ptr->m_debug) - CUtils::dump(1U, "IMRS Network Data Sent", buffer, length); + CUtils::dump(1U, "IMRS Network Data Sent", buffer, 47U); - m_socket.write(buffer, length, (*it)->m_addr, (*it)->m_addrLen); + m_socket.write(buffer, 47U, (*it)->m_addr, (*it)->m_addrLen); } +#endif + + ptr->m_seqNo++; return true; } @@ -163,15 +177,15 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* // Create the header switch (dt) { case YSF_DT_VD_MODE1: - // Copy the DCH (10 bytes) + // Copy the DCH (20 bytes) payload.readVDMode1Data(data + 35U, buffer + 7U); // Copy the audio - ::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; + ::memcpy(buffer + 27U + 0U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 9U, 9U); + ::memcpy(buffer + 27U + 9U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 27U, 9U); + ::memcpy(buffer + 27U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, 9U); + ::memcpy(buffer + 27U + 27U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 63U, 9U); + ::memcpy(buffer + 27U + 36U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 81U, 9U); + length = 72U; break; case YSF_DT_DATA_FR_MODE: // Copy the data @@ -202,6 +216,12 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* return false; } + fich.getRaw(buffer + 3U); + CUtils::dump(1U, "IMRS Network Data Sent", buffer, length); + + readData(ptr, fich, buffer); + +#ifdef notdef 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); @@ -213,12 +233,129 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* m_socket.write(buffer, length, (*it)->m_addr, (*it)->m_addrLen); } +#endif ptr->m_seqNo++; return true; } +void CIMRSNetwork::readHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data) +{ + assert(ptr != NULL); + assert(data != NULL); + + unsigned char buffer[155U]; + + ::memcpy(buffer + 0U, "YSFD", 4U); + + unsigned char fi = fich.getFI(); + if (fi == YSF_FI_HEADER) { + ::memcpy(ptr->m_source, data + 17U + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH); + + unsigned char cm = fich.getCM(); + if (cm == YSF_CM_GROUP1 || cm == YSF_CM_GROUP2) + ::memcpy(ptr->m_dest, "ALL ", YSF_CALLSIGN_LENGTH); + else + ::memcpy(ptr->m_dest, data + 17U + 0U, YSF_CALLSIGN_LENGTH); + + buffer[34U] = 0x00U; + } else { + uint16_t seqNo = (buffer[1U] << 8) + (buffer[2U] << 0); + buffer[34U] = 0x01U | ((seqNo - 1U) & 0x7FU) << 1; + } + + ::memcpy(buffer + 4U, "IMRS ", YSF_CALLSIGN_LENGTH); + ::memcpy(buffer + 14U, ptr->m_source, YSF_CALLSIGN_LENGTH); + ::memcpy(buffer + 24U, ptr->m_dest, YSF_CALLSIGN_LENGTH); + + ::memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES); + + fich.encode(buffer + 35U); + + CYSFPayload payload; + payload.writeHeaderData(data + 7U, buffer + 35U); + + CUtils::dump("YSF Data Transmitted", buffer, 155U); + +#ifdef notdef + unsigned char len = 155U; + ptr->m_buffer.addData(&len, 1U); + ptr->m_buffer.addData(buffer, len); +#endif +} + +void CIMRSNetwork::readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data) +{ + assert(ptr != NULL); + assert(data != NULL); + + unsigned char buffer[155U]; + + ::memcpy(buffer + 0U, "YSFD", 4U); + + ::memcpy(buffer + 4U, "IMRS ", YSF_CALLSIGN_LENGTH); + ::memcpy(buffer + 14U, ptr->m_source, YSF_CALLSIGN_LENGTH); + ::memcpy(buffer + 24U, ptr->m_dest, YSF_CALLSIGN_LENGTH); + + uint16_t seqNo = (buffer[1U] << 8) + (buffer[2U] << 0); + buffer[34U] = ((seqNo - 1U) & 0x7FU) << 1; + + ::memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES); + + fich.encode(buffer + 35U); + + unsigned char dt = fich.getDT(); + + CYSFPayload payload; + + // Create the header + switch (dt) { + case YSF_DT_VD_MODE1: + // Copy the DCH + payload.writeVDMode1Data(data + 7U, buffer + 35U); + // Copy the audio + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 9U, data + 27U + 0U, 9U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 27U, data + 27U + 9U, 9U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, data + 27U + 18U, 9U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 63U, data + 27U + 27U, 9U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 81U, data + 27U + 36U, 9U); + break; + case YSF_DT_DATA_FR_MODE: + // Copy the data + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U, data + 17U + 0U, 90U); + break; + case YSF_DT_VD_MODE2: + // Copy the DCH + payload.writeVDMode2Data(data + 7U, buffer + 35U); + // Copy the audio + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 5U, data + 17U + 0U, 13U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 23U, data + 17U + 13U, 13U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 41U, data + 17U + 26U, 13U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 59U, data + 17U + 39U, 13U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 77U, data + 17U + 52U, 13U); + break; + case YSF_DT_VOICE_FR_MODE: + // Copy the audio + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U, data + 17U + 0U, 18U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 18U, data + 17U + 18U, 18U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 36U, data + 17U + 36U, 18U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, data + 17U + 54U, 18U); + ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, data + 17U + 72U, 18U); + break; + default: + return; + } + + CUtils::dump("YSF Data Transmitted", buffer, 155U); + +#ifdef notdef + unsigned char len = 155U; + ptr->m_buffer.addData(&len, 1U); + ptr->m_buffer.addData(buffer, len); +#endif +} + void CIMRSNetwork::link() { } @@ -249,9 +386,21 @@ void CIMRSNetwork::clock(unsigned int ms) if (ptr->m_debug) CUtils::dump(1U, "IMRS Network Data Received", buffer, length); - unsigned char len = length; - ptr->m_buffer.addData(&len, 1U); - ptr->m_buffer.addData(buffer, len); + CYSFFICH fich; + fich.setRaw(buffer + 3U); + + unsigned char fi = fich.getFI(); + switch (fi) { + case YSF_FI_HEADER: + case YSF_FI_TERMINATOR: + readHeaderTrailer(ptr, fich, buffer); + break; + case YSF_FI_COMMUNICATIONS: + readData(ptr, fich, buffer); + break; + default: + break; + } } unsigned int CIMRSNetwork::read(unsigned int dgId, unsigned char* data) @@ -303,4 +452,3 @@ IMRSDGId* CIMRSNetwork::find(unsigned int dgId) const return NULL; } - diff --git a/DGIdGateway/IMRSNetwork.h b/DGIdGateway/IMRSNetwork.h index 5591d59..7766c2a 100644 --- a/DGIdGateway/IMRSNetwork.h +++ b/DGIdGateway/IMRSNetwork.h @@ -23,6 +23,7 @@ #include "YSFDefines.h" #include "UDPSocket.h" #include "RingBuffer.h" +#include "YSFDefines.h" #include "YSFFICH.h" #include @@ -42,13 +43,26 @@ public: m_name(), m_seqNo(0U), m_destinations(), + m_source(NULL), + m_dest(NULL), m_debug(false), m_buffer(1000U, "IMRS Buffer") - {} + { + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + } + + ~IMRSDGId() + { + delete[] m_source; + delete[] m_dest; + } unsigned int m_dgId; std::string m_name; uint16_t m_seqNo; + unsigned char* m_source; + unsigned char* m_dest; std::vector m_destinations; bool m_debug; CRingBuffer m_buffer; @@ -91,6 +105,9 @@ private: bool writeHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data); bool writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data); + + void readHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data); + void readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char* data); }; #endif diff --git a/DGIdGateway/Version.h b/DGIdGateway/Version.h index 1006c28..28b79b8 100644 --- a/DGIdGateway/Version.h +++ b/DGIdGateway/Version.h @@ -19,6 +19,6 @@ #if !defined(VERSION_H) #define VERSION_H -const char* VERSION = "20200923"; +const char* VERSION = "20200924"; #endif diff --git a/DGIdGateway/YSFFICH.cpp b/DGIdGateway/YSFFICH.cpp index 8547e54..36ec305 100644 --- a/DGIdGateway/YSFFICH.cpp +++ b/DGIdGateway/YSFFICH.cpp @@ -171,12 +171,12 @@ void CYSFFICH::encode(unsigned char* bytes) void CYSFFICH::setRaw(const unsigned char* bytes) { - ::memcpy(m_fich, bytes, 6U); + ::memcpy(m_fich, bytes, 4U); } void CYSFFICH::getRaw(unsigned char* bytes) const { - ::memcpy(bytes, m_fich, 6U); + ::memcpy(bytes, m_fich, 4U); } unsigned char CYSFFICH::getFI() const diff --git a/DGIdGateway/YSFPayload.cpp b/DGIdGateway/YSFPayload.cpp index 16b4476..80bf01e 100644 --- a/DGIdGateway/YSFPayload.cpp +++ b/DGIdGateway/YSFPayload.cpp @@ -1,5 +1,5 @@ /* -* Copyright (C) 2016 Jonathan Naylor, G4KLX +* Copyright (C) 2016,2020 Jonathan Naylor, G4KLX * Copyright (C) 2016 Mathias Weyland, HB9FRV * * This program is free software; you can redistribute it and/or modify @@ -84,6 +84,74 @@ CYSFPayload::~CYSFPayload() { } +bool CYSFPayload::readHeaderData(const unsigned char* data, unsigned char* dt) +{ + assert(data != NULL); + assert(dt != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char dch[45U]; + + const unsigned char* p1 = data; + unsigned char* p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + CYSFConvolution conv; + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + unsigned char output[23U]; + conv.chainback(output, 176U); + + bool valid1 = CCRC::checkCCITT16(output, 22U); + if (valid1) { + for (unsigned int i = 0U; i < 20U; i++) + dt[i] = output[i] ^ WHITENING_DATA[i]; + } + + p1 = data + 9U; + p2 = dch; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p2, p1, 9U); + p1 += 18U; p2 += 9U; + } + + conv.start(); + + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + uint8_t s0 = READ_BIT1(dch, n) ? 1U : 0U; + + n++; + uint8_t s1 = READ_BIT1(dch, n) ? 1U : 0U; + + conv.decode(s0, s1); + } + + conv.chainback(output, 176U); + + bool valid2 = CCRC::checkCCITT16(output, 22U); + if (valid2) { + for (unsigned int i = 0U; i < 20U; i++) + dt[i + 20U] = output[i] ^ WHITENING_DATA[i]; + } + + return valid1 && valid2; +} + bool CYSFPayload::readVDMode1Data(const unsigned char* data, unsigned char* dt) { assert(data != NULL); @@ -119,11 +187,7 @@ bool CYSFPayload::readVDMode1Data(const unsigned char* data, unsigned char* dt) bool ret = CCRC::checkCCITT16(output, 22U); if (ret) { for (unsigned int i = 0U; i < 20U; i++) - output[i] ^= WHITENING_DATA[i]; - - CUtils::dump(1U, "V/D Mode 1 Data", output, 20U); - - ::memcpy(dt, output, 20U); + dt[i] = output[i] ^ WHITENING_DATA[i]; } return ret; @@ -164,11 +228,7 @@ bool CYSFPayload::readVDMode2Data(const unsigned char* data, unsigned char* dt) bool ret = CCRC::checkCCITT16(output, 12U); if (ret) { for (unsigned int i = 0U; i < 10U; i++) - output[i] ^= WHITENING_DATA[i]; - - CUtils::dump(1U, "V/D Mode 2 Data", output, YSF_CALLSIGN_LENGTH); - - ::memcpy(dt, output, YSF_CALLSIGN_LENGTH); + dt[i] = output[i] ^ WHITENING_DATA[i]; } return ret; @@ -211,11 +271,7 @@ bool CYSFPayload::readDataFRModeData1(const unsigned char* data, unsigned char* bool ret = CCRC::checkCCITT16(output, 22U); if (ret) { for (unsigned int i = 0U; i < 20U; i++) - output[i] ^= WHITENING_DATA[i]; - - CUtils::dump(1U, "FR Mode Data 1", output, 20U); - - ::memcpy(dt, output, 20U); + dt[i] = output[i] ^ WHITENING_DATA[i]; } return ret; @@ -258,14 +314,172 @@ bool CYSFPayload::readDataFRModeData2(const unsigned char* data, unsigned char* bool ret = CCRC::checkCCITT16(output, 22U); if (ret) { for (unsigned int i = 0U; i < 20U; i++) - output[i] ^= WHITENING_DATA[i]; + dt[i] = output[i] ^ WHITENING_DATA[i]; + } - CUtils::dump(1U, "FR Mode Data 2", output, 20U); + return ret; +} + +void CYSFPayload::writeHeaderData(const unsigned char* dt, unsigned char* data) +{ + assert(dt != NULL); + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char output[23U]; + for (unsigned int i = 0U; i < 20U; i++) + output[i] = dt[i] ^ WHITENING_DATA[i]; + + CCRC::addCCITT16(output, 22U); + output[22U] = 0x00U; - ::memcpy(dt, output, 20U); + unsigned char convolved[45U]; + + CYSFConvolution conv; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); } - return ret; + unsigned char* p1 = data + 0U; + unsigned char* p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + + for (unsigned int i = 0U; i < 20U; i++) + output[i] = dt[i + 20U] ^ WHITENING_DATA[i]; + + CCRC::addCCITT16(output, 22U); + output[22U] = 0x00U; + + conv.encode(output, convolved, 180U); + + for (unsigned int i = 0U, j = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + p1 = data + 9U; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } +} + +void CYSFPayload::writeVDMode1Data(const unsigned char* dt, unsigned char* data) +{ + assert(dt != NULL); + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char output[23U]; + for (unsigned int i = 0U; i < 20U; i++) + output[i] = dt[i] ^ WHITENING_DATA[i]; + + CCRC::addCCITT16(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + + CYSFConvolution conv; + conv.encode(output, convolved, 180U); + + unsigned char bytes[45U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 180U; i++) { + unsigned int n = INTERLEAVE_TABLE_9_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + unsigned char* p1 = data; + unsigned char* p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } +} + +void CYSFPayload::writeVDMode2Data(const unsigned char* dt, unsigned char* data) +{ + assert(dt != NULL); + assert(data != NULL); + + data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES; + + unsigned char output[13U]; + for (unsigned int i = 0U; i < 10U; i++) + output[i] = dt[i] ^ WHITENING_DATA[i]; + + CCRC::addCCITT16(output, 12U); + output[12U] = 0x00U; + + unsigned char convolved[25U]; + + CYSFConvolution conv; + conv.encode(output, convolved, 100U); + + unsigned char bytes[25U]; + unsigned int j = 0U; + for (unsigned int i = 0U; i < 100U; i++) { + unsigned int n = INTERLEAVE_TABLE_5_20[i]; + + bool s0 = READ_BIT1(convolved, j) != 0U; + j++; + + bool s1 = READ_BIT1(convolved, j) != 0U; + j++; + + WRITE_BIT1(bytes, n, s0); + + n++; + WRITE_BIT1(bytes, n, s1); + } + + unsigned char* p1 = data; + unsigned char* p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 5U); + p1 += 18U; p2 += 5U; + } } void CYSFPayload::writeDataFRModeData1(const unsigned char* dt, unsigned char* data) diff --git a/DGIdGateway/YSFPayload.h b/DGIdGateway/YSFPayload.h index 96d4cf3..a6b8c37 100644 --- a/DGIdGateway/YSFPayload.h +++ b/DGIdGateway/YSFPayload.h @@ -26,14 +26,19 @@ public: CYSFPayload(); ~CYSFPayload(); + bool readHeaderData(const unsigned char* data, unsigned char* dt); + void writeHeaderData(const unsigned char* dt, unsigned char* data); + bool readVDMode1Data(const unsigned char* data, unsigned char* dt); + void writeVDMode1Data(const unsigned char* dt, unsigned char* data); bool readVDMode2Data(const unsigned char* data, unsigned char* dt); + void writeVDMode2Data(const unsigned char* dt, unsigned char* data); bool readDataFRModeData1(const unsigned char* data, unsigned char* dt); - bool readDataFRModeData2(const unsigned char* data, unsigned char* dt); - void writeDataFRModeData1(const unsigned char* dt, unsigned char* data); + + bool readDataFRModeData2(const unsigned char* data, unsigned char* dt); void writeDataFRModeData2(const unsigned char* dt, unsigned char* data); private: