mirror of https://github.com/ShaYmez/MMDVM_CM.git
140 lines
4.3 KiB
C++
140 lines
4.3 KiB
C++
/*
|
|
Copyright (C) 2010 Perens LLC <bruce@perens.com>
|
|
|
|
All rights reserved.
|
|
|
|
This program is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU Lesser General Public License version 2.1, as
|
|
published by the Free Software Foundation. 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 Lesser General Public License
|
|
along with this program; if not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "defines.h"
|
|
#include "quantise.h"
|
|
#include <stdio.h>
|
|
|
|
/* Compile-time constants */
|
|
/* Size of unsigned char in bits. Assumes 8 bits-per-char. */
|
|
static const unsigned int WordSize = 8;
|
|
|
|
/* Mask to pick the bit component out of bitIndex. */
|
|
static const unsigned int IndexMask = 0x7;
|
|
|
|
/* Used to pick the word component out of bitIndex. */
|
|
static const unsigned int ShiftRight = 3;
|
|
|
|
/** Pack a bit field into a bit string, encoding the field in Gray code.
|
|
*
|
|
* The output is an array of unsigned char data. The fields are efficiently
|
|
* packed into the bit string. The Gray coding is a naive attempt to reduce
|
|
* the effect of single-bit errors, we expect to do a better job as the
|
|
* codec develops.
|
|
*
|
|
* This code would be simpler if it just set one bit at a time in the string,
|
|
* but would hit the same cache line more often. I'm not sure the complexity
|
|
* gains us anything here.
|
|
*
|
|
* Although field is currently of int type rather than unsigned for
|
|
* compatibility with the rest of the code, indices are always expected to
|
|
* be >= 0.
|
|
*/
|
|
void CQuantize::pack(
|
|
unsigned char *bitArray, /* The output bit string. */
|
|
unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/
|
|
int field, /* The bit field to be packed. */
|
|
unsigned int fieldWidth /* Width of the field in BITS, not bytes. */
|
|
)
|
|
{
|
|
pack_natural_or_gray(bitArray, bitIndex, field, fieldWidth, 1);
|
|
}
|
|
|
|
void CQuantize::pack_natural_or_gray(
|
|
unsigned char *bitArray, /* The output bit string. */
|
|
unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/
|
|
int field, /* The bit field to be packed. */
|
|
unsigned int fieldWidth, /* Width of the field in BITS, not bytes. */
|
|
unsigned int gray /* non-zero for gray coding */
|
|
)
|
|
{
|
|
if (gray)
|
|
{
|
|
/* Convert the field to Gray code */
|
|
field = (field >> 1) ^ field;
|
|
}
|
|
|
|
do
|
|
{
|
|
unsigned int bI = *bitIndex;
|
|
unsigned int bitsLeft = WordSize - (bI & IndexMask);
|
|
unsigned int sliceWidth = bitsLeft < fieldWidth ? bitsLeft : fieldWidth;
|
|
unsigned int wordIndex = bI >> ShiftRight;
|
|
|
|
bitArray[wordIndex] |= ((unsigned char)((field >> (fieldWidth - sliceWidth)) << (bitsLeft - sliceWidth)));
|
|
|
|
*bitIndex = bI + sliceWidth;
|
|
fieldWidth -= sliceWidth;
|
|
}
|
|
while ( fieldWidth != 0 );
|
|
}
|
|
|
|
/** Unpack a field from a bit string, converting from Gray code to binary.
|
|
*
|
|
*/
|
|
int CQuantize::unpack(
|
|
const unsigned char *bitArray, /* The input bit string. */
|
|
unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/
|
|
unsigned int fieldWidth/* Width of the field in BITS, not bytes. */
|
|
)
|
|
{
|
|
return unpack_natural_or_gray(bitArray, bitIndex, fieldWidth, 1);
|
|
}
|
|
|
|
/** Unpack a field from a bit string, to binary, optionally using
|
|
* natural or Gray code.
|
|
*
|
|
*/
|
|
int CQuantize::unpack_natural_or_gray(
|
|
const unsigned char *bitArray, /* The input bit string. */
|
|
unsigned int *bitIndex, /* Index into the string in BITS, not bytes.*/
|
|
unsigned int fieldWidth,/* Width of the field in BITS, not bytes. */
|
|
unsigned int gray /* non-zero for Gray coding */
|
|
)
|
|
{
|
|
unsigned int field = 0;
|
|
unsigned int t;
|
|
|
|
do
|
|
{
|
|
unsigned int bI = *bitIndex;
|
|
unsigned int bitsLeft = WordSize - (bI & IndexMask);
|
|
unsigned int sliceWidth = bitsLeft < fieldWidth ? bitsLeft : fieldWidth;
|
|
|
|
field |= (((bitArray[bI >> ShiftRight] >> (bitsLeft - sliceWidth)) & ((1 << sliceWidth) - 1)) << (fieldWidth - sliceWidth));
|
|
|
|
*bitIndex = bI + sliceWidth;
|
|
fieldWidth -= sliceWidth;
|
|
}
|
|
while ( fieldWidth != 0 );
|
|
|
|
if (gray)
|
|
{
|
|
/* Convert from Gray code to binary. Works for maximum 8-bit fields. */
|
|
t = field ^ (field >> 8);
|
|
t ^= (t >> 4);
|
|
t ^= (t >> 2);
|
|
t ^= (t >> 1);
|
|
}
|
|
else
|
|
{
|
|
t = field;
|
|
}
|
|
|
|
return t;
|
|
}
|