//
// FX25_extract.c
//	Author: Jim McGuire KB3MPL
//	Date: 	23 October 2007
//
//
// Accepts an FX.25 byte stream on STDIN, finds the correlation tag, stores 256 bytes,
//   corrects errors with FEC, removes the bit-stuffing, and outputs the resultant AX.25 
//   byte stream out STDOUT.
//
// STDERR prints a bunch of status information about the packet being processed.
//   
//
// Usage : FX25_extract < infile > outfile [2> logfile]
//
//
//
// This program is a single-file implementation of the FX.25 extraction/decode 
// structure for use with FX.25 data frames.  Details of the FX.25 
// specification are available at:
//     http://www.stensat.org/Docs/Docs.htm
//
// This program implements a single RS(255,239) FEC structure.  Future
// releases will incorporate more capabilities as accommodated in the FX.25
// spec.  
//
// The Reed Solomon encoding routines are based on work performed by
// Phil Karn.  Phil was kind enough to release his code under the GPL, as
// noted below.  Consequently, this FX.25 implementation is also released
// under the terms of the GPL.  
//
// Phil Karn's original copyright notice:
  /* Test the Reed-Solomon codecs
   * for various block sizes and with random data and random error patterns
   *
   * Copyright 2002 Phil Karn, KA9Q
   * May be used under the terms of the GNU General Public License (GPL)
   *
   */

#include <stdio.h>
#include <stdlib.h>

//#define	BLOCKSIZE	246		// number of bytes in FEC block	- test value
#define	BLOCKSIZE	255		// number of bytes in FEC block	


#define		CRC_fault	0x01
#define		strip_fault	0x02
#define		autocorr_threshold 35   // max value is 63

#define PC 1  // comment this out for AVR cross-compilation
#define ram_init 1  // use ram init instead of const-array init; comment for AVR, uncomment for PC
//#define DEBUG 5    
               
// select one of the following for embedded applications
#define RS255_239 1     // 16-bytes of FEC
//#define RS255_223 1     // 32-bytes of FEC
//#define RS255_191 1     // 64-bytes of FEC

#define version "v0.0.1"
//-----------------------------------------------------------------------
// Revision History
//-----------------------------------------------------------------------
// 0.0.1  - initial release
//          Modifications from Phil Karn's GPL source code.
//          Initially added code to run with PC file 
//          I/O and use the (255,239) decoder exclusively.  Confirmed that the
//          code produces the correct results.
//  
//-----------------------------------------------------------------------
// 0.0.2  - 


//#include "rs.h"
/* General purpose RS codec, 8-bit symbols */
//int  decode_rs_char(void *rs,unsigned char *data,unsigned char *parity);
//void *init_rs_char(unsigned int symsize,unsigned int gfpoly,unsigned int fcr,unsigned int prim,unsigned int nroots);
//void free_rs_char(void *rs);


#define	min(a,b)	((a) < (b) ? (a) : (b))

//#include "char.h"
#define DTYPE unsigned char

/* Reed-Solomon codec control block */
struct rs {
  unsigned int mm;              /* Bits per symbol */
  unsigned int nn;              /* Symbols per block (= (1<<mm)-1) */
  unsigned char *alpha_to;      /* log lookup table */
  unsigned char *index_of;      /* Antilog lookup table */
  unsigned char *genpoly;       /* Generator polynomial */
  unsigned int nroots;     /* Number of generator roots = number of parity symbols */
  unsigned char fcr;        /* First consecutive root, index form */
  unsigned char prim;       /* Primitive element, index form */
  unsigned char iprim;      /* prim-th root of 1, index form */
};

#ifndef ram_init
  char rs_ram[sizeof(struct rs)];
#endif

static inline int modnn(struct rs *rs,int x){
  while (x >= rs->nn) {
    x -= rs->nn;
    x = (x >> rs->mm) + (x & rs->nn);
  }
  return x;
}
#define MODNN(x) modnn(rs,x)

#define MM (rs->mm)
#define NN (rs->nn)
#define ALPHA_TO (rs->alpha_to) 
#define INDEX_OF (rs->index_of)
#define GENPOLY (rs->genpoly)
#define NROOTS (rs->nroots)
#define FCR (rs->fcr)
#define PRIM (rs->prim)
#define IPRIM (rs->iprim)
#define A0 (NN)

#define ENCODE_RS encode_rs_char
#define DECODE_RS decode_rs_char
#define INIT_RS init_rs_char

#ifdef ram_init
#define FREE_RS free_rs_char
#endif


