mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-07-23 04:12:56 +00:00
Code Issues Releases Wiki Activity

Adapt to gnss-sdr coding style

Browse Source
This commit is contained in:
Carles Fernandez 2017-03-28 19:32:42 +02:00
parent fa4eb25920
commit cff63b378a
6 changed files with 561 additions and 504 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

175
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c
View File

@ -52,11 +52,11 @@ static const u8 bitn[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4};
*/ */
u8 parity(u32 x) u8 parity(u32 x)
{ {
x ^= x >> 16; x ^= x >> 16;
x ^= x >> 8; x ^= x >> 8;
x ^= x >> 4; x ^= x >> 4;
x &= 0xF; x &= 0xF;
return (0x6996 >> x) & 1; return (0x6996 >> x) & 1;
} }
@ -72,11 +72,12 @@ u8 parity(u32 x)
u32 getbitu(const u8 *buff, u32 pos, u8 len) u32 getbitu(const u8 *buff, u32 pos, u8 len)
{ {
u32 bits = 0; u32 bits = 0;
u32 i=0; u32 i=0;
for ( i= pos; i < pos + len; i++) { for (i = pos; i < pos + len; i++)
bits = (bits << 1) + {
((buff[i/8] >> (7 - i%8)) & 1u); bits = (bits << 1) +
} ((buff[i/8] >> (7 - i%8)) & 1u);
}
return bits; return bits;
} }
@ -114,17 +115,18 @@ s32 getbits(const u8 *buff, u32 pos, u8 len)
*/ */
void setbitu(u8 *buff, u32 pos, u32 len, u32 data) void setbitu(u8 *buff, u32 pos, u32 len, u32 data)
{ {
u32 mask = 1u << (len - 1); u32 mask = 1u << (len - 1);
if (len <= 0 || 32 < len) if (len <= 0 || 32 < len)
return; return;
u32 i=0; u32 i = 0;
for (i = pos; i < pos + len; i++, mask >>= 1) { for (i = pos; i < pos + len; i++, mask >>= 1)
if (data & mask) {
buff[i/8] |= 1u << (7 - i % 8); if (data & mask)
else buff[i/8] |= 1u << (7 - i % 8);
buff[i/8] &= ~(1u << (7 - i % 8)); else
} buff[i/8] &= ~(1u << (7 - i % 8));
}
} }
/** Set bit field in buffer from a signed integer. /** Set bit field in buffer from a signed integer.
@ -138,7 +140,7 @@ void setbitu(u8 *buff, u32 pos, u32 len, u32 data)
*/ */
void setbits(u8 *buff, u32 pos, u32 len, s32 data) void setbits(u8 *buff, u32 pos, u32 len, s32 data)
{ {
setbitu(buff, pos, len, (u32)data); setbitu(buff, pos, len, (u32)data);
} }
/** /**
@ -153,38 +155,43 @@ void setbits(u8 *buff, u32 pos, u32 len, s32 data)
*/ */
void bitshl(void *buf, u32 size, u32 shift) void bitshl(void *buf, u32 size, u32 shift)
{ {
if (shift > size * CHAR_BIT) { if (shift > size * CHAR_BIT)
/* Quick check: if the shift is larger, than the buffer, zero the data */ {
memset(buf, 0, size); /* Quick check: if the shift is larger, than the buffer, zero the data */
return; memset(buf, 0, size);
} return;
}
unsigned char *dst = buf; /* Destination byte. */ unsigned char *dst = buf; /* Destination byte. */
const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, possibly incomplete. */
* possibly incomplete. */
u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */ u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */
u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */ u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */
u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */ u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */
if (0 == byte_shift) { if (0 == byte_shift)
/* When moving data in character boundaries, use built-in functions: move {
* data, and then zero the tail. */ /* When moving data in character boundaries, use built-in functions: move
memmove(dst, src, full_bytes); * data, and then zero the tail. */
memset(dst + full_bytes, 0, size - full_bytes); memmove(dst, src, full_bytes);
} else { memset(dst + full_bytes, 0, size - full_bytes);
/* Create an accumulator: it will hold a value of two consecutive bytes */ }
u32 acc = *src++; else
u32 i=0; {
for (i = 0; i < full_bytes; ++i) { /* Create an accumulator: it will hold a value of two consecutive bytes */
acc = (acc << CHAR_BIT) | *src++; u32 acc = *src++;
*dst++ = acc >> byte_shift; u32 i = 0;
} for (i = 0; i < full_bytes; ++i)
*dst++ = acc << CHAR_BIT >> byte_shift; {
if (full_bytes + 1 < size) { acc = (acc << CHAR_BIT) | *src++;
memset(dst, 0, size - full_bytes - 1); *dst++ = acc >> byte_shift;
} }
} *dst++ = acc << CHAR_BIT >> byte_shift;
if (full_bytes + 1 < size)
{
memset(dst, 0, size - full_bytes - 1);
}
}
} }
/** /**
@ -206,19 +213,21 @@ void bitshl(void *buf, u32 size, u32 shift)
void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index, void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index,
u32 count) u32 count)
{ {
u32 limit1 = count / 32; u32 limit1 = count / 32;
u32 limit2 = count % 32; u32 limit2 = count % 32;
u32 idx=0; u32 idx = 0;
for (idx = 0; idx < limit1; ++idx) { for (idx = 0; idx < limit1; ++idx)
u32 tmp = getbitu(src, src_index, 32); {
setbitu(dst, dst_index, 32, tmp); u32 tmp = getbitu(src, src_index, 32);
src_index += 32; setbitu(dst, dst_index, 32, tmp);
dst_index += 32; src_index += 32;
} dst_index += 32;
if (0 != limit2) { }
u32 tmp = getbitu(src, src_index, limit2); if (0 != limit2)
setbitu(dst, dst_index, limit2, tmp); {
} u32 tmp = getbitu(src, src_index, limit2);
setbitu(dst, dst_index, limit2, tmp);
}
} }
/** /**
@ -231,11 +240,11 @@ void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index,
*/ */
u8 count_bits_u64(u64 v, u8 bv) u8 count_bits_u64(u64 v, u8 bv)
{ {
u8 r = 0; u8 r = 0;
int i =0; int i = 0;
for (i = 0; i < 16; i++) for (i = 0; i < 16; i++)
r += bitn[(v >> (i*4)) & 0xf]; r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 64 - r; return bv == 1 ? r : 64 - r;
} }
/** /**
@ -248,11 +257,11 @@ int i =0;
*/ */
u8 count_bits_u32(u32 v, u8 bv) u8 count_bits_u32(u32 v, u8 bv)
{ {
u8 r = 0; u8 r = 0;
int i=0; int i = 0;
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
r += bitn[(v >> (i*4)) & 0xf]; r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 32 - r; return bv == 1 ? r : 32 - r;
} }
/** /**
@ -265,11 +274,11 @@ u8 count_bits_u32(u32 v, u8 bv)
*/ */
u8 count_bits_u16(u16 v, u8 bv) u8 count_bits_u16(u16 v, u8 bv)
{ {
u8 r = 0; u8 r = 0;
int i =0; int i = 0;
for (i= 0; i < 4; i++) for (i= 0; i < 4; i++)
r += bitn[(v >> (i*4)) & 0xf]; r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 16 - r; return bv == 1 ? r : 16 - r;
} }
/** /**
@ -282,11 +291,11 @@ int i =0;
*/ */
u8 count_bits_u8(u8 v, u8 bv) u8 count_bits_u8(u8 v, u8 bv)
{ {
u8 r = 0; u8 r = 0;
int i=0; int i = 0;
for (i = 0; i < 2; i++) for (i = 0; i < 2; i++)
r += bitn[(v >> (i*4)) & 0xf]; r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 8 - r; return bv == 1 ? r : 8 - r;
} }
/** \} */ /** \} */

