From cff63b378a4e18abb0c5c437cc8d3d3781803790 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 28 Mar 2017 19:32:42 +0200 Subject: [PATCH] Adapt to gnss-sdr coding style --- .../libs/libswiftcnav/bits.c | 175 +++---- .../libs/libswiftcnav/cnav_msg.c | 457 ++++++++++-------- .../libs/libswiftcnav/cnav_msg.h | 50 +- .../telemetry_decoder/libs/libswiftcnav/edc.c | 109 +++-- .../telemetry_decoder/libs/libswiftcnav/fec.h | 22 +- .../libs/libswiftcnav/viterbi27.c | 252 +++++----- 6 files changed, 561 insertions(+), 504 deletions(-) diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c index fc1243789..33edf91f8 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c @@ -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) { - x ^= x >> 16; - x ^= x >> 8; - x ^= x >> 4; - x &= 0xF; - return (0x6996 >> x) & 1; + x ^= x >> 16; + x ^= x >> 8; + x ^= x >> 4; + x &= 0xF; + return (0x6996 >> x) & 1; } @@ -72,11 +72,12 @@ u8 parity(u32 x) u32 getbitu(const u8 *buff, u32 pos, u8 len) { u32 bits = 0; - u32 i=0; - for ( i= pos; i < pos + len; i++) { - bits = (bits << 1) + - ((buff[i/8] >> (7 - i%8)) & 1u); - } + u32 i=0; + for (i = pos; i < pos + len; i++) + { + bits = (bits << 1) + + ((buff[i/8] >> (7 - i%8)) & 1u); + } 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) { - u32 mask = 1u << (len - 1); + u32 mask = 1u << (len - 1); - if (len <= 0 || 32 < len) - return; - u32 i=0; - for (i = pos; i < pos + len; i++, mask >>= 1) { - if (data & mask) - buff[i/8] |= 1u << (7 - i % 8); - else - buff[i/8] &= ~(1u << (7 - i % 8)); - } + if (len <= 0 || 32 < len) + return; + u32 i = 0; + for (i = pos; i < pos + len; i++, mask >>= 1) + { + if (data & mask) + buff[i/8] |= 1u << (7 - i % 8); + else + buff[i/8] &= ~(1u << (7 - i % 8)); + } } /** 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) { - 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) { - if (shift > size * CHAR_BIT) { - /* Quick check: if the shift is larger, than the buffer, zero the data */ - memset(buf, 0, size); - return; - } + if (shift > size * CHAR_BIT) + { + /* Quick check: if the shift is larger, than the buffer, zero the data */ + memset(buf, 0, size); + return; + } - unsigned char *dst = buf; /* Destination byte. */ - const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, - * possibly incomplete. */ + unsigned char *dst = buf; /* Destination byte. */ + const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, possibly incomplete. */ - u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */ - u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */ - u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes 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 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */ - if (0 == byte_shift) { - /* When moving data in character boundaries, use built-in functions: move - * data, and then zero the tail. */ - memmove(dst, src, full_bytes); - memset(dst + full_bytes, 0, size - full_bytes); - } else { - /* Create an accumulator: it will hold a value of two consecutive bytes */ - u32 acc = *src++; - u32 i=0; - for (i = 0; i < full_bytes; ++i) { - acc = (acc << CHAR_BIT) | *src++; - *dst++ = acc >> byte_shift; - } - *dst++ = acc << CHAR_BIT >> byte_shift; - if (full_bytes + 1 < size) { - memset(dst, 0, size - full_bytes - 1); - } - } + if (0 == byte_shift) + { + /* When moving data in character boundaries, use built-in functions: move + * data, and then zero the tail. */ + memmove(dst, src, full_bytes); + memset(dst + full_bytes, 0, size - full_bytes); + } + else + { + /* Create an accumulator: it will hold a value of two consecutive bytes */ + u32 acc = *src++; + u32 i = 0; + for (i = 0; i < full_bytes; ++i) + { + acc = (acc << CHAR_BIT) | *src++; + *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, u32 count) { - u32 limit1 = count / 32; - u32 limit2 = count % 32; - u32 idx=0; - for (idx = 0; idx < limit1; ++idx) { - u32 tmp = getbitu(src, src_index, 32); - setbitu(dst, dst_index, 32, tmp); - src_index += 32; - dst_index += 32; - } - if (0 != limit2) { - u32 tmp = getbitu(src, src_index, limit2); - setbitu(dst, dst_index, limit2, tmp); - } + u32 limit1 = count / 32; + u32 limit2 = count % 32; + u32 idx = 0; + for (idx = 0; idx < limit1; ++idx) + { + u32 tmp = getbitu(src, src_index, 32); + setbitu(dst, dst_index, 32, tmp); + src_index += 32; + dst_index += 32; + } + if (0 != limit2) + { + 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 r = 0; -int i =0; - for (i = 0; i < 16; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 64 - r; + u8 r = 0; + int i = 0; + for (i = 0; i < 16; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 64 - r; } /** @@ -248,11 +257,11 @@ int i =0; */ u8 count_bits_u32(u32 v, u8 bv) { - u8 r = 0; - int i=0; - for (i = 0; i < 8; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 32 - r; + u8 r = 0; + int i = 0; + for (i = 0; i < 8; i++) + r += bitn[(v >> (i*4)) & 0xf]; + 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 r = 0; -int i =0; - for (i= 0; i < 4; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 16 - r; + u8 r = 0; + int i = 0; + for (i= 0; i < 4; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 16 - r; } /** @@ -282,11 +291,11 @@ int i =0; */ u8 count_bits_u8(u8 v, u8 bv) { - u8 r = 0; -int i=0; - for (i = 0; i < 2; i++) - r += bitn[(v >> (i*4)) & 0xf]; - return bv == 1 ? r : 8 - r; + u8 r = 0; + int i = 0; + for (i = 0; i < 2; i++) + r += bitn[(v >> (i*4)) & 0xf]; + return bv == 1 ? r : 8 - r; } /** \} */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index 5e33c4ba9..bbb9a92f6 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -84,12 +84,13 @@ */ static u32 _cnav_compute_crc(cnav_v27_part_t *part) { - u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH, - part->invert); + u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH, + part->invert); - return crc; + return crc; } + /** * Extracts CRC-24Q from a CNAV 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) { - u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH, - GPS_CNAV_MSG_CRC_LENGTH); - if (part->invert) { - crc ^= 0xFFFFFF; - } - return crc; + u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH, + GPS_CNAV_MSG_CRC_LENGTH); + if (part->invert) + { + crc ^= 0xFFFFFF; + } + return crc; } + /** * Helper to rescan for preamble in the received buffer. * 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) { - part->preamble_seen = false; + part->preamble_seen = false; - if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1) { - size_t i=0; - size_t j=0; - 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) { - part->preamble_seen = true; - part->invert = (GPS_CNAV_PREAMBLE2 == c); - /* We shift the accumulated bits to the beginning of the buffer */ - 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); - 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; + 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) + { + part->preamble_seen = true; + part->invert = (GPS_CNAV_PREAMBLE2 == c); + /* We shift the accumulated bits to the beginning of the buffer */ + 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); + part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1; + } } + /** * 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) { - part->symbols[part->n_symbols++] = ch; + part->symbols[part->n_symbols++] = ch; - 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) { - return; - } - part->init = false; - } - else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2) { - /* Wait until decoding block is accumulated */ - return; - } - - /* Feed accumulated symbols into the buffer, reset the number of accumulated - * 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; - } + 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) + { + return; + } + part->init = false; + } + else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2) + { + /* Wait until decoding block is accumulated */ + return; } - } 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 */ - } - } + /* Feed accumulated symbols into the buffer, reset the number of accumulated + * 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 */ + retry = true; + } + } + else + { + /* No preamble or preamble and less than 300 bits decoded */ + } + } } + /** * 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) { - size_t i = 0; - for (i = 0; i < sizeof(part->decoded); i++) { - part->decoded[i] ^= 0xFFu; - } + size_t i = 0; + for (i = 0; i < sizeof(part->decoded); i++) + { + part->decoded[i] ^= 0xFFu; + } } + /** * 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) { - bool res = false; - if (GPS_CNAV_MSG_LENGTH <= part->n_decoded) { - if (part->crc_ok) { - /* CRC is OK */ - if (part->invert) { - _cnav_msg_invert(part); - } + bool res = false; + if (GPS_CNAV_MSG_LENGTH <= part->n_decoded) + { + if (part->crc_ok) + { + /* CRC is OK */ + if (part->invert) + { + _cnav_msg_invert(part); + } - msg->prn = getbitu(part->decoded, 8, 6); - msg->msg_id = getbitu(part->decoded, 14, 6); - msg->tow = getbitu(part->decoded, 20, 17); - msg->alert = getbitu(part->decoded, 37, 1) ? true : false; + msg->prn = getbitu(part->decoded, 8, 6); + msg->msg_id = getbitu(part->decoded, 14, 6); + msg->tow = getbitu(part->decoded, 20, 17); + msg->alert = getbitu(part->decoded, 37, 1) ? true : false; - /* copy RAW message for GNSS-SDR */ - memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS); + /* copy RAW message for GNSS-SDR */ + 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) - * 2 + part->n_symbols; + *delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols; - if (part->invert) { - _cnav_msg_invert(part); - } - res = true; - } else { - /* CRC mismatch - no decoding */ - } - bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH); - part->n_decoded -= GPS_CNAV_MSG_LENGTH; - } + if (part->invert) + { + _cnav_msg_invert(part); + } + res = true; + } + else + { + /* 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. * @@ -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) { - memset(dec, 0, sizeof(*dec)); - v27_init(&dec->part1.dec, - dec->part1.decisions, - GPS_L2_V27_HISTORY_LENGTH_BITS, - cnav_msg_decoder_get_poly(), - 0); - v27_init(&dec->part2.dec, - dec->part2.decisions, - GPS_L2_V27_HISTORY_LENGTH_BITS, - cnav_msg_decoder_get_poly(), - 0); - dec->part1.init = true; - dec->part2.init = true; - _cnav_add_symbol(&dec->part2, 0x80); + memset(dec, 0, sizeof(*dec)); + v27_init(&dec->part1.dec, + dec->part1.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); + v27_init(&dec->part2.dec, + dec->part2.decisions, + GPS_L2_V27_HISTORY_LENGTH_BITS, + cnav_msg_decoder_get_poly(), + 0); + dec->part1.init = true; + dec->part2.init = true; + _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. */ bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec, - u8 symbol, - cnav_msg_t *msg, - u32 *pdelay) + u8 symbol, + cnav_msg_t *msg, + u32 *pdelay) { - _cnav_add_symbol(&dec->part1, symbol); - _cnav_add_symbol(&dec->part2, symbol); + _cnav_add_symbol(&dec->part1, symbol); + _cnav_add_symbol(&dec->part2, symbol); - if (dec->part1.message_lock) { - /* Flush data in decoder. */ - dec->part2.n_decoded = 0; - dec->part2.n_symbols = 0; - return _cnav_msg_decode(&dec->part1, msg, pdelay); - } - if (dec->part2.message_lock) { - /* Flush data in decoder. */ - dec->part1.n_decoded = 0; - dec->part1.n_symbols = 0; - return _cnav_msg_decode(&dec->part2, msg, pdelay); - } + if (dec->part1.message_lock) + { + /* Flush data in decoder. */ + dec->part2.n_decoded = 0; + dec->part2.n_symbols = 0; + return _cnav_msg_decode(&dec->part1, msg, pdelay); + } + if (dec->part2.message_lock) + { + /* Flush data in decoder. */ + 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. * @@ -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) { - static v27_poly_t instance; - static bool initialized = false; + static v27_poly_t instance; + static bool initialized = false; - if (!initialized) { - /* Coefficients for polynomial object */ - const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B }; + if (!initialized) + { + /* 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 - * than once in case multiple threads request concurrent access. However, - * nature of the v27_poly_init() function and data alignment ensure that - * the data returned from the earlier finished call is consistent and can - * be used even when re-initialization is happening. - * - * Other possible approaches are: - * - Replace late initialization with an explicit call. - * - Use POSIX synchronization objects like pthread_once_t. - */ - v27_poly_init(&instance, coeffs); - initialized = true; - } - return &instance; + /* Racing condition handling: the data can be potential initialized more + * than once in case multiple threads request concurrent access. However, + * nature of the v27_poly_init() function and data alignment ensure that + * the data returned from the earlier finished call is consistent and can + * be used even when re-initialization is happening. + * + * Other possible approaches are: + * - Replace late initialization with an explicit call. + * - Use POSIX synchronization objects like pthread_once_t. + */ + v27_poly_init(&instance, coeffs); + initialized = true; + } + return &instance; } /** \} */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h index dba1c8d6b..8b8141358 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h @@ -62,11 +62,11 @@ */ typedef struct { - u8 prn; /**< SV PRN. 0..31 */ - u8 msg_id; /**< Message id. 0..31 */ - u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ - 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 prn; /**< SV PRN. 0..31 */ + u8 msg_id; /**< Message id. 0..31 */ + u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ + bool alert; /**< CNAV message alert flag */ + u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ } cnav_msg_t; /** @@ -76,24 +76,24 @@ typedef struct * @sa cnav_msg_decoder_t */ typedef struct { - v27_t dec; /**< Viterbi block decoder object */ - v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS]; - /**< Decision graph */ - unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2]; - /**< 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]; - /**< Decode buffer */ - size_t n_decoded; /**< Number of bits in the decode buffer */ - bool preamble_seen; /**< When true, the decode buffer is aligned on - * preamble. */ - bool invert; /**< When true, indicates the bits are inverted */ - bool message_lock; /**< When true, indicates the message boundary - * is found. */ - bool crc_ok; /**< Flag that the last message had good CRC */ - size_t n_crc_fail; /**< Counter for CRC failures */ - bool init; /**< Initial state flag. When true, initial bits - * do not produce output. */ + v27_t dec; /**< Viterbi block decoder object */ + v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS]; + /**< Decision graph */ + unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2]; + /**< 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]; + /**< Decode buffer */ + size_t n_decoded; /**< Number of bits in the decode buffer */ + bool preamble_seen; /**< When true, the decode buffer is aligned on + * preamble. */ + bool invert; /**< When true, indicates the bits are inverted */ + bool message_lock; /**< When true, indicates the message boundary + * is found. */ + bool crc_ok; /**< Flag that the last message had good CRC */ + size_t n_crc_fail; /**< Counter for CRC failures */ + bool init; /**< Initial state flag. When true, initial bits + * do not produce output. */ } cnav_v27_part_t; /** @@ -104,8 +104,8 @@ typedef struct { */ typedef struct { - cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */ - cnav_v27_part_t part2; /**< Decoder for even symbol pairs */ + cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */ + cnav_v27_part_t part2; /**< Decoder for even symbol pairs */ } cnav_msg_decoder_t; const v27_poly_t *cnav_msg_decoder_get_poly(void); diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c index 370304d04..dc5fee6b7 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c @@ -40,38 +40,38 @@ * \{ */ static const u32 crc24qtab[256] = { - 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, - 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, - 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, - 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, - 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, - 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, - 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, - 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, - 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, - 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, - 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, - 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, - 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, - 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, - 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, - 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, - 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, - 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, - 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, - 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, - 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, - 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, - 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, - 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, - 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, - 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, - 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, - 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, - 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, - 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, - 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, - 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, + 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, + 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, + 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, + 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, + 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, + 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, + 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, + 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, + 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, + 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, + 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, + 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, + 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, + 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, + 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, + 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, + 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, + 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, + 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, + 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, + 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, + 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, + 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, + 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, + 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, + 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, + 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, + 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 }; /** 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 i=0; - for ( i= 0; i < len; i++) - crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; - return crc; + u32 i = 0; + for (i = 0; i < len; i++) + crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff]; + 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) { - u16 acc = 0; - u8 b = 0; - u32 shift = 8 - n_bits % 8; + u16 acc = 0; + u8 b = 0; + u32 shift = 8 - n_bits % 8; - u32 i=0; - for ( i= 0; i < n_bits / 8; ++i) { - acc = (acc << 8) | *buf++; - if (invert) { - acc ^= 0xFFu; - } + u32 i = 0; + for (i = 0; i < n_bits / 8; ++i) + { + acc = (acc << 8) | *buf++; + 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; 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; } diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h index 8842f49bf..d5196dcc4 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h @@ -40,25 +40,25 @@ #define V27POLYB 0x6d typedef struct { - unsigned char c0[32]; - unsigned char c1[32]; + unsigned char c0[32]; + unsigned char c1[32]; } v27_poly_t; typedef struct { - unsigned int w[2]; + unsigned int w[2]; } v27_decision_t; /* State info for instance of r=1/2 k=7 Viterbi decoder */ typedef struct { - unsigned int metrics1[64]; /* Path metric buffer 1 */ - unsigned int metrics2[64]; /* Path metric buffer 2 */ - /* Pointers to path metrics, swapped on every bit */ - unsigned int *old_metrics, *new_metrics; - const v27_poly_t *poly; /* Polynomial to use */ - v27_decision_t *decisions; /* Beginning of decisions for block */ - unsigned int decisions_index; /* Index of current decision */ - unsigned int decisions_count; /* Number of decisions in history */ + unsigned int metrics1[64]; /* Path metric buffer 1 */ + unsigned int metrics2[64]; /* Path metric buffer 2 */ + /* Pointers to path metrics, swapped on every bit */ + unsigned int *old_metrics, *new_metrics; + const v27_poly_t *poly; /* Polynomial to use */ + v27_decision_t *decisions; /* Beginning of decisions for block */ + unsigned int decisions_index; /* Index of current decision */ + unsigned int decisions_count; /* Number of decisions in history */ } v27_t; void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]); diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c index c2227a2d8..958b0e635 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c @@ -35,13 +35,14 @@ static inline int parity(int x) { - x ^= x >> 16; - x ^= x >> 8; - x ^= x >> 4; - x &= 0xf; - return (0x6996 >> x) & 1; + x ^= x >> 16; + x ^= x >> 8; + x ^= x >> 4; + x &= 0xf; + return (0x6996 >> x) & 1; } + /** Initialize a v27_poly_t struct for use with a v27_t decoder. * * \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]) { - int state; + int 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; - } + 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; + } } + /** Initialize a v27_t struct for Viterbi decoding. * * \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, const v27_poly_t *poly, unsigned char initial_state) { - int i; + int i; - v->old_metrics = v->metrics1; - v->new_metrics = v->metrics2; - v->poly = poly; - v->decisions = decisions; - v->decisions_index = 0; - v->decisions_count = decisions_count; + v->old_metrics = v->metrics1; + v->new_metrics = v->metrics2; + v->poly = poly; + v->decisions = decisions; + v->decisions_index = 0; + v->decisions_count = decisions_count; - for(i = 0; i < 64; i++) - v->old_metrics[i] = 63; + for(i = 0; i < 64; i++) + 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 */ #define BFLY(i) {\ -unsigned int metric,m0,m1,decision;\ - metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\ - m0 = v->old_metrics[i] + metric;\ - m1 = v->old_metrics[i+32] + (510 - metric);\ - decision = (signed int)(m0-m1) > 0;\ - v->new_metrics[2*i] = decision ? m1 : m0;\ - d->w[i/16] |= decision << ((2*i)&31);\ - m0 -= (metric+metric-510);\ - m1 += (metric+metric-510);\ - decision = (signed int)(m0-m1) > 0;\ - v->new_metrics[2*i+1] = decision ? m1 : m0;\ - d->w[i/16] |= decision << ((2*i+1)&31);\ + unsigned int metric,m0,m1,decision;\ + metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\ + m0 = v->old_metrics[i] + metric;\ + m1 = v->old_metrics[i+32] + (510 - metric);\ + decision = (signed int)(m0-m1) > 0;\ + v->new_metrics[2*i] = decision ? m1 : m0;\ + d->w[i/16] |= decision << ((2*i)&31);\ + m0 -= (metric+metric-510);\ + m1 += (metric+metric-510);\ + decision = (signed int)(m0-m1) > 0;\ + v->new_metrics[2*i+1] = decision ? m1 : m0;\ + d->w[i/16] |= decision << ((2*i+1)&31);\ } /** 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) { - unsigned char sym0, sym1; - unsigned int *tmp; - int normalize = 0; + unsigned char sym0, sym1; + unsigned int *tmp; + int normalize = 0; - while(nbits--) { - v27_decision_t *d = &v->decisions[v->decisions_index]; + while(nbits--) + { + v27_decision_t *d = &v->decisions[v->decisions_index]; - d->w[0] = d->w[1] = 0; - sym0 = *syms++; - sym1 = *syms++; + d->w[0] = d->w[1] = 0; + sym0 = *syms++; + sym1 = *syms++; - BFLY(0); - BFLY(1); - BFLY(2); - BFLY(3); - BFLY(4); - BFLY(5); - BFLY(6); - BFLY(7); - BFLY(8); - BFLY(9); - BFLY(10); - BFLY(11); - BFLY(12); - BFLY(13); - BFLY(14); - BFLY(15); - BFLY(16); - BFLY(17); - BFLY(18); - BFLY(19); - BFLY(20); - BFLY(21); - BFLY(22); - BFLY(23); - BFLY(24); - BFLY(25); - BFLY(26); - BFLY(27); - BFLY(28); - BFLY(29); - BFLY(30); - BFLY(31); + BFLY(0); + BFLY(1); + BFLY(2); + BFLY(3); + BFLY(4); + BFLY(5); + BFLY(6); + BFLY(7); + BFLY(8); + BFLY(9); + BFLY(10); + BFLY(11); + BFLY(12); + BFLY(13); + BFLY(14); + BFLY(15); + BFLY(16); + BFLY(17); + BFLY(18); + BFLY(19); + BFLY(20); + BFLY(21); + BFLY(22); + BFLY(23); + BFLY(24); + BFLY(25); + BFLY(26); + BFLY(27); + BFLY(28); + BFLY(29); + BFLY(30); + BFLY(31); - /* Normalize metrics if they are nearing overflow */ - if(v->new_metrics[0] > (1<<30)) { - int i; - unsigned int minmetric = 1<<31; + /* Normalize metrics if they are nearing overflow */ + if(v->new_metrics[0] > (1 << 30)) + { + int i; + unsigned int minmetric = 1 << 31; - for(i=0; i<64; i++) { - if(v->new_metrics[i] < minmetric) - minmetric = v->new_metrics[i]; - } + for(i = 0; i < 64; i++) + { + if(v->new_metrics[i] < minmetric) + minmetric = v->new_metrics[i]; + } - for(i=0; i<64; i++) - v->new_metrics[i] -= minmetric; + for(i = 0; i < 64; i++) + v->new_metrics[i] -= minmetric; - normalize += minmetric; - } + normalize += minmetric; + } - /* Advance decision index */ - if(++v->decisions_index >= v->decisions_count) - v->decisions_index = 0; + /* Advance decision index */ + if(++v->decisions_index >= v->decisions_count) + v->decisions_index = 0; - /* Swap pointers to old and new metrics */ - tmp = v->old_metrics; - v->old_metrics = v->new_metrics; - v->new_metrics = tmp; - } + /* Swap pointers to old and new metrics */ + tmp = v->old_metrics; + v->old_metrics = v->new_metrics; + v->new_metrics = tmp; + } } + /** Retrieve the most likely output bit sequence with known final state from * 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, unsigned char final_state) { - int k; - unsigned int decisions_index = v->decisions_index; + int k; + unsigned int decisions_index = v->decisions_index; - final_state %= 64; - final_state <<= 2; + final_state %= 64; + final_state <<= 2; - while(nbits-- != 0) { + while(nbits-- != 0) + { - /* Decrement decision index */ - decisions_index = (decisions_index == 0) ? - v->decisions_count-1 : decisions_index-1; + /* Decrement decision index */ + decisions_index = (decisions_index == 0) ? + v->decisions_count-1 : decisions_index-1; - v27_decision_t *d = &v->decisions[decisions_index]; - k = (d->w[(final_state>>2)/32] >> ((final_state>>2)%32)) & 1; - /* The store into data[] only needs to be done every 8 bits. - * But this avoids a conditional branch, and the writes will - * combine in the cache anyway - */ - data[nbits>>3] = final_state = (final_state >> 1) | (k << 7); - } + v27_decision_t *d = &v->decisions[decisions_index]; + k = (d->w[(final_state >> 2) / 32] >> ((final_state >> 2) % 32)) & 1; + /* The store into data[] only needs to be done every 8 bits. + * But this avoids a conditional branch, and the writes will + * combine in the cache anyway + */ + data[nbits >> 3] = final_state = (final_state >> 1) | (k << 7); + } } + /** Retrieve the most likely output bit sequence with unknown final state from * 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) { - /* Determine state with minimum metric */ + /* Determine state with minimum metric */ - int i; - unsigned int best_metric = 0xffffffff; - unsigned char best_state = 0; - for(i=0; i<64; i++) - { - if(v->new_metrics[i] < best_metric) - { - best_metric = v->new_metrics[i]; - best_state = i; - } - } + int i; + unsigned int best_metric = 0xffffffff; + unsigned char best_state = 0; + for(i = 0; i < 64; i++) + { + if(v->new_metrics[i] < best_metric) + { + best_metric = v->new_metrics[i]; + best_state = i; + } + } - v27_chainback_fixed(v, data, nbits, best_state); + v27_chainback_fixed(v, data, nbits, best_state); }