struct {
  int symsize;
  int genpoly;
  int fcs;
  int prim;
  int nroots;
//  int ntrials;  // not used in embedded app
} Tab[] = {
  {8, 0x11d,   1,   1, 16 },  // RS(255,239)
  {8, 0x11d,   1,   1, 32 },  // RS(255,223)
  {8, 0x11d,   1,   1, 64 },  // RS(255,191)
//  {8, 0x11d,   1,   1, 16, 10 },
//  {3, 0xb,     1,   1, 2, 10 },  // comment out unused code elements
//  {4, 0x13,    1,   1, 4, 10 },
//  {5, 0x25,    1,   1, 6, 10 },
//  {6, 0x43,    1,   1, 8, 10 },
//  {7, 0x89,    1,   1, 10, 10 },
//  {8, 0x11d,   1,   1, 32, 10 },
//  {8, 0x187,   112,11, 32, 10 }, /* Duplicates CCSDS codec */
//  {9, 0x211,   1,   1, 32, 10 },
//  {10,0x409,   1,   1, 32, 10 },
//  {11,0x805,   1,   1, 32, 10 },
//  {12,0x1053,  1,   1, 32, 5 },
//  {13,0x201b,  1,   1, 32, 2 },
//  {14,0x4443,  1,   1, 32, 1 },
//  {15,0x8003,  1,   1, 32, 1 },
//  {16,0x1100b, 1,   1, 32, 1 },
  {0, 0, 0, 0, 0}  // removed the trailing zero for the embedded app
};


#define NULL ((void *)0)

#ifndef ram_init
  static const DTYPE alpha_to_const[] = {
	0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x1d,0x3a,0x74,0xe8,0xcd,0x87,0x13,0x26,  // data for RS(255,239)
	0x4c,0x98,0x2d,0x5a,0xb4,0x75,0xea,0xc9,0x8f,0x03,0x06,0x0c,0x18,0x30,0x60,0xc0,
	0x9d,0x27,0x4e,0x9c,0x25,0x4a,0x94,0x35,0x6a,0xd4,0xb5,0x77,0xee,0xc1,0x9f,0x23,
	0x46,0x8c,0x05,0x0a,0x14,0x28,0x50,0xa0,0x5d,0xba,0x69,0xd2,0xb9,0x6f,0xde,0xa1,
	0x5f,0xbe,0x61,0xc2,0x99,0x2f,0x5e,0xbc,0x65,0xca,0x89,0x0f,0x1e,0x3c,0x78,0xf0,
	0xfd,0xe7,0xd3,0xbb,0x6b,0xd6,0xb1,0x7f,0xfe,0xe1,0xdf,0xa3,0x5b,0xb6,0x71,0xe2,
	0xd9,0xaf,0x43,0x86,0x11,0x22,0x44,0x88,0x0d,0x1a,0x34,0x68,0xd0,0xbd,0x67,0xce,
	0x81,0x1f,0x3e,0x7c,0xf8,0xed,0xc7,0x93,0x3b,0x76,0xec,0xc5,0x97,0x33,0x66,0xcc,
	0x85,0x17,0x2e,0x5c,0xb8,0x6d,0xda,0xa9,0x4f,0x9e,0x21,0x42,0x84,0x15,0x2a,0x54,
	0xa8,0x4d,0x9a,0x29,0x52,0xa4,0x55,0xaa,0x49,0x92,0x39,0x72,0xe4,0xd5,0xb7,0x73,
	0xe6,0xd1,0xbf,0x63,0xc6,0x91,0x3f,0x7e,0xfc,0xe5,0xd7,0xb3,0x7b,0xf6,0xf1,0xff,
	0xe3,0xdb,0xab,0x4b,0x96,0x31,0x62,0xc4,0x95,0x37,0x6e,0xdc,0xa5,0x57,0xae,0x41,
	0x82,0x19,0x32,0x64,0xc8,0x8d,0x07,0x0e,0x1c,0x38,0x70,0xe0,0xdd,0xa7,0x53,0xa6,
	0x51,0xa2,0x59,0xb2,0x79,0xf2,0xf9,0xef,0xc3,0x9b,0x2b,0x56,0xac,0x45,0x8a,0x09,
	0x12,0x24,0x48,0x90,0x3d,0x7a,0xf4,0xf5,0xf7,0xf3,0xfb,0xeb,0xcb,0x8b,0x0b,0x16,
	0x2c,0x58,0xb0,0x7d,0xfa,0xe9,0xcf,0x83,0x1b,0x36,0x6c,0xd8,0xad,0x47,0x8e,0x00 };
  static const DTYPE index_of_const[] = {
	0xff,0x00,0x01,0x19,0x02,0x32,0x1a,0xc6,0x03,0xdf,0x33,0xee,0x1b,0x68,0xc7,0x4b,  // data for RS(255,239)
	0x04,0x64,0xe0,0x0e,0x34,0x8d,0xef,0x81,0x1c,0xc1,0x69,0xf8,0xc8,0x08,0x4c,0x71,
	0x05,0x8a,0x65,0x2f,0xe1,0x24,0x0f,0x21,0x35,0x93,0x8e,0xda,0xf0,0x12,0x82,0x45,
	0x1d,0xb5,0xc2,0x7d,0x6a,0x27,0xf9,0xb9,0xc9,0x9a,0x09,0x78,0x4d,0xe4,0x72,0xa6,
	0x06,0xbf,0x8b,0x62,0x66,0xdd,0x30,0xfd,0xe2,0x98,0x25,0xb3,0x10,0x91,0x22,0x88,
	0x36,0xd0,0x94,0xce,0x8f,0x96,0xdb,0xbd,0xf1,0xd2,0x13,0x5c,0x83,0x38,0x46,0x40,
	0x1e,0x42,0xb6,0xa3,0xc3,0x48,0x7e,0x6e,0x6b,0x3a,0x28,0x54,0xfa,0x85,0xba,0x3d,
	0xca,0x5e,0x9b,0x9f,0x0a,0x15,0x79,0x2b,0x4e,0xd4,0xe5,0xac,0x73,0xf3,0xa7,0x57,
	0x07,0x70,0xc0,0xf7,0x8c,0x80,0x63,0x0d,0x67,0x4a,0xde,0xed,0x31,0xc5,0xfe,0x18,
	0xe3,0xa5,0x99,0x77,0x26,0xb8,0xb4,0x7c,0x11,0x44,0x92,0xd9,0x23,0x20,0x89,0x2e,
	0x37,0x3f,0xd1,0x5b,0x95,0xbc,0xcf,0xcd,0x90,0x87,0x97,0xb2,0xdc,0xfc,0xbe,0x61,
	0xf2,0x56,0xd3,0xab,0x14,0x2a,0x5d,0x9e,0x84,0x3c,0x39,0x53,0x47,0x6d,0x41,0xa2,
	0x1f,0x2d,0x43,0xd8,0xb7,0x7b,0xa4,0x76,0xc4,0x17,0x49,0xec,0x7f,0x0c,0x6f,0xf6,
	0x6c,0xa1,0x3b,0x52,0x29,0x9d,0x55,0xaa,0xfb,0x60,0x86,0xb1,0xbb,0xcc,0x3e,0x5a,
	0xcb,0x59,0x5f,0xb0,0x9c,0xa9,0xa0,0x51,0x0b,0xf5,0x16,0xeb,0x7a,0x75,0x2c,0xd7,
	0x4f,0xae,0xd5,0xe9,0xe6,0xe7,0xad,0xe8,0x74,0xd6,0xf4,0xea,0xa8,0x50,0x58,0xaf };
  static const DTYPE genpoly_const[] = {
	0x88,0xf0,0xd0,0xc3,0xb5,0x9e,0xc9,0x64,0x0b,0x53,0xa7,0x6b,0x71,0x6e,0x6a,0x79,0x00 };  // data for RS(255,239)
