/* K=7 r=1/2 Viterbi decoder with Intel SIMD
 * May 2001, Phil Karn, KA9Q
 */
#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include "viterbi27.h"
#include "parity.h"

static int V27_init;

int cpu_features(void);

#if defined(MMX)
/*
  Branch metric lookup tables, indexed by state and input symbol
  Mettab27_1 is for the first symbol, Mettab27_2 for the second
  The first index is the input symbol (0-15)
  The second index is the state number
*/
unsigned char Mettab27_1[16][32] __attribute__((aligned(32)));
unsigned char Mettab27_2[16][32] __attribute__((aligned(32)));

#else
unsigned char Branchtab27_1[32] __attribute__ ((aligned(32)));
unsigned char Branchtab27_2[32] __attribute__ ((aligned(32)));
#endif

/* Create a new instance of a Viterbi decoder */
struct v27 *create_viterbi27(int len){
  void *blk;
  struct v27 *vp;
  int state;

  if(!V27_init){
    /* Check that CPU has required features */
#if defined(SSE2)
    if(!(cpu_features() & (1 << 26))){
      fprintf(stderr,"CPU does not support SSE2 instructions\n");
      exit(1);
    }
#elif defined(SSE)
    if(!(cpu_features() & (1 << 25))){
      fprintf(stderr,"CPU does not support SSE instructions\n");
      exit(1);
    }
#elif defined(MMX)
    if(!(cpu_features() & (1 << 23))){
      fprintf(stderr,"CPU does not support MMX instructions\n");
      exit(1);
    }
#endif
    /* Initialize metric tables */
    for(state=0;state < 32;state++){
#ifdef MMX
      int symbol;
      for(symbol = 0;symbol < 16;symbol++){
	Mettab27_1[symbol][state] = parity((2*state) & V27POLYA) ? (15-symbol):symbol;
	Mettab27_2[symbol][state] = parity((2*state) & V27POLYB) ? (15-symbol):symbol;
      }
#else
      Branchtab27_1[state] = parity((2*state) & V27POLYA) ? 15:0;
      Branchtab27_2[state] = parity((2*state) & V27POLYB) ? 15:0;
#endif
    }
    V27_init = 1;
  }
  /* Malloc only guarantees 8-byte alignment, but we want to ensure that
   * the path metric arrays are on 32-byte boundaries. At least 16-byte
   * alignment is mandatory in the SSE2 version, but the Pentium III
   * cache line size is 32 bytes
   */
  blk = malloc(sizeof(struct v27)+32);
  if((int)blk & 31){
    /* Not on 32-byte boundary; shift up */
    vp = (struct v27 *)(((int)blk + 32) & ~31);
  } else {
    vp = (struct v27 *)blk;
  }
  vp->alloc_blk = blk; /* Record original pointer from malloc for use by free() */

  /* The decisions only need be 32-bit aligned */
#if defined(MMX)
  vp->dp = vp->decisions = malloc((len+6)*64);
#else
  vp->dp = vp->decisions = malloc((len+6)*8);
#endif

  vp->old_metrics = vp->metrics1;
  vp->new_metrics = vp->metrics2;
  return vp;
}

/* Initialize Viterbi decoder for start of new frame */
int init_viterbi27(struct v27 *vp,int starting_state){
  memset(vp->metrics1,60,64);
  vp->old_metrics = vp->metrics1;
  vp->new_metrics = vp->metrics2;
  vp->dp = vp->decisions;
  vp->old_metrics[starting_state & 63] = 0; /* Bias known start state */
  return 0;
}

/* Do Viterbi chainback */
int chainback_viterbi27(
      struct v27 *vp,
      unsigned char *data, /* Decoded output data */
      unsigned int nbits, /* Number of data bits */
      unsigned int endstate){ /* Terminal encoder state */

  int k;
  decision_t *decisions = vp->decisions;

  /* Make room beyond the end of the encoder register so we can
   * accumulate a full byte of decoded data
   */
  endstate %= 64;
  endstate <<= 2;
  decisions += 6; /* Look past tail */

  while(nbits-- != 0){
    k = EXTRACT_DECISION(&decisions[nbits],endstate >> 2);
    /* The store into data[] only needs to be done every 8 bits.
     * But this avoids a conditional branch, and the writes will
     * combine in the cache anyway
     */
    data[nbits>>3] = endstate = (endstate >> 1) | (k << 7);
  }
  return 0;
}

/* Delete instance of a Viterbi decoder */
void delete_viterbi27(struct v27 *vp){
  if(vp != NULL){
    free(vp->metrics1);
    free(vp->metrics2);
    free(vp->decisions);
    free(vp->alloc_blk);
  }
}
/* C-language butterfly */
#define BFLY(i) {\
char metric,m0,m1,decision;\
    metric = ((Branchtab27_1[i] ^ sym1) + (Branchtab27_2[i] ^ sym2) + 1)/2;\
    m0 = vp->old_metrics[i] + metric;\
    m1 = vp->old_metrics[i+32] + (15 - metric);\
    decision = (m0-m1) > 0;\
    vp->new_metrics[2*i] = decision ? m1 : m0;\
    vp->dp->w[i/16] |= decision << ((2*i)&31);\
    m0 -= (metric+metric-15);\
    m1 += (metric+metric-15);\
    decision = (m0-m1) > 0;\
    vp->new_metrics[2*i+1] = decision ? m1 : m0;\
    vp->dp->w[i/16] |= decision << ((2*i+1)&31);\
}

#if !defined(MMX) && !defined(SSE) & !defined(SSE2)
int update_viterbi27(struct v27 *vp,unsigned char sym1,
     unsigned char sym2){

  unsigned char *tmp;
  vp->dp->w[0] = vp->dp->w[1] = 0;

  BFLY(0);
  BFLY(1);
  BFLY(2);
  BFLY(3);
  BFLY(4);
  BFLY(5);
  BFLY(6);
  BFLY(7);
  BFLY(8);
  BFLY(9);
  BFLY(10);
  BFLY(11);
  BFLY(12);
  BFLY(13);
  BFLY(14);
  BFLY(15);
  BFLY(16);
  BFLY(17);
  BFLY(18);
  BFLY(19);
  BFLY(20);
  BFLY(21);
  BFLY(22);
  BFLY(23);
  BFLY(24);
  BFLY(25);
  BFLY(26);
  BFLY(27);
  BFLY(28);
  BFLY(29);
  BFLY(30);
  BFLY(31);

  vp->dp++;
  tmp = vp->old_metrics;
  vp->old_metrics = vp->new_metrics;
  vp->new_metrics = tmp;
  
  return 0;
}
#endif