ShaYmez
/
MMDVM_CM
mirror of https://github.com/ShaYmez/MMDVM_CM.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
MMDVM_CM/YSF2NXDN/ModeConv.cpp

403 lines
9.4 KiB
C++
Raw Permalink Blame History

/*
* 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;
}
Reference in New Issue View Git Blame Copy Permalink