#endif


#ifdef ram_init
void FREE_RS(void *p){
  struct rs *rs = (struct rs *)p;

  free(rs->alpha_to);
  free(rs->index_of);
  free(rs->genpoly);
  free(rs);
}
#endif

/* Initialize a Reed-Solomon codec
 * symsize = symbol size, bits (1-8)
 * gfpoly = Field generator polynomial coefficients
 * fcr = first root of RS code generator polynomial, index form
 * prim = primitive element to generate polynomial roots
 * nroots = RS code generator polynomial degree (number of roots)
 */
#ifdef ram_init    // use ram-based init on the PC

void *INIT_RS(unsigned int symsize,unsigned int gfpoly,unsigned fcr,unsigned prim, unsigned int nroots){
  struct rs *rs;
  int i, j, sr,root,iprim;

  fprintf(stderr,"Using ram init ...\n\n\r");

  if(symsize > 8*sizeof(DTYPE))
    return NULL; /* Need version with ints rather than chars */

  if(fcr >= (1<<symsize))
    return NULL;
  if(prim == 0 || prim >= (1<<symsize))
    return NULL;
  if(nroots >= (1<<symsize))
    return NULL; /* Can't have more roots than symbol values! */

  rs = (struct rs *)calloc(1,sizeof(struct rs));
  rs->mm = symsize;
  rs->nn = (1<<symsize)-1;

  rs->alpha_to = (DTYPE *)malloc(sizeof(DTYPE)*(rs->nn+1));
  if(rs->alpha_to == NULL){
    free(rs);
    return NULL;
  }
  rs->index_of = (DTYPE *)malloc(sizeof(DTYPE)*(rs->nn+1));
  if(rs->index_of == NULL){
    free(rs->alpha_to);
    free(rs);
    return NULL;
  }

  /* Generate Galois field lookup tables */
  rs->index_of[0] = A0; /* log(zero) = -inf */
  rs->alpha_to[A0] = 0; /* alpha**-inf = 0 */
  sr = 1;
  for(i=0;i<rs->nn;i++){
    rs->index_of[sr] = i;
    rs->alpha_to[i] = sr;
    sr <<= 1;
    if(sr & (1<<symsize))
      sr ^= gfpoly;
    sr &= rs->nn;
  }
  if(sr != 1){
    /* field generator polynomial is not primitive! */
    free(rs->alpha_to);
    free(rs->index_of);
    free(rs);
    return NULL;
  }

  /* Form RS code generator polynomial from its roots */
  rs->genpoly = (DTYPE *)malloc(sizeof(DTYPE)*(nroots+1));
  if(rs->genpoly == NULL){
    free(rs->alpha_to);
    free(rs->index_of);
    free(rs);
    return NULL;
  }
  rs->fcr = fcr;
  rs->prim = prim;
  rs->nroots = nroots;

  /* Find prim-th root of 1, used in decoding */
  for(iprim=1;(iprim % prim) != 0;iprim += rs->nn)
    ;
  rs->iprim = iprim / prim;

  rs->genpoly[0] = 1;
  for (i = 0,root=fcr*prim; i < nroots; i++,root += prim) {
    rs->genpoly[i+1] = 1;

    /* Multiply rs->genpoly[] by  @**(root + x) */
    for (j = i; j > 0; j--){
      if (rs->genpoly[j] != 0)
	rs->genpoly[j] = rs->genpoly[j-1] ^ rs->alpha_to[modnn(rs,rs->index_of[rs->genpoly[j]] + root)];
      else
	rs->genpoly[j] = rs->genpoly[j-1];
    }
    /* rs->genpoly[0] can never be zero */
    rs->genpoly[0] = rs->alpha_to[modnn(rs,rs->index_of[rs->genpoly[0]] + root)];
  }
    /* convert rs->genpoly[] to index form for quicker encoding */
  for (i = 0; i <= nroots; i++) {
    rs->genpoly[i] = rs->index_of[rs->genpoly[i]];
  }
  
// diagnostic prints
/*
  printf("Alpha To:\n\r");
  for (i=0; i < sizeof(DTYPE)*(rs->nn+1); i++) 
    printf("0x%2x,", rs->alpha_to[i]);
  printf("\n\r");

  printf("Index Of:\n\r");
  for (i=0; i < sizeof(DTYPE)*(rs->nn+1); i++) 
    printf("0x%2x,", rs->index_of[i]);
  printf("\n\r");
  
  printf("GenPoly:\n\r");
  for (i = 0; i <= nroots; i++) 
    printf("0x%2x,", rs->genpoly[i]);
  printf("\n\r");
*/
  return rs;
}

