mirror of
https://github.com/ShaYmez/MMDVM_CM.git
synced 2024-11-15 20:51:49 -05:00
403 lines
9.4 KiB
C++
403 lines
9.4 KiB
C++
/*
|
|
* Copyright (C) 2010,2014,2016 and 2018 by Jonathan Naylor G4KLX
|
|
* Copyright (C) 2016 Mathias Weyland, HB9FRV
|
|
* Copyright (C) 2018 by Andy Uribe CA6JAU
|
|
*
|
|
* 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
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
|
*/
|
|
|
|
#include "ModeConv.h"
|
|
#include "YSFConvolution.h"
|
|
#include "CRC.h"
|
|
#include "Utils.h"
|
|
|
|
#include "Log.h"
|
|
|
|
#include <cstdio>
|
|
#include <cassert>
|
|
|
|
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])
|
|
#define READ_BIT(p,i) (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
|
|
|
|
const unsigned int INTERLEAVE_TABLE_26_4[] = {
|
|
0U, 4U, 8U, 12U, 16U, 20U, 24U, 28U, 32U, 36U, 40U, 44U, 48U, 52U, 56U, 60U, 64U, 68U, 72U, 76U, 80U, 84U, 88U, 92U, 96U, 100U,
|
|
1U, 5U, 9U, 13U, 17U, 21U, 25U, 29U, 33U, 37U, 41U, 45U, 49U, 53U, 57U, 61U, 65U, 69U, 73U, 77U, 81U, 85U, 89U, 93U, 97U, 101U,
|
|
2U, 6U, 10U, 14U, 18U, 22U, 26U, 30U, 34U, 38U, 42U, 46U, 50U, 54U, 58U, 62U, 66U, 70U, 74U, 78U, 82U, 86U, 90U, 94U, 98U, 102U,
|
|
3U, 7U, 11U, 15U, 19U, 23U, 27U, 31U, 35U, 39U, 43U, 47U, 51U, 55U, 59U, 63U, 67U, 71U, 75U, 79U, 83U, 87U, 91U, 95U, 99U, 103U};
|
|
|
|
const unsigned char WHITENING_DATA[] = {0x93U, 0xD7U, 0x51U, 0x21U, 0x9CU, 0x2FU, 0x6CU, 0xD0U, 0xEFU, 0x0FU,
|
|
0xF8U, 0x3DU, 0xF1U, 0x73U, 0x20U, 0x94U, 0xEDU, 0x1EU, 0x7CU, 0xD8U};
|
|
|
|
const unsigned char AMBE_SILENCE[] = {0xF8U, 0x01U, 0xA9U, 0x9FU, 0x8CU, 0xE0U, 0x80U};
|
|
const unsigned char YSF_SILENCE[] = {0x7BU, 0xB2U, 0x8EU, 0x43U, 0x36U, 0xE4U, 0xA2U, 0x39U, 0x78U, 0x49U, 0x33U, 0x68U, 0x33U};
|
|
|
|
CModeConv::CModeConv() :
|
|
m_ysfN(0U),
|
|
m_nxdnN(0U),
|
|
m_YSF(5000U, "NXDN2YSF"),
|
|
m_NXDN(5000U, "YSF2NXDN")
|
|
{
|
|
}
|
|
|
|
CModeConv::~CModeConv()
|
|
{
|
|
}
|
|
|
|
void CModeConv::putNXDN(unsigned char* data)
|
|
{
|
|
assert(data != NULL);
|
|
|
|
data += 5U;
|
|
|
|
unsigned int dat_a1 = 0U, dat_b1 = 0U, dat_c1 = 0U;
|
|
unsigned int dat_a2 = 0U, dat_b2 = 0U, dat_c2 = 0U;
|
|
unsigned int dat_a3 = 0U, dat_b3 = 0U, dat_c3 = 0U;
|
|
unsigned int dat_a4 = 0U, dat_b4 = 0U, dat_c4 = 0U;
|
|
|
|
unsigned int MASK = 0x800U;
|
|
for (unsigned int i = 0U; i < 12U; i++, MASK >>= 1) {
|
|
if (READ_BIT(data, i + 0U))
|
|
dat_a1 |= MASK;
|
|
if (READ_BIT(data, i + 12U))
|
|
dat_b1 |= MASK;
|
|
if (READ_BIT(data, i + 49U))
|
|
dat_a2 |= MASK;
|
|
if (READ_BIT(data, i + 61U))
|
|
dat_b2 |= MASK;
|
|
if (READ_BIT(data, i + 112U))
|
|
dat_a3 |= MASK;
|
|
if (READ_BIT(data, i + 124U))
|
|
dat_b3 |= MASK;
|
|
if (READ_BIT(data, i + 161U))
|
|
dat_a4 |= MASK;
|
|
if (READ_BIT(data, i + 173U))
|
|
dat_b4 |= MASK;
|
|
}
|
|
|
|
MASK = 0x1000000U;
|
|
for (unsigned int i = 0U; i < 25U; i++, MASK >>= 1) {
|
|
if (READ_BIT(data, i + 24U))
|
|
dat_c1 |= MASK;
|
|
if (READ_BIT(data, i + 73U))
|
|
dat_c2 |= MASK;
|
|
if (READ_BIT(data, i + 136U))
|
|
dat_c3 |= MASK;
|
|
if (READ_BIT(data, i + 185U))
|
|
dat_c4 |= MASK;
|
|
}
|
|
|
|
putAMBE2YSF(dat_a1, dat_b1, dat_c1);
|
|
putAMBE2YSF(dat_a2, dat_b2, dat_c2);
|
|
putAMBE2YSF(dat_a3, dat_b3, dat_c3);
|
|
putAMBE2YSF(dat_a4, dat_b4, dat_c4);
|
|
}
|
|
|
|
void CModeConv::putAMBE2YSF(unsigned int dat_a, unsigned int dat_b, unsigned int dat_c)
|
|
{
|
|
unsigned char vch[13U];
|
|
unsigned char ysfFrame[13U];
|
|
::memset(vch, 0U, 13U);
|
|
::memset(ysfFrame, 0, 13U);
|
|
|
|
for (unsigned int i = 0U; i < 12U; i++) {
|
|
bool s = (dat_a << (20U + i)) & 0x80000000U;
|
|
WRITE_BIT(vch, 3*i + 0U, s);
|
|
WRITE_BIT(vch, 3*i + 1U, s);
|
|
WRITE_BIT(vch, 3*i + 2U, s);
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 12U; i++) {
|
|
bool s = (dat_b << (20U + i)) & 0x80000000U;
|
|
WRITE_BIT(vch, 3*(i + 12U) + 0U, s);
|
|
WRITE_BIT(vch, 3*(i + 12U) + 1U, s);
|
|
WRITE_BIT(vch, 3*(i + 12U) + 2U, s);
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 3U; i++) {
|
|
bool s = (dat_c << (7U + i)) & 0x80000000U;
|
|
WRITE_BIT(vch, 3*(i + 24U) + 0U, s);
|
|
WRITE_BIT(vch, 3*(i + 24U) + 1U, s);
|
|
WRITE_BIT(vch, 3*(i + 24U) + 2U, s);
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 22U; i++) {
|
|
bool s = (dat_c << (10U + i)) & 0x80000000U;
|
|
WRITE_BIT(vch, i + 81U, s);
|
|
}
|
|
|
|
WRITE_BIT(vch, 103U, 0U);
|
|
|
|
// Scramble
|
|
for (unsigned int i = 0U; i < 13U; i++)
|
|
vch[i] ^= WHITENING_DATA[i];
|
|
|
|
// Interleave
|
|
for (unsigned int i = 0U; i < 104U; i++) {
|
|
unsigned int n = INTERLEAVE_TABLE_26_4[i];
|
|
bool s = READ_BIT(vch, i);
|
|
WRITE_BIT(ysfFrame, n, s);
|
|
}
|
|
|
|
m_YSF.addData(&TAG_DATA, 1U);
|
|
m_YSF.addData(ysfFrame, 13U);
|
|
//CUtils::dump(1U, "VCH V/D type 2:", ysfFrame, 13U);
|
|
|
|
m_ysfN += 1U;
|
|
}
|
|
|
|
void CModeConv::putYSF(unsigned char* data)
|
|
{
|
|
unsigned char v_tmp[7U];
|
|
|
|
assert(data != NULL);
|
|
|
|
::memset(v_tmp, 0, 7U);
|
|
|
|
data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES;
|
|
|
|
unsigned int offset = 40U; // DCH(0)
|
|
|
|
// We have a total of 5 VCH sections, iterate through each
|
|
for (unsigned int j = 0U; j < 5U; j++, offset += 144U) {
|
|
|
|
unsigned char vch[13U];
|
|
unsigned int dat_a = 0U;
|
|
unsigned int dat_b = 0U;
|
|
unsigned int dat_c = 0U;
|
|
|
|
// Deinterleave
|
|
for (unsigned int i = 0U; i < 104U; i++) {
|
|
unsigned int n = INTERLEAVE_TABLE_26_4[i];
|
|
bool s = READ_BIT(data, offset + n);
|
|
WRITE_BIT(vch, i, s);
|
|
}
|
|
|
|
// "Un-whiten" (descramble)
|
|
for (unsigned int i = 0U; i < 13U; i++)
|
|
vch[i] ^= WHITENING_DATA[i];
|
|
|
|
for (unsigned int i = 0U; i < 12U; i++) {
|
|
dat_a <<= 1U;
|
|
if (READ_BIT(vch, 3U*i + 1U))
|
|
dat_a |= 0x01U;;
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 12U; i++) {
|
|
dat_b <<= 1U;
|
|
if (READ_BIT(vch, 3U*(i + 12U) + 1U))
|
|
dat_b |= 0x01U;;
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 3U; i++) {
|
|
dat_c <<= 1U;
|
|
if (READ_BIT(vch, 3U*(i + 24U) + 1U))
|
|
dat_c |= 0x01U;;
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 22U; i++) {
|
|
dat_c <<= 1U;
|
|
if (READ_BIT(vch, i + 81U))
|
|
dat_c |= 0x01U;;
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 12U; i++) {
|
|
bool s1 = (dat_a << (i + 20U)) & 0x80000000;
|
|
bool s2 = (dat_b << (i + 20U)) & 0x80000000;
|
|
WRITE_BIT(v_tmp, i, s1);
|
|
WRITE_BIT(v_tmp, i + 12U, s2);
|
|
}
|
|
|
|
for (unsigned int i = 0U; i < 25U; i++) {
|
|
bool s = (dat_c << (i + 7U)) & 0x80000000;
|
|
WRITE_BIT(v_tmp, i + 24U, s);
|
|
}
|
|
|
|
m_NXDN.addData(&TAG_DATA, 1U);
|
|
m_NXDN.addData(v_tmp, 7U);
|
|
|
|
//CUtils::dump(1U, "NXDN Voice:", v_tmp, 7U);
|
|
|
|
m_nxdnN += 1U;
|
|
}
|
|
}
|
|
|
|
void CModeConv::putNXDNHeader()
|
|
{
|
|
unsigned char vch[13U];
|
|
|
|
::memset(vch, 0, 13U);
|
|
|
|
m_YSF.addData(&TAG_HEADER, 1U);
|
|
m_YSF.addData(vch, 13U);
|
|
m_ysfN += 1U;
|
|
}
|
|
|
|
void CModeConv::putNXDNEOT()
|
|
{
|
|
unsigned char vch[13U];
|
|
|
|
::memset(vch, 0, 13U);
|
|
|
|
unsigned int fill = 5U - (m_ysfN % 5U);
|
|
for (unsigned int i = 0U; i < fill; i++) {
|
|
m_YSF.addData(&TAG_DATA, 1U);
|
|
m_YSF.addData(YSF_SILENCE, 13U);
|
|
m_ysfN += 1U;
|
|
}
|
|
|
|
m_YSF.addData(&TAG_EOT, 1U);
|
|
m_YSF.addData(vch, 13U);
|
|
m_ysfN += 1U;
|
|
}
|
|
|
|
void CModeConv::putYSFHeader()
|
|
{
|
|
unsigned char v_nxdn[7U];
|
|
|
|
::memset(v_nxdn, 0U, 7U);
|
|
|
|
m_NXDN.addData(&TAG_HEADER, 1U);
|
|
m_NXDN.addData(v_nxdn, 7U);
|
|
m_nxdnN += 1U;
|
|
}
|
|
|
|
void CModeConv::putYSFEOT()
|
|
{
|
|
unsigned char v_nxdn[7U];
|
|
|
|
::memset(v_nxdn, 0U, 7U);
|
|
|
|
unsigned int fill = 4U - (m_nxdnN % 4U);
|
|
for (unsigned int i = 0U; i < fill; i++) {
|
|
m_NXDN.addData(&TAG_DATA, 1U);
|
|
m_NXDN.addData(AMBE_SILENCE, 7U);
|
|
m_nxdnN += 1U;
|
|
}
|
|
|
|
m_NXDN.addData(&TAG_EOT, 1U);
|
|
m_NXDN.addData(v_nxdn, 7U);
|
|
m_nxdnN += 1U;
|
|
}
|
|
|
|
unsigned int CModeConv::getNXDN(unsigned char* data)
|
|
{
|
|
unsigned char tmp[7U];
|
|
unsigned char tag[1U];
|
|
|
|
tag[0U] = TAG_NODATA;
|
|
|
|
if (m_nxdnN >= 1U) {
|
|
m_NXDN.peek(tag, 1U);
|
|
|
|
if (tag[0U] != TAG_DATA) {
|
|
m_NXDN.getData(tag, 1U);
|
|
m_NXDN.getData(data, 7U);
|
|
m_nxdnN -= 1U;
|
|
return tag[0U];
|
|
}
|
|
}
|
|
|
|
if (m_nxdnN >= 4U) {
|
|
data += 5U;
|
|
|
|
m_NXDN.getData(tag, 1U);
|
|
m_NXDN.getData(tmp, 7U);
|
|
for (unsigned int i = 0U; i < 49U; i++) {
|
|
bool s = READ_BIT(tmp, i);
|
|
WRITE_BIT(data, i + 0U, s);
|
|
}
|
|
|
|
m_NXDN.getData(tag, 1U);
|
|
m_NXDN.getData(tmp, 7U);
|
|
for (unsigned int i = 0U; i < 49U; i++) {
|
|
bool s = READ_BIT(tmp, i);
|
|
WRITE_BIT(data, i + 49U, s);
|
|
}
|
|
|
|
m_NXDN.getData(tag, 1U);
|
|
m_NXDN.getData(tmp, 7U);
|
|
for (unsigned int i = 0U; i < 49U; i++) {
|
|
bool s = READ_BIT(tmp, i);
|
|
WRITE_BIT(data, i + 112U, s);
|
|
}
|
|
|
|
m_NXDN.getData(tag, 1U);
|
|
m_NXDN.getData(tmp, 7U);
|
|
for (unsigned int i = 0U; i < 49U; i++) {
|
|
bool s = READ_BIT(tmp, i);
|
|
WRITE_BIT(data, i + 161U, s);
|
|
}
|
|
|
|
m_nxdnN -= 4U;
|
|
|
|
return TAG_DATA;
|
|
}
|
|
else
|
|
return TAG_NODATA;
|
|
}
|
|
|
|
unsigned int CModeConv::getYSF(unsigned char* data)
|
|
{
|
|
unsigned char tag[1U];
|
|
|
|
tag[0U] = TAG_NODATA;
|
|
|
|
data += YSF_SYNC_LENGTH_BYTES + YSF_FICH_LENGTH_BYTES;
|
|
|
|
if (m_ysfN >= 1U) {
|
|
m_YSF.peek(tag, 1U);
|
|
|
|
if (tag[0U] != TAG_DATA) {
|
|
m_YSF.getData(tag, 1U);
|
|
m_YSF.getData(data, 13U);
|
|
m_ysfN -= 1U;
|
|
return tag[0U];
|
|
}
|
|
}
|
|
|
|
if (m_ysfN >= 5U) {
|
|
data += 5U;
|
|
m_YSF.getData(tag, 1U);
|
|
m_YSF.getData(data, 13U);
|
|
m_ysfN -= 1U;
|
|
|
|
data += 18U;
|
|
m_YSF.getData(tag, 1U);
|
|
m_YSF.getData(data, 13U);
|
|
m_ysfN -= 1U;
|
|
|
|
data += 18U;
|
|
m_YSF.getData(tag, 1U);
|
|
m_YSF.getData(data, 13U);
|
|
m_ysfN -= 1U;
|
|
|
|
data += 18U;
|
|
m_YSF.getData(tag, 1U);
|
|
m_YSF.getData(data, 13U);
|
|
m_ysfN -= 1U;
|
|
|
|
data += 18U;
|
|
m_YSF.getData(tag, 1U);
|
|
m_YSF.getData(data, 13U);
|
|
m_ysfN -= 1U;
|
|
|
|
return TAG_DATA;
|
|
}
|
|
else
|
|
return TAG_NODATA;
|
|
}
|