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 struct ao_soft_sym {
  27         uint8_t a, b;
  28 };
  29
  30 static const struct ao_soft_sym ao_fec_decode_table[16] = {
  31 /* next        0              1          state */
  32         { 0x00, 0x00 }, { 0xff, 0xff }, /* 000 */
  33         { 0x00, 0xff }, { 0xff, 0x00 }, /* 001 */
  34         { 0xff, 0xff }, { 0x00, 0x00 }, /* 010 */
  35         { 0xff, 0x00 }, { 0x00, 0xff }, /* 011 */
  36         { 0xff, 0xff }, { 0x00, 0x00 }, /* 100 */
  37         { 0xff, 0x00 }, { 0x00, 0xff }, /* 101 */
  38         { 0x00, 0x00 }, { 0xff, 0xff }, /* 110 */
  39         { 0x00, 0xff }, { 0xff, 0x00 }  /* 111 */
  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 static inline uint8_t
  53 ao_next_state(uint8_t state, uint8_t bit)
  54 {
  55         return ((state << 1) | bit) & 0x7;
  56 }
  57
  58 static inline uint16_t ao_abs(int16_t x) { return x < 0 ? -x : x; }
  59
  60 static inline uint16_t
  61 ao_cost(struct ao_soft_sym a, struct ao_soft_sym b)
  62 {
  63         return ao_abs(a.a - b.a) + ao_abs(a.b - b.b);
  64 }
  65
  66 #define NUM_STATE       8
  67
  68 uint8_t
  69 ao_fec_decode(uint8_t *in, int len, uint8_t *out)
  70 {
  71         uint16_t        cost[2][NUM_STATE], min_cost;
  72         uint8_t         prev[len/2 + 1][NUM_STATE];
  73         uint16_t        prev_bits[2][NUM_STATE];
  74         int             i, b, dump;
  75         uint8_t         p, n;
  76         uint8_t         state = 0, min_state;
  77
  78         p = 0;
  79         for (state = 0; state < NUM_STATE; state++) {
  80                 cost[0][state] = 0xffff;
  81                 prev_bits[0][state] = 0;
  82         }
  83         cost[0][0] = 0;
  84
  85         min_state = 0;
  86         min_cost = 0;
  87         dump = 0;
  88         for (i = 0; i < len; i += 2) {
  89                 b = i/2;
  90                 n = p ^ 1;
  91                 struct ao_soft_sym s = { .a = in[i], .b = in[i+1] };
  92
  93                 for (state = 0; state < NUM_STATE; state++)
  94                         cost[n][state] = 0xffff;
  95
  96                 for (state = 0; state < NUM_STATE; state++) {
  97                         int     zero_cost = ao_cost(s, ao_fec_decode_table[state * 2 + 0]);
  98                         int     one_cost = ao_cost(s, ao_fec_decode_table[state * 2 + 1]);
  99
 100                         uint8_t zero_state = ao_next_state(state, 0);
 101                         uint8_t one_state = ao_next_state(state, 1);
 102
 103                         zero_cost += cost[p][state];
 104                         one_cost += cost[p][state];
 105                         if (zero_cost < cost[n][zero_state]) {
 106                                 prev[b+1][zero_state] = state;
 107                                 cost[n][zero_state] = zero_cost;
 108                                 prev_bits[n][zero_state] = (prev_bits[p][state] << 1) | (state & 1);
 109                         }
 110
 111                         if (one_cost < cost[n][one_state]) {
 112                                 prev[b+1][one_state] = state;
 113                                 cost[n][one_state] = one_cost;
 114                                 prev_bits[n][one_state] = (prev_bits[p][state] << 1) | (state & 1);
 115                         }
 116                 }
 117
 118 #if 0
 119                 printf ("bit %3d symbol %2x %2x:", i/2, s.a, s.b);
 120                 for (state = 0; state < NUM_STATE; state++) {
 121                         printf (" %5d(%04x)", cost[n][state], prev_bits[n][state]);
 122                 }
 123                 printf ("\n");
 124 #endif
 125                 p = n;
 126
 127                 b++;
 128                 if ((b - dump) > 16 || i + 2 >= len) {
 129                         uint8_t dist = b - (dump + 9);
 130                         uint8_t rev;
 131                         min_cost = cost[p][0];
 132                         min_state = 0;
 133                         for (state = 1; state < NUM_STATE; state++) {
 134                                 if (cost[p][state] < min_cost) {
 135                                         min_cost = cost[p][state];
 136                                         min_state = state;
 137                                 }
 138                         }
 139                         for (rev = 0; rev < dist; rev++) {
 140                                 min_state = prev[b][min_state];
 141                                 b--;
 142                         }
 143 #if 0
 144                         printf ("\tbit %3d min_cost %5d old bit %3d old_state %x bits %02x\n",
 145                                 i/2, min_cost, b-1, min_state, (prev_bits[p][min_state] >> dist) & 0xff);
 146 #endif
 147                         out[dump/8] = prev_bits[p][min_state] >> dist;
 148                         dump = b - 1;
 149                 }
 150
 151         }
 152         return len/16;
 153 }