#endif   


int DECODE_RS(void *p, DTYPE *data, int *eras_pos, int no_eras) {

  struct rs *rs = (struct rs *)p;
  int deg_lambda, el, deg_omega;
  int i, j, r,k;
  DTYPE u,q,tmp,num1,num2,den,discr_r;
  DTYPE lambda[NROOTS+1], s[NROOTS];	/* Err+Eras Locator poly and syndrome poly */
  DTYPE b[NROOTS+1], t[NROOTS+1], omega[NROOTS+1];
  DTYPE root[NROOTS], reg[NROOTS+1], loc[NROOTS];
  int syn_error, count;

  fprintf(stderr,"Decode_RS entry\n");
  
  /* form the syndromes; i.e., evaluate data(x) at roots of g(x) */
  for(i=0;i<NROOTS;i++)
    s[i] = data[0];

  for(j=1;j<NN;j++){
    for(i=0;i<NROOTS;i++){
      if(s[i] == 0){
	s[i] = data[j];
      } else {
	s[i] = data[j] ^ ALPHA_TO[MODNN(INDEX_OF[s[i]] + (FCR+i)*PRIM)];
      }
    }
  }

  /* Convert syndromes to index form, checking for nonzero condition */
  syn_error = 0;
  for(i=0;i<NROOTS;i++){
    syn_error |= s[i];
    s[i] = INDEX_OF[s[i]];
  }

  // fprintf(stderr,"syn_error = %4x\n",syn_error);
  if (!syn_error) {
    /* if syndrome is zero, data[] is a codeword and there are no
     * errors to correct. So return data[] unmodified
     */
    count = 0;
    goto finish;
  }
  memset(&lambda[1],0,NROOTS*sizeof(lambda[0]));
  lambda[0] = 1;

  if (no_eras > 0) {
    /* Init lambda to be the erasure locator polynomial */
    lambda[1] = ALPHA_TO[MODNN(PRIM*(NN-1-eras_pos[0]))];
    for (i = 1; i < no_eras; i++) {
      u = MODNN(PRIM*(NN-1-eras_pos[i]));
      for (j = i+1; j > 0; j--) {
	tmp = INDEX_OF[lambda[j - 1]];
	if(tmp != A0)
	  lambda[j] ^= ALPHA_TO[MODNN(u + tmp)];
      }
    }

#if DEBUG >= 1
    /* Test code that verifies the erasure locator polynomial just constructed
       Needed only for decoder debugging. */
    
    /* find roots of the erasure location polynomial */
    for(i=1;i<=no_eras;i++)
      reg[i] = INDEX_OF[lambda[i]];

    count = 0;
    for (i = 1,k=IPRIM-1; i <= NN; i++,k = MODNN(k+IPRIM)) {
      q = 1;
      for (j = 1; j <= no_eras; j++)
	if (reg[j] != A0) {
	  reg[j] = MODNN(reg[j] + j);
	  q ^= ALPHA_TO[reg[j]];
	}
      if (q != 0)
	continue;
      /* store root and error location number indices */
      root[count] = i;
      loc[count] = k;
      count++;
    }
    if (count != no_eras) {
      fprintf(stderr,"count = %d no_eras = %d\n lambda(x) is WRONG\n",count,no_eras);
      count = -1;
      goto finish;
    }
#if DEBUG >= 2
    fprintf(stderr,"\n Erasure positions as determined by roots of Eras Loc Poly:\n");
    for (i = 0; i < count; i++)
      fprintf(stderr,"%d ", loc[i]);
    fprintf(stderr,"\n");
#endif
#endif
  }
  for(i=0;i<NROOTS+1;i++)
    b[i] = INDEX_OF[lambda[i]];
  
  /*
   * Begin Berlekamp-Massey algorithm to determine error+erasure
   * locator polynomial
   */
  r = no_eras;
  el = no_eras;
  while (++r <= NROOTS) {	/* r is the step number */
    /* Compute discrepancy at the r-th step in poly-form */
    discr_r = 0;
    for (i = 0; i < r; i++){
      if ((lambda[i] != 0) && (s[r-i-1] != A0)) {
	discr_r ^= ALPHA_TO[MODNN(INDEX_OF[lambda[i]] + s[r-i-1])];
      }
    }
    discr_r = INDEX_OF[discr_r];	/* Index form */
    if (discr_r == A0) {
      /* 2 lines below: B(x) <-- x*B(x) */
      memmove(&b[1],b,NROOTS*sizeof(b[0]));
      b[0] = A0;
    } else {
      /* 7 lines below: T(x) <-- lambda(x) - discr_r*x*b(x) */
      t[0] = lambda[0];
      for (i = 0 ; i < NROOTS; i++) {
	if(b[i] != A0)
	  t[i+1] = lambda[i+1] ^ ALPHA_TO[MODNN(discr_r + b[i])];
	else
	  t[i+1] = lambda[i+1];
      }
      if (2 * el <= r + no_eras - 1) {
	el = r + no_eras - el;
	/*
	 * 2 lines below: B(x) <-- inv(discr_r) *
	 * lambda(x)
	 */
	for (i = 0; i <= NROOTS; i++)
	  b[i] = (lambda[i] == 0) ? A0 : MODNN(INDEX_OF[lambda[i]] - discr_r + NN);
      } else {
	/* 2 lines below: B(x) <-- x*B(x) */
	memmove(&b[1],b,NROOTS*sizeof(b[0]));
	b[0] = A0;
      }
      memcpy(lambda,t,(NROOTS+1)*sizeof(t[0]));
    }
  }

  /* Convert lambda to index form and compute deg(lambda(x)) */
  deg_lambda = 0;
  for(i=0;i<NROOTS+1;i++){
    lambda[i] = INDEX_OF[lambda[i]];
    if(lambda[i] != A0)
      deg_lambda = i;
  }
  /* Find roots of the error+erasure locator polynomial by Chien search */
  memcpy(&reg[1],&lambda[1],NROOTS*sizeof(reg[0]));
  count = 0;		/* Number of roots of lambda(x) */
  for (i = 1,k=IPRIM-1; i <= NN; i++,k = MODNN(k+IPRIM)) {
    q = 1; /* lambda[0] is always 0 */
    for (j = deg_lambda; j > 0; j--){
      if (reg[j] != A0) {
	reg[j] = MODNN(reg[j] + j);
	q ^= ALPHA_TO[reg[j]];
      }
    }
    if (q != 0)
      continue; /* Not a root */
    /* store root (index-form) and error location number */
#if DEBUG>=2
    fprintf(stderr,"count %d root %d loc %d\n",count,i,k);
#endif
    root[count] = i;
    loc[count] = k;
    /* If we've already found max possible roots,
     * abort the search to save time
     */
    if(++count == deg_lambda)
      break;
  }
  if (deg_lambda != count) {
    /*
     * deg(lambda) unequal to number of roots => uncorrectable
     * error detected
     */
    count = -1;
    goto finish;
  }
  /*
   * Compute err+eras evaluator poly omega(x) = s(x)*lambda(x) (modulo
   * x**NROOTS). in index form. Also find deg(omega).
   */
  deg_omega = 0;
  for (i = 0; i < NROOTS;i++){
    tmp = 0;
    j = (deg_lambda < i) ? deg_lambda : i;
    for(;j >= 0; j--){
      if ((s[i - j] != A0) && (lambda[j] != A0))
	tmp ^= ALPHA_TO[MODNN(s[i - j] + lambda[j])];
    }
    if(tmp != 0)
      deg_omega = i;
    omega[i] = INDEX_OF[tmp];
  }
  omega[NROOTS] = A0;
  
  /*
   * Compute error values in poly-form. num1 = omega(inv(X(l))), num2 =
   * inv(X(l))**(FCR-1) and den = lambda_pr(inv(X(l))) all in poly-form
   */
  for (j = count-1; j >=0; j--) {
    num1 = 0;
    for (i = deg_omega; i >= 0; i--) {
      if (omega[i] != A0)
	num1  ^= ALPHA_TO[MODNN(omega[i] + i * root[j])];
    }
    num2 = ALPHA_TO[MODNN(root[j] * (FCR - 1) + NN)];
    den = 0;
    
    /* lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] */
    for (i = min(deg_lambda,NROOTS-1) & ~1; i >= 0; i -=2) {
      if(lambda[i+1] != A0)
	den ^= ALPHA_TO[MODNN(lambda[i+1] + i * root[j])];
    }
    if (den == 0) {
#if DEBUG >= 1
      fprintf(stderr,"\n ERROR: denominator = 0\n");
#endif
      count = -1;
      goto finish;
    }
    /* Apply error to data */
    if (num1 != 0) {
      data[loc[j]] ^= ALPHA_TO[MODNN(INDEX_OF[num1] + INDEX_OF[num2] + NN - INDEX_OF[den])];
    }
  }
 finish:
  if(eras_pos != NULL){
    for(i=0;i<count;i++)
      eras_pos[i] = loc[i];
  }
  return count;
}




