108 lines
3.5 KiB
C++
108 lines
3.5 KiB
C++
|
/*
|
||
|
* 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++;
|
||
|
}
|
||
|
}
|
||
|
}
|