1
0
Fork 0

Handle VW mode DCH and the YSF sequence number.

ycs232-kbc
Jonathan Naylor 4 years ago
parent 5ac1bbe599
commit 7b65cfddff

@ -171,6 +171,8 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
buffer[2U] = (ptr->m_seqNo << 0) & 0xFFU; buffer[2U] = (ptr->m_seqNo << 0) & 0xFFU;
unsigned char dt = fich.getDT(); unsigned char dt = fich.getDT();
unsigned char ft = fich.getFT();
unsigned char fn = fich.getFN();
CYSFPayload payload; CYSFPayload payload;
@ -204,6 +206,14 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
length = 82U; length = 82U;
break; break;
case YSF_DT_VOICE_FR_MODE: case YSF_DT_VOICE_FR_MODE:
if (fn == 0U && ft == 1U) {
// Copy the DCH (20 bytes)
payload.readVoiceFRModeData(data + 35U, buffer + 7U);
// Copy the audio
::memcpy(buffer + 27U + 0U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, 18U);
::memcpy(buffer + 27U + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, 18U);
length = 107U;
} else {
// Copy the audio // Copy the audio
::memcpy(buffer + 17U + 0U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 0U, 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 + 18U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 18U, 18U);
@ -211,6 +221,7 @@ bool CIMRSNetwork::writeData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
::memcpy(buffer + 17U + 54U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, 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); ::memcpy(buffer + 17U + 72U, data + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, 18U);
length = 107U; length = 107U;
}
break; break;
default: default:
return false; return false;
@ -261,8 +272,8 @@ void CIMRSNetwork::readHeaderTrailer(IMRSDGId* ptr, CYSFFICH& fich, const unsign
buffer[34U] = 0x00U; buffer[34U] = 0x00U;
} else { } else {
uint16_t seqNo = (buffer[1U] << 8) + (buffer[2U] << 0); uint16_t seqNo = (data[1U] << 8) + (data[2U] << 0);
buffer[34U] = 0x01U | ((seqNo - 1U) & 0x7FU) << 1; buffer[34U] = 0x01U | ((seqNo & 0x7FU) << 1);
} }
::memcpy(buffer + 4U, "IMRS ", YSF_CALLSIGN_LENGTH); ::memcpy(buffer + 4U, "IMRS ", YSF_CALLSIGN_LENGTH);
@ -298,14 +309,16 @@ void CIMRSNetwork::readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
::memcpy(buffer + 14U, ptr->m_source, YSF_CALLSIGN_LENGTH); ::memcpy(buffer + 14U, ptr->m_source, YSF_CALLSIGN_LENGTH);
::memcpy(buffer + 24U, ptr->m_dest, YSF_CALLSIGN_LENGTH); ::memcpy(buffer + 24U, ptr->m_dest, YSF_CALLSIGN_LENGTH);
uint16_t seqNo = (buffer[1U] << 8) + (buffer[2U] << 0); uint16_t seqNo = (data[1U] << 8) + (data[2U] << 0);
buffer[34U] = ((seqNo - 1U) & 0x7FU) << 1; buffer[34U] = (seqNo & 0x7FU) << 1;
::memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES); ::memcpy(buffer + 35U, YSF_SYNC_BYTES, YSF_SYNC_LENGTH_BYTES);
fich.encode(buffer + 35U); fich.encode(buffer + 35U);
unsigned char dt = fich.getDT(); unsigned char dt = fich.getDT();
unsigned char fn = fich.getFN();
unsigned char ft = fich.getFT();
CYSFPayload payload; CYSFPayload payload;
@ -336,12 +349,22 @@ void CIMRSNetwork::readData(IMRSDGId* ptr, CYSFFICH& fich, const unsigned char*
::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 77U, data + 17U + 52U, 13U); ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 77U, data + 17U + 52U, 13U);
break; break;
case YSF_DT_VOICE_FR_MODE: case YSF_DT_VOICE_FR_MODE:
if (fn == 0U && ft == 1U) {
// Copy the DCH
payload.writeVoiceFRModeData(data + 7U, buffer + 35U);
// NULL the unused section
::memset(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 45U, 0x00U, 9U);
// Copy the audio
::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 54U, data + 27U + 0U, 18U);
::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, data + 27U + 18U, 18U);
} else {
// Copy the audio // 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 + 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 + 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 + 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 + 54U, data + 17U + 54U, 18U);
::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, data + 17U + 72U, 18U); ::memcpy(buffer + 35U + YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES + 72U, data + 17U + 72U, 18U);
}
break; break;
default: default:
return; return;

@ -277,6 +277,41 @@ bool CYSFPayload::readDataFRModeData1(const unsigned char* data, unsigned char*
return ret; return ret;
} }
bool CYSFPayload::readVoiceFRModeData(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];
::memcpy(dch, data, 45U);
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 ret = CCRC::checkCCITT16(output, 22U);
if (ret) {
for (unsigned int i = 0U; i < 20U; i++)
dt[i] = output[i] ^ WHITENING_DATA[i];
}
return ret;
}
bool CYSFPayload::readDataFRModeData2(const unsigned char* data, unsigned char* dt) bool CYSFPayload::readDataFRModeData2(const unsigned char* data, unsigned char* dt)
{ {
assert(data != NULL); assert(data != NULL);
@ -482,6 +517,45 @@ void CYSFPayload::writeVDMode2Data(const unsigned char* dt, unsigned char* data)
} }
} }
void CYSFPayload::writeVoiceFRModeData(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);
}
::memcpy(data, bytes, 45U);
}
void CYSFPayload::writeDataFRModeData1(const unsigned char* dt, unsigned char* data) void CYSFPayload::writeDataFRModeData1(const unsigned char* dt, unsigned char* data)
{ {
assert(dt != NULL); assert(dt != NULL);

@ -35,6 +35,9 @@ public:
bool readVDMode2Data(const unsigned char* data, unsigned char* dt); bool readVDMode2Data(const unsigned char* data, unsigned char* dt);
void writeVDMode2Data(const unsigned char* dt, unsigned char* data); void writeVDMode2Data(const unsigned char* dt, unsigned char* data);
bool readVoiceFRModeData(const unsigned char* data, unsigned char* dt);
void writeVoiceFRModeData(const unsigned char* dt, unsigned char* data);
bool readDataFRModeData1(const unsigned char* data, unsigned char* dt); bool readDataFRModeData1(const unsigned char* data, unsigned char* dt);
void writeDataFRModeData1(const unsigned char* dt, unsigned char* data); void writeDataFRModeData1(const unsigned char* dt, unsigned char* data);

Loading…
Cancel
Save