// calculate the CRC on the AX.25 packet
// return the byte count - including the CRC and final flag bytes
//
int check_CRC(unsigned char *block) {
	
	unsigned int	i,j,t;
	unsigned int 	crc;
	unsigned int	bytecount;
	unsigned char	pass;
	
	pass = 0;
	bytecount = 0;
	crc = 0xFFFF;
	
	while ((bytecount < BLOCKSIZE) && (pass == 0)) {	
		for (i=0; i < 8; i++) {  // cycle through all 8 bits
			t = (((block[bytecount] >> i) & 0x01) ^ crc) & 0x0001;  // xor the shifted input data with the LSb of the CRC register
			crc = (crc >> 1) & 0x7FFF;  // right-shift the CRC value one bit
			if (t != 0) {
				crc = crc ^ 0x8408;  // if the above xor resulted in a 1, xor the equivalent feedback taps
			}
		}
		// compare calculated and transmitted values
		bytecount++;
		t = (block[bytecount+1] << 8) + (block[bytecount] & 0x00FF);
		t ^= 0xFFFF;  // invert the transmitted value
		if (t == crc) pass = 1;
		
		// diagnostic prints
		/* 
		fprintf(stderr,"CRC:d=%2x,nt=%4x,it=%4x,calc=%4x ",block[bytecount-1],(t ^ 0xFFFF),t,crc);
		if ((block[bytecount-1] > 0x10) && (block[bytecount-1] < 0x7E)) {
			fputc(block[bytecount-1],stderr);
			fprintf(stderr,"\n");
		} else {
			fprintf(stderr,".\n");
		}
		*/ // end diagnostic prints
	}
	if (pass == 1) return (bytecount+2);  // include the CRC and flag bytes in count
	else return (0);
}


