Crash_Override
/
MMDVMHost-Private
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
CRC.cpp
View File

@ -238,23 +238,3 @@ unsigned char CCRC::crc8(const unsigned char *in, unsigned int length)
return crc; 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 bool checkCCITT162(const unsigned char* in, unsigned int length);
static unsigned char crc8(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 #endif

27
YSFControl.cpp
View File

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

71
YSFConvolution.cpp
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 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]) #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 uint8_t 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_TABLE2[] = {0U, 1U, 1U, 0U, 0U, 1U, 1U, 0U};
CYSFConvolution::CYSFConvolution() : CYSFConvolution::CYSFConvolution() :
m_metrics1(NULL), m_metrics1(NULL),
@ -40,9 +40,9 @@ m_newMetrics(NULL),
m_decisions(NULL), m_decisions(NULL),
m_dp(NULL) m_dp(NULL)
{ {
m_metrics1 = new unsigned short[16U]; m_metrics1 = new uint16_t[16U];
m_metrics2 = new unsigned short[16U]; m_metrics2 = new uint16_t[16U];
m_decisions = new unsigned long long[100U]; m_decisions = new uint64_t[100U];
} }
CYSFConvolution::~CYSFConvolution() CYSFConvolution::~CYSFConvolution()
@ -54,44 +54,44 @@ CYSFConvolution::~CYSFConvolution()
const unsigned int NUM_OF_STATES_D2 = 8U; const unsigned int NUM_OF_STATES_D2 = 8U;
const unsigned int NUM_OF_STATES = 16U; const unsigned int NUM_OF_STATES = 16U;
const unsigned int M = 3U; const uint32_t M = 3U;
const unsigned int K = 5U; const unsigned int K = 5U;
void CYSFConvolution::start() void CYSFConvolution::start()
{ {
::memset(m_metrics1, 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(unsigned short)); ::memset(m_metrics2, 0x00U, NUM_OF_STATES * sizeof(uint16_t));
m_oldMetrics = m_metrics1; m_oldMetrics = m_metrics1;
m_newMetrics = m_metrics2; m_newMetrics = m_metrics2;
m_dp = m_decisions; 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; *m_dp = 0U;
for (unsigned int i = 0U; i < NUM_OF_STATES_D2; i++) { for (uint8_t i = 0U; i < NUM_OF_STATES_D2; i++) {
unsigned int j = i * 2U; 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; uint16_t m0 = m_oldMetrics[i] + metric;
unsigned short m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + (M - metric); uint16_t m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + (M - metric);
unsigned char decision0 = (m0 >= m1) ? 1U : 0U; uint8_t decision0 = (m0 >= m1) ? 1U : 0U;
m_newMetrics[j + 0U] = decision0 != 0U ? m1 : m0; m_newMetrics[j + 0U] = decision0 != 0U ? m1 : m0;
m0 = m_oldMetrics[i] + (M - metric); m0 = m_oldMetrics[i] + (M - metric);
m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + 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_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; ++m_dp;
unsigned short* tmp = m_oldMetrics; uint16_t* tmp = m_oldMetrics;
m_oldMetrics = m_newMetrics; m_oldMetrics = m_newMetrics;
m_newMetrics = tmp; m_newMetrics = tmp;
} }
@ -100,16 +100,43 @@ void CYSFConvolution::chainback(unsigned char* out)
{ {
assert(out != NULL); assert(out != NULL);
unsigned int state = 0U; uint32_t state = 0U;
unsigned char nbits = 96U; uint8_t nbits = 96U;
while (nbits-- > 0) { while (nbits-- > 0) {
--m_dp; --m_dp;
unsigned int i = state >> (9 - K); uint32_t i = state >> (9 - K);
unsigned char bit = (unsigned char)(*m_dp >> i) & 1; uint8_t bit = uint8_t(*m_dp >> i) & 1;
state = (bit << 7) | (state >> 1); state = (bit << 7) | (state >> 1);
WRITE_BIT1(out, nbits, bit != 0U); 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++;
}
}

18
YSFConvolution.h
View File

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

6
YSFDefines.h
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_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 #endif

123
YSFFICH.cpp
View File

@ -17,9 +17,11 @@
*/ */
#include "YSFConvolution.h" #include "YSFConvolution.h"
#include "YSFDefines.h"
#include "Golay24128.h" #include "Golay24128.h"
#include "YSFFICH.h" #include "YSFFICH.h"
#include "CRC.h" #include "CRC.h"
#include "Log.h"
#include <cstdio> #include <cstdio>
#include <cassert> #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 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]) #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, 0U, 40U, 80U, 120U, 160U,
2U, 42U, 82U, 122U, 162U, 2U, 42U, 82U, 122U, 162U,
4U, 44U, 84U, 124U, 164U, 4U, 44U, 84U, 124U, 164U,
@ -52,32 +54,37 @@ const unsigned int INTERLEAVE_TABLE_RX[] = {
36U, 76U, 116U, 156U, 196U, 36U, 76U, 116U, 156U, 196U,
38U, 78U, 118U, 158U, 198U}; 38U, 78U, 118U, 158U, 198U};
CYSFFICH::CYSFFICH() CYSFFICH::CYSFFICH() :
m_fich(NULL)
{ {
m_fich = new unsigned char[6U];
} }
CYSFFICH::~CYSFFICH() 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(bytes != NULL);
assert(fich != NULL);
// Skip the sync bytes
bytes += YSF_SYNC_LENGTH_BYTES;
CYSFConvolution viterbi; CYSFConvolution viterbi;
viterbi.start(); viterbi.start();
// Deinterleave the FICH and send bits to the Viterbi decoder // Deinterleave the FICH and send bits to the Viterbi decoder
for (unsigned int i = 0U; i < 100U; i++) { for (unsigned int i = 0U; i < 100U; i++) {
unsigned int n = INTERLEAVE_TABLE_RX[i]; unsigned int n = INTERLEAVE_TABLE[i];
unsigned int s0 = READ_BIT1(data, n) ? 1U : 0U; uint8_t s0 = READ_BIT1(bytes, n) ? 1U : 0U;
n++; n++;
unsigned int s1 = READ_BIT1(data, n) ? 1U : 0U; uint8_t s1 = READ_BIT1(bytes, n) ? 1U : 0U;
viterbi.decode(s0, s1); viterbi.decode(s0, s1);
} }
unsigned char output[13U]; unsigned char output[13U];
viterbi.chainback(output); 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 b2 = CGolay24128::decode24128(output + 6U);
unsigned int b3 = CGolay24128::decode24128(output + 9U); unsigned int b3 = CGolay24128::decode24128(output + 9U);
fich[0U] = (b0 >> 16) & 0xFFU; m_fich[0U] = (b0 >> 4) & 0xFFU;
fich[1U] = ((b0 >> 8) & 0xF0U) | ((b1 >> 20) & 0x0FU); m_fich[1U] = ((b0 << 4) & 0xF0U) | ((b1 >> 8) & 0x0FU);
fich[2U] = (b1 >> 12) & 0xFFU; m_fich[2U] = (b1 >> 0) & 0xFFU;
fich[3U] = (b2 >> 16) & 0xFFU; m_fich[3U] = (b2 >> 4) & 0xFFU;
fich[4U] = ((b2 >> 8) & 0xF0U) | ((b3 >> 20) & 0x0FU); m_fich[4U] = ((b2 << 4) & 0xF0U) | ((b3 >> 8) & 0x0FU);
fich[5U] = (b3 >> 12) & 0xFFU; 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;
} }

12
YSFFICH.h
View File

@ -24,9 +24,19 @@ public:
CYSFFICH(); CYSFFICH();
~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: private:
unsigned char* m_fich;
}; };
#endif #endif

12
YSFParrot.cpp
View File

@ -51,14 +51,16 @@ bool CYSFParrot::write(const unsigned char* data)
::memcpy(m_data + m_used, data, YSF_FRAME_LENGTH_BYTES + 2U); ::memcpy(m_data + m_used, data, YSF_FRAME_LENGTH_BYTES + 2U);
m_used += 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; return true;
} }
void CYSFParrot::end()
{
m_timer.start();
m_ptr = 0U;
}
void CYSFParrot::read(unsigned char* data) void CYSFParrot::read(unsigned char* data)
{ {
assert(data != NULL); assert(data != NULL);

2
YSFParrot.h
View File

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