From 4b323a9d202bc34fbd91ca28c1c1d2ecfc6e809a Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Mon, 23 May 2016 21:45:52 +0100 Subject: [PATCH] Decode FR mode data, and generate cleanups and bug fixing. --- YSFControl.cpp | 87 ++++++++++++++---- YSFControl.h | 3 +- YSFPayload.cpp | 239 +++++++++++++++++++++++++++---------------------- 3 files changed, 201 insertions(+), 128 deletions(-) diff --git a/YSFControl.cpp b/YSFControl.cpp index 924ae81..c9246cc 100644 --- a/YSFControl.cpp +++ b/YSFControl.cpp @@ -43,13 +43,16 @@ m_rfSource(NULL), m_rfDest(NULL), m_netSource(NULL), m_netDest(NULL), -m_payload(), +m_rfPayload(), +m_netPayload(), m_fp(NULL) { assert(display != NULL); - m_payload.setUplink(callsign); - m_payload.setDownlink(callsign); + m_rfPayload.setUplink(callsign); + m_rfPayload.setDownlink(callsign); + + m_netPayload.setDownlink(callsign); m_netSource = new unsigned char[YSF_CALLSIGN_LENGTH]; m_netDest = new unsigned char[YSF_CALLSIGN_LENGTH]; @@ -90,7 +93,7 @@ bool CYSFControl::writeModem(unsigned char *data) m_rfErrs = 0U; m_rfBits = 1U; m_rfTimeoutTimer.start(); - m_payload.reset(); + m_rfPayload.reset(); m_rfState = RS_RF_AUDIO; #if defined(DUMP_YSF) openFile(); @@ -104,19 +107,21 @@ bool CYSFControl::writeModem(unsigned char *data) if (valid && fi == YSF_FI_HEADER) { CSync::addYSFSync(data + 2U); + // LogDebug("YSF, FICH: FI: %u, DT: %u, FN: %u, FT: %u", fich.getFI(), fich.getDT(), fich.getFN(), fich.getFT()); + m_rfFrames++; - valid = m_payload.processHeaderData(data + 2U); + valid = m_rfPayload.processHeaderData(data + 2U); if (valid) - m_rfSource = m_payload.getSource(); + m_rfSource = m_rfPayload.getSource(); unsigned char cm = fich.getCM(); if (cm == YSF_CM_GROUP) { m_rfDest = (unsigned char*)"ALL "; } else { if (valid) - m_rfDest = m_payload.getDest(); + m_rfDest = m_rfPayload.getDest(); } if (m_rfSource != NULL && m_rfDest != NULL) { @@ -150,9 +155,11 @@ bool CYSFControl::writeModem(unsigned char *data) } else if (valid && fi == YSF_FI_TERMINATOR) { CSync::addYSFSync(data + 2U); + // LogDebug("YSF, FICH: FI: %u, DT: %u, FN: %u, FT: %u", fich.getFI(), fich.getDT(), fich.getFN(), fich.getFT()); + m_rfFrames++; - m_payload.processHeaderData(data + 2U); + m_rfPayload.processHeaderData(data + 2U); data[0U] = TAG_EOT; data[1U] = 0x00U; @@ -180,29 +187,31 @@ bool CYSFControl::writeModem(unsigned char *data) unsigned char ft = fich.getFT(); unsigned char dt = fich.getDT(); + // LogDebug("YSF, FICH: FI: %u, DT: %u, FN: %u, FT: %u", fich.getFI(), fich.getDT(), fich.getFN(), fich.getFT()); + m_rfFrames++; switch (dt) { case YSF_DT_VD_MODE1: - valid = m_payload.processVDMode1Data(data + 2U, fn); - m_rfErrs += m_payload.processVDMode1Audio(data + 2U); + valid = m_rfPayload.processVDMode1Data(data + 2U, fn); + m_rfErrs += m_rfPayload.processVDMode1Audio(data + 2U); m_rfBits += 235U; break; case YSF_DT_VD_MODE2: - valid = m_payload.processVDMode2Data(data + 2U, fn); - m_rfErrs += m_payload.processVDMode2Audio(data + 2U); + valid = m_rfPayload.processVDMode2Data(data + 2U, fn); + m_rfErrs += m_rfPayload.processVDMode2Audio(data + 2U); m_rfBits += 135U; break; case YSF_DT_DATA_FR_MODE: - valid = m_payload.processDataFRModeData(data + 2U, fn); + valid = m_rfPayload.processDataFRModeData(data + 2U, fn); break; case YSF_DT_VOICE_FR_MODE: if (fn != 0U || ft != 1U) { // The first packet after the header is odd, don't try and regenerate it - m_rfErrs += m_payload.processVoiceFRModeAudio(data + 2U); + m_rfErrs += m_rfPayload.processVoiceFRModeAudio(data + 2U); m_rfBits += 720U; } valid = false; @@ -220,14 +229,14 @@ bool CYSFControl::writeModem(unsigned char *data) m_rfDest = (unsigned char*)"ALL "; change = true; } else if (valid) { - m_rfDest = m_payload.getDest(); + m_rfDest = m_rfPayload.getDest(); if (m_rfDest != NULL) change = true; } } if (valid && m_rfSource == NULL) { - m_rfSource = m_payload.getSource(); + m_rfSource = m_rfPayload.getSource(); if (m_rfSource != NULL) change = true; } @@ -306,7 +315,7 @@ void CYSFControl::writeEndRF() m_rfState = RS_RF_LISTENING; m_rfTimeoutTimer.stop(); - m_payload.reset(); + m_rfPayload.reset(); // These variables are free'd by YSFPayload m_rfSource = NULL; @@ -331,6 +340,8 @@ void CYSFControl::writeEndNet() m_netTimeoutTimer.stop(); m_networkWatchdog.stop(); + m_netPayload.reset(); + m_display->clearFusion(); if (m_network != NULL) @@ -364,6 +375,7 @@ void CYSFControl::writeNetwork() LogMessage("YSF, received network data from %10.10s to %10.10s at %10.10s", m_netSource, m_netDest, data + 4U); m_netTimeoutTimer.start(); + m_netPayload.reset(); m_netState = RS_NET_AUDIO; m_netFrames = 0U; } else { @@ -395,10 +407,47 @@ void CYSFControl::writeNetwork() CYSFFICH fich; bool valid = fich.decode(data + 35U); if (valid) { - // XXX Should set the downlink callsign + unsigned char dt = fich.getDT(); + unsigned char fn = fich.getFN(); + unsigned char ft = fich.getFT(); + unsigned char fi = fich.getFI(); + + // Set the downlink callsign + switch (fi) { + case YSF_FI_HEADER: + case YSF_FI_TERMINATOR: + m_netPayload.processHeaderData(data + 35U); + break; + + case YSF_FI_COMMUNICATIONS: + switch (dt) { + case YSF_DT_VD_MODE1: + m_netPayload.processVDMode1Data(data + 35U, fn); + break; + + case YSF_DT_VD_MODE2: + m_netPayload.processVDMode2Data(data + 35U, fn); + break; + + case YSF_DT_DATA_FR_MODE: + m_netPayload.processDataFRModeData(data + 35U, fn); + break; + + case YSF_DT_VOICE_FR_MODE: + break; + + default: + break; + } + break; + + default: + break; + } + fich.setVoIP(true); fich.setMR(YSF_MR_NOT_BUSY); - fich.encode(data + 2U); + fich.encode(data + 35U); } writeQueueNet(data + 33U); diff --git a/YSFControl.h b/YSFControl.h index 0ac2a3d..edcb811 100644 --- a/YSFControl.h +++ b/YSFControl.h @@ -60,7 +60,8 @@ private: unsigned char* m_rfDest; unsigned char* m_netSource; unsigned char* m_netDest; - CYSFPayload m_payload; + CYSFPayload m_rfPayload; + CYSFPayload m_netPayload; FILE* m_fp; void writeQueueRF(const unsigned char* data); diff --git a/YSFPayload.cpp b/YSFPayload.cpp index 2b0ceaf..20a8232 100644 --- a/YSFPayload.cpp +++ b/YSFPayload.cpp @@ -131,19 +131,19 @@ bool CYSFPayload::processHeaderData(unsigned char* data) unsigned char output[23U]; conv.chainback(output, 176U); - bool valid = CCRC::checkCCITT162(output, 22U); - if (valid) { + bool valid1 = CCRC::checkCCITT162(output, 22U); + if (valid1) { for (unsigned int i = 0U; i < 20U; i++) output[i] ^= WHITENING_DATA[i]; if (m_dest == NULL) { - m_dest = new unsigned char[10U]; - ::memcpy(m_dest, output + 0U, 10U); + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output + 0U, YSF_CALLSIGN_LENGTH); } if (m_source == NULL) { - m_source = new unsigned char[10U]; - ::memcpy(m_source, output + 10U, 10U); + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH); } for (unsigned int i = 0U; i < 20U; i++) @@ -180,45 +180,73 @@ bool CYSFPayload::processHeaderData(unsigned char* data) } } - ::memset(output, ' ', 20U); - if (m_downlink != NULL) - ::memcpy(output + 0U, m_downlink, 10U); - if (m_uplink != NULL) - ::memcpy(output + 10U, m_uplink, 10U); - for (unsigned int i = 0U; i < 20U; i++) - output[i] ^= WHITENING_DATA[i]; - - CCRC::addCCITT162(output, 22U); - output[22U] = 0x00U; - - unsigned char convolved[45U]; - 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); - } - p1 = data + 9U; - p2 = bytes; + p2 = dch; for (unsigned int i = 0U; i < 5U; i++) { - ::memcpy(p1, p2, 9U); + ::memcpy(p2, p1, 9U); p1 += 18U; p2 += 9U; } - return valid; + 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::checkCCITT162(output, 22U); + if (valid2) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + if (m_downlink != NULL) + ::memcpy(output + 0U, m_downlink, YSF_CALLSIGN_LENGTH); + + if (m_uplink != NULL) + ::memcpy(output + YSF_CALLSIGN_LENGTH, m_uplink, YSF_CALLSIGN_LENGTH); + + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + CCRC::addCCITT162(output, 22U); + output[22U] = 0x00U; + + unsigned char convolved[45U]; + 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); + } + + p1 = data + 9U; + p2 = bytes; + for (unsigned int i = 0U; i < 5U; i++) { + ::memcpy(p1, p2, 9U); + p1 += 18U; p2 += 9U; + } + } + + return valid1; } unsigned int CYSFPayload::processVDMode1Audio(unsigned char* data) @@ -279,36 +307,30 @@ bool CYSFPayload::processVDMode1Data(unsigned char* data, unsigned char fn) switch (fn) { case 0U: if (m_dest == NULL) { - m_dest = new unsigned char[10U]; - ::memcpy(m_dest, output + 0U, 10U); + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output + 0U, YSF_CALLSIGN_LENGTH); } if (m_source == NULL) { - m_source = new unsigned char[10U]; - ::memcpy(m_source, output + 10U, 10U); + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH); } break; + case 1U: + if (m_downlink != NULL) + ::memcpy(output + 0U, m_downlink, YSF_CALLSIGN_LENGTH); + + if (m_uplink != NULL) + ::memcpy(output + YSF_CALLSIGN_LENGTH, m_uplink, YSF_CALLSIGN_LENGTH); + + break; + default: break; } - } - if (fn == 1U) { - ::memset(output, ' ', 20U); - - if (m_downlink != NULL) - ::memcpy(output + 0U, m_downlink, 10U); - - if (m_uplink != NULL) - ::memcpy(output + 10U, m_uplink, 10U); - - ret = true; - } - - // Data isn't corrupt so regenerate it - if (!ret) { for (unsigned int i = 0U; i < 20U; i++) output[i] ^= WHITENING_DATA[i]; @@ -441,46 +463,39 @@ bool CYSFPayload::processVDMode2Data(unsigned char* data, unsigned char fn) bool ret = CCRC::checkCCITT162(output, 12U); if (ret) { - for (unsigned int i = 0U; i < 10U; i++) + for (unsigned int i = 0U; i < 20U; i++) output[i] ^= WHITENING_DATA[i]; switch (fn) { case 0U: if (m_dest == NULL) { - m_dest = new unsigned char[10U]; - ::memcpy(m_dest, output, 10U); + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output, YSF_CALLSIGN_LENGTH); } break; + case 1U: if (m_source == NULL) { - m_source = new unsigned char[10U]; - ::memcpy(m_source, output, 10U); + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output, YSF_CALLSIGN_LENGTH); } break; + + case 2U: + if (m_downlink != NULL) + ::memcpy(output, m_downlink, YSF_CALLSIGN_LENGTH); + break; + + case 3U: + if (m_uplink != NULL) + ::memcpy(output, m_uplink, YSF_CALLSIGN_LENGTH); + break; + default: break; } - } - if (fn == 2U) { - if (m_downlink != NULL) - ::memcpy(output, m_downlink, 10U); - else - ::memset(output, ' ', 10U); - ret = true; - } - - if (fn == 3U) { - if (m_uplink != NULL) - ::memcpy(output, m_uplink, 10U); - else - ::memset(output, ' ', 10U); - ret = true; - } - - // Data isn't corrupt so regenerate it - if (ret) { - for (unsigned int i = 0U; i < 10U; i++) + for (unsigned int i = 0U; i < 20U; i++) output[i] ^= WHITENING_DATA[i]; CCRC::addCCITT162(output, 12U); @@ -556,13 +571,13 @@ bool CYSFPayload::processDataFRModeData(unsigned char* data, unsigned char fn) switch (fn) { case 0U: if (m_dest == NULL) { - m_dest = new unsigned char[10U]; - ::memcpy(m_dest, output + 0U, 10U); + m_dest = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_dest, output + 0U, YSF_CALLSIGN_LENGTH); } if (m_source == NULL) { - m_source = new unsigned char[10U]; - ::memcpy(m_source, output + 10U, 10U); + m_source = new unsigned char[YSF_CALLSIGN_LENGTH]; + ::memcpy(m_source, output + YSF_CALLSIGN_LENGTH, YSF_CALLSIGN_LENGTH); } break; @@ -627,21 +642,29 @@ bool CYSFPayload::processDataFRModeData(unsigned char* data, unsigned char fn) conv.chainback(output, 176U); bool ret2 = CCRC::checkCCITT162(output, 22U); - - if (fn == 0U) { - ::memset(output, ' ', 20U); - - if (m_downlink != NULL) - ::memcpy(output + 0U, m_downlink, 10U); - - if (m_uplink != NULL) - ::memcpy(output + 10U, m_uplink, 10U); - - ret2 = true; - } - - // Data isn't corrupt so regenerate it if (ret2) { + for (unsigned int i = 0U; i < 20U; i++) + output[i] ^= WHITENING_DATA[i]; + + switch (fn) { + case 0U: + if (m_downlink != NULL) + ::memcpy(output + 0U, m_downlink, YSF_CALLSIGN_LENGTH); + + if (m_uplink != NULL) + ::memcpy(output + YSF_CALLSIGN_LENGTH, m_uplink, YSF_CALLSIGN_LENGTH); + + break; + + case 1U: + CUtils::dump(1U, "FR Mode Data", output + 10U, 10U); + break; + + default: + CUtils::dump(1U, "FR Mode Data", output + 0U, 20U); + break; + } + for (unsigned int i = 0U; i < 20U; i++) output[i] ^= WHITENING_DATA[i]; @@ -700,23 +723,23 @@ unsigned int CYSFPayload::processVoiceFRModeAudio(unsigned char* data) void CYSFPayload::setUplink(const std::string& callsign) { - m_uplink = new unsigned char[10U]; + m_uplink = new unsigned char[YSF_CALLSIGN_LENGTH]; std::string uplink = callsign; - uplink.resize(10U, ' '); + uplink.resize(YSF_CALLSIGN_LENGTH, ' '); - for (unsigned int i = 0U; i < 10U; i++) + for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++) m_uplink[i] = uplink.at(i); } void CYSFPayload::setDownlink(const std::string& callsign) { - m_downlink = new unsigned char[10U]; + m_downlink = new unsigned char[YSF_CALLSIGN_LENGTH]; std::string downlink = callsign; - downlink.resize(10U, ' '); + downlink.resize(YSF_CALLSIGN_LENGTH, ' '); - for (unsigned int i = 0U; i < 10U; i++) + for (unsigned int i = 0U; i < YSF_CALLSIGN_LENGTH; i++) m_downlink[i] = downlink.at(i); }