// strip leading 0x7E flags and bit-stuffing from AX.25 data in block array
// 
char strip (unsigned char *block) {
	int				i,j;
	unsigned int 	shift;
	unsigned int	byte_ptr;
	unsigned char	out_buffer, out_bits;
	unsigned char	consec_ones;
	
	
	
	i = 0;
	shift = 0;
	
	if (block[0] != 0x7E) {
		fprintf(stderr,"\nNo initial AX.25 flag =%2x\n",block[0]);
		return(0);  // fault if first AX.25 byte in block buffer isn't a flag
	}
	while ((i < BLOCKSIZE) && (shift == 0)) {  // strip any additional leading flags
		if (block[i] == 0x7E) {
			i++;
		} else {
			shift = i;
		}
	}
	for (i=shift; i < BLOCKSIZE; i++) {  // shift data toward 0-index
		block[i-shift] = block[i];
	}
	/* // diagnostic print
	
	fprintf(stderr,"\nFlags-removed, shift=%2x\n",shift);
	for (i=0; i < BLOCKSIZE; i++) {
		fprintf(stderr,"%2x",block[i]);
	}
	fprintf(stderr,"\n");
	*/ // end diagnostic print
	
	// destuff AX.25 packet
	//
	consec_ones = 0;
	out_bits = 0;
	byte_ptr = 0;  // init output data pointer
	
	for (j=0; j < BLOCKSIZE; j++) {
	  for (i=0; i<8; i++) {   //shift in all 8 input bits
		if (((block[j] >> i) & 0x01) == 0) {	// next bit is 0
			//fprintf(stderr,"0");
	  		if (consec_ones != 5) {
		  		out_buffer = (out_buffer >> 1) & 0x7F;  // shift in a zero into the MSb position
		  		out_bits++;
	  		} else {
		  		//fprintf(stderr,"x");  //indicate that zero was de-stuffed
	  		}
	  		consec_ones = 0;		//a zero clears the consecutive-ones counter
	  		
  		} else {   // next bit is 1
  			//fprintf(stderr,"1");
			consec_ones++;
			out_buffer = (out_buffer >> 1) | 0x80;  // shift in a one into the MSb position
			out_bits++;
		}
		if (out_bits >= 8) {  //store complete output byte in output array
			block[byte_ptr] = out_buffer;
			out_bits = 0;
			byte_ptr++;
		}
	  }  // for i
	}  // for j
	return (1);  // indicate success
}  // strip

