git.gag.com Git - fw/altos/blob - src/core/ao_fec_rx.c

   1 /*
   2  * Copyright © 2012 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include <ao_fec.h>
  19 #include <stdio.h>
  20
  21 /*
  22  * byte order repeats through 3 2 1 0
  23  *
  24  * bit-pair order repeats through
  25  *
  26  *  1/0 3/2 5/4 7/6
  27  *
  28  * So, the over all order is:
  29  *
  30  *      3,1/0   2,1/0   1,1/0   0,1/0
  31  *      3,3/2   2,3/2   1,3/2   0,3/2
  32  *      3,5/4   2,5/4   1,5/4   0,5/4
  33  *      3,7/6   2,7/6   1,7/6   0,7/6
  34  *
  35  * The raw bit order is thus
  36  *
  37  *      1e/1f   16/17   0e/0f   06/07
  38  *      1c/1d   14/15   0c/0d   04/05
  39  *      1a/1b   12/13   0a/0b   02/03
  40  *      18/19   10/11   08/09   00/01
  41  */
  42
  43 static inline uint16_t ao_interleave_index(uint16_t i) {
  44         uint8_t         l = i & 0x1e;
  45         uint16_t        h = i & ~0x1e;
  46         uint8_t         o = 0x1e ^ (((l >> 2) & 0x6) | ((l << 2) & 0x18));
  47         return h | o;
  48 }
  49
  50 struct ao_soft_sym {
  51         uint8_t a, b;
  52 };
  53
  54 #define NUM_STATE       8
  55 #define NUM_HIST        8
  56 #define MOD_HIST(b)     ((b) & 7)
  57
  58 #define V_0             0xc0
  59 #define V_1             0x40
  60
  61 static const struct ao_soft_sym ao_fec_decode_table[NUM_STATE][2] = {
  62 /* next        0              1          state */
  63         { { V_0, V_0 }, { V_1, V_1 } } ,        /* 000 */
  64         { { V_0, V_1 }, { V_1, V_0 } }, /* 001 */
  65         { { V_1, V_1 }, { V_0, V_0 } }, /* 010 */
  66         { { V_1, V_0 }, { V_0, V_1 } }, /* 011 */
  67         { { V_1, V_1 }, { V_0, V_0 } }, /* 100 */
  68         { { V_1, V_0 }, { V_0, V_1 } }, /* 101 */
  69         { { V_0, V_0 }, { V_1, V_1 } }, /* 110 */
  70         { { V_0, V_1 }, { V_1, V_0 } }  /* 111 */
  71 };
  72
  73 static inline uint8_t
  74 ao_next_state(uint8_t state, uint8_t bit)
  75 {
  76         return ((state << 1) | bit) & 0x7;
  77 }
  78
  79 static inline uint16_t ao_abs(int16_t x) { return x < 0 ? -x : x; }
  80
  81 static inline uint16_t
  82 ao_cost(struct ao_soft_sym a, struct ao_soft_sym b)
  83 {
  84         return ao_abs(a.a - b.a) + ao_abs(a.b - b.b);
  85 }
  86
  87 /*
  88  * 'in' is 8-bits per symbol soft decision data
  89  * 'len' is input byte length. 'out' must be
  90  * 'len'/16 bytes long
  91  */
  92
  93 uint8_t
  94 ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out, uint8_t out_len, uint16_t (*callback)())
  95 {
  96         static uint16_t cost[2][NUM_STATE];             /* path cost */
  97         static uint16_t bits[2][NUM_STATE];             /* save bits to quickly output them */
  98         uint16_t        i;                              /* input byte index */
  99         uint16_t        b;                              /* encoded symbol index (bytes/2) */
 100         uint16_t        o;                              /* output bit index */
 101         uint8_t         p;                              /* previous cost/bits index */
 102         uint8_t         n;                              /* next cost/bits index */
 103         uint8_t         state;                          /* state index */
 104         uint8_t         bit;                            /* original encoded bit index */
 105         const uint8_t   *whiten = ao_fec_whiten_table;
 106         uint16_t        interleave;                     /* input byte array index */
 107         struct ao_soft_sym      s;                      /* input symbol pair */
 108         uint16_t        avail;
 109
 110         p = 0;
 111         for (state = 0; state < NUM_STATE; state++) {
 112                 cost[0][state] = 0xffff;
 113                 bits[0][state] = 0;
 114         }
 115         cost[0][0] = 0;
 116
 117         if (callback)
 118                 avail = 0;
 119         else
 120                 avail = len;
 121
 122         o = 0;
 123         for (i = 0; i < len; i += 2) {
 124                 b = i/2;
 125                 n = p ^ 1;
 126
 127                 if (!avail) {
 128                         avail = callback();
 129                         if (!avail)
 130                                 break;
 131                 }
 132
 133                 /* Fetch one pair of input bytes, de-interleaving
 134                  * the input.
 135                  */
 136                 interleave = ao_interleave_index(i);
 137                 s.a = in[interleave];
 138                 s.b = in[interleave+1];
 139
 140                 /* Reset next costs to 'impossibly high' values so that
 141                  * the first path through this state is cheaper than this
 142                  */
 143                 for (state = 0; state < NUM_STATE; state++)
 144                         cost[n][state] = 0xffff;
 145
 146                 /* Compute path costs and accumulate output bit path
 147                  * for each state and encoded bit value
 148                  */
 149                 for (state = 0; state < NUM_STATE; state++) {
 150                         for (bit = 0; bit < 2; bit++) {
 151                                 int     bit_cost = cost[p][state] + ao_cost(s, ao_fec_decode_table[state][bit]);
 152                                 uint8_t bit_state = ao_next_state(state, bit);
 153
 154                                 /* Only track the minimal cost to reach
 155                                  * this state; the best path can never
 156                                  * go through the higher cost paths as
 157                                  * total path cost is cumulative
 158                                  */
 159                                 if (bit_cost < cost[n][bit_state]) {
 160                                         cost[n][bit_state] = bit_cost;
 161                                         bits[n][bit_state] = (bits[p][state] << 1) | (state & 1);
 162                                 }
 163                         }
 164                 }
 165
 166 #if 0
 167                 printf ("bit %3d symbol %2x %2x:", i/2, s.a, s.b);
 168                 for (state = 0; state < NUM_STATE; state++) {
 169                         printf (" %5d(%04x)", cost[n][state], bits[n][state]);
 170                 }
 171                 printf ("\n");
 172 #endif
 173                 p = n;
 174
 175                 /* A loop is needed to handle the last output byte. It
 176                  * won't have any bits of future data to perform full
 177                  * error correction, but we might as well give the
 178                  * best possible answer anyways.
 179                  */
 180                 while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) {
 181
 182                         /* Compute number of bits to the end of the
 183                          * last full byte of data. This is generally
 184                          * NUM_HIST, unless we've reached
 185                          * the end of the input, in which case
 186                          * it will be seven.
 187                          */
 188                         int8_t          dist = b - (o + 8);     /* distance to last ready-for-writing bit */
 189                         uint16_t        min_cost;               /* lowest cost */
 190                         uint8_t         min_state;              /* lowest cost state */
 191
 192                         /* Find the best fit at the current point
 193                          * of the decode.
 194                          */
 195                         min_cost = cost[p][0];
 196                         min_state = 0;
 197                         for (state = 1; state < NUM_STATE; state++) {
 198                                 if (cost[p][state] < min_cost) {
 199                                         min_cost = cost[p][state];
 200                                         min_state = state;
 201                                 }
 202                         }
 203
 204                         /* The very last byte of data has the very last bit
 205                          * of data left in the state value; just smash the
 206                          * bits value in place and reset the 'dist' from
 207                          * -1 to 0 so that the full byte is read out
 208                          */
 209                         if (dist < 0) {
 210                                 bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1);
 211                                 dist = 0;
 212                         }
 213
 214 #if 0
 215                         printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n",
 216                                 i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten);
 217 #endif
 218                         if (out_len) {
 219                                 *out++ = (bits[p][min_state] >> dist) ^ *whiten++;
 220                                 --out_len;
 221                         }
 222                         o += 8;
 223                 }
 224         }
 225         return len/16;
 226 }