457
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c
View File

@ -84,12 +84,13 @@
*/ */
static u32 _cnav_compute_crc(cnav_v27_part_t *part) static u32 _cnav_compute_crc(cnav_v27_part_t *part)
{ {
u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH, u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
part->invert); part->invert);
return crc; return crc;
} }
/** /**
* Extracts CRC-24Q from a CNAV message buffer. * Extracts CRC-24Q from a CNAV message buffer.
* CRC-24Q value is the last 24 bits from 300 bits message buffer. * CRC-24Q value is the last 24 bits from 300 bits message buffer.
@ -102,14 +103,16 @@ static u32 _cnav_compute_crc(cnav_v27_part_t *part)
*/ */
static u32 _cnav_extract_crc(const cnav_v27_part_t *part) static u32 _cnav_extract_crc(const cnav_v27_part_t *part)
{ {
u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH, u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
GPS_CNAV_MSG_CRC_LENGTH); GPS_CNAV_MSG_CRC_LENGTH);
if (part->invert) { if (part->invert)
crc ^= 0xFFFFFF; {
} crc ^= 0xFFFFFF;
return crc; }
return crc;
} }
/** /**
* Helper to rescan for preamble in the received buffer. * Helper to rescan for preamble in the received buffer.
* Occasionally there could be a false lock on message contents if it the * Occasionally there could be a false lock on message contents if it the
@ -126,31 +129,35 @@ static u32 _cnav_extract_crc(const cnav_v27_part_t *part)
*/ */
static void _cnav_rescan_preamble(cnav_v27_part_t *part) static void _cnav_rescan_preamble(cnav_v27_part_t *part)
{ {
part->preamble_seen = false; part->preamble_seen = false;
if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) { if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1)
size_t i=0; {
size_t j=0; size_t i = 0;
for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; size_t j = 0;
i < j; ++i) { for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; i < j; ++i)
u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH); {
if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c) { u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH);
part->preamble_seen = true; if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c)
part->invert = (GPS_CNAV_PREAMBLE2 == c); {
/* We shift the accumulated bits to the beginning of the buffer */ part->preamble_seen = true;
bitshl(part->decoded, sizeof(part->decoded), i); part->invert = (GPS_CNAV_PREAMBLE2 == c);
part->n_decoded -= i; /* We shift the accumulated bits to the beginning of the buffer */
break; bitshl(part->decoded, sizeof(part->decoded), i);
} part->n_decoded -= i;
} break;
} }
if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH) { }
bitshl(part->decoded, sizeof(part->decoded), }
part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1); if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH)
part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1; {
} bitshl(part->decoded, sizeof(part->decoded),
part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1);
part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1;
}
} }
/** /**
* Feed a symbol into Viterbi decoder instance. * Feed a symbol into Viterbi decoder instance.
* *
@ -167,110 +174,126 @@ static void _cnav_rescan_preamble(cnav_v27_part_t *part)
*/ */
static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch) static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
{ {
part->symbols[part->n_symbols++] = ch; part->symbols[part->n_symbols++] = ch;
if (part->init) { if (part->init)
/* Initial step - load more symbols without decoding. */ {
if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2) { /* Initial step - load more symbols without decoding. */
return; if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2)
} {
part->init = false; return;
} }
else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2) { part->init = false;
/* Wait until decoding block is accumulated */ }
return; else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2)
} {
/* Wait until decoding block is accumulated */
/* Feed accumulated symbols into the buffer, reset the number of accumulated return;
* symbols. */
v27_update(&part->dec, part->symbols, part->n_symbols / 2);
part->n_symbols = 0;
/* Decode N+M bits, where:
* - N - Number of bits to put into decoded buffer
* - M - Number of bits in the tail to ignore.
*/
unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
CHAR_BIT - 1) / CHAR_BIT];
v27_chainback_likely(&part->dec, tmp_bits,
GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
/* Read decoded bits and add them to the decoded buffer */
bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS);
part->n_decoded += GPS_L2C_V27_DECODE_BITS;
/* Depending on the decoder state, one of the following actions are
* possible:
* - If no message lock
* - If no preamble seen - look for preamble
* - If preamble seen - collect 300 bits
* - If 300 bits are collected - verify CRC
* - If CRC is OK - message lock is acquired
* - If CRC fails - rescan for preamble
* - If found - continue collecting 300 bits
* - If not found - continue preamble wait
* - If message lock
* - If 300 bits collected, compute CRC
* - If CRC is OK, message can be decoded
* - If CRC is not OK, discard data
*/
bool retry = true;
while (retry) {
retry = false;
if (!part->preamble_seen) {
/* Rescan for preamble if possible. The first bit is ignored. */
_cnav_rescan_preamble(part);
}
if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded) {
/* We have collected 300 bits starting from message preamble. Now try
* to compute CRC-24Q */
u32 crc = _cnav_compute_crc(part);
u32 crc2 = _cnav_extract_crc(part);
if (part->message_lock) {
/* We have message lock */
part->crc_ok = (crc == crc2);
if (part->crc_ok) {
/* Reset message lock counter */
part->n_crc_fail = 0;
} else {
/* Increment message lock counter */
part->n_crc_fail++;
if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS) {
/* CRC has failed too many times - drop the lock. */
part->n_crc_fail = 0;
part->message_lock = false;
part->preamble_seen = false;
/* Try to find a new preamble, reuse data from buffer. */
retry = true;
}
} }
} else if (crc == crc2) {
/* CRC match - message can be decoded */
part->message_lock = true;
part->crc_ok = true;
part->n_crc_fail = 0;
} else {
/* There is no message lock and the CRC check fails. Assume there is
* false positive lock - rescan for preamble. */
part->crc_ok = false;
part->preamble_seen = false;
/* CRC mismatch - try to re-scan for preamble */ /* Feed accumulated symbols into the buffer, reset the number of accumulated
retry = true; * symbols. */
} v27_update(&part->dec, part->symbols, part->n_symbols / 2);
} part->n_symbols = 0;
else
{ /* Decode N+M bits, where:
/* No preamble or preamble and less than 300 bits decoded */ * - N - Number of bits to put into decoded buffer
} * - M - Number of bits in the tail to ignore.
} */
unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
CHAR_BIT - 1) / CHAR_BIT];
v27_chainback_likely(&part->dec, tmp_bits,
GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
/* Read decoded bits and add them to the decoded buffer */
bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS);
part->n_decoded += GPS_L2C_V27_DECODE_BITS;
/* Depending on the decoder state, one of the following actions are
* possible:
* - If no message lock
* - If no preamble seen - look for preamble
* - If preamble seen - collect 300 bits
* - If 300 bits are collected - verify CRC
* - If CRC is OK - message lock is acquired
* - If CRC fails - rescan for preamble
* - If found - continue collecting 300 bits
* - If not found - continue preamble wait
* - If message lock
* - If 300 bits collected, compute CRC
* - If CRC is OK, message can be decoded
* - If CRC is not OK, discard data
*/
bool retry = true;
while (retry)
{
retry = false;
if (!part->preamble_seen)
{
/* Rescan for preamble if possible. The first bit is ignored. */
_cnav_rescan_preamble(part);
}
if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded)
{
/* We have collected 300 bits starting from message preamble. Now try
* to compute CRC-24Q */
u32 crc = _cnav_compute_crc(part);
u32 crc2 = _cnav_extract_crc(part);
if (part->message_lock)
{
/* We have message lock */
part->crc_ok = (crc == crc2);
if (part->crc_ok)
{
/* Reset message lock counter */
part->n_crc_fail = 0;
}
else
{
/* Increment message lock counter */
part->n_crc_fail++;
if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS)
{
/* CRC has failed too many times - drop the lock. */
part->n_crc_fail = 0;
part->message_lock = false;
part->preamble_seen = false;
/* Try to find a new preamble, reuse data from buffer. */
retry = true;
}
}
}
else if (crc == crc2)
{
/* CRC match - message can be decoded */
part->message_lock = true;
part->crc_ok = true;
part->n_crc_fail = 0;
}
else
{
/* There is no message lock and the CRC check fails. Assume there is
* false positive lock - rescan for preamble. */
part->crc_ok = false;
part->preamble_seen = false;
/* CRC mismatch - try to re-scan for preamble */
retry = true;
}
}
else
{
/* No preamble or preamble and less than 300 bits decoded */
}
}
} }
/** /**
* Invert message bits in the buffer. * Invert message bits in the buffer.
* *
@ -282,12 +305,14 @@ static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
*/ */
static void _cnav_msg_invert(cnav_v27_part_t *part) static void _cnav_msg_invert(cnav_v27_part_t *part)
{ {
size_t i = 0; size_t i = 0;
for (i = 0; i < sizeof(part->decoded); i++) { for (i = 0; i < sizeof(part->decoded); i++)
part->decoded[i] ^= 0xFFu; {
} part->decoded[i] ^= 0xFFu;
}
} }
/** /**
* Performs CNAV message decoding. * Performs CNAV message decoding.
* *
@ -310,39 +335,45 @@ static void _cnav_msg_invert(cnav_v27_part_t *part)
*/ */
static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay) static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay)
{ {
bool res = false; bool res = false;
if (GPS_CNAV_MSG_LENGTH <= part->n_decoded) { if (GPS_CNAV_MSG_LENGTH <= part->n_decoded)
if (part->crc_ok) { {
/* CRC is OK */ if (part->crc_ok)
if (part->invert) { {
_cnav_msg_invert(part); /* CRC is OK */
} if (part->invert)
{
_cnav_msg_invert(part);
}
msg->prn = getbitu(part->decoded, 8, 6); msg->prn = getbitu(part->decoded, 8, 6);
msg->msg_id = getbitu(part->decoded, 14, 6); msg->msg_id = getbitu(part->decoded, 14, 6);
msg->tow = getbitu(part->decoded, 20, 17); msg->tow = getbitu(part->decoded, 20, 17);
msg->alert = getbitu(part->decoded, 37, 1) ? true : false; msg->alert = getbitu(part->decoded, 37, 1) ? true : false;
/* copy RAW message for GNSS-SDR */ /* copy RAW message for GNSS-SDR */
memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
*delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols;
* 2 + part->n_symbols;
if (part->invert) { if (part->invert)
_cnav_msg_invert(part); {
} _cnav_msg_invert(part);
res = true; }
} else { res = true;
/* CRC mismatch - no decoding */ }
} else
bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH); {
part->n_decoded -= GPS_CNAV_MSG_LENGTH; /* CRC mismatch - no decoding */
} }
bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH);
part->n_decoded -= GPS_CNAV_MSG_LENGTH;
}
return res; return res;
} }
/** /**
* Initialize CNAV decoder. * Initialize CNAV decoder.
* *
@ -355,20 +386,20 @@ static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay)
*/ */
void cnav_msg_decoder_init(cnav_msg_decoder_t *dec) void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
{ {
memset(dec, 0, sizeof(*dec)); memset(dec, 0, sizeof(*dec));
v27_init(&dec->part1.dec, v27_init(&dec->part1.dec,
dec->part1.decisions, dec->part1.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS, GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(), cnav_msg_decoder_get_poly(),
0); 0);
v27_init(&dec->part2.dec, v27_init(&dec->part2.dec,
dec->part2.decisions, dec->part2.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS, GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(), cnav_msg_decoder_get_poly(),
0); 0);
dec->part1.init = true; dec->part1.init = true;
dec->part2.init = true; dec->part2.init = true;
_cnav_add_symbol(&dec->part2, 0x80); _cnav_add_symbol(&dec->part2, 0x80);
} }
/** /**
@ -394,29 +425,32 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
* \retval false More data is required. * \retval false More data is required.
*/ */
bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec, bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
u8 symbol, u8 symbol,
cnav_msg_t *msg, cnav_msg_t *msg,
u32 *pdelay) u32 *pdelay)
{ {
_cnav_add_symbol(&dec->part1, symbol); _cnav_add_symbol(&dec->part1, symbol);
_cnav_add_symbol(&dec->part2, symbol); _cnav_add_symbol(&dec->part2, symbol);
if (dec->part1.message_lock) { if (dec->part1.message_lock)
/* Flush data in decoder. */ {
dec->part2.n_decoded = 0; /* Flush data in decoder. */
dec->part2.n_symbols = 0; dec->part2.n_decoded = 0;
return _cnav_msg_decode(&dec->part1, msg, pdelay); dec->part2.n_symbols = 0;
} return _cnav_msg_decode(&dec->part1, msg, pdelay);
if (dec->part2.message_lock) { }
/* Flush data in decoder. */ if (dec->part2.message_lock)
dec->part1.n_decoded = 0; {
dec->part1.n_symbols = 0; /* Flush data in decoder. */
return _cnav_msg_decode(&dec->part2, msg, pdelay); dec->part1.n_decoded = 0;
} dec->part1.n_symbols = 0;
return _cnav_msg_decode(&dec->part2, msg, pdelay);
}
return false; return false;
} }
/** /**
* Provides a singleton polynomial object. * Provides a singleton polynomial object.
* *
@ -429,27 +463,28 @@ bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
*/ */
const v27_poly_t *cnav_msg_decoder_get_poly(void) const v27_poly_t *cnav_msg_decoder_get_poly(void)
{ {
static v27_poly_t instance; static v27_poly_t instance;
static bool initialized = false; static bool initialized = false;
if (!initialized) { if (!initialized)
/* Coefficients for polynomial object */ {
const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B }; /* Coefficients for polynomial object */
const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B };
/* Racing condition handling: the data can be potential initialized more /* Racing condition handling: the data can be potential initialized more
* than once in case multiple threads request concurrent access. However, * than once in case multiple threads request concurrent access. However,
* nature of the v27_poly_init() function and data alignment ensure that * nature of the v27_poly_init() function and data alignment ensure that
* the data returned from the earlier finished call is consistent and can * the data returned from the earlier finished call is consistent and can
* be used even when re-initialization is happening. * be used even when re-initialization is happening.
* *
* Other possible approaches are: * Other possible approaches are:
* - Replace late initialization with an explicit call. * - Replace late initialization with an explicit call.
* - Use POSIX synchronization objects like pthread_once_t. * - Use POSIX synchronization objects like pthread_once_t.
*/ */
v27_poly_init(&instance, coeffs); v27_poly_init(&instance, coeffs);
initialized = true; initialized = true;
} }
return &instance; return &instance;
} }
/** \} */ /** \} */