//
// Search for AX.25 framing information
//
void process_ax25(unsigned char *block, unsigned int bytecount) {
	unsigned int			crc;
	int						i,j;
	unsigned char			currbyte, count;
	static unsigned char	state;
	
	
	
	for (j=0; j <= bytecount; j++) {
		currbyte = block[j];
		if ((currbyte == 0x7E) && (state < 0x08)) {
			fprintf(stderr,"\n** Abnormal flag location\n");
			return;  // force resync
		} else {   // flags have been purged, process packet data
			// fprintf(stderr,"\nj=%2x S:%2x:cnt=%2x:",j,state,count);
			switch (state) {
				default   :
				case 0x01 : count = 1;  // first byte of header
							state = 0x02;
							fprintf(stderr,"Call:");
							fputc((currbyte>>1),stderr);
							break;
				case 0x02 : count++;
							fputc((currbyte>>1),stderr);
							if (count == 6) {
								state = 0x03;		// next byte is SSID field
								fprintf(stderr,"\n");
							}
							break;
				case 0x03 : fprintf(stderr,"SSID: %2x\n",currbyte);
							if ((currbyte & 0x01) == 0x01) {
								state = 0x04;   // end of header
							} else {
								state = 0x01;   // more header info to come
							}
							break;
				case 0x04 : fprintf(stderr,"CTRL: %2x\n",currbyte);
							state = 0x05;
							break;
				case 0x05 : fprintf(stderr,"PID : %2x\n",currbyte);
							state = 0x06;
							break;
				case 0x06 : fputc(currbyte,stderr);  // print current payload byte
							if (j == bytecount - 3) {
								state = 0x07;
							}
							break;
				case 0x07 : fprintf(stderr,"\nCRC : %2x",currbyte);
							state = 0x08;
							break;
				case 0x08 : fprintf(stderr,"%2x (low-byte, high-byte)\n",currbyte);  // print second CRC byte
							state = 0x09;
							break;
				case 0x09 : if (currbyte == 0x7e) {
								fprintf(stderr,"\nPacket terminated normally\n");
							} else {
								fprintf(stderr,"\n** Packet missing final flag **\n");
								j = bytecount;  // error handler; punch out
							}
							state = 1;  // done, start over
							break;
			}  // switch
		}  // else
	}  // for j
	
	// if you got here, the packet was valid.  dump the binary output to STDOUT
	putc(0x7E,stdout); // initial flag
	for (j=0; j <= bytecount; j++) {
		putc(block[j],stdout);  // AX.25 packet including trailing flag
	}
	
	return;
}

//
// Find the correlation tag in the input data stream
//   Input data is presented as an INT.  Correlation tag may be any phase of bit-shift 0 thru 7
//   Return a value of 8 if it's not found yet.  Otherwise, return the bit phase - offset relative
//   to bit[0] of dbuffer, range 0-7.
//


/***************************************************************************/
// Autocorrelation calculation for a 64-bit correlation tag value
//
// Data is stored locally in a static 64-bit variable.  The most recent byte
// provides the next 8 bits in time sequence, and is shifted-in to the LSB
// position of the holding register.  The MSB of the holding register is the 
// oldest in time-sequence.  
//
// The calculated autocorrelation values are compared to the threshold parameter.  If the 
// threshold isn't met, a phase value of 8 is returned - indicating that the tag wasn't found.
// Valid bit-phase values of 7:0 indicate that the tag was found.
//
// So what does this all mean?  If the autocorrelation value exceeds your threshold, that
// indicates a "match."  The correlation tag begins in the data[0] byte at the bit-offset,
// and terminates at the bit-offset in the MSB of ltemp1.  For the degenerate case where 
// the bit-offset is zero, "data" represents the last byte of the correlationt tag, and
// the next byte in time sequence will be an information byte.
//
//
/***************************************************************************/
char find_corr_tag (unsigned char newbyte, char threshold) {
	
	static unsigned long long const tag = INT64_C(0x3E2F538ADFB74DB7);		// fixed correlation tag value
	int 	i, j;
	static unsigned long long 	ltemp1 = 0;  // this one needs to be persistent
	unsigned long long 			ltemp2, tag_temp;
	char 	sum, phase;
	

	phase = 8;  // default value = "not found"
	
	for (j=0; j<8; j++) {						// iterate through 8 bit-phase shifts (that's zero plus 7 offsets)
		ltemp2 = (ltemp1 << (8-j)) + (newbyte >> j);	// load initial value
		sum = 0;
		for (i=0; i<64; i++) {
			if (((ltemp2 >> i) & 0x01) == ((tag >> i) & 0x01))
				sum++;
			else
				sum--;
		}
		// fprintf(stderr,"autocorr: data = %ll16x, corr_val = 0x%2x\n",ltemp2,sum);
		if ((sum >= threshold) && (phase == 8)) phase = j;  // store bit phase value if it meets criteria
	}  // for
	
	//printf("autocorr, tag  = %ll16x\n",tag);
	
	// load data set into working register for next iteration
	//   this method may seem odd, but it works around
	//   compiler limitations that restrict certain ops to 32-bits
	ltemp1 <<= 8;							
	ltemp1 += newbyte;
	
	return(phase);
}


