Skip to content

Commit

Permalink
implement proper LDU2 encode/decode; implement proper HDU (Header) en…
Browse files Browse the repository at this point in the history 
…code/decode
  • Loading branch information
@gatekeep
gatekeep committed Oct 4, 2018
1 parent f5a70aa commit 85bc835
Show file tree
Hide file tree
Showing 6 changed files with 431 additions and 106 deletions.
27 changes: 26 additions & 1 deletion P25Control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,9 +208,24 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
m_rssiCount++;
}

if (duid == P25_DUID_LDU1) {
if (duid == P25_DUID_HEADER) {
if (m_rfState == RS_RF_LISTENING) {
m_rfData.reset();
bool ret = m_rfData.decodeHeader(data + 2U);
if (!ret) {
m_lastDUID = duid;
return false;
}

LogMessage("P25, received RF header");

m_lastDUID = duid;
return true;
}
}
else if (duid == P25_DUID_LDU1) {
if (m_rfState == RS_RF_LISTENING) {
//m_rfData.reset();
bool ret = m_rfData.decodeLDU1(data + 2U);
if (!ret) {
m_lastDUID = duid;
Expand Down Expand Up @@ -262,6 +277,11 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
}

if (m_rfState == RS_RF_AUDIO) {
bool ret = m_rfData.decodeLDU1(data + 2U);
if (!ret) {
return false;
}

// Regenerate Sync
CSync::addP25Sync(data + 2U);

Expand Down Expand Up @@ -306,6 +326,11 @@ bool CP25Control::writeModem(unsigned char* data, unsigned int len)
}
} else if (duid == P25_DUID_LDU2) {
if (m_rfState == RS_RF_AUDIO) {
bool ret = m_rfData.decodeLDU2(data + 2U);
if (!ret) {
return false;
}

writeNetwork(m_rfLDU, m_lastDUID, false);

// Regenerate Sync
Expand Down
214 changes: 194 additions & 20 deletions P25Data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,25 +22,13 @@
#include "P25Utils.h"
#include "CRC.h"
#include "Hamming.h"
#include "Golay24128.h"
#include "Utils.h"
#include "Log.h"

#include <cstdio>
#include <cassert>
#include <cstring>

const unsigned char DUMMY_HEADER[] = {
0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U,
0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x08U, 0xDCU, 0x60U,
0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x02U, 0x93U, 0xE7U, 0x73U, 0x77U, 0x57U, 0xD6U, 0xD3U, 0xCFU, 0x77U,
0xEEU, 0x82U, 0x93U, 0xE2U, 0x2FU, 0xF3U, 0xD5U, 0xF5U, 0xBEU, 0xBCU, 0x54U, 0x0DU, 0x9CU, 0x29U, 0x3EU, 0x46U,
0xE3U, 0x28U, 0xB0U, 0xB7U, 0x73U, 0x76U, 0x1EU, 0x26U, 0x0CU, 0x75U, 0x5BU, 0xF7U, 0x4DU, 0x5FU, 0x5AU, 0x37U,
0x18U};

const unsigned char DUMMY_LDU2[] = {
0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x80U, 0x00U, 0x00U, 0xACU, 0xB8U, 0xA4U, 0x9BU,
0xDCU, 0x75U
};

const unsigned char BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U };

Expand All @@ -67,11 +55,70 @@ CP25Data::~CP25Data()
delete[] m_mi;
}

bool CP25Data::decodeHeader(const unsigned char* data)
{
assert(data != NULL);

// deinterleave
unsigned char rs[81U];
unsigned char raw[81U];
CP25Utils::decode(data, raw, 114U, 780U);

// decode Golay (18,6,8) FEC
decodeHeaderGolay(raw, rs);

// decode RS (36,20,17) FEC
try {
bool ret = m_rs241213.decode362017(rs);
if (!ret)
return false;
} catch (...) {
CUtils::dump(2U, "P25, RS crashed with input data", rs, 81U);
return false;
}

m_mfId = rs[9U]; // Mfg Id.
m_algId = rs[10U]; // Algorithm ID
if (m_algId != P25_ALGO_UNENCRYPT) {
m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES);
::memcpy(m_mi, rs, P25_MI_LENGTH_BYTES); // Message Indicator

m_kId = (rs[11U] << 8) + rs[12U]; // Key ID
}
else {
m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES);

m_kId = 0x0000U;
}

return true;
}