50
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h
View File

@ -62,11 +62,11 @@
*/ */
typedef struct typedef struct
{ {
u8 prn; /**< SV PRN. 0..31 */ u8 prn; /**< SV PRN. 0..31 */
u8 msg_id; /**< Message id. 0..31 */ u8 msg_id; /**< Message id. 0..31 */
u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */
bool alert; /**< CNAV message alert flag */ bool alert; /**< CNAV message alert flag */
u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */
} cnav_msg_t; } cnav_msg_t;
/** /**
@ -76,24 +76,24 @@ typedef struct
* @sa cnav_msg_decoder_t * @sa cnav_msg_decoder_t
*/ */
typedef struct { typedef struct {
v27_t dec; /**< Viterbi block decoder object */ v27_t dec; /**< Viterbi block decoder object */
v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS]; v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS];
/**< Decision graph */ /**< Decision graph */
unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2]; unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2];
/**< Symbol buffer */ /**< Symbol buffer */
size_t n_symbols; /**< Count of symbols in the symbol buffer */ size_t n_symbols; /**< Count of symbols in the symbol buffer */
unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS];
/**< Decode buffer */ /**< Decode buffer */
size_t n_decoded; /**< Number of bits in the decode buffer */ size_t n_decoded; /**< Number of bits in the decode buffer */
bool preamble_seen; /**< When true, the decode buffer is aligned on bool preamble_seen; /**< When true, the decode buffer is aligned on
* preamble. */ * preamble. */
bool invert; /**< When true, indicates the bits are inverted */ bool invert; /**< When true, indicates the bits are inverted */
bool message_lock; /**< When true, indicates the message boundary bool message_lock; /**< When true, indicates the message boundary
* is found. */ * is found. */
bool crc_ok; /**< Flag that the last message had good CRC */ bool crc_ok; /**< Flag that the last message had good CRC */
size_t n_crc_fail; /**< Counter for CRC failures */ size_t n_crc_fail; /**< Counter for CRC failures */
bool init; /**< Initial state flag. When true, initial bits bool init; /**< Initial state flag. When true, initial bits
* do not produce output. */ * do not produce output. */
} cnav_v27_part_t; } cnav_v27_part_t;
/** /**
@ -104,8 +104,8 @@ typedef struct {
*/ */
typedef struct typedef struct
{ {
cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */ cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */
cnav_v27_part_t part2; /**< Decoder for even symbol pairs */ cnav_v27_part_t part2; /**< Decoder for even symbol pairs */
} cnav_msg_decoder_t; } cnav_msg_decoder_t;
const v27_poly_t *cnav_msg_decoder_get_poly(void); const v27_poly_t *cnav_msg_decoder_get_poly(void);

