UHSDR/UHSDR-active-devel/mchf-eclipse/drivers/freedv/pack.c
2022-08-24 08:39:13 +02:00

141 lines
4.3 KiB
C
Executable File

/*
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
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
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
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
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;
}