Decode and encode the System Fusion FICH.

This commit is contained in:
Jonathan Naylor 2016-02-23 19:30:35 +00:00
parent 6792894c00
commit dc819b8821
10 changed files with 219 additions and 74 deletions

20
CRC.cpp
View File

@ -238,23 +238,3 @@ unsigned char CCRC::crc8(const unsigned char *in, unsigned int length)
return crc;
}
bool CCRC::crcFICH(const unsigned char* fich)
{
assert(fich != NULL);
union {
uint16_t crc16;
uint8_t crc8[2U];
};
crc16 = 0U;
crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[0U]];
crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[1U]];
crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[2U]];
crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[3U]];
crc16 = ~crc16;
return crc8[0U] == fich[5U] && crc8[1U] == fich[4U];
}

2
CRC.h
View File

@ -32,8 +32,6 @@ public:
static bool checkCCITT162(const unsigned char* in, unsigned int length);
static unsigned char crc8(const unsigned char* in, unsigned int length);
static bool crcFICH(const unsigned char* fich);
};
#endif

View File

@ -12,6 +12,7 @@
*/
#include "YSFControl.h"
#include "YSFFICH.h"
#include "Utils.h"
#include "Sync.h"
#include "Log.h"
@ -24,7 +25,6 @@
/*
* TODO:
* AMBE FEC reconstruction.
* FICH regeneration.
* Callsign extraction + late entry.
* Uplink and downlink callsign addition.
*/
@ -56,6 +56,10 @@ bool CYSFControl::writeModem(unsigned char *data)
if (type == TAG_LOST && m_state == RS_RELAYING_RF_AUDIO) {
LogMessage("YSF, transmission lost, %.1f seconds", float(m_frames) / 10.0F);
if (m_parrot != NULL)
m_parrot->end();
writeEndOfTransmission();
return false;
}
@ -84,15 +88,24 @@ bool CYSFControl::writeModem(unsigned char *data)
if (type == TAG_EOT) {
CSync::addYSFSync(data + 2U);
CYSFFICH fich;
fich.decode(data + 2U);
m_frames++;
if (m_duplex) {
fich.setMR(YSF_MR_BUSY);
fich.encode(data + 2U);
data[0U] = TAG_EOT;
data[1U] = 0x00U;
writeQueue(data);
}
if (m_parrot != NULL) {
fich.setMR(YSF_MR_NOT_BUSY);
fich.encode(data + 2U);
data[0U] = TAG_EOT;
data[1U] = 0x00U;
writeParrot(data);
@ -109,15 +122,24 @@ bool CYSFControl::writeModem(unsigned char *data)
} else {
CSync::addYSFSync(data + 2U);
CYSFFICH fich;
fich.decode(data + 2U);
m_frames++;
if (m_duplex) {
fich.setMR(YSF_MR_BUSY);
fich.encode(data + 2U);
data[0U] = TAG_DATA;
data[1U] = 0x00U;
writeQueue(data);
}
if (m_parrot != NULL) {
fich.setMR(YSF_MR_NOT_BUSY);
fich.encode(data + 2U);
data[0U] = TAG_DATA;
data[1U] = 0x00U;
writeParrot(data);
@ -196,6 +218,9 @@ void CYSFControl::writeParrot(const unsigned char *data)
return;
m_parrot->write(data);
if (data[0U] == TAG_EOT)
m_parrot->end();
}
bool CYSFControl::openFile()

View File

@ -29,8 +29,8 @@ const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U
#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
#define READ_BIT1(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
const unsigned char BRANCH_TABLE1[] = {0U, 0U, 0U, 0U, 1U, 1U, 1U, 1U};
const unsigned char BRANCH_TABLE2[] = {0U, 1U, 1U, 0U, 0U, 1U, 1U, 0U};
const uint8_t BRANCH_TABLE1[] = {0U, 0U, 0U, 0U, 1U, 1U, 1U, 1U};
const uint8_t BRANCH_TABLE2[] = {0U, 1U, 1U, 0U, 0U, 1U, 1U, 0U};
CYSFConvolution::CYSFConvolution() :
m_metrics1(NULL),
@ -40,9 +40,9 @@ m_newMetrics(NULL),
m_decisions(NULL),
m_dp(NULL)
{
m_metrics1 = new unsigned short[16U];
m_metrics2 = new unsigned short[16U];
m_decisions = new unsigned long long[100U];
m_metrics1 = new uint16_t[16U];
m_metrics2 = new uint16_t[16U];
m_decisions = new uint64_t[100U];
}
CYSFConvolution::~CYSFConvolution()
@ -54,44 +54,44 @@ CYSFConvolution::~CYSFConvolution()
const unsigned int NUM_OF_STATES_D2 = 8U;
const unsigned int NUM_OF_STATES = 16U;
const unsigned int M = 3U;
const uint32_t M = 3U;
const unsigned int K = 5U;
void CYSFConvolution::start()
{
::memset(m_metrics1, 0x00U, NUM_OF_STATES * sizeof(unsigned short));
::memset(m_metrics2, 0x00U, NUM_OF_STATES * sizeof(unsigned short));
::memset(m_metrics1, 0x00U, NUM_OF_STATES * sizeof(uint16_t));
::memset(m_metrics2, 0x00U, NUM_OF_STATES * sizeof(uint16_t));
m_oldMetrics = m_metrics1;
m_newMetrics = m_metrics2;
m_dp = m_decisions;
}
void CYSFConvolution::decode(unsigned char s0, unsigned char s1)
void CYSFConvolution::decode(uint8_t s0, uint8_t s1)
{
*m_dp = 0U;
for (unsigned int i = 0U; i < NUM_OF_STATES_D2; i++) {
unsigned int j = i * 2U;
for (uint8_t i = 0U; i < NUM_OF_STATES_D2; i++) {
uint8_t j = i * 2U;
unsigned short metric = (BRANCH_TABLE1[i] ^ s0) + (BRANCH_TABLE2[i] ^ s1);
uint16_t metric = (BRANCH_TABLE1[i] ^ s0) + (BRANCH_TABLE2[i] ^ s1);
unsigned short m0 = m_oldMetrics[i] + metric;
unsigned short m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + (M - metric);
unsigned char decision0 = (m0 >= m1) ? 1U : 0U;
uint16_t m0 = m_oldMetrics[i] + metric;
uint16_t m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + (M - metric);
uint8_t decision0 = (m0 >= m1) ? 1U : 0U;
m_newMetrics[j + 0U] = decision0 != 0U ? m1 : m0;
m0 = m_oldMetrics[i] + (M - metric);
m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + metric;
unsigned char decision1 = (m0 >= m1) ? 1U : 0U;
uint8_t decision1 = (m0 >= m1) ? 1U : 0U;
m_newMetrics[j + 1U] = decision1 != 0U ? m1 : m0;
*m_dp |= ((unsigned long long)(decision1) << (j + 1U)) | ((unsigned long long)(decision0) << (j + 0U));
*m_dp |= (uint64_t(decision1) << (j + 1U)) | (uint64_t(decision0) << (j + 0U));
}
++m_dp;
unsigned short* tmp = m_oldMetrics;
uint16_t* tmp = m_oldMetrics;
m_oldMetrics = m_newMetrics;
m_newMetrics = tmp;
}
@ -100,16 +100,43 @@ void CYSFConvolution::chainback(unsigned char* out)
{
assert(out != NULL);
unsigned int state = 0U;
uint32_t state = 0U;
unsigned char nbits = 96U;
uint8_t nbits = 96U;
while (nbits-- > 0) {
--m_dp;
unsigned int i = state >> (9 - K);
unsigned char bit = (unsigned char)(*m_dp >> i) & 1;
uint32_t i = state >> (9 - K);
uint8_t bit = uint8_t(*m_dp >> i) & 1;
state = (bit << 7) | (state >> 1);
WRITE_BIT1(out, nbits, bit != 0U);
}
}
void CYSFConvolution::encode(const unsigned char* in, unsigned char* out, unsigned int nBits) const
{
assert(in != NULL);
assert(out != NULL);
assert(nBits > 0U);
uint8_t d1 = 0U, d2 = 0U, d3 = 0U, d4 = 0U;
uint32_t k = 0U;
for (unsigned int i = 0U; i < nBits; i++) {
uint8_t d = READ_BIT1(in, i) ? 1U : 0U;
uint8_t g1 = (d + d3 + d4) & 1;
uint8_t g2 = (d + d1 + d2 + d4) & 1;
d4 = d3;
d3 = d2;
d2 = d1;
d1 = d;
WRITE_BIT1(out, k, g1 != 0U);
k++;
WRITE_BIT1(out, k, g2 != 0U);
k++;
}
}

View File

@ -21,22 +21,26 @@
#include "YSFConvolution.h"
#include <cstdint>
class CYSFConvolution {
public:
CYSFConvolution();
~CYSFConvolution();
void start();
void decode(unsigned char s0, unsigned char s1);
void decode(uint8_t s0, uint8_t s1);
void chainback(unsigned char* out);
void encode(const unsigned char* in, unsigned char* out, unsigned int nBits) const;
private:
unsigned short* m_metrics1;
unsigned short* m_metrics2;
unsigned short* m_oldMetrics;
unsigned short* m_newMetrics;
unsigned long long* m_decisions;
unsigned long long* m_dp;
uint16_t* m_metrics1;
uint16_t* m_metrics2;
uint16_t* m_oldMetrics;
uint16_t* m_newMetrics;
uint64_t* m_decisions;
uint64_t* m_dp;
};
#endif

View File

@ -32,4 +32,10 @@ const unsigned char YSF_DT_TERMINATOR_CHANNEL = 0x80U;
const unsigned char YSF_CKSUM_OK = 0x01U;
const unsigned char YSF_CM_GROUP = 0x00U;
const unsigned char YSF_CM_INDIVIDUAL = 0x0CU;
const unsigned char YSF_MR_NOT_BUSY = 0x08U;
const unsigned char YSF_MR_BUSY = 0x10U;
#endif

View File

@ -17,9 +17,11 @@
*/
#include "YSFConvolution.h"
#include "YSFDefines.h"
#include "Golay24128.h"
#include "YSFFICH.h"
#include "CRC.h"
#include "Log.h"
#include <cstdio>
#include <cassert>
@ -30,7 +32,7 @@ const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U
#define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
#define READ_BIT1(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
const unsigned int INTERLEAVE_TABLE_RX[] = {
const unsigned int INTERLEAVE_TABLE[] = {
0U, 40U, 80U, 120U, 160U,
2U, 42U, 82U, 122U, 162U,
4U, 44U, 84U, 124U, 164U,
@ -52,32 +54,37 @@ const unsigned int INTERLEAVE_TABLE_RX[] = {
36U, 76U, 116U, 156U, 196U,
38U, 78U, 118U, 158U, 198U};
CYSFFICH::CYSFFICH()
CYSFFICH::CYSFFICH() :
m_fich(NULL)
{
m_fich = new unsigned char[6U];
}
CYSFFICH::~CYSFFICH()
{
delete[] m_fich;
}
bool CYSFFICH::decode(const unsigned char* data, unsigned char* fich) const
bool CYSFFICH::decode(const unsigned char* bytes)
{
assert(data != NULL);
assert(fich != NULL);
assert(bytes != NULL);
// Skip the sync bytes
bytes += YSF_SYNC_LENGTH_BYTES;
CYSFConvolution viterbi;
viterbi.start();
// Deinterleave the FICH and send bits to the Viterbi decoder
for (unsigned int i = 0U; i < 100U; i++) {
unsigned int n = INTERLEAVE_TABLE_RX[i];
unsigned int s0 = READ_BIT1(data, n) ? 1U : 0U;
unsigned int n = INTERLEAVE_TABLE[i];
uint8_t s0 = READ_BIT1(bytes, n) ? 1U : 0U;
n++;
unsigned int s1 = READ_BIT1(data, n) ? 1U : 0U;
uint8_t s1 = READ_BIT1(bytes, n) ? 1U : 0U;
viterbi.decode(s0, s1);
}
}
unsigned char output[13U];
viterbi.chainback(output);
@ -87,12 +94,96 @@ bool CYSFFICH::decode(const unsigned char* data, unsigned char* fich) const
unsigned int b2 = CGolay24128::decode24128(output + 6U);
unsigned int b3 = CGolay24128::decode24128(output + 9U);
fich[0U] = (b0 >> 16) & 0xFFU;
fich[1U] = ((b0 >> 8) & 0xF0U) | ((b1 >> 20) & 0x0FU);
fich[2U] = (b1 >> 12) & 0xFFU;
fich[3U] = (b2 >> 16) & 0xFFU;
fich[4U] = ((b2 >> 8) & 0xF0U) | ((b3 >> 20) & 0x0FU);
fich[5U] = (b3 >> 12) & 0xFFU;
m_fich[0U] = (b0 >> 4) & 0xFFU;
m_fich[1U] = ((b0 << 4) & 0xF0U) | ((b1 >> 8) & 0x0FU);
m_fich[2U] = (b1 >> 0) & 0xFFU;
m_fich[3U] = (b2 >> 4) & 0xFFU;
m_fich[4U] = ((b2 << 4) & 0xF0U) | ((b3 >> 8) & 0x0FU);
m_fich[5U] = (b3 >> 0) & 0xFFU;
return CCRC::crcFICH(fich);
return CCRC::checkCCITT162(m_fich, 6U);
}
void CYSFFICH::encode(unsigned char* bytes)
{
assert(bytes != NULL);
// Skip the sync bytes
bytes += YSF_SYNC_LENGTH_BYTES;
CCRC::addCCITT162(m_fich, 6U);
unsigned int b0 = ((m_fich[0U] << 4) & 0xFF0U) | ((m_fich[1U] >> 4) & 0x00FU);
unsigned int b1 = ((m_fich[1U] << 8) & 0xF00U) | ((m_fich[2U] >> 0) & 0x0FFU);
unsigned int b2 = ((m_fich[3U] << 4) & 0xFF0U) | ((m_fich[4U] >> 4) & 0x00FU);
unsigned int b3 = ((m_fich[4U] << 8) & 0xF00U) | ((m_fich[5U] >> 0) & 0x0FFU);
unsigned int c0 = CGolay24128::encode24128(b0);
unsigned int c1 = CGolay24128::encode24128(b1);
unsigned int c2 = CGolay24128::encode24128(b2);
unsigned int c3 = CGolay24128::encode24128(b3);
unsigned char conv[13U];
conv[0U] = (c0 >> 16) & 0xFFU;
conv[1U] = (c0 >> 8) & 0xFFU;
conv[2U] = (c0 >> 0) & 0xFFU;
conv[3U] = (c1 >> 16) & 0xFFU;
conv[4U] = (c1 >> 8) & 0xFFU;
conv[5U] = (c1 >> 0) & 0xFFU;
conv[6U] = (c2 >> 16) & 0xFFU;
conv[7U] = (c2 >> 8) & 0xFFU;
conv[8U] = (c2 >> 0) & 0xFFU;
conv[9U] = (c3 >> 16) & 0xFFU;
conv[10U] = (c3 >> 8) & 0xFFU;
conv[11U] = (c3 >> 0) & 0xFFU;
conv[12U] = 0x00U;
CYSFConvolution convolution;
unsigned char convolved[25U];
convolution.encode(conv, convolved, 100U);
unsigned int j = 0U;
for (unsigned int i = 0U; i < 100U; i++) {
unsigned int n = INTERLEAVE_TABLE[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 CYSFFICH::getCM() const
{
return m_fich[0U] & 0x0CU;
}
unsigned char CYSFFICH::getFT() const
{
return m_fich[1U] & 0x07U;
}
unsigned char CYSFFICH::getFN() const
{
return (m_fich[1U] & 0x38U) >> 3;
}
void CYSFFICH::setMR(unsigned char mr)
{
m_fich[2U] &= 0xC7U;
m_fich[2U] |= mr;
}
void CYSFFICH::setVoIP(bool on)
{
if (on)
m_fich[2U] |= 0x04U;
else
m_fich[2U] &= 0xFBU;
}

View File

@ -24,9 +24,19 @@ public:
CYSFFICH();
~CYSFFICH();
bool decode(const unsigned char* frame, unsigned char* fich) const;
bool decode(const unsigned char* bytes);
void encode(unsigned char* bytes);
unsigned char getCM() const;
unsigned char getFT() const;
unsigned char getFN() const;
void setMR(unsigned char mr);
void setVoIP(bool set);
private:
unsigned char* m_fich;
};
#endif

View File

@ -51,14 +51,16 @@ bool CYSFParrot::write(const unsigned char* data)
::memcpy(m_data + m_used, data, YSF_FRAME_LENGTH_BYTES + 2U);
m_used += YSF_FRAME_LENGTH_BYTES + 2U;
if (data[0U] == TAG_EOT) {
m_timer.start();
m_ptr = 0U;
}
return true;
}
void CYSFParrot::end()
{
m_timer.start();
m_ptr = 0U;
}
void CYSFParrot::read(unsigned char* data)
{
assert(data != NULL);

View File

@ -31,6 +31,8 @@ public:
void read(unsigned char* data);
void end();
bool hasData();
void clock(unsigned int ms);