109
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c
View File

@ -40,38 +40,38 @@
* \{ */ * \{ */
static const u32 crc24qtab[256] = { static const u32 crc24qtab[256] = {
0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E,
0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E,
0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7,
0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE,
0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077,
0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E,
0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5,
0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC,
0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375,
0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C,
0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15,
0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5,
0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3,
0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A,
0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A,
0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703,
0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3,
0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA,
0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61,
0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8,
0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1,
0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
}; };
/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q). /** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q).
@ -91,10 +91,10 @@ static const u32 crc24qtab[256] = {
*/ */
u32 crc24q(const u8 *buf, u32 len, u32 crc) u32 crc24q(const u8 *buf, u32 len, u32 crc)
{ {
u32 i=0; u32 i = 0;
for ( i= 0; i < len; i++) for (i = 0; i < len; i++)
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff];
return crc; return crc;
} }
/** /**
@ -114,27 +114,30 @@ u32 crc24q(const u8 *buf, u32 len, u32 crc)
*/ */
u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert) u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert)
{ {
u16 acc = 0; u16 acc = 0;
u8 b = 0; u8 b = 0;
u32 shift = 8 - n_bits % 8; u32 shift = 8 - n_bits % 8;
u32 i=0; u32 i = 0;
for ( i= 0; i < n_bits / 8; ++i) { for (i = 0; i < n_bits / 8; ++i)
acc = (acc << 8) | *buf++; {
if (invert) { acc = (acc << 8) | *buf++;
acc ^= 0xFFu; if (invert)
} {
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
}
acc = (acc << 8) | *buf;
if (invert)
{
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu; b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu]; crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
}
acc = (acc << 8) | *buf;
if (invert) {
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
return crc; return crc;
} }

22
src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h
View File

@ -40,25 +40,25 @@
#define V27POLYB 0x6d #define V27POLYB 0x6d
typedef struct { typedef struct {
unsigned char c0[32]; unsigned char c0[32];
unsigned char c1[32]; unsigned char c1[32];
} v27_poly_t; } v27_poly_t;
typedef struct { typedef struct {
unsigned int w[2]; unsigned int w[2];
} v27_decision_t; } v27_decision_t;
/* State info for instance of r=1/2 k=7 Viterbi decoder /* State info for instance of r=1/2 k=7 Viterbi decoder
*/ */
typedef struct { typedef struct {
unsigned int metrics1[64]; /* Path metric buffer 1 */ unsigned int metrics1[64]; /* Path metric buffer 1 */
unsigned int metrics2[64]; /* Path metric buffer 2 */ unsigned int metrics2[64]; /* Path metric buffer 2 */
/* Pointers to path metrics, swapped on every bit */ /* Pointers to path metrics, swapped on every bit */
unsigned int *old_metrics, *new_metrics; unsigned int *old_metrics, *new_metrics;
const v27_poly_t *poly; /* Polynomial to use */ const v27_poly_t *poly; /* Polynomial to use */
v27_decision_t *decisions; /* Beginning of decisions for block */ v27_decision_t *decisions; /* Beginning of decisions for block */
unsigned int decisions_index; /* Index of current decision */ unsigned int decisions_index; /* Index of current decision */
unsigned int decisions_count; /* Number of decisions in history */ unsigned int decisions_count; /* Number of decisions in history */
} v27_t; } v27_t;
void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]); void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]);

