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  * 'input' is 8-bits per symbol soft decision data
  23  * 'len' is output byte length
  24  */
  25
  26 static const uint8_t ao_fec_encode_table[16] = {
  27 /* next 0  1      state */
  28         0, 3,   /* 000 */
  29         1, 2,   /* 001 */
  30         3, 0,   /* 010 */
  31         2, 1,   /* 011 */
  32         3, 0,   /* 100 */
  33         2, 1,   /* 101 */
  34         0, 3,   /* 110 */
  35         1, 2    /* 111 */
  36 };
  37
  38 struct ao_soft_sym {
  39         uint8_t a, b;
  40 };
  41
  42 struct ao_soft_sym
  43 ao_soft_sym(uint8_t bits)
  44 {
  45         struct ao_soft_sym      s;
  46
  47         s.a = ((bits & 2) >> 1) * 0xff;
  48         s.b = (bits & 1) * 0xff;
  49         return s;
  50 }
  51
  52 uint8_t
  53 ao_next_state(uint8_t state, uint8_t bit)
  54 {
  55         return ((state << 1) | bit) & 0x7;
  56 }
  57
  58 static inline abs(int x) { return x < 0 ? -x : x; }
  59
  60 int
  61 ao_cost(struct ao_soft_sym a, struct ao_soft_sym b)
  62 {
  63         return abs(a.a - b.a) + abs(a.b - b.b);
  64 }
  65
  66 uint8_t
  67 ao_fec_decode(uint8_t *in, int len, uint8_t *out)
  68 {
  69         int     cost[len/2 + 1][8];
  70         uint8_t prev[len/2 + 1][8];
  71         int     c;
  72         int     i, b;
  73         uint8_t state = 0, min_state;
  74         uint8_t bits[len/2];
  75
  76         for (c = 0; c < 8; c++)
  77                 cost[0][c] = 10000;
  78         cost[0][0] = 0;
  79
  80         for (i = 0; i < len; i += 2) {
  81                 b = i/2;
  82                 struct ao_soft_sym s = { .a = in[i], .b = in[i+1] };
  83
  84                 for (state = 0; state < 8; state++)
  85                         cost[b+1][state] = 10000;
  86
  87                 for (state = 0; state < 8; state++) {
  88                         struct ao_soft_sym zero = ao_soft_sym(ao_fec_encode_table[state * 2 + 0]);
  89                         struct ao_soft_sym one = ao_soft_sym(ao_fec_encode_table[state * 2 + 1]);
  90                         uint8_t zero_state = ao_next_state(state, 0);
  91                         uint8_t one_state = ao_next_state(state, 1);
  92                         int     zero_cost = ao_cost(s, zero);
  93                         int     one_cost = ao_cost(s, one);
  94
  95 #if 0
  96                         printf ("saw %02x %02x expected %02x %02x (%d) or %02x %02x (%d)\n",
  97                                 s.a, s.b, zero.a, zero.b, zero_cost, one.a, one.b, one_cost);
  98 #endif
  99                         zero_cost += cost[b][state];
 100                         one_cost += cost[b][state];
 101                         if (zero_cost < cost[b+1][zero_state]) {
 102                                 prev[b+1][zero_state] = state;
 103                                 cost[b+1][zero_state] = zero_cost;
 104                         }
 105
 106                         if (one_cost < cost[b+1][one_state]) {
 107                                 prev[b+1][one_state] = state;
 108                                 cost[b+1][one_state] = one_cost;
 109                         }
 110                 }
 111
 112                 printf ("bit %3d symbol %2x %2x:", i/2, s.a, s.b);
 113                 for (state = 0; state < 8; state++) {
 114                         printf (" %5d", cost[b+1][state]);
 115                 }
 116                 printf ("\n");
 117         }
 118
 119         b = len / 2;
 120         c = cost[b][0];
 121         min_state = 0;
 122         for (state = 1; state < 8; state++) {
 123                 if (cost[b][state] < c) {
 124                         c = cost[b][state];
 125                         min_state = state;
 126                 }
 127         }
 128
 129         for (b = len/2; b > 0; b--) {
 130                 bits[b-1] = min_state & 1;
 131                 min_state = prev[b][min_state];
 132         }
 133
 134         for (i = 0; i < len/2; i += 8) {
 135                 uint8_t byte;
 136
 137                 byte = 0;
 138                 for (b = 0; b < 8; b++)
 139                         byte = (byte << 1) | bits[i + b];
 140                 out[i/8] = byte;
 141         }
 142         return len/16;
 143 }