MMDVMHost-Private/P25Audio.cpp

108 lines
3.5 KiB
C++
Raw Normal View History

/*
* Copyright (C) 2016 by Jonathan Naylor G4KLX
*
* 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 "P25Audio.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])
CP25Audio::CP25Audio() :
m_fec()
{
}
CP25Audio::~CP25Audio()
{
}
unsigned int CP25Audio::process(unsigned char* data)
{
assert(data != NULL);
unsigned int errs = 0U;
unsigned char imbe[18U];
read(data, imbe, 114U, 262U, 142U, 143U, 214U, 215U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 114U, 262U, 142U, 143U, 214U, 215U);
read(data, imbe, 262U, 410U, 286U, 287U, 358U, 359U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 262U, 410U, 286U, 287U, 358U, 359U);
read(data, imbe, 452U, 600U, 502U, 503U, 574U, 575U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 452U, 600U, 502U, 503U, 574U, 575U);
read(data, imbe, 640U, 788U, 646U, 647U, 718U, 719U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 640U, 788U, 646U, 647U, 718U, 719U);
read(data, imbe, 830U, 978U, 862U, 863U, 934U, 935U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 830U, 978U, 862U, 863U, 934U, 935U);
read(data, imbe, 1020U, 1168U, 1078U, 1079U, 1150U, 1151U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 1020U, 1168U, 1078U, 1079U, 1150U, 1151U);
read(data, imbe, 1208U, 1356U, 1222U, 1223U, 1294U, 1295U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 1208U, 1356U, 1222U, 1223U, 1294U, 1295U);
read(data, imbe, 1398U, 1546U, 1438U, 1439U, 1510U, 1511U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 1398U, 1546U, 1438U, 1439U, 1510U, 1511U);
read(data, imbe, 1578U, 1726U, 1582U, 1583U, 1654U, 1655U);
errs += m_fec.regenerateIMBE(imbe);
write(data, imbe, 1578U, 1726U, 1582U, 1583U, 1654U, 1655U);
return errs;
}
void CP25Audio::read(const unsigned char* data, unsigned char* out, unsigned int start, unsigned int stop, unsigned int avoid1, unsigned int avoid2, unsigned int avoid3, unsigned int avoid4)
{
unsigned int n = 0U;
for (unsigned int offset = start; offset < stop; offset++) {
if (offset != avoid1 && offset != avoid2 && offset != avoid3 && offset != avoid4) {
bool b = READ_BIT(data, offset);
WRITE_BIT(out, n, b);
n++;
}
}
}
void CP25Audio::write(unsigned char* data, const unsigned char* in, unsigned int start, unsigned int stop, unsigned int avoid1, unsigned int avoid2, unsigned int avoid3, unsigned int avoid4)
{
unsigned int n = 0U;
for (unsigned int offset = start; offset < stop; offset++) {
if (offset != avoid1 && offset != avoid2 && offset != avoid3 && offset != avoid4) {
bool b = READ_BIT(in, n);
WRITE_BIT(data, offset, b);
n++;
}
}
}