void CP25Data::encodeHeader(unsigned char* data)
{
assert(data != NULL);
assert(m_mi != NULL);

CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U);
unsigned char rs[81U];
::memset(rs, 0x00U, 81U);

rs[9U] = m_mfId; // Mfg Id.
rs[13U] = (m_dstId >> 8) & 0xFFU; // Talkgroup Address MSB
rs[14U] = (m_dstId >> 0) & 0xFFU; // Talkgroup Address LSB

// encode RS (36,20,17) FEC
m_rs241213.encode362017(rs);

unsigned char raw[81U];
::memset(raw, 0x00U, 81U);

// encode Golay (18,6,8) FEC
encodeHeaderGolay(raw, rs);

// interleave
CP25Utils::encode(raw, data, 114U, 780U);
}

bool CP25Data::decodeLDU1(const unsigned char* data)
Expand Down Expand Up @@ -185,27 +232,98 @@ void CP25Data::encodeLDU1(unsigned char* data)
CP25Utils::encode(raw, data, 1356U, 1398U);
}

bool CP25Data::decodeLDU2(const unsigned char* data)
{
assert(data != NULL);

unsigned char rs[18U];

// deinterleave and decode Hamming (10,6,3) for LC data
unsigned char raw[5U];
CP25Utils::decode(data, raw, 410U, 452U);
decodeLDUHamming(raw, rs + 0U);

CP25Utils::decode(data, raw, 600U, 640U);
decodeLDUHamming(raw, rs + 3U);

CP25Utils::decode(data, raw, 788U, 830U);
decodeLDUHamming(raw, rs + 6U);

CP25Utils::decode(data, raw, 978U, 1020U);
decodeLDUHamming(raw, rs + 9U);

CP25Utils::decode(data, raw, 1168U, 1208U);
decodeLDUHamming(raw, rs + 12U);

CP25Utils::decode(data, raw, 1356U, 1398U);
decodeLDUHamming(raw, rs + 15U);

// CUtils::dump(2U, "LDU2 RS", rs, P25_LDU_LC_LENGTH_BYTES);

// decode RS (24,16,9) FEC
try {
bool ret = m_rs241213.decode24169(rs);
if (!ret)
return false;
} catch (...) {
CUtils::dump(2U, "P25, RS crashed with input data", rs, 18U);
return false;
}

m_algId = rs[9U]; // Algorithm ID
if (m_algId != P25_ALGO_UNENCRYPT) {
m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES);
::memcpy(m_mi, rs, P25_MI_LENGTH_BYTES); // Message Indicator

m_kId = (rs[10U] << 8) + rs[11U]; // Key ID
}
else {
m_mi = new unsigned char[P25_MI_LENGTH_BYTES];
::memset(m_mi, 0x00U, P25_MI_LENGTH_BYTES);

m_kId = 0x0000U;
}

return true;
}

void CP25Data::encodeLDU2(unsigned char* data)
{
assert(data != NULL);
assert(m_mi != NULL);

unsigned char rs[18U];
::memset(rs, 0x00U, 18U);

for (unsigned int i = 0; i < P25_MI_LENGTH_BYTES; i++)
rs[i] = m_mi[i]; // Message Indicator

rs[9U] = m_algId; // Algorithm ID
rs[10U] = (m_kId >> 8) & 0xFFU; // Key ID MSB
rs[11U] = (m_kId >> 0) & 0xFFU; // Key ID LSB

// encode RS (24,16,9) FEC
m_rs241213.encode24169(rs);

// encode Hamming (10,6,3) FEC and interleave for LC data
unsigned char raw[5U];
encodeLDUHamming(raw, DUMMY_LDU2 + 0U);
encodeLDUHamming(raw, rs + 0U);
CP25Utils::encode(raw, data, 410U, 452U);

encodeLDUHamming(raw, DUMMY_LDU2 + 3U);
encodeLDUHamming(raw, rs + 3U);
CP25Utils::encode(raw, data, 600U, 640U);

encodeLDUHamming(raw, DUMMY_LDU2 + 6U);
encodeLDUHamming(raw, rs + 6U);
CP25Utils::encode(raw, data, 788U, 830U);

encodeLDUHamming(raw, DUMMY_LDU2 + 9U);
encodeLDUHamming(raw, rs + 9U);
CP25Utils::encode(raw, data, 978U, 1020U);

encodeLDUHamming(raw, DUMMY_LDU2 + 12U);
encodeLDUHamming(raw, rs + 12U);
CP25Utils::encode(raw, data, 1168U, 1208U);

encodeLDUHamming(raw, DUMMY_LDU2 + 15U);
encodeLDUHamming(raw, rs + 15U);
CP25Utils::encode(raw, data, 1356U, 1398U);
}

