git.gag.com Git - fw/altos/blob - src/core/ao_viterbi.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 static const struct ao_soft_sym ao_fec_decode_table[NUM_STATE][2] = {
  59 /* next        0              1          state */
  60         { { 0x00, 0x00 }, { 0xff, 0xff } } ,    /* 000 */
  61         { { 0x00, 0xff }, { 0xff, 0x00 } },     /* 001 */
  62         { { 0xff, 0xff }, { 0x00, 0x00 } },     /* 010 */
  63         { { 0xff, 0x00 }, { 0x00, 0xff } },     /* 011 */
  64         { { 0xff, 0xff }, { 0x00, 0x00 } },     /* 100 */
  65         { { 0xff, 0x00 }, { 0x00, 0xff } },     /* 101 */
  66         { { 0x00, 0x00 }, { 0xff, 0xff } },     /* 110 */
  67         { { 0x00, 0xff }, { 0xff, 0x00 } }      /* 111 */
  68 };
  69
  70 static inline uint8_t
  71 ao_next_state(uint8_t state, uint8_t bit)
  72 {
  73         return ((state << 1) | bit) & 0x7;
  74 }
  75
  76 static inline uint16_t ao_abs(int16_t x) { return x < 0 ? -x : x; }
  77
  78 static inline uint16_t
  79 ao_cost(struct ao_soft_sym a, struct ao_soft_sym b)
  80 {
  81         return ao_abs(a.a - b.a) + ao_abs(a.b - b.b);
  82 }
  83
  84 /*
  85  * 'in' is 8-bits per symbol soft decision data
  86  * 'len' is input byte length. 'out' must be
  87  * 'len'/16 bytes long
  88  */
  89
  90 uint8_t
  91 ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out)
  92 {
  93         static uint16_t cost[2][NUM_STATE];             /* path cost */
  94         static uint16_t bits[2][NUM_STATE];             /* save bits to quickly output them */
  95         uint16_t        i;                              /* input byte index */
  96         uint16_t        b;                              /* encoded symbol index (bytes/2) */
  97         uint16_t        o;                              /* output bit index */
  98         uint8_t         p;                              /* previous cost/bits index */
  99         uint8_t         n;                              /* next cost/bits index */
 100         uint8_t         state;                          /* state index */
 101         uint8_t         bit;                            /* original encoded bit index */
 102         const uint8_t   *whiten = ao_fec_whiten_table;
 103         uint16_t        interleave;                     /* input byte array index */
 104         struct ao_soft_sym      s;                      /* input symbol pair */
 105
 106         p = 0;
 107         for (state = 0; state < NUM_STATE; state++) {
 108                 cost[0][state] = 0xffff;
 109                 bits[0][state] = 0;
 110         }
 111         cost[0][0] = 0;
 112
 113         o = 0;
 114         for (i = 0; i < len; i += 2) {
 115                 b = i/2;
 116                 n = p ^ 1;
 117
 118                 /* Fetch one pair of input bytes, de-interleaving
 119                  * the input.
 120                  */
 121                 interleave = ao_interleave_index(i);
 122                 s.a = in[interleave];
 123                 s.b = in[interleave+1];
 124
 125                 /* Reset next costs to 'impossibly high' values so that
 126                  * the first path through this state is cheaper than this
 127                  */
 128                 for (state = 0; state < NUM_STATE; state++)
 129                         cost[n][state] = 0xffff;
 130
 131                 /* Compute path costs and accumulate output bit path
 132                  * for each state and encoded bit value
 133                  */
 134                 for (state = 0; state < NUM_STATE; state++) {
 135                         for (bit = 0; bit < 2; bit++) {
 136                                 int     bit_cost = cost[p][state] + ao_cost(s, ao_fec_decode_table[state][bit]);
 137                                 uint8_t bit_state = ao_next_state(state, bit);
 138
 139                                 /* Only track the minimal cost to reach
 140                                  * this state; the best path can never
 141                                  * go through the higher cost paths as
 142                                  * total path cost is cumulative
 143                                  */
 144                                 if (bit_cost < cost[n][bit_state]) {
 145                                         cost[n][bit_state] = bit_cost;
 146                                         bits[n][bit_state] = (bits[p][state] << 1) | (state & 1);
 147                                 }
 148                         }
 149                 }
 150
 151 #if 0
 152                 printf ("bit %3d symbol %2x %2x:", i/2, s.a, s.b);
 153                 for (state = 0; state < NUM_STATE; state++) {
 154                         printf (" %5d(%04x)", cost[n][state], bits[n][state]);
 155                 }
 156                 printf ("\n");
 157 #endif
 158                 p = n;
 159
 160                 /* A loop is needed to handle the last output byte. It
 161                  * won't have any bits of future data to perform full
 162                  * error correction, but we might as well give the
 163                  * best possible answer anyways.
 164                  */
 165                 while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) {
 166
 167                         /* Compute number of bits to the end of the
 168                          * last full byte of data. This is generally
 169                          * NUM_HIST, unless we've reached
 170                          * the end of the input, in which case
 171                          * it will be seven.
 172                          */
 173                         int8_t          dist = b - (o + 8);     /* distance to last ready-for-writing bit */
 174                         uint16_t        min_cost;               /* lowest cost */
 175                         uint8_t         min_state;              /* lowest cost state */
 176
 177                         /* Find the best fit at the current point
 178                          * of the decode.
 179                          */
 180                         min_cost = cost[p][0];
 181                         min_state = 0;
 182                         for (state = 1; state < NUM_STATE; state++) {
 183                                 if (cost[p][state] < min_cost) {
 184                                         min_cost = cost[p][state];
 185                                         min_state = state;
 186                                 }
 187                         }
 188
 189                         /* The very last byte of data has the very last bit
 190                          * of data left in the state value; just smash the
 191                          * bits value in place and reset the 'dist' from
 192                          * -1 to 0 so that the full byte is read out
 193                          */
 194                         if (dist < 0) {
 195                                 bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1);
 196                                 dist = 0;
 197                         }
 198
 199 #if 0
 200                         printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x whiten %0x\n",
 201                                 i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff, *whiten);
 202 #endif
 203                         *out++ = (bits[p][min_state] >> dist) ^ *whiten++;
 204                         o += 8;
 205                 }
 206         }
 207         return len/16;
 208 }