252
src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c
View File

@ -35,13 +35,14 @@
static inline int parity(int x) static inline int parity(int x)
{ {
x ^= x >> 16; x ^= x >> 16;
x ^= x >> 8; x ^= x >> 8;
x ^= x >> 4; x ^= x >> 4;
x &= 0xf; x &= 0xf;
return (0x6996 >> x) & 1; return (0x6996 >> x) & 1;
} }
/** Initialize a v27_poly_t struct for use with a v27_t decoder. /** Initialize a v27_poly_t struct for use with a v27_t decoder.
* *
* \param poly Structure to initialize. * \param poly Structure to initialize.
@ -49,14 +50,16 @@ static inline int parity(int x)
*/ */
void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]) void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2])
{ {
int state; int state;
for(state = 0; state < 32; state++) { for(state = 0; state < 32; state++)
poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0; {
poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0; poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0;
} poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0;
}
} }
/** Initialize a v27_t struct for Viterbi decoding. /** Initialize a v27_t struct for Viterbi decoding.
* *
* \param v Structure to initialize * \param v Structure to initialize
@ -71,35 +74,36 @@ void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2])
void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count, void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count,
const v27_poly_t *poly, unsigned char initial_state) const v27_poly_t *poly, unsigned char initial_state)
{ {
int i; int i;
v->old_metrics = v->metrics1; v->old_metrics = v->metrics1;
v->new_metrics = v->metrics2; v->new_metrics = v->metrics2;
v->poly = poly; v->poly = poly;
v->decisions = decisions; v->decisions = decisions;
v->decisions_index = 0; v->decisions_index = 0;
v->decisions_count = decisions_count; v->decisions_count = decisions_count;
for(i = 0; i < 64; i++) for(i = 0; i < 64; i++)
v->old_metrics[i] = 63; v->old_metrics[i] = 63;
v->old_metrics[initial_state & 63] = 0; /* Bias known start state */ v->old_metrics[initial_state & 63] = 0; /* Bias known start state */
} }
/* C-language butterfly */ /* C-language butterfly */
#define BFLY(i) {\ #define BFLY(i) {\
unsigned int metric,m0,m1,decision;\ unsigned int metric,m0,m1,decision;\
metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\ metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\
m0 = v->old_metrics[i] + metric;\ m0 = v->old_metrics[i] + metric;\
m1 = v->old_metrics[i+32] + (510 - metric);\ m1 = v->old_metrics[i+32] + (510 - metric);\
decision = (signed int)(m0-m1) > 0;\ decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i] = decision ? m1 : m0;\ v->new_metrics[2*i] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i)&31);\ d->w[i/16] |= decision << ((2*i)&31);\
m0 -= (metric+metric-510);\ m0 -= (metric+metric-510);\
m1 += (metric+metric-510);\ m1 += (metric+metric-510);\
decision = (signed int)(m0-m1) > 0;\ decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i+1] = decision ? m1 : m0;\ v->new_metrics[2*i+1] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i+1)&31);\ d->w[i/16] |= decision << ((2*i+1)&31);\
} }
/** Update a v27_t decoder with a block of symbols. /** Update a v27_t decoder with a block of symbols.
@ -111,77 +115,81 @@ unsigned int metric,m0,m1,decision;\
*/ */
void v27_update(v27_t *v, const unsigned char *syms, int nbits) void v27_update(v27_t *v, const unsigned char *syms, int nbits)
{ {
unsigned char sym0, sym1; unsigned char sym0, sym1;
unsigned int *tmp; unsigned int *tmp;
int normalize = 0; int normalize = 0;
while(nbits--) { while(nbits--)
v27_decision_t *d = &v->decisions[v->decisions_index]; {
v27_decision_t *d = &v->decisions[v->decisions_index];
d->w[0] = d->w[1] = 0; d->w[0] = d->w[1] = 0;
sym0 = *syms++; sym0 = *syms++;
sym1 = *syms++; sym1 = *syms++;
BFLY(0); BFLY(0);
BFLY(1); BFLY(1);
BFLY(2); BFLY(2);
BFLY(3); BFLY(3);
BFLY(4); BFLY(4);
BFLY(5); BFLY(5);
BFLY(6); BFLY(6);
BFLY(7); BFLY(7);
BFLY(8); BFLY(8);
BFLY(9); BFLY(9);
BFLY(10); BFLY(10);
BFLY(11); BFLY(11);
BFLY(12); BFLY(12);
BFLY(13); BFLY(13);
BFLY(14); BFLY(14);
BFLY(15); BFLY(15);
BFLY(16); BFLY(16);
BFLY(17); BFLY(17);
BFLY(18); BFLY(18);
BFLY(19); BFLY(19);
BFLY(20); BFLY(20);
BFLY(21); BFLY(21);
BFLY(22); BFLY(22);
BFLY(23); BFLY(23);
BFLY(24); BFLY(24);
BFLY(25); BFLY(25);
BFLY(26); BFLY(26);
BFLY(27); BFLY(27);
BFLY(28); BFLY(28);
BFLY(29); BFLY(29);
BFLY(30); BFLY(30);
BFLY(31); BFLY(31);
/* Normalize metrics if they are nearing overflow */ /* Normalize metrics if they are nearing overflow */
if(v->new_metrics[0] > (1<<30)) { if(v->new_metrics[0] > (1 << 30))
int i; {
unsigned int minmetric = 1<<31; int i;
unsigned int minmetric = 1 << 31;
for(i=0; i<64; i++) { for(i = 0; i < 64; i++)
if(v->new_metrics[i] < minmetric) {
minmetric = v->new_metrics[i]; if(v->new_metrics[i] < minmetric)
} minmetric = v->new_metrics[i];
}
for(i=0; i<64; i++) for(i = 0; i < 64; i++)
v->new_metrics[i] -= minmetric; v->new_metrics[i] -= minmetric;
normalize += minmetric; normalize += minmetric;
} }
/* Advance decision index */ /* Advance decision index */
if(++v->decisions_index >= v->decisions_count) if(++v->decisions_index >= v->decisions_count)
v->decisions_index = 0; v->decisions_index = 0;
/* Swap pointers to old and new metrics */ /* Swap pointers to old and new metrics */
tmp = v->old_metrics; tmp = v->old_metrics;
v->old_metrics = v->new_metrics; v->old_metrics = v->new_metrics;
v->new_metrics = tmp; v->new_metrics = tmp;
} }
} }
/** Retrieve the most likely output bit sequence with known final state from /** Retrieve the most likely output bit sequence with known final state from
* a v27_t decoder. * a v27_t decoder.
* *
@ -193,28 +201,30 @@ void v27_update(v27_t *v, const unsigned char *syms, int nbits)
void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits, void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits,
unsigned char final_state) unsigned char final_state)
{ {
int k; int k;
unsigned int decisions_index = v->decisions_index; unsigned int decisions_index = v->decisions_index;
final_state %= 64; final_state %= 64;
final_state <<= 2; final_state <<= 2;
while(nbits-- != 0) { while(nbits-- != 0)
{
/* Decrement decision index */ /* Decrement decision index */
decisions_index = (decisions_index == 0) ? decisions_index = (decisions_index == 0) ?
v->decisions_count-1 : decisions_index-1; v->decisions_count-1 : decisions_index-1;
v27_decision_t *d = &v->decisions[decisions_index]; v27_decision_t *d = &v->decisions[decisions_index];
k = (d->w[(final_state>>2)/32] >> ((final_state>>2)%32)) & 1; k = (d->w[(final_state >> 2) / 32] >> ((final_state >> 2) % 32)) & 1;
/* The store into data[] only needs to be done every 8 bits. /* The store into data[] only needs to be done every 8 bits.
* But this avoids a conditional branch, and the writes will * But this avoids a conditional branch, and the writes will
* combine in the cache anyway * combine in the cache anyway
*/ */
data[nbits>>3] = final_state = (final_state >> 1) | (k << 7); data[nbits >> 3] = final_state = (final_state >> 1) | (k << 7);
} }
} }
/** Retrieve the most likely output bit sequence with unknown final state from /** Retrieve the most likely output bit sequence with unknown final state from
* a v27_t decoder. * a v27_t decoder.
* *
@ -224,19 +234,19 @@ void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits,
*/ */
void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits) void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits)
{ {
/* Determine state with minimum metric */ /* Determine state with minimum metric */
int i; int i;
unsigned int best_metric = 0xffffffff; unsigned int best_metric = 0xffffffff;
unsigned char best_state = 0; unsigned char best_state = 0;
for(i=0; i<64; i++) for(i = 0; i < 64; i++)
{ {
if(v->new_metrics[i] < best_metric) if(v->new_metrics[i] < best_metric)
{ {
best_metric = v->new_metrics[i]; best_metric = v->new_metrics[i];
best_state = i; best_state = i;
} }
} }
v27_chainback_fixed(v, data, nbits, best_state); v27_chainback_fixed(v, data, nbits, best_state);
} }