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 struct ao_soft_sym {
  22         uint8_t a, b;
  23 };
  24
  25 #define NUM_STATE       8
  26 #define NUM_HIST        8
  27 #define MOD_HIST(b)     ((b) & 7)
  28
  29 static const struct ao_soft_sym ao_fec_decode_table[NUM_STATE][2] = {
  30 /* next        0              1          state */
  31         { { 0x00, 0x00 }, { 0xff, 0xff } } ,    /* 000 */
  32         { { 0x00, 0xff }, { 0xff, 0x00 } },     /* 001 */
  33         { { 0xff, 0xff }, { 0x00, 0x00 } },     /* 010 */
  34         { { 0xff, 0x00 }, { 0x00, 0xff } },     /* 011 */
  35         { { 0xff, 0xff }, { 0x00, 0x00 } },     /* 100 */
  36         { { 0xff, 0x00 }, { 0x00, 0xff } },     /* 101 */
  37         { { 0x00, 0x00 }, { 0xff, 0xff } },     /* 110 */
  38         { { 0x00, 0xff }, { 0xff, 0x00 } }      /* 111 */
  39 };
  40
  41 static inline uint8_t
  42 ao_next_state(uint8_t state, uint8_t bit)
  43 {
  44         return ((state << 1) | bit) & 0x7;
  45 }
  46
  47 static inline uint16_t ao_abs(int16_t x) { return x < 0 ? -x : x; }
  48
  49 static inline uint16_t
  50 ao_cost(struct ao_soft_sym a, struct ao_soft_sym b)
  51 {
  52         return ao_abs(a.a - b.a) + ao_abs(a.b - b.b);
  53 }
  54
  55 /*
  56  * 'in' is 8-bits per symbol soft decision data
  57  * 'len' is input byte length. 'out' must be
  58  * 'len'/16 bytes long
  59  */
  60
  61 uint8_t
  62 ao_fec_decode(uint8_t *in, uint16_t len, uint8_t *out)
  63 {
  64         static uint16_t cost[2][NUM_STATE];             /* path cost */
  65         static uint16_t bits[2][NUM_STATE];             /* save bits to quickly output them */
  66         uint16_t        i;                              /* input byte index */
  67         uint16_t        b;                              /* encoded symbol index (bytes/2) */
  68         uint16_t        o;                              /* output bit index */
  69         uint8_t         p;                              /* previous cost/bits index */
  70         uint8_t         n;                              /* next cost/bits index */
  71         uint8_t         state;                          /* state index */
  72         uint8_t         bit;                            /* original encoded bit index */
  73
  74         p = 0;
  75         for (state = 0; state < NUM_STATE; state++) {
  76                 cost[0][state] = 0xffff;
  77                 bits[0][state] = 0;
  78         }
  79         cost[0][0] = 0;
  80
  81         o = 0;
  82         for (i = 0; i < len; i += 2) {
  83                 b = i/2;
  84                 n = p ^ 1;
  85                 struct ao_soft_sym s = { .a = in[i], .b = in[i+1] };
  86
  87                 /* Reset next costs to 'impossibly high' values so that
  88                  * the first path through this state is cheaper than this
  89                  */
  90                 for (state = 0; state < NUM_STATE; state++)
  91                         cost[n][state] = 0xffff;
  92
  93                 /* Compute path costs and accumulate output bit path
  94                  * for each state and encoded bit value
  95                  */
  96                 for (state = 0; state < NUM_STATE; state++) {
  97                         for (bit = 0; bit < 2; bit++) {
  98                                 int     bit_cost = cost[p][state] + ao_cost(s, ao_fec_decode_table[state][bit]);
  99                                 uint8_t bit_state = ao_next_state(state, bit);
 100
 101                                 /* Only track the minimal cost to reach
 102                                  * this state; the best path can never
 103                                  * go through the higher cost paths as
 104                                  * total path cost is cumulative
 105                                  */
 106                                 if (bit_cost < cost[n][bit_state]) {
 107                                         cost[n][bit_state] = bit_cost;
 108                                         bits[n][bit_state] = (bits[p][state] << 1) | (state & 1);
 109                                 }
 110                         }
 111                 }
 112
 113 #if 0
 114                 printf ("bit %3d symbol %2x %2x:", i/2, s.a, s.b);
 115                 for (state = 0; state < NUM_STATE; state++) {
 116                         printf (" %5d(%04x)", cost[n][state], bits[n][state]);
 117                 }
 118                 printf ("\n");
 119 #endif
 120                 p = n;
 121
 122                 /* A loop is needed to handle the last output byte. It
 123                  * won't have a full NUM_HIST bits of future data to
 124                  * perform full error correction, but we might as well
 125                  * give the best possible answer anyways.
 126                  */
 127                 while ((b - o) >= (8 + NUM_HIST) || (i + 2 >= len && b > o)) {
 128
 129                         /* Compute number of bits to the end of the
 130                          * last full byte of data. This is generally
 131                          * NUM_HIST, unless we've reached
 132                          * the end of the input, in which case
 133                          * it will be seven.
 134                          */
 135                         int8_t          dist = b - (o + 8);     /* distance to last ready-for-writing bit */
 136                         uint16_t        min_cost;               /* lowest cost */
 137                         uint8_t         min_state;              /* lowest cost state */
 138
 139                         /* Find the best fit at the current point
 140                          * of the decode.
 141                          */
 142                         min_cost = cost[p][0];
 143                         min_state = 0;
 144                         for (state = 1; state < NUM_STATE; state++) {
 145                                 if (cost[p][state] < min_cost) {
 146                                         min_cost = cost[p][state];
 147                                         min_state = state;
 148                                 }
 149                         }
 150
 151                         /* The very last byte of data has the very last bit
 152                          * of data left in the state value; just smash the
 153                          * bits value in place and reset the 'dist' from
 154                          * -1 to 0 so that the full byte is read out
 155                          */
 156                         if (dist < 0) {
 157                                 bits[p][min_state] = (bits[p][min_state] << 1) | (min_state & 1);
 158                                 dist = 0;
 159                         }
 160
 161 #if 0
 162                         printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x\n",
 163                                 i/2, min_cost, o + 8, min_state, (bits[p][min_state] >> dist) & 0xff);
 164 #endif
 165                         out[o >> 3] = bits[p][min_state] >> dist;
 166                         o += 8;
 167                 }
 168         }
 169         return len/16;
 170 }