From b292c14790fc225029cba3f80ce8ad6c5652bc4e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 23 Jun 2012 16:05:42 -0700 Subject: [PATCH] altos: improve FEC apis to reduce data copying Integrate interleaving and whitening into encode and decode steps. Add CRC checking function for receive. Make ao_fec_test program round-trip the data and verify correctness. Signed-off-by: Keith Packard --- src/core/ao_fec.h | 26 ++-- src/core/ao_fec_tx.c | 118 +++++++++++------ src/core/ao_viterbi.c | 46 ++++++- src/test/Makefile | 6 +- src/test/{ao_fec_tx_test.c => ao_fec_test.c} | 132 ++++++++++--------- 5 files changed, 208 insertions(+), 120 deletions(-) rename src/test/{ao_fec_tx_test.c => ao_fec_test.c} (55%) diff --git a/src/core/ao_fec.h b/src/core/ao_fec.h index d4f64b74..f3f55fa8 100644 --- a/src/core/ao_fec.h +++ b/src/core/ao_fec.h @@ -24,12 +24,22 @@ #define AO_FEC_TRELLIS_TERMINATOR 0x0b #define AO_FEC_PREPARE_EXTRA 4 +extern const uint8_t ao_fec_whiten_table[]; + void -ao_fec_dump_bytes(uint8_t *bytes, uint8_t len, char *name); +ao_fec_dump_bytes(uint8_t *bytes, uint16_t len, char *name); uint16_t ao_fec_crc(uint8_t *bytes, uint8_t len); +/* + * 'len' is the length of the original data; 'bytes' + * must be four bytes longer than that, and the first + * two after 'len' must be the received crc + */ +uint8_t +ao_fec_check_crc(uint8_t *bytes, uint8_t len); + /* * Append CRC and terminator bytes, returns resulting length. * 'out' must be at least len + AO_FEC_PREPARE_EXTRA bytes long @@ -46,17 +56,11 @@ void ao_fec_whiten(uint8_t *in, uint8_t len, uint8_t *out); /* - * Encode data. 'out' must be len*2 bytes long + * Encode and interleave data. 'out' must be len*2 bytes long */ uint8_t ao_fec_encode(uint8_t *in, uint8_t len, uint8_t *out); -/* - * Interleave data. 'out' must be 'len' bytes long - */ -uint8_t -ao_fec_interleave(uint8_t *in, uint8_t len, uint8_t *out); - /* * Decode data. 'in' is one byte per bit, soft decision * 'out' must be len/8 bytes long @@ -65,4 +69,10 @@ ao_fec_interleave(uint8_t *in, uint8_t len, uint8_t *out); uint8_t ao_fec_decode(uint8_t *in, uint16_t in_len, uint8_t *out); +/* + * Interleave data packed in bytes. 'out' must be 'len' bytes long. + */ +uint16_t +ao_fec_interleave_bytes(uint8_t *in, uint16_t len, uint8_t *out); + #endif /* _AO_FEC_H_ */ diff --git a/src/core/ao_fec_tx.c b/src/core/ao_fec_tx.c index c5f410b8..c9c0a3d6 100644 --- a/src/core/ao_fec_tx.c +++ b/src/core/ao_fec_tx.c @@ -19,9 +19,9 @@ #include void -ao_fec_dump_bytes(uint8_t *bytes, uint8_t len, char *name) +ao_fec_dump_bytes(uint8_t *bytes, uint16_t len, char *name) { - uint8_t i; + uint16_t i; printf ("%s (%d):", name, len); for (i = 0; i < len; i++) { @@ -57,41 +57,76 @@ ao_fec_crc(uint8_t *bytes, uint8_t len) return crc; } +/* + * len is the length of the data; the crc will be + * the fist two bytes after that + */ + uint8_t -ao_fec_prepare(uint8_t *in, uint8_t len, uint8_t *out) +ao_fec_check_crc(uint8_t *bytes, uint8_t len) +{ + uint16_t computed_crc = ao_fec_crc(bytes, len); + uint16_t received_crc = (bytes[len] << 8) | (bytes[len+1]); + + return computed_crc == received_crc; +} + +uint8_t +ao_fec_prepare(uint8_t *in, uint8_t len, uint8_t *extra) { uint16_t crc = ao_fec_crc (in, len); - uint8_t i; + uint8_t i = 0; uint8_t num_fec; - /* Copy data */ - for (i = 0; i < len; i++) - out[i] = in[i]; - /* Append CRC */ - out[i++] = crc >> 8; - out[i++] = crc; + extra[i++] = crc >> 8; + extra[i++] = crc; /* Append FEC -- 1 byte if odd, two bytes if even */ num_fec = 2 - (i & 1); while (num_fec--) - out[i++] = AO_FEC_TRELLIS_TERMINATOR; + extra[i++] = AO_FEC_TRELLIS_TERMINATOR; return i; } -static const uint8_t whiten[] = { +const uint8_t ao_fec_whiten_table[] = { #include "ao_whiten.h" }; +#if 0 void ao_fec_whiten(uint8_t *in, uint8_t len, uint8_t *out) { - const uint8_t *w = whiten; + const uint8_t *w = ao_fec_whiten_table; while (len--) *out++ = *in++ ^ *w++; } +/* + * Unused as interleaving is now built in to ao_fec_encode + */ + +static void +ao_fec_interleave(uint8_t *d, uint8_t len) +{ + uint8_t i, j; + + for (i = 0; i < len; i += 4) { + uint32_t interleaved = 0; + + for (j = 0; j < 4 * 4; j++) { + interleaved <<= 2; + interleaved |= (d[i + (~j & 0x3)] >> (2 * ((j & 0xc) >> 2))) & 0x03; + } + d[i+0] = interleaved >> 24; + d[i+1] = interleaved >> 16; + d[i+2] = interleaved >> 8; + d[i+3] = interleaved; + } +} +#endif + static const uint8_t ao_fec_encode_table[16] = { /* next 0 1 state */ 0, 3, /* 000 */ @@ -107,38 +142,37 @@ static const uint8_t ao_fec_encode_table[16] = { uint8_t ao_fec_encode(uint8_t *in, uint8_t len, uint8_t *out) { - uint16_t fec = 0, output; - uint8_t byte, bit; - - for (byte = 0; byte < len; byte++) { - fec = (fec & 0x700) | in[byte]; - output = 0; - for (bit = 0; bit < 8; bit++) { - output = output << 2 | ao_fec_encode_table[fec >> 7]; - fec = (fec << 1) & 0x7ff; + uint8_t extra[AO_FEC_PREPARE_EXTRA]; + uint8_t extra_len; + uint32_t encode, interleave; + uint8_t pair, byte, bit; + uint16_t fec = 0; + const uint8_t *whiten = ao_fec_whiten_table; + + extra_len = ao_fec_prepare(in, len, extra); + for (pair = 0; pair < len + extra_len; pair += 2) { + encode = 0; + for (byte = 0; byte < 2; byte++) { + if (pair + byte == len) + in = extra; + fec |= *in++ ^ *whiten++; + for (bit = 0; bit < 8; bit++) { + encode = encode << 2 | ao_fec_encode_table[fec >> 7]; + fec = (fec << 1) & 0x7ff; + } } - out[byte * 2] = output >> 8; - out[byte * 2 + 1] = output; - } - return len * 2; -} -uint8_t -ao_fec_interleave(uint8_t *in, uint8_t len, uint8_t *out) -{ - uint8_t i, j; - - for (i = 0; i < len; i += 4) { - uint32_t interleaved = 0; + interleave = 0; + for (bit = 0; bit < 4 * 4; bit++) { + uint8_t byte_shift = (bit & 0x3) << 3; + uint8_t bit_shift = (bit & 0xc) >> 1; - for (j = 0; j < 4 * 4; j++) { - interleaved <<= 2; - interleaved |= (in[i + (~j & 0x3)] >> (2 * ((j & 0xc) >> 2))) & 0x03; + interleave = (interleave << 2) | ((encode >> (byte_shift + bit_shift)) & 0x3); } - out[i+0] = interleaved >> 24; - out[i+1] = interleaved >> 16; - out[i+2] = interleaved >> 8; - out[i+3] = interleaved; + *out++ = interleave >> 24; + *out++ = interleave >> 16; + *out++ = interleave >> 8; + *out++ = interleave >> 0; } - return len; + return (len + extra_len) * 2; } diff --git a/src/core/ao_viterbi.c b/src/core/ao_viterbi.c index 17464cd1..bb93bc83 100644 --- a/src/core/ao_viterbi.c +++ b/src/core/ao_viterbi.c @@ -18,6 +18,35 @@ #include #include +/* + * byte order repeats through 3 2 1 0 + * + * bit-pair order repeats through + * + * 1/0 3/2 5/4 7/6 + * + * So, the over all order is: + * + * 3,1/0 2,1/0 1,1/0 0,1/0 + * 3,3/2 2,3/2 1,3/2 0,3/2 + * 3,5/4 2,5/4 1,5/4 0,5/4 + * 3,7/6 2,7/6 1,7/6 0,7/6 + * + * The raw bit order is thus + * + * 1e/1f 16/17 0e/0f 06/07 + * 1c/1d 14/15 0c/0d 04/05 + * 1a/1b 12/13 0a/0b 02/03 + * 18/19 10/11 08/09 00/01 + */ + +static inline uint16_t ao_interleave_index(uint16_t i) { + uint8_t l = i & 0x1e; + uint16_t h = i & ~0x1e; + uint8_t o = 0x1e ^ (((l >> 2) & 0x6) | ((l << 2) & 0x18)); + return h | o; +} + struct ao_soft_sym { uint8_t a, b; }; @@ -70,6 +99,9 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out) uint8_t n; /* next cost/bits index */ uint8_t state; /* state index */ uint8_t bit; /* original encoded bit index */ + const uint8_t *whiten = ao_fec_whiten_table; + uint16_t interleave; /* input byte array index */ + struct ao_soft_sym s; /* input symbol pair */ p = 0; for (state = 0; state < NUM_STATE; state++) { @@ -82,7 +114,13 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out) for (i = 0; i < len; i += 2) { b = i/2; n = p ^ 1; - struct ao_soft_sym s = { .a = in[i], .b = in[i+1] }; + + /* Fetch one pair of input bytes, de-interleaving + * the input. + */ + interleave = ao_interleave_index(i); + s.a = in[interleave]; + s.b = in[interleave+1]; /* Reset next costs to 'impossibly high' values so that * the first path through this state is cheaper than this @@ -159,10 +197,10 @@ ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out) } #if 0 - printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x\n", - i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff); + printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n", + i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten); #endif - out[o >> 3] = bits[p][min_state] >> dist; + *out++ = (bits[p][min_state] >> dist) ^ *whiten++; o += 8; } } diff --git a/src/test/Makefile b/src/test/Makefile index 7bcf24e1..cdcecd51 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,6 +1,6 @@ vpath % ..:../core:../drivers:../util -PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_tx_test +PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test KALMAN=make-kalman @@ -40,5 +40,5 @@ ao_convert_pa_test: ao_convert_pa_test.c ao_convert_pa.c altitude-pa.h ao_kalman.h: $(KALMAN) (cd .. && make ao_kalman.h) -ao_fec_tx_test: ao_fec_tx_test.c ao_fec_tx.c ao_viterbi.c - cc $(CFLAGS) -o $@ ao_fec_tx_test.c ../core/ao_fec_tx.c ../core/ao_viterbi.c -lm +ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_viterbi.c + cc $(CFLAGS) -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_viterbi.c -lm diff --git a/src/test/ao_fec_tx_test.c b/src/test/ao_fec_test.c similarity index 55% rename from src/test/ao_fec_tx_test.c rename to src/test/ao_fec_test.c index 1b1fd56d..c7509728 100644 --- a/src/test/ao_fec_tx_test.c +++ b/src/test/ao_fec_test.c @@ -20,6 +20,7 @@ #include #include #include +#include #ifndef RANDOM_MAX #define RANDOM_MAX 0x7fffffff @@ -70,55 +71,21 @@ gaussian_random(double mean, double dev) #define PREPARE_LEN(input_len) ((input_len) + AO_FEC_PREPARE_EXTRA) #define ENCODE_LEN(input_len) (PREPARE_LEN(input_len) * 2) -#define INTERLEAVE_LEN(input_len) ENCODE_LEN(input_len) +#define DECODE_LEN(input_len) ((input_len) + AO_FEC_PREPARE_EXTRA) +#define EXPAND_LEN(input_len) (ENCODE_LEN(input_len) * 8) static int -ao_encode(uint8_t *input, int input_len, uint8_t *output) +ao_expand(uint8_t *bits, int bits_len, uint8_t *bytes) { - uint8_t prepare[PREPARE_LEN(input_len)]; - uint8_t encode[ENCODE_LEN(input_len)]; - uint8_t prepare_len; - uint8_t encode_len; - uint8_t interleave_len; - - ao_fec_dump_bytes(input, input_len, "Input"); - - prepare_len = ao_fec_prepare(input, input_len, prepare); - - ao_fec_dump_bytes(prepare, prepare_len, "Prepare"); - - encode_len = ao_fec_encode(prepare, prepare_len, encode); - - ao_fec_dump_bytes(encode, encode_len, "Encode"); - - interleave_len = ao_fec_interleave(encode, encode_len, output); - - ao_fec_dump_bytes(output, interleave_len, "Interleave"); - - return interleave_len; -} - -#define RADIO_LEN(input_len) (INTERLEAVE_LEN(input_len) * 8) - -static int -ao_radio(uint8_t *bits, int bits_len, uint8_t *bytes) -{ - uint8_t b, *bytes_orig = bytes; - uint8_t interleave[bits_len]; int i, bit; - - ao_fec_interleave(bits, bits_len, interleave); - - ao_fec_dump_bytes(interleave, bits_len, "De-interleave"); + uint8_t b; for (i = 0; i < bits_len; i++) { - b = interleave[i]; + b = bits[i]; for (bit = 7; bit >= 0; bit--) *bytes++ = ((b >> bit) & 1) * 0xff; } - ao_fec_dump_bytes(bytes_orig, bits_len * 8, "Bytes"); - return bits_len * 8; } @@ -144,49 +111,88 @@ ao_fuzz (uint8_t *in, int in_len, uint8_t *out, double dev) } out[i] = byte; } - - printf ("Introduced %d errors\n", errors); - ao_fec_dump_bytes(out, in_len, "Fuzz"); - return in_len; + return errors; } -static int -ao_decode(uint8_t *bytes, int bytes_len, uint8_t *bits) +static uint8_t +ao_random_data(uint8_t *out, uint8_t out_len) { - int bits_len; - - bits_len = ao_fec_decode(bytes, bytes_len, bits); + uint8_t len = random() % (out_len + 1); + uint8_t i; + + for (i = 0; i < len; i++) + out[i] = random(); + return len; +} - ao_fec_dump_bytes(bits, bits_len, "Decode"); - return bits_len; -} int main(int argc, char **argv) { - uint8_t original[4] = { 3, 1, 2, 3 }; - uint8_t encode[INTERLEAVE_LEN(sizeof(original))]; + int trial; + + uint8_t original[120]; + uint8_t original_len; + + uint8_t encode[ENCODE_LEN(sizeof(original))]; int encode_len; - uint8_t transmit[RADIO_LEN(sizeof(original))]; + uint8_t transmit[EXPAND_LEN(sizeof(original))]; int transmit_len; - uint8_t receive[RADIO_LEN(sizeof(original))]; - int receive_len; + uint8_t receive[EXPAND_LEN(sizeof(original))]; + int receive_len, receive_errors; - uint8_t decode[INTERLEAVE_LEN(sizeof(original))]; + uint8_t decode[DECODE_LEN(sizeof(original))]; int decode_len; - encode_len = ao_encode(original, sizeof(original), encode); + int errors = 0; + int error; + + srandom(0); + for (trial = 0; trial < 10000; trial++) { + + /* Compute some random data */ + original_len = ao_random_data(original, sizeof(original)); - transmit_len = ao_radio(encode, encode_len, transmit); + /* Encode it */ + encode_len = ao_fec_encode(original, original_len, encode); - /* apply gaussian noise to test viterbi code against errors */ - receive_len = ao_fuzz(transmit, transmit_len, receive, 0x70); + /* Expand from 1-bit-per-symbol to 1-byte-per-symbol */ + transmit_len = ao_expand(encode, encode_len, transmit); - decode_len = ao_decode(receive, receive_len, decode); + /* Add gaussian noise to the signal */ + receive_errors = ao_fuzz(transmit, transmit_len, receive, 0x30); + receive_len = transmit_len; + + /* Decode it */ + decode_len = ao_fec_decode(receive, receive_len, decode); - return decode_len >= sizeof(original); + /* Check to see if we received the right data */ + error = 0; + + if (decode_len < original_len + 2) { + printf ("len mis-match\n"); + error++; + } + + if (!ao_fec_check_crc(decode, original_len)) { + printf ("crc mis-match\n"); + error++; + } + + if (memcmp(original, decode, original_len) != 0) { + printf ("data mis-match\n"); + error++; + } + if (error) { + printf ("Errors: %d\n", receive_errors); + ao_fec_dump_bytes(original, original_len, "Input"); + ao_fec_dump_bytes(decode, original_len, "Decode"); + errors += error; + } + } + return errors; } -- 2.30.2