implement proper LDU2 encode/decode; implement proper HDU (Header) encode/decode
This commit is contained in:
parent
f5a70aa6ce
commit
85bc8357aa
6 changed files with 433 additions and 108 deletions
|
@ -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;
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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
|
||||
|
|
218
P25Data.cpp
218
P25Data.cpp
|
@ -22,6 +22,7 @@
|
|||
#include "P25Utils.h"
|
||||
#include "CRC.h"
|
||||
#include "Hamming.h"
|
||||
#include "Golay24128.h"
|
||||
#include "Utils.h"
|
||||
#include "Log.h"
|
||||
|
||||
|
@ -29,19 +30,6 @@
|
|||
#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 };
|
||||
|
||||
#define WRITE_BIT(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
|
||||
|
@ -67,11 +55,70 @@ CP25Data::~CP25Data()
|
|||
delete[] m_mi;
|
||||
}
|
||||
|
||||
void CP25Data::encodeHeader(unsigned char* data)
|
||||
bool CP25Data::decodeHeader(const unsigned char* data)
|
||||
{
|
||||
assert(data != NULL);
|
||||
|
||||
CP25Utils::encode(DUMMY_HEADER, data, 114U, 780U);
|
||||
// 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);
|
||||
|
||||
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)
|
||||
|
@ -185,27 +232,98 @@ void CP25Data::encodeLDU1(unsigned char* data)
|
|||
CP25Utils::encode(raw, data, 1356U, 1398U);
|
||||
}
|
||||
|
||||
void CP25Data::encodeLDU2(unsigned char* data)
|
||||
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];
|
||||
encodeLDUHamming(raw, DUMMY_LDU2 + 0U);
|
||||
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, 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);
|
||||
}
|
||||
|
||||
|
@ -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++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -28,11 +28,13 @@ public:
|
|||
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);
|
||||
|
@ -82,6 +84,9 @@ private:
|
|||
|
||||
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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
225
RS241213.cpp
225
RS241213.cpp
|
@ -1,5 +1,6 @@
|
|||
/*
|
||||
* Copyright (C) 2016 by Jonathan Naylor G4KLX
|
||||
* Copyright (C) 2018 by Bryan Biedenkapp <gatekeep@gmail.com>
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
|
@ -20,6 +21,7 @@
|
|||
|
||||
#include <cstdio>
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
|
||||
const unsigned char ENCODE_MATRIX[12U][24U] = {
|
||||
{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 062, 044, 003, 025, 014, 016, 027, 003, 053, 004, 036, 047},
|
||||
|
@ -35,6 +37,46 @@ const unsigned char ENCODE_MATRIX[12U][24U] = {
|
|||
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 073, 065, 036, 061, 042, 022, 017, 004, 044, 020, 025, 005},
|
||||
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 071, 005, 055, 003, 071, 034, 060, 011, 074, 002, 041, 050}};
|
||||
|
||||
const unsigned char ENCODE_MATRIX_24169[16U][24U] = {
|
||||
{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 051, 045, 067, 015, 064, 067, 052, 012 },
|
||||
{ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 057, 025, 063, 073, 071, 022, 040, 015 },
|
||||
{ 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 005, 001, 031, 004, 016, 054, 025, 076 },
|
||||
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 073, 007, 047, 014, 041, 077, 047, 011 },
|
||||
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 075, 015, 051, 051, 017, 067, 017, 057 },
|
||||
{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 020, 032, 014, 042, 075, 042, 070, 054 },
|
||||
{ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 002, 075, 043, 005, 001, 040, 012, 064 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 024, 074, 015, 072, 024, 026, 074, 061 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 042, 064, 007, 022, 061, 020, 040, 065 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 032, 032, 055, 041, 057, 066, 021, 077 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 065, 036, 025, 007, 050, 016, 040, 051 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 064, 006, 054, 032, 076, 046, 014, 036 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 062, 063, 074, 070, 005, 027, 037, 046 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 055, 043, 034, 071, 057, 076, 050, 064 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 024, 023, 023, 005, 050, 070, 042, 023 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 067, 075, 045, 060, 057, 024, 006, 026 } };
|
||||
|
||||
const unsigned char ENCODE_MATRIX_362017[20U][36U] = {
|
||||
{ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 074, 037, 034, 006, 002, 007, 044, 064, 026, 014, 026, 044, 054, 013, 077, 005 },
|
||||
{ 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 004, 017, 050, 024, 011, 005, 030, 057, 033, 003, 002, 002, 015, 016, 025, 026 },
|
||||
{ 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 007, 023, 037, 046, 056, 075, 043, 045, 055, 021, 050, 031, 045, 027, 071, 062 },
|
||||
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 026, 005, 007, 063, 063, 027, 063, 040, 006, 004, 040, 045, 047, 030, 075, 007 },
|
||||
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 023, 073, 073, 041, 072, 034, 021, 051, 067, 016, 031, 074, 011, 021, 012, 021 },
|
||||
{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 024, 051, 025, 023, 022, 041, 074, 066, 074, 065, 070, 036, 067, 045, 064, 001 },
|
||||
{ 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 052, 033, 014, 002, 020, 006, 014, 025, 052, 023, 035, 074, 075, 075, 043, 027 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 055, 062, 056, 025, 073, 060, 015, 030, 013, 017, 020, 002, 070, 055, 014, 047 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 054, 051, 032, 065, 077, 012, 054, 013, 035, 032, 056, 012, 075, 001, 072, 063 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 074, 041, 030, 041, 043, 022, 051, 006, 064, 033, 003, 047, 027, 012, 055, 047 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 054, 070, 011, 003, 013, 022, 016, 057, 003, 045, 072, 031, 030, 056, 035, 022 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 051, 007, 072, 030, 065, 054, 006, 021, 036, 063, 050, 061, 064, 052, 001, 060 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 001, 065, 032, 070, 013, 044, 073, 024, 012, 052, 021, 055, 012, 035, 014, 072 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 011, 070, 005, 010, 065, 024, 015, 077, 022, 024, 024, 074, 007, 044, 007, 046 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 006, 002, 065, 011, 041, 020, 045, 042, 046, 054, 035, 012, 040, 064, 065, 033 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 034, 031, 001, 015, 044, 064, 016, 024, 052, 016, 006, 062, 020, 013, 055, 057 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 063, 043, 025, 044, 077, 063, 017, 017, 064, 014, 040, 074, 031, 072, 054, 006 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 071, 021, 070, 044, 056, 004, 030, 074, 004, 023, 071, 070, 063, 045, 056, 043 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 002, 001, 053, 074, 002, 014, 052, 074, 012, 057, 024, 063, 015, 042, 052, 033 },
|
||||
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 034, 035, 002, 023, 021, 027, 022, 033, 064, 042, 005, 073, 051, 046, 073, 060 } };
|
||||
|
||||
const unsigned int rsGFexp[64] = {
|
||||
1, 2, 4, 8, 16, 32, 3, 6, 12, 24, 48, 35, 5, 10, 20, 40,
|
||||
19, 38, 15, 30, 60, 59, 53, 41, 17, 34, 7, 14, 28, 56, 51, 37,
|
||||
|
@ -85,19 +127,119 @@ CRS241213::~CRS241213()
|
|||
}
|
||||
|
||||
bool CRS241213::decode(unsigned char* data)
|
||||
{
|
||||
return decode(data, 24U, 39, 12);
|
||||
}
|
||||
|
||||
void CRS241213::encode(unsigned char* data)
|
||||
{
|
||||
assert(data != NULL);
|
||||
|
||||
unsigned char HB[24U];
|
||||
unsigned char codeword[24U];
|
||||
|
||||
for (unsigned int i = 0U; i < 24U; i++) {
|
||||
codeword[i] = 0x00U;
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int j = 0U; j < 12U; j++, offset += 6U) {
|
||||
unsigned char hexbit = bin2Hex(data, offset);
|
||||
codeword[i] ^= gf6Mult(hexbit, ENCODE_MATRIX[j][i]);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int i = 0U; i < 24U; i++, offset += 6U)
|
||||
hex2Bin(codeword[i], data, offset);
|
||||
}
|
||||
|
||||
bool CRS241213::decode24169(unsigned char* data)
|
||||
{
|
||||
return decode(data, 24U, 39, 8);
|
||||
}
|
||||
|
||||
void CRS241213::encode24169(unsigned char* data)
|
||||
{
|
||||
assert(data != NULL);
|
||||
|
||||
unsigned char codeword[24U];
|
||||
|
||||
for (unsigned int i = 0U; i < 24U; i++) {
|
||||
codeword[i] = 0x00U;
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int j = 0U; j < 16U; j++, offset += 6U) {
|
||||
unsigned char hexbit = bin2Hex(data, offset);
|
||||
codeword[i] ^= gf6Mult(hexbit, ENCODE_MATRIX_24169[j][i]);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int i = 0U; i < 24U; i++, offset += 6U)
|
||||
hex2Bin(codeword[i], data, offset);
|
||||
}
|
||||
|
||||
bool CRS241213::decode362017(unsigned char* data)
|
||||
{
|
||||
return decode(data, 36U, 27, 16);
|
||||
}
|
||||
|
||||
void CRS241213::encode362017(unsigned char* data)
|
||||
{
|
||||
assert(data != NULL);
|
||||
|
||||
unsigned char codeword[36U];
|
||||
|
||||
for (unsigned int i = 0U; i < 36U; i++) {
|
||||
codeword[i] = 0x00U;
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int j = 0U; j < 20U; j++, offset += 6U) {
|
||||
unsigned char hexbit = bin2Hex(data, offset);
|
||||
codeword[i] ^= gf6Mult(hexbit, ENCODE_MATRIX_362017[j][i]);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int i = 0U; i < 36U; i++, offset += 6U)
|
||||
hex2Bin(codeword[i], data, offset);
|
||||
}
|
||||
|
||||
// GF(2 ^ 6) multiply(for Reed - Solomon encoder)
|
||||
unsigned char CRS241213::gf6Mult(unsigned char a, unsigned char b) const
|
||||
{
|
||||
unsigned char p = 0x00U;
|
||||
|
||||
for (unsigned int i = 0U; i < 6U; i++) {
|
||||
if ((b & 0x01U) == 0x01U)
|
||||
p ^= a;
|
||||
|
||||
a <<= 1;
|
||||
|
||||
if ((a & 0x40U) == 0x40U)
|
||||
a ^= 0x43U; // primitive polynomial : x ^ 6 + x + 1
|
||||
|
||||
b >>= 1;
|
||||
}
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
bool CRS241213::decode(unsigned char* data, const unsigned int bitLength, const int firstData, const int roots)
|
||||
{
|
||||
assert(data != NULL);
|
||||
|
||||
//unsigned char HB[24U];
|
||||
unsigned char HB[63U];
|
||||
::memset(HB, 0x00U, 63U);
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int i = 0U; i < bitLength; i++, offset += 6)
|
||||
HB[i] = bin2Hex(data, offset);
|
||||
|
||||
//RS (63,63-nroots,nroots+1) decoder where nroots = number of parity bits
|
||||
// rsDec(8, 39) rsDec(16, 27) rsDec(12, 39)
|
||||
|
||||
const int nroots = 12;
|
||||
const int nroots = roots;
|
||||
int lambda[18]; // Err+Eras Locator poly
|
||||
int S[17]; // syndrome poly
|
||||
int b[18];
|
||||
|
@ -114,11 +256,14 @@ bool CRS241213::decode(unsigned char* data)
|
|||
S[i] = HB[0];
|
||||
}
|
||||
|
||||
for (j = 1; j <= 23; j++) { // XXX was 62
|
||||
//for (j = 1; j <= 24; j++) { // XXX was 62
|
||||
//for (j = 1; j <= (int)(bitLength - 1); j++) {
|
||||
for (j = 1; j <= 62; j++) {
|
||||
for (i = 0; i <= nroots - 1; i++) {
|
||||
if (S[i] == 0) {
|
||||
S[i] = HB[j];
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
S[i] = HB[j] ^ rsGFexp[(rsGFlog[S[i]] + i + 1) % 63];
|
||||
}
|
||||
}
|
||||
|
@ -153,8 +298,9 @@ bool CRS241213::decode(unsigned char* data)
|
|||
// locator polynomial
|
||||
r = 0;
|
||||
el = 0;
|
||||
while (r < nroots) { //r is the step number
|
||||
r = r + 1;
|
||||
while (++r <= nroots) {
|
||||
// r is the step number
|
||||
//r = r + 1;
|
||||
// compute discrepancy at the r-th step in poly-form
|
||||
DiscrR = 0;
|
||||
|
||||
|
@ -173,14 +319,16 @@ bool CRS241213::decode(unsigned char* data)
|
|||
}
|
||||
|
||||
b[0] = 63;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
// t(x) <-- lambda(x) - DiscrR*x*b(x)
|
||||
t[0] = lambda[0];
|
||||
|
||||
for (i = 0; i <= nroots - 1; i++) {
|
||||
if (b[i] != 63) {
|
||||
t[i + 1] = lambda[i + 1] ^ rsGFexp[(DiscrR + b[i]) % 63];
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
t[i + 1] = lambda[i + 1];
|
||||
}
|
||||
}
|
||||
|
@ -192,11 +340,13 @@ bool CRS241213::decode(unsigned char* data)
|
|||
for (i = 0; i <= nroots; i++) {
|
||||
if (lambda[i]) {
|
||||
b[i] = (rsGFlog[lambda[i]] - DiscrR + 63) % 63;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
b[i] = 63;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
// shift elements upward one step
|
||||
for (i = nroots; i >= 1; i += -1) {
|
||||
b[i] = b[i - 1];
|
||||
|
@ -238,7 +388,8 @@ bool CRS241213::decode(unsigned char* data)
|
|||
}
|
||||
}
|
||||
|
||||
if (q == 0) { //it is a root
|
||||
// it is a root
|
||||
if (q == 0) {
|
||||
// store root (index-form) and error location number
|
||||
root[count] = i;
|
||||
locn[count] = i - 40;
|
||||
|
@ -263,7 +414,8 @@ bool CRS241213::decode(unsigned char* data)
|
|||
tmp = 0;
|
||||
if (DegLambda < i) {
|
||||
j = DegLambda;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
j = i;
|
||||
}
|
||||
|
||||
|
@ -301,7 +453,8 @@ bool CRS241213::decode(unsigned char* data)
|
|||
// lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i]
|
||||
if (DegLambda < nroots) {
|
||||
i = DegLambda;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
i = nroots;
|
||||
}
|
||||
|
||||
|
@ -317,55 +470,15 @@ bool CRS241213::decode(unsigned char* data)
|
|||
|
||||
// apply error to data
|
||||
if (num1 != 0) {
|
||||
if(locn[j] < 24)
|
||||
if (locn[j] < firstData)
|
||||
return false;
|
||||
HB[locn[j]] = HB[locn[j]] ^ (rsGFexp[(rsGFlog[num1] + rsGFlog[num2] + 63 - rsGFlog[den]) % 63]);
|
||||
}
|
||||
}
|
||||
|
||||
offset = 0U;
|
||||
for (unsigned int i = 0U; i < 12U; i++, offset += 6U)
|
||||
for (unsigned int i = 0U; i < (unsigned int)nroots; i++, offset += 6)
|
||||
hex2Bin(HB[i], data, offset);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void CRS241213::encode(unsigned char* data)
|
||||
{
|
||||
assert(data != NULL);
|
||||
|
||||
unsigned char codeword[24U];
|
||||
|
||||
for (unsigned int i = 0U; i < 24U; i++) {
|
||||
codeword[i] = 0x00U;
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int j = 0U; j < 12U; j++, offset += 6U) {
|
||||
unsigned char hexbit = bin2Hex(data, offset);
|
||||
codeword[i] ^= gf6Mult(hexbit, ENCODE_MATRIX[j][i]);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int offset = 0U;
|
||||
for (unsigned int i = 0U; i < 24U; i++, offset += 6U)
|
||||
hex2Bin(codeword[i], data, offset);
|
||||
}
|
||||
|
||||
// GF(2 ^ 6) multiply(for Reed - Solomon encoder)
|
||||
unsigned char CRS241213::gf6Mult(unsigned char a, unsigned char b) const
|
||||
{
|
||||
unsigned char p = 0x00U;
|
||||
|
||||
for (unsigned int i = 0U; i < 6U; i++) {
|
||||
if ((b & 0x01U) == 0x01U)
|
||||
p ^= a;
|
||||
|
||||
a <<= 1;
|
||||
|
||||
if ((a & 0x40U) == 0x40U)
|
||||
a ^= 0x43U; // primitive polynomial : x ^ 6 + x + 1
|
||||
|
||||
b >>= 1;
|
||||
}
|
||||
|
||||
return p;
|
||||
}
|
||||
|
|
|
@ -1,5 +1,6 @@
|
|||
/*
|
||||
* Copyright (C) 2016 by Jonathan Naylor G4KLX
|
||||
* Copyright (C) 2018 by Bryan Biedenkapp <gatekeep@gmail.com>
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
|
@ -26,11 +27,16 @@ public:
|
|||
~CRS241213();
|
||||
|
||||
bool decode(unsigned char* data);
|
||||
bool decode24169(unsigned char* data);
|
||||
bool decode362017(unsigned char* data);
|
||||
|
||||
void encode(unsigned char* data);
|
||||
void encode24169(unsigned char* data);
|
||||
void encode362017(unsigned char* data);
|
||||
|
||||
private:
|
||||
unsigned char gf6Mult(unsigned char a, unsigned char b) const;
|
||||
bool decode(unsigned char* data, const unsigned int bitLength, const int firstData, const int roots);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Reference in a new issue