Expand Down Expand Up @@ -467,3 +585,59 @@ void CP25Data::encodeLDUHamming(unsigned char* data, const unsigned char* raw)
}
}
}

void CP25Data::decodeHeaderGolay(const unsigned char* data, unsigned char* raw)
{
// shortened Golay (18,6,8) decode
unsigned int n = 0U;
unsigned int m = 0U;
for (unsigned int i = 0U; i < 36U; i++) {
bool golay[18U];
for (unsigned int j = 0U; j < 18U; j++) {
golay[j] = READ_BIT(data, n);
n++;
}

unsigned int g0 = 0U;
for (unsigned int j = 0U; j < 18U; j++)
g0 = (g0 << 1) | (golay[j] ? 0x01U : 0x00U);
unsigned int c0data = CGolay24128::decode24128(g0);
for (int j = 5; j >= 0; j--) {
golay[j] = (c0data & 0x01U) == 0x01U;
c0data >>= 1;
}

for (unsigned int j = 0U; j < 6U; j++) {
WRITE_BIT(raw, m, golay[j]);
m++;
}
}
}

void CP25Data::encodeHeaderGolay(unsigned char* data, const unsigned char* raw)
{
// shortened Golay (18,6,8) encode
unsigned int n = 0U;
unsigned int m = 0U;
for (unsigned int i = 0U; i < 36U; i++) {
bool golay[18U];
for (unsigned int j = 0U; j < 6U; j++) {
golay[j] = READ_BIT(raw, m);
m++;
}

unsigned int c0data = 0U;
for (unsigned int j = 0U; j < 6U; j++)
c0data = (c0data << 1) | (golay[j] ? 0x01U : 0x00U);
unsigned int g0 = CGolay24128::encode24128(c0data);
for (int j = 17; j >= 0; j--) {
golay[j] = (g0 & 0x01U) == 0x01U;
g0 >>= 1;
}

for (unsigned int j = 0U; j < 18U; j++) {
WRITE_BIT(data, n, golay[j]);
n++;
}
}
}
5 changes: 5 additions & 0 deletions P25Data.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,13 @@ class CP25Data {
CP25Data();
~CP25Data();

bool decodeHeader(const unsigned char* data);
void encodeHeader(unsigned char* data);

bool decodeLDU1(const unsigned char* data);
void encodeLDU1(unsigned char* data);

bool decodeLDU2(const unsigned char* data);
void encodeLDU2(unsigned char* data);

bool decodeTSDU(const unsigned char* data);
Expand Down Expand Up @@ -82,6 +84,9 @@ class CP25Data {

void decodeLDUHamming(const unsigned char* raw, unsigned char* data);
void encodeLDUHamming(unsigned char* data, const unsigned char* raw);

void decodeHeaderGolay(const unsigned char* raw, unsigned char* data);
void encodeHeaderGolay(unsigned char* data, const unsigned char* raw);
};

#endif
2 changes: 2 additions & 0 deletions P25Defines.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,8 @@ const unsigned int P25_PDU_FEC_LENGTH_BITS = P25_PDU_FEC_LENGTH_BYTES

const unsigned int P25_MI_LENGTH_BYTES = 9U;

const unsigned char P25_ALGO_UNENCRYPT = 0x81U;

const unsigned char P25_LCF_GROUP = 0x00U;
const unsigned char P25_LCF_PRIVATE = 0x03U;

Expand Down
Loading

0 comments on commit 85bc835

Please sign in to comment.