//
// Process an FX.25 frame with the following steps:
// 		find correlation tag
// 		store 256 bytes that follow
// 		perform RS FEC
// 		strip leading 0x7E flags
// 		de-stuff stored data
// 		find trailing 0x7E flag
// 		calculate CRC - validate AX.25 packet	
// 		dump out AX.25 packet data
// 
// 
// Process data LSb first
//
//
int main(int argc, char **argv)
{
	unsigned char			out_buffer = 0;
	int						out_bits = 0;
	int						consec_ones = 0;
	unsigned char			out_byte;
	unsigned char			block[BLOCKSIZE];
	int						i;
	unsigned char			mstate, phase;
	unsigned char			sample_char;
	unsigned char			fault;
	unsigned int			dbuffer, bytecount;
	unsigned char			derrors;
	int						erasures;
	
	int						in_size;
	unsigned				*in_buf;

	struct 	rs *rs;
	int 	index;
	int 	nn,kk;

	in_size = sizeof (char);
	in_buf = (unsigned *)&sample_char;


	index = 0;			// use fixed index into RS coefficient table for this application
						// index will be useful when additional FEC rates are supported
	
    nn = (1<<Tab[index].symsize) - 1;
    kk = nn - Tab[index].nroots;
	int						derrlocs[Tab[index].nroots];

	
	/* Process encoding argument, if any */
	while ((argc > 1) && (argv[1][0] == '-')) {
		switch (argv[1][1]) {
		case 'a':
			// process command line paramater here
			// break;  // uncomment if used
		default:
			fprintf(stderr, "FX.25 frame extractor -- usage:\n");
			fprintf(stderr, "\t./fx25_extract < infile > outfile [2> logfile]\n");
			fprintf(stderr, "where:\n");
			fprintf(stderr, "\tinfile and outfile are binary data\n");
			exit(1);
		}
		argc--;
		argv++;
	}

	fprintf(stderr,"\nFX.25 Decoder, " version "\n");
	mstate = 0;		// init variables
	dbuffer = 0;
	fault = 0;
	derrors = 0;
#ifdef PC    
    fprintf(stderr,"Testing (%d,%d) RS encoder, table index = %d\n",nn,kk,index);
#endif   

#ifdef ram_init 
    if((rs = init_rs_char(Tab[index].symsize,Tab[index].genpoly,Tab[index].fcs,Tab[index].prim,Tab[index].nroots)) == NULL){
		fprintf(stderr,"init_rs_char failed!\n");
    }
#else
	printf("Using const-array init ...\n\n\r");

//  rs = (struct rs *)calloc(1,sizeof(struct rs));
	rs = (struct rs *)rs_ram;
	rs->mm = Tab[index].symsize;
	rs->nn = (1<<Tab[index].symsize)-1;
  
	rs->fcr = Tab[index].fcs;
	rs->prim = Tab[index].prim;
	rs->nroots = Tab[index].nroots;

// Find prim-th root of 1, used in decoding 
//  for(iprim=1;(iprim % prim) != 0;iprim += rs->nn)
//    ;
//  rs->iprim = iprim / prim;
  
	rs->alpha_to = (DTYPE *)alpha_to_const;  // set up pointers to static init arrays
	rs->index_of = (DTYPE *)index_of_const;
	rs->genpoly = (DTYPE *)genpoly_const;
#endif      
    

	/* Read input file and process */
	while (fread(in_buf, in_size, 1, stdin) == 1) {
		dbuffer = (dbuffer >> 8) & 0x00FF;  //shift the buffered data over 8 bits
		dbuffer |= (sample_char << 8);  // or in the new byte
		switch (mstate) {
			case 0x00 :	phase = find_corr_tag((dbuffer & 0x00FF), autocorr_threshold);
						// fprintf(stderr,".%4x",dbuffer);  // diagnostic print

						if (phase != 8) {
							mstate = 0x01;  // promote to next state
							bytecount = 0;  // init buffer count
							fprintf(stderr,"\nCorrelation Tag found, Phase=%2x\n",phase);
						}
						break;
			case 0x01 : block[bytecount] = (char)((dbuffer >> phase) & 0x00FF);  // store the phase-shifted byte in the block array
						// fprintf(stderr,"%4x:%2x ",bytecount,block[bytecount]);
						bytecount++;
						if (bytecount == BLOCKSIZE) {
							mstate = 0x00;  // promote to initial state, then process FX.25 frame
							
							// diagnostic - dump contents of FEC block prior to decode
							fprintf(stderr,"\nFEC block data\n");
							for (i=0; i < BLOCKSIZE; i++) fprintf(stderr,"%2x ",block[i]);
							fprintf(stderr,"\n\n");
							//
							
							derrors = DECODE_RS(rs,&block[0],derrlocs,0);  // perform RS FEC correction
							fprintf(stderr,"FEC complete, detected %2d errors\n\n",derrors);
							if (strip(&block[0]) != 0) {  // valid struture found
								bytecount = check_CRC(&block[0]);
								if (bytecount != 0) {
									process_ax25(&block[0], bytecount);
								} else {
									fault = CRC_fault;
									fprintf(stderr,"\n\nFault code = CRC");
								}
							} else {
								fault = strip_fault;
								fprintf(stderr,"\n\nFault code = Strip");
							}
						}
						break;
		}  // switch
	}  // while
	
	fclose(stdout);

	return 0;
}