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 <keithp@keithp.com>
#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
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
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_ */
#include <stdio.h>
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++) {
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 */
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;
}
#include <ao_fec.h>
#include <stdio.h>
+/*
+ * 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;
};
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++) {
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
}
#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;
}
}
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
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
--- /dev/null
+/*
+ * Copyright © 2012 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include <ao_fec.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+#ifndef RANDOM_MAX
+#define RANDOM_MAX 0x7fffffff
+#endif
+
+static double
+rand_real(void) {
+ return (double) random() / (double) RANDOM_MAX;
+}
+
+static double
+gaussian_random(double mean, double dev)
+{
+ static int save_x_valid = 0;
+ static double save_x;
+ double x;
+
+ if (save_x_valid)
+ {
+ x = save_x;
+ save_x_valid = 0;
+ }
+ else
+ {
+ double w;
+ double normal_x1, normal_x2;
+
+ do {
+ normal_x1 = 2 * rand_real () - 1;
+ normal_x2 = 2 * rand_real () - 1;
+ w = normal_x1*normal_x1 + normal_x2*normal_x2;
+ } while (w >= 1 || w < 1E-30);
+
+ w = sqrt(log(w)*(-2./w));
+
+ /*
+ * normal_x1 and normal_x2 are independent normally
+ * distributed variates
+ */
+
+ x = normal_x1 * w;
+ /* save normal_x2 for next call */
+ save_x = normal_x2 * w;
+ save_x_valid = 1;
+ }
+ return x * dev + mean;
+}
+
+#define PREPARE_LEN(input_len) ((input_len) + AO_FEC_PREPARE_EXTRA)
+#define ENCODE_LEN(input_len) (PREPARE_LEN(input_len) * 2)
+#define DECODE_LEN(input_len) ((input_len) + AO_FEC_PREPARE_EXTRA)
+#define EXPAND_LEN(input_len) (ENCODE_LEN(input_len) * 8)
+
+static int
+ao_expand(uint8_t *bits, int bits_len, uint8_t *bytes)
+{
+ int i, bit;
+ uint8_t b;
+
+ for (i = 0; i < bits_len; i++) {
+ b = bits[i];
+ for (bit = 7; bit >= 0; bit--)
+ *bytes++ = ((b >> bit) & 1) * 0xff;
+ }
+
+ return bits_len * 8;
+}
+
+static int
+ao_fuzz (uint8_t *in, int in_len, uint8_t *out, double dev)
+{
+ int i;
+ int errors = 0;
+
+ for (i = 0; i < in_len; i++) {
+ double error = gaussian_random(0, dev);
+ uint8_t byte = in[i];
+
+ if (error > 0) {
+ if (error > 0xff)
+ error = 0xff;
+ if (error >= 0x80)
+ errors++;
+ if (byte < 0x80)
+ byte += error;
+ else
+ byte -= error;
+ }
+ out[i] = byte;
+ }
+ return errors;
+}
+
+static uint8_t
+ao_random_data(uint8_t *out, uint8_t out_len)
+{
+ uint8_t len = random() % (out_len + 1);
+ uint8_t i;
+
+ for (i = 0; i < len; i++)
+ out[i] = random();
+ return len;
+}
+
+
+int
+main(int argc, char **argv)
+{
+ int trial;
+
+ uint8_t original[120];
+ uint8_t original_len;
+
+ uint8_t encode[ENCODE_LEN(sizeof(original))];
+ int encode_len;
+
+ uint8_t transmit[EXPAND_LEN(sizeof(original))];
+ int transmit_len;
+
+ uint8_t receive[EXPAND_LEN(sizeof(original))];
+ int receive_len, receive_errors;
+
+ uint8_t decode[DECODE_LEN(sizeof(original))];
+ int decode_len;
+
+ 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));
+
+ /* Encode it */
+ encode_len = ao_fec_encode(original, original_len, encode);
+
+ /* Expand from 1-bit-per-symbol to 1-byte-per-symbol */
+ transmit_len = ao_expand(encode, encode_len, transmit);
+
+ /* 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);
+
+ /* 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;
+}
+
+
+++ /dev/null
-/*
- * Copyright © 2012 Keith Packard <keithp@keithp.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- */
-
-#include <ao_fec.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-
-#ifndef RANDOM_MAX
-#define RANDOM_MAX 0x7fffffff
-#endif
-
-static double
-rand_real(void) {
- return (double) random() / (double) RANDOM_MAX;
-}
-
-static double
-gaussian_random(double mean, double dev)
-{
- static int save_x_valid = 0;
- static double save_x;
- double x;
-
- if (save_x_valid)
- {
- x = save_x;
- save_x_valid = 0;
- }
- else
- {
- double w;
- double normal_x1, normal_x2;
-
- do {
- normal_x1 = 2 * rand_real () - 1;
- normal_x2 = 2 * rand_real () - 1;
- w = normal_x1*normal_x1 + normal_x2*normal_x2;
- } while (w >= 1 || w < 1E-30);
-
- w = sqrt(log(w)*(-2./w));
-
- /*
- * normal_x1 and normal_x2 are independent normally
- * distributed variates
- */
-
- x = normal_x1 * w;
- /* save normal_x2 for next call */
- save_x = normal_x2 * w;
- save_x_valid = 1;
- }
- return x * dev + mean;
-}
-
-#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)
-
-static int
-ao_encode(uint8_t *input, int input_len, uint8_t *output)
-{
- 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");
-
- for (i = 0; i < bits_len; i++) {
- b = interleave[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;
-}
-
-static int
-ao_fuzz (uint8_t *in, int in_len, uint8_t *out, double dev)
-{
- int i;
- int errors = 0;
-
- for (i = 0; i < in_len; i++) {
- double error = gaussian_random(0, dev);
- uint8_t byte = in[i];
-
- if (error > 0) {
- if (error > 0xff)
- error = 0xff;
- if (error >= 0x80)
- errors++;
- if (byte < 0x80)
- byte += error;
- else
- byte -= error;
- }
- out[i] = byte;
- }
-
- printf ("Introduced %d errors\n", errors);
- ao_fec_dump_bytes(out, in_len, "Fuzz");
- return in_len;
-}
-
-static int
-ao_decode(uint8_t *bytes, int bytes_len, uint8_t *bits)
-{
- int bits_len;
-
- bits_len = ao_fec_decode(bytes, bytes_len, bits);
-
- 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 encode_len;
-
- uint8_t transmit[RADIO_LEN(sizeof(original))];
- int transmit_len;
-
- uint8_t receive[RADIO_LEN(sizeof(original))];
- int receive_len;
-
- uint8_t decode[INTERLEAVE_LEN(sizeof(original))];
- int decode_len;
-
- encode_len = ao_encode(original, sizeof(original), encode);
-
- transmit_len = ao_radio(encode, encode_len, transmit);
-
- /* apply gaussian noise to test viterbi code against errors */
- receive_len = ao_fuzz(transmit, transmit_len, receive, 0x70);
-
- decode_len = ao_decode(receive, receive_len, decode);
-
- return decode_len >= sizeof(original);
-}
-
-
projects / fw / altos / commitdiff