/* 
 * This software is licensed under terms of the GNU GENERAL PUBLIC LICENSE
 *
 * This software is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This software  is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this software.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Source code is available at http://ppcenter.free.fr
 */


#include "sc_common.h"

#ifdef _BCB
#ifdef  __cplusplus
extern "C" {
#endif
#endif // _BCB


/* Main CRC (CRC16-CCITT), used to verify packets and messages integrity. */
unsigned short crc16_start(void)
{
//return crc8_init();
    return 0x1664;
    //return 0xFFFF;
}
unsigned short crc16_update(unsigned short old_crc, unsigned char data)
{
//return crc8_update((old_crc)&0x00FF, data);
    unsigned short crc;
    unsigned short x;

    x = ((old_crc>>8) ^ data) & 0xff;//make8(old_crc,1) ^ data;
    x ^= x>>4;

    crc = (old_crc << 8) ^ (x << 12) ^ (x <<5) ^ x;

    crc &= 0xffff;

    return crc;
}


//----------------------------------------------------------------------
// 8 bits CRC
//http://forums.freescale.com/t5/8-Bit-Microcontrollers/CRC-8-bits/m-p/3678
// 
//----------------------------------------------------------------------
const unsigned char crc8_Table[ ] =
{
     0,  94, 188, 226,  97,  63, 221, 131, 194, 156, 126,  32, 163, 253,  31,  65,
   157, 195,  33, 127, 252, 162,  64,  30,  95,   1, 227, 189,  62,  96, 130, 220,
    35, 125, 159, 193,  66,  28, 254, 160, 225, 191,  93,   3, 128, 222,  60,  98,
   190, 224,   2,  92, 223, 129,  99,  61, 124,  34, 192, 158,  29,  67, 161, 255,
    70,  24, 250, 164,  39, 121, 155, 197, 132, 218,  56, 102, 229, 187,  89,   7,
   219, 133, 103,  57, 186, 228,   6,  88,  25,  71, 165, 251, 120,  38, 196, 154,
   101,  59, 217, 135,   4,  90, 184, 230, 167, 249,  27,  69, 198, 152, 122,  36,
   248, 166,  68,  26, 153, 199,  37, 123,  58, 100, 134, 216,  91,   5, 231, 185,
   140, 210,  48, 110, 237, 179,  81,  15,  78,  16, 242, 172,  47, 113, 147, 205,
    17,  79, 173, 243, 112,  46, 204, 146, 211, 141, 111,  49, 178, 236,  14,  80,
   175, 241,  19,  77, 206, 144, 114,  44, 109,  51, 209, 143,  12,  82, 176, 238,
    50, 108, 142, 208,  83,  13, 239, 177, 240, 174,  76,  18, 145, 207,  45, 115,
   202, 148, 118,  40, 171, 245,  23,  73,   8,  86, 180, 234, 105,  55, 213, 139,
    87,   9, 235, 181,  54, 104, 138, 212, 149, 203,  41, 119, 244, 170,  72,  22,
   233, 183,  85,  11, 136, 214,  52, 106,  43, 117, 151, 201,  74,  20, 246, 168,
   116,  42, 200, 150,  21,  75, 169, 247, 182, 232,  10,  84, 215, 137, 107,  53
} ;


/* Alternate CRC, used to check RS decoding. */
unsigned char crc8_start(void)
{
    return 0x42;
}
unsigned char crc8_update(unsigned char crc, unsigned char val)
{
    // Some CRC implementations xor the final value with some other value.
    //crc = crc ^ xor_Value

    return crc8_Table[ crc ^ val ];
    //return ((((crc) >> 3) | (((crc) & 0x07) << 5)) + (val)) & 0xFF;
    //return (crc + val) & 0xFF;
}





//----------------------------------------------------------------------
// RS Code
// Source code is available at http://rscode.sourceforge.net
// 
//----------------------------------------------------------------------
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include "common/ecc.h"


#if 0 /* <- not used */
//----------------------------------------------------------------------
//----- crcgen.c
// CRC-CCITT generator simulator for byte wide data.
// CRC-CCITT = x^16 + x^12 + x^5 + 1

BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum);

/* Computes the CRC-CCITT checksum on array of byte data, length len
*/
BIT16 crc_ccitt(unsigned char *msg, int len)
{
	int i;
	BIT16 acc = 0;

	for (i = 0; i < len; i++) {
		acc = crchware((BIT16) msg[i], (BIT16) 0x1021, acc);
	}
	
	return(acc);
}
	
/* models crc hardware (minor variation on polynomial division algorithm) */
BIT16 crchware(BIT16 data, BIT16 genpoly, BIT16 accum)
{
	static BIT16 i;
	data <<= 8;
	for (i = 8; i > 0; i--) {
		if ((data ^ accum) & 0x8000)
			accum = ((accum << 1) ^ genpoly) & 0xFFFF;
		else
			accum = (accum<<1) & 0xFFFF;
		data = (data<<1) & 0xFFFF;
	}
	return (accum);
}
#endif



//----------------------------------------------------------------------
//----- galois.c
// Multiplication and Arithmetic on Galois Field GF(256)
//
// From Mee, Daniel, "Magnetic Recording, Volume III", Ch. 5 by Patel.

/* This is one of 14 irreducible polynomials
 * of degree 8 and cycle length 255. (Ch 5, pp. 275, Magnetic Recording)
 * The high order 1 bit is implicit */
/* x^8 + x^4 + x^3 + x^2 + 1 */
#define PPOLY 0x1D 


int gexp[512];
int glog[256];


static void init_exp_table (void);


void
init_galois_tables (void)
{	
  /* initialize the table of powers of alpha */
  init_exp_table();
}


static void
init_exp_table (void)
{
  int i, z;
  int pinit,p1,p2,p3,p4,p5,p6,p7,p8;

  pinit = p2 = p3 = p4 = p5 = p6 = p7 = p8 = 0;
  p1 = 1;
	
  gexp[0] = 1;
  gexp[255] = gexp[0];
  glog[0] = 0;			/* shouldn't log[0] be an error? */
	
  for (i = 1; i < 256; i++) {
    pinit = p8;
    p8 = p7;
    p7 = p6;
    p6 = p5;
    p5 = p4 ^ pinit;
    p4 = p3 ^ pinit;
    p3 = p2 ^ pinit;
    p2 = p1;
    p1 = pinit;
    gexp[i] = p1 + p2*2 + p3*4 + p4*8 + p5*16 + p6*32 + p7*64 + p8*128;
    gexp[i+255] = gexp[i];
  }
	
  for (i = 1; i < 256; i++) {
    for (z = 0; z < 256; z++) {
      if (gexp[z] == i) {
	glog[i] = z;
	break;
      }
    }
  }
}

/* multiplication using logarithms */
int gmult(int a, int b)
{
  int i,j;
  if (a==0 || b == 0) return (0);
  i = glog[a];
  j = glog[b];
  return (gexp[i+j]);
}
		

int ginv (int elt) 
{ 
  return (gexp[255-glog[elt]]);
}




//----------------------------------------------------------------------
//----- berlekamp.c

/* The Error Locator Polynomial, also known as Lambda or Sigma. Lambda[0] == 1 */
static int Lambda[MAXDEG];

/* The Error Evaluator Polynomial */
static int Omega[MAXDEG];

/* local ANSI declarations */
static int compute_discrepancy(int lambda[], int S[], int L, int n);
static void init_gamma(int gamma[]);
static void compute_modified_omega (void);
static void mul_z_poly (int src[]);

/* error locations found using Chien's search*/
static int ErrorLocs[256];
static int NErrors;

/* erasure flags */
static int ErasureLocs[256];
static int NErasures;

/* From  Cain, Clark, "Error-Correction Coding For Digital Communications", pp. 216. */
void
Modified_Berlekamp_Massey (void)
{	
  int n, L, L2, k, d, i;
  int psi[MAXDEG], psi2[MAXDEG], D[MAXDEG];
  int gamma[MAXDEG];
	
  /* initialize Gamma, the erasure locator polynomial */
  init_gamma(gamma);

  /* initialize to z */
  copy_poly(D, gamma);
  mul_z_poly(D);
	
  copy_poly(psi, gamma);	
  k = -1; L = NErasures;
	
  for (n = NErasures; n < NPAR; n++) {
	
    d = compute_discrepancy(psi, synBytes, L, n);
		
    if (d != 0) {
		
      /* psi2 = psi - d*D */
      for (i = 0; i < MAXDEG; i++) psi2[i] = psi[i] ^ gmult(d, D[i]);
		
		
      if (L < (n-k)) {
	L2 = n-k;
	k = n-L;
	/* D = scale_poly(ginv(d), psi); */
	for (i = 0; i < MAXDEG; i++) D[i] = gmult(psi[i], ginv(d));
	L = L2;
      }
			
      /* psi = psi2 */
      for (i = 0; i < MAXDEG; i++) psi[i] = psi2[i];
    }
		
    mul_z_poly(D);
  }
	
  for(i = 0; i < MAXDEG; i++) Lambda[i] = psi[i];
  compute_modified_omega();

	
}

/* given Psi (called Lambda in Modified_Berlekamp_Massey) and synBytes,
   compute the combined erasure/error evaluator polynomial as 
   Psi*S mod z^4
  */
void
compute_modified_omega ()
{
  int i;
  int product[MAXDEG*2];
	
  mult_polys(product, Lambda, synBytes);	
  zero_poly(Omega);
  for(i = 0; i < NPAR; i++) Omega[i] = product[i];

}

/* polynomial multiplication */
void
mult_polys (int dst[], int p1[], int p2[])
{
  int i, j;
  int tmp1[MAXDEG*2];
	
  for (i=0; i < (MAXDEG*2); i++) dst[i] = 0;
	
  for (i = 0; i < MAXDEG; i++) {
    for(j=MAXDEG; j<(MAXDEG*2); j++) tmp1[j]=0;
		
    /* scale tmp1 by p1[i] */
    for(j=0; j<MAXDEG; j++) tmp1[j]=gmult(p2[j], p1[i]);
    /* and mult (shift) tmp1 right by i */
    for (j = (MAXDEG*2)-1; j >= i; j--) tmp1[j] = tmp1[j-i];
    for (j = 0; j < i; j++) tmp1[j] = 0;
		
    /* add into partial product */
    for(j=0; j < (MAXDEG*2); j++) dst[j] ^= tmp1[j];
  }
}



/* gamma = product (1-z*a^Ij) for erasure locs Ij */
void
init_gamma (int gamma[])
{
  int e, tmp[MAXDEG];

  zero_poly(gamma);
  zero_poly(tmp);
  gamma[0] = 1;

  for (e = 0; e < NErasures; e++) {
    copy_poly(tmp, gamma);
    scale_poly(gexp[ErasureLocs[e]], tmp);
    mul_z_poly(tmp);
    add_polys(gamma, tmp);
  }
}



void 
compute_next_omega (int d, int A[], int dst[], int src[])
{
  int i;
  for ( i = 0; i < MAXDEG;  i++) {
    dst[i] = src[i] ^ gmult(d, A[i]);
  }
}



int
compute_discrepancy (int lambda[], int S[], int L, int n)
{
  int i, sum=0;

  for (i = 0; i <= L; i++) 
    sum ^= gmult(lambda[i], S[n-i]);
  return (sum);
}

/********** polynomial arithmetic *******************/

void add_polys (int dst[], int src[]) 
{
  int i;
  for (i = 0; i < MAXDEG; i++) dst[i] ^= src[i];
}

void copy_poly (int dst[], int src[]) 
{
  int i;
  for (i = 0; i < MAXDEG; i++) dst[i] = src[i];
}

void scale_poly (int k, int poly[]) 
{
  int i;
  for (i = 0; i < MAXDEG; i++) poly[i] = gmult(k, poly[i]);
}


void zero_poly (int poly[]) 
{
  int i;
  for (i = 0; i < MAXDEG; i++) poly[i] = 0;
}


/* multiply by z, i.e., shift right by 1 */
static void mul_z_poly (int src[])
{
  int i;
  for (i = MAXDEG-1; i > 0; i--) src[i] = src[i-1];
  src[0] = 0;
}


/* Finds all the roots of an error-locator polynomial with coefficients
 * Lambda[j] by evaluating Lambda at successive values of alpha. 
 * 
 * This can be tested with the decoder's equations case.
 */


void 
Find_Roots (void)
{
  int sum, r, k;
  NErrors = 0;
  
  for (r = 1; r < 256; r++) {
    sum = 0;
    /* evaluate lambda at r */
    for (k = 0; k < NPAR+1; k++) {
      sum ^= gmult(gexp[(k*r)%255], Lambda[k]);
    }
    if (sum == 0) 
      {
        ErrorLocs[NErrors] = (255-r); NErrors++; 
        sc_dprintf("Root found at r = %d, (255-r) = %d\n", r, (255-r));
      }
  }
}

/* Combined Erasure And Error Magnitude Computation 
 * 
 * Pass in the codeword, its size in bytes, as well as
 * an array of any known erasure locations, along the number
 * of these erasures.
 * 
 * Evaluate Omega(actually Psi)/Lambda' at the roots
 * alpha^(-i) for error locs i. 
 *
 * Returns 1 if everything ok, or 0 if an out-of-bounds error is found
 *
 */

int
correct_errors_erasures (unsigned char codeword[], 
             int csize,
             int nerasures,
             int erasures[])
{
  int r, i, j, err;

  /* If you want to take advantage of erasure correction, be sure to
     set NErasures and ErasureLocs[] with the locations of erasures. 
     */
  NErasures = nerasures;
  for (i = 0; i < NErasures; i++) ErasureLocs[i] = erasures[i];

  Modified_Berlekamp_Massey();
  Find_Roots();
  

  if ((NErrors <= NPAR) && NErrors > 0) { 

    /* first check for illegal error locs */
    for (r = 0; r < NErrors; r++) {
      if (ErrorLocs[r] >= csize) {
        sc_dprintf("Error loc i=%d outside of codeword length %d\n", i, csize);
        return(0);
      }
    }

    for (r = 0; r < NErrors; r++) {
      int num, denom;
      i = ErrorLocs[r];
      /* evaluate Omega at alpha^(-i) */

      num = 0;
      for (j = 0; j < MAXDEG; j++) 
        num ^= gmult(Omega[j], gexp[((255-i)*j)%255]);
      
      /* evaluate Lambda' (derivative) at alpha^(-i) ; all odd powers disappear */
      denom = 0;
      for (j = 1; j < MAXDEG; j += 2) {
        denom ^= gmult(Lambda[j], gexp[((255-i)*(j-1)) % 255]);
      }
      
      err = gmult(num, ginv(denom));
      sc_dprintf("Error magnitude %#x at loc %d\n", err, csize-i);
      
      codeword[csize-i-1] ^= err;
    }
    return(1);
  }
  else {
    if (NErrors) sc_dprintf("Uncorrectable codeword\n");
    return(0);
  }
}




//----------------------------------------------------------------------
//----- rs.c

/* Encoder parity bytes */
int pBytes[MAXDEG];

/* Decoder syndrome bytes */
int synBytes[MAXDEG];

/* generator polynomial */
int genPoly[MAXDEG*2];

//int DEBUG = FALSE;

static void
compute_genpoly (int nbytes, int genpoly[]);

/* Initialize lookup tables, polynomials, etc. */
void
initialize_ecc ()
{
  /* Initialize the galois field arithmetic tables */
    init_galois_tables();

    /* Compute the encoder generator polynomial */
    compute_genpoly(NPAR, genPoly);
}

void
zero_fill_from (unsigned char buf[], int from, int to)
{
  int i;
  for (i = from; i < to; i++) buf[i] = 0;
}

/* debugging routines */
void
print_parity (void)
{ 
  int i;
  sc_dprintf("Parity Bytes: ");
  for (i = 0; i < NPAR; i++) 
    sc_dprintf("[%d]:%x, ",i,pBytes[i]);
  sc_dprintf("\n");
}


void
print_syndrome (void)
{ 
  int i;
  sc_dprintf("Syndrome Bytes: ");
  for (i = 0; i < NPAR; i++) 
    sc_dprintf("[%d]:%x, ",i,synBytes[i]);
  sc_dprintf("\n");
}

/* Append the parity bytes onto the end of the message */
void
build_codeword (unsigned char msg[], int nbytes, unsigned char dst[])
{
  int i;

  for (i = 0; i < nbytes; i++) dst[i] = msg[i];

  for (i = 0; i < NPAR; i++) {
    dst[i+nbytes] = pBytes[NPAR-1-i];
  }
}

/**********************************************************
 * Reed Solomon Decoder 
 *
 * Computes the syndrome of a codeword. Puts the results
 * into the synBytes[] array.
 */
 
void
decode_data(unsigned char data[], int nbytes)
{
  int i, j, sum;
  for (j = 0; j < NPAR;  j++) {
    sum	= 0;
    for (i = 0; i < nbytes; i++) {
      sum = data[i] ^ gmult(gexp[j+1], sum);
    }
    synBytes[j]  = sum;
  }
}


/* Check if the syndrome is zero */
int
check_syndrome (void)
{
 int i, nz = 0;
 for (i =0 ; i < NPAR; i++) {
  if (synBytes[i] != 0) {
      nz = 1;
      break;
  }
 }
 return nz;
}


void
debug_check_syndrome (void)
{
  int i;

  for (i = 0; i < 3; i++) {
    sc_dprintf(" inv log S[%d]/S[%d] = %d\n", i, i+1, 
           glog[gmult(synBytes[i], ginv(synBytes[i+1]))]);
  }
}


/* Create a generator polynomial for an n byte RS code. 
 * The coefficients are returned in the genPoly arg.
 * Make sure that the genPoly array which is passed in is 
 * at least n+1 bytes long.
 */

static void
compute_genpoly (int nbytes, int genpoly[])
{
  int i, tp[256], tp1[256];

  /* multiply (x + a^n) for n = 1 to nbytes */

  zero_poly(tp1);
  tp1[0] = 1;

  for (i = 1; i <= nbytes; i++) {
    zero_poly(tp);
    tp[0] = gexp[i];		/* set up x+a^n */
    tp[1] = 1;

    mult_polys(genpoly, tp, tp1);
    copy_poly(tp1, genpoly);
  }
}

/* Simulate a LFSR with generator polynomial for n byte RS code. 
 * Pass in a pointer to the data array, and amount of data. 
 *
 * The parity bytes are deposited into pBytes[], and the whole message
 * and parity are copied to dest to make a codeword.
 * 
 */

void
encode_data (unsigned char msg[], int nbytes, unsigned char dst[])
{
  int i, LFSR[NPAR+1],dbyte, j;

  for(i=0; i < NPAR+1; i++) LFSR[i]=0;

  for (i = 0; i < nbytes; i++) {
    dbyte = msg[i] ^ LFSR[NPAR-1];
    for (j = NPAR-1; j > 0; j--) {
      LFSR[j] = LFSR[j-1] ^ gmult(genPoly[j], dbyte);
    }
    LFSR[0] = gmult(genPoly[0], dbyte);
  }

  for (i = 0; i < NPAR; i++) 
    pBytes[i] = LFSR[i];

  build_codeword(msg, nbytes, dst);
}







void rs_start(void)
{
    initialize_ecc();
}

void rse_init(unsigned char* rs)
{
    int i;
    for(i=0; i < NPAR+1; i++) rs[i]=0;

    /* Decoding confirmation CRC. */
    rs[NPAR] = crc8_start();
}
void rse_update(unsigned char* rs, unsigned short index, unsigned char val)
{
    int dbyte, j;

    dbyte = val ^ rs[NPAR-1];
    for (j = NPAR-1; j > 0; j--) {
        rs[j] = rs[j-1] ^ gmult(genPoly[j], dbyte);
    }
    rs[0] = gmult(genPoly[0], dbyte);

    /* Decoding confirmation CRC. */
    rs[NPAR] = crc8_update(rs[NPAR], val);
}

int rsd_execute(unsigned char* msg, unsigned short msg_len, unsigned short msg_crc, 
                unsigned char* rs, 
                unsigned short recov_start, unsigned short recov_end, 
                unsigned char recov_pattern)
{
    unsigned char codeword[256];

    int erasures[16];
    int nerasures = 0;

    int ML = msg_len + NPAR;
    unsigned char msg_crca = rs[NPAR];
    int k, i, ret;





    if(recov_pattern & SC_RSD_NOISE)
    {
        /*------------------------------------------------*/
        /* Try to recover the message from an error byte. */
        /* Clear the codeword data. */
        memset(codeword, 0, sizeof(codeword));

        /* Copy the message in codeword. */
        memcpy(codeword, msg, msg_len);
        /* Append RS data to codeword. */
        for (i = 0; i < NPAR; i++)
        {
            codeword[msg_len + i] = rs[NPAR-1-i];
        }


        nerasures = 0;
        /* Now decode -- encoded codeword size must be passed */
        decode_data(codeword, ML);
        /* check if syndrome is all zeros */
        if (check_syndrome () != 0)
        {
            ret = correct_errors_erasures (codeword, 
                         ML,
                         nerasures, 
                         erasures);

            if(ret == 1)
            {
                /* Compute decoded messages' CRCs */
                unsigned short crcm;
                unsigned char crca;
                crcm = crc16_start();
                crca = crc8_start();
                for(i=0; i<msg_len; i++)
                {
                    crcm = crc16_update(crcm, codeword[i]);
                    crca = crc8_update(crca, codeword[i]);
                }

                /* CRCs OK ? */
                if((crcm == msg_crc)
                && (crca == msg_crca))
                {
                    /* Save recovered data. */
                    memcpy(msg, codeword, msg_len);
                    return 0;
                } /* CRCs */
            } /* correct_errors_erasures */
        } /* check_syndrome */
    }



    if(recov_pattern & SC_RSD_ERASR)
    {
        /*-------------------------------------------------*/
        /* Try to recover the message from a missing byte. */
        //for(k=0; k<msg_len; k++)
        for(k=recov_start; k<recov_end; k++)
        {
            /* Clear the codeword data. */
            memset(codeword, 0, sizeof(codeword));

            /* Shift message from k-th byte, and copy the result in codeword. */
            if(k != 0)
            {
                memcpy(codeword, msg, k);
            }
            if((k+1) != msg_len)
            {
                memcpy(codeword + k+1, msg+k, msg_len-(k+1));
            }

            /* Append RS data to codeword. */
            for (i = 0; i < NPAR; i++)
            {
                codeword[msg_len + i] = rs[NPAR-1-i];
            }



            nerasures = 0;
            erasures[nerasures++] = ML-k;

         
            /* Now decode -- encoded codeword size must be passed */
            decode_data(codeword, ML);
            /* check if syndrome is all zeros */
            if (check_syndrome () != 0)
            {
                ret = correct_errors_erasures (codeword, 
                             ML,
                             nerasures, 
                             erasures);

                if(ret == 1)
                {
                    /* Compute decoded messages' CRCs */
                    unsigned short crcm;
                    unsigned char crca;
                    crcm = crc16_start();
                    crca = crc8_start();
                    for(i=0; i<msg_len; i++)
                    {
                        crcm = crc16_update(crcm, codeword[i]);
                        crca = crc8_update(crca, codeword[i]);
                    }

                    /* CRCs OK ? */
                    if((crcm == msg_crc)
                    && (crca == msg_crca))
                    {
                        /* Save recovered data. */
                        memcpy(msg, codeword, msg_len);
                        return 0;
                    } /* CRCs */
                } /* correct_errors_erasures */
            } /* check_syndrome */
        }
    }


    if(recov_pattern & SC_RSD_INSRT)
    {
        /*---------------------------------------------------*/
        /* Try to recover the message from an inserted byte. */
        //for(k=0; k<msg_len; k++)
        for(k=recov_start; k<recov_end; k++)
        {
            /* Clear the codeword data. */
            memset(codeword, 0, sizeof(codeword));

            /* Remove the inserted byte. */
            if(k != 0)
            {
                memcpy(codeword, msg, k);
            }
            if((k+1) != msg_len)
            {
                memcpy(codeword + k, msg+k+1, msg_len-(k+1));
            }

            /* Append RS data to codeword. */
            for (i = 0; i < NPAR; i++)
            {
                codeword[msg_len + i] = rs[NPAR-1-i];
            }



            nerasures = 0;
            //erasures[nerasures++] = ML-k;
            /* Since we shifted the data in order to delete
             * the inserted byte, the byte to recove is
             * located at the end of the message.
             */
            erasures[nerasures++] = ML-(msg_len-1);

         
            /* Now decode -- encoded codeword size must be passed */
            decode_data(codeword, ML);
            /* check if syndrome is all zeros */
            if (check_syndrome () != 0)
            {
                ret = correct_errors_erasures (codeword, 
                             ML,
                             nerasures, 
                             erasures);

                if(ret == 1)
                {
                    /* Compute decoded messages' CRCs */
                    unsigned short crcm;
                    unsigned char crca;
                    crcm = crc16_start();
                    crca = crc8_start();
                    for(i=0; i<msg_len; i++)
                    {
                        crcm = crc16_update(crcm, codeword[i]);
                        crca = crc8_update(crca, codeword[i]);
                    }

                    /* CRCs OK ? */
                    if((crcm == msg_crc)
                    && (crca == msg_crca))
                    {
                        /* Save recovered data. */
                        memcpy(msg, codeword, msg_len);
                        return 0;
                    } /* CRCs */
                } /* correct_errors_erasures */
            } /* check_syndrome */
        }
    }


    return 1;
}




//----------------------------------------------------------------------
// LZF compression
// 
//----------------------------------------------------------------------



//----------------------------------------------------------------------
//----- lzf_c.c
#include "common/lzfP.h"

#define HSIZE (1 << (HLOG))

/*
 * don't play with this unless you benchmark!
 * decompression is not dependent on the hash function
 * the hashing function might seem strange, just believe me
 * it works ;)
 */
#ifndef FRST
# define FRST(p) (((p[0]) << 8) | p[1])
# define NEXT(v,p) (((v) << 8) | p[2])
# if ULTRA_FAST
#  define IDX(h) ((( h             >> (3*8 - HLOG)) - h  ) & (HSIZE - 1))
# elif VERY_FAST
#  define IDX(h) ((( h             >> (3*8 - HLOG)) - h*5) & (HSIZE - 1))
# else
#  define IDX(h) ((((h ^ (h << 5)) >> (3*8 - HLOG)) - h*5) & (HSIZE - 1))
# endif
#endif
/*
 * IDX works because it is very similar to a multiplicative hash, e.g.
 * ((h * 57321 >> (3*8 - HLOG)) & (HSIZE - 1))
 * the latter is also quite fast on newer CPUs, and compresses similarly.
 *
 * the next one is also quite good, albeit slow ;)
 * (int)(cos(h & 0xffffff) * 1e6)
 */

#if 0
/* original lzv-like hash function, much worse and thus slower */
# define FRST(p) (p[0] << 5) ^ p[1]
# define NEXT(v,p) ((v) << 5) ^ p[2]
# define IDX(h) ((h) & (HSIZE - 1))
#endif

#define        MAX_LIT        (1 <<  5)
#define        MAX_OFF        (1 << 13)
#define        MAX_REF        ((1 << 8) + (1 << 3))

#if __GNUC__ >= 3
# define expect(expr,value)         __builtin_expect ((expr),(value))
# define inline                     inline
#else
# define expect(expr,value)         (expr)
# define inline                     static
#endif

#define expect_false(expr) expect ((expr) != 0, 0)
#define expect_true(expr)  expect ((expr) != 0, 1)

/*
 * compressed format
 *
 * 000LLLLL <L+1>    ; literal
 * LLLooooo oooooooo ; backref L
 * 111ooooo LLLLLLLL oooooooo ; backref L+7
 *
 */

unsigned int
lzf_compress (const void *const in_data, unsigned int in_len,
	      void *out_data, unsigned int out_len
#if LZF_STATE_ARG
              , LZF_STATE htab
#endif
              )
{
#if !LZF_STATE_ARG
  LZF_STATE htab;
#endif
  const u8 **hslot;
  const u8 *ip = (const u8 *)in_data;
        u8 *op = (u8 *)out_data;
  const u8 *in_end  = ip + in_len;
        u8 *out_end = op + out_len;
  const u8 *ref;

  /* off requires a type wide enough to hold a general pointer difference.
   * ISO C doesn't have that (size_t might not be enough and ptrdiff_t only
   * works for differences within a single object). We also assume that no
   * no bit pattern traps. Since the only platform that is both non-POSIX
   * and fails to support both assumptions is windows 64 bit, we make a
   * special workaround for it.
   */
#if defined (WIN32) && defined (_M_X64)
  unsigned _int64 off; /* workaround for missing POSIX compliance */
#else
  unsigned long off;
#endif
  unsigned int hval;
  int lit;

  if (!in_len || !out_len)
    return 0;

#if INIT_HTAB
  memset (htab, 0, sizeof (htab));
# if 0
  for (hslot = htab; hslot < htab + HSIZE; hslot++)
    *hslot++ = ip;
# endif
#endif

  lit = 0; op++; /* start run */

  hval = FRST (ip);
  while (ip < in_end - 2)
    {
      hval = NEXT (hval, ip);
      hslot = htab + IDX (hval);
      ref = *hslot; *hslot = ip;

      if (1
#if INIT_HTAB
          && ref < ip /* the next test will actually take care of this, but this is faster */
#endif
          && (off = ip - ref - 1) < MAX_OFF
          && ip + 4 < in_end
          && ref > (u8 *)in_data
#if STRICT_ALIGN
          && ref[0] == ip[0]
          && ref[1] == ip[1]
          && ref[2] == ip[2]
#else
          && *(u16 *)ref == *(u16 *)ip
          && ref[2] == ip[2]
#endif
        )
        {
          /* match found at *ref++ */
          unsigned int len = 2;
          unsigned int maxlen = in_end - ip - len;
          maxlen = maxlen > MAX_REF ? MAX_REF : maxlen;

          if (expect_false (op + 3 + 1 >= out_end)) /* first a faster conservative test */
            if (op - !lit + 3 + 1 >= out_end) /* second the exact but rare test */
              return 0;

          op [- lit - 1] = lit - 1; /* stop run */
          op -= !lit; /* undo run if length is zero */

          for (;;)
            {
              if (expect_true (maxlen > 16))
                {
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;

                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;

                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;

                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                  len++; if (ref [len] != ip [len]) break;
                }

              do
                len++;
              while (len < maxlen && ref[len] == ip[len]);

              break;
            }

          len -= 2; /* len is now #octets - 1 */
          ip++;

          if (len < 7)
            {
              *op++ = (off >> 8) + (len << 5);
            }
          else
            {
              *op++ = (off >> 8) + (  7 << 5);
              *op++ = len - 7;
            }

          *op++ = off;
          lit = 0; op++; /* start run */

          ip += len + 1;

          if (expect_false (ip >= in_end - 2))
            break;

#if ULTRA_FAST || VERY_FAST
          --ip;
# if VERY_FAST && !ULTRA_FAST
          --ip;
# endif
          hval = FRST (ip);

          hval = NEXT (hval, ip);
          htab[IDX (hval)] = ip;
          ip++;

# if VERY_FAST && !ULTRA_FAST
          hval = NEXT (hval, ip);
          htab[IDX (hval)] = ip;
          ip++;
# endif
#else
          ip -= len + 1;

          do
            {
              hval = NEXT (hval, ip);
              htab[IDX (hval)] = ip;
              ip++;
            }
          while (len--);
#endif
        }
      else
        {
          /* one more literal byte we must copy */
          if (expect_false (op >= out_end))
            return 0;

          lit++; *op++ = *ip++;

          if (expect_false (lit == MAX_LIT))
            {
              op [- lit - 1] = lit - 1; /* stop run */
              lit = 0; op++; /* start run */
            }
        }
    }

  if (op + 3 > out_end) /* at most 3 bytes can be missing here */
    return 0;

  while (ip < in_end)
    {
      lit++; *op++ = *ip++;

      if (expect_false (lit == MAX_LIT))
        {
          op [- lit - 1] = lit - 1; /* stop run */
          lit = 0; op++; /* start run */
        }
    }

  op [- lit - 1] = lit - 1; /* end run */
  op -= !lit; /* undo run if length is zero */

  return op - (u8 *)out_data;
}



/*
 * Compress in_len bytes stored at the memory block starting at
 * in_data and write the result to out_data, up to a maximum length
 * of out_len bytes.
 *
 * If the output buffer is not large enough or any error occurs return 0,
 * otherwise return the number of bytes used, which might be considerably
 * more than in_len (but less than 104% of the original size), so it
 * makes sense to always use out_len == in_len - 1), to ensure _some_
 * compression, and store the data uncompressed otherwise (with a flag, of
 * course.
 *
 * lzf_compress might use different algorithms on different systems and
 * even different runs, thus might result in different compressed strings
 * depending on the phase of the moon or similar factors. However, all
 * these strings are architecture-independent and will result in the
 * original data when decompressed using lzf_decompress.
 *
 * The buffers must not be overlapping.
 *
 * If the option LZF_STATE_ARG is enabled, an extra argument must be
 * supplied which is not reflected in this header file. Refer to lzfP.h
 * and lzf_c.c.
 *
 */
//unsigned int 
//lzf_compress (const void *const in_data,  unsigned int in_len,
//              void             *out_data, unsigned int out_len);

/**
 * Compress data.
 *  ind : input data.
 *  inl : input data length.
 *  outd: output data.
 * At most inl+SC_COMP_HEADER_MAXSIZE bytes are required in the output buffer.
 * Return compressed data size.
**/
unsigned long _comp_header_get_data(unsigned char* header, int data_size, int index);
void _comp_header_set_data(unsigned char* header, int data_size, int index, unsigned long val);
unsigned long comp_exec(unsigned char* ind, unsigned long inl, unsigned char* outd)
{
    unsigned long ret;
    int header_len;
    unsigned long lzf_ret;

    int data_size;
    if(inl > 65500)    data_size = 4;
    else if(inl > 240) data_size = 2;
    else               data_size = 1;

    /* By default, set the header length in the case of compressible data. */
    header_len = 1+data_size;
    lzf_ret = 0;
    if(inl > 2)
    { /* As a caution, don't compress very small data (it can cause troubles ?). */
        //unsigned int 
        //lzf_compress (const void *const in_data,  unsigned int in_len,
        //              void             *out_data, unsigned int out_len);
        lzf_ret = lzf_compress(ind,  inl, outd+header_len, inl-header_len-1);
    }
    if(lzf_ret == 0)
    { /* Bad luck, could not compress, hence copy data. */
        header_len = 1;

        /* Set header version & datasize. */
        outd[0] = (0<<4) | (data_size&0x7);
        /* Append uncompressed data.
         * Note: the case ind==0 seems to covered by memcpy. */
        memcpy(outd+header_len, ind, inl);

        /* Set return value */
        ret = header_len + inl;
    }
    else
    { /* Could compress data. Fill header. */

        /* Set header version & datasize. */
        outd[0] = (1<<4) | (data_size&0x7);
        /* Set decompressed size in header. */
        _comp_header_set_data(outd, data_size, 0/*index*/, inl);
        /* Don't need to append compressed data, 
         * since it has been done in lzf_compress function. */

        /* Set return value */
        ret = header_len + lzf_ret;
    }

    /* Return header size +compressed data size. */
    return ret;
}


//----------------------------------------------------------------------
//----- lzf_d.c

#include "common/lzfP.h"

#if AVOID_ERRNO
# define SET_ERRNO(n)
#else
# include <errno.h>
# define SET_ERRNO(n) errno = (n)
#endif

#if (__i386 || __amd64) && __GNUC__ >= 3
# define lzf_movsb(dst, src, len)                \
   asm ("rep movsb"                              \
        : "=D" (dst), "=S" (src), "=c" (len)     \
        :  "0" (dst),  "1" (src),  "2" (len));
#endif

unsigned int 
lzf_decompress (const void *const in_data,  unsigned int in_len,
                void             *out_data, unsigned int out_len)
{
  u8 const *ip = (const u8 *)in_data;
  u8       *op = (u8 *)out_data;
  u8 const *const in_end  = ip + in_len;
  u8       *const out_end = op + out_len;

  do
    {
      unsigned int ctrl = *ip++;

      if (ctrl < (1 << 5)) /* literal run */
        {
          ctrl++;

          if (op + ctrl > out_end)
            {
              SET_ERRNO (E2BIG);
              return 0;
            }

#if CHECK_INPUT
          if (ip + ctrl > in_end)
            {
              SET_ERRNO (EINVAL);
              return 0;
            }
#endif

#ifdef lzf_movsb
          lzf_movsb (op, ip, ctrl);
#else
          do
            *op++ = *ip++;
          while (--ctrl);
#endif
        }
      else /* back reference */
        {
          unsigned int len = ctrl >> 5;

          u8 *ref = op - ((ctrl & 0x1f) << 8) - 1;

#if CHECK_INPUT
          if (ip >= in_end)
            {
              SET_ERRNO (EINVAL);
              return 0;
            }
#endif
          if (len == 7)
            {
              len += *ip++;
#if CHECK_INPUT
              if (ip >= in_end)
                {
                  SET_ERRNO (EINVAL);
                  return 0;
                }
#endif
            }

          ref -= *ip++;

          if (op + len + 2 > out_end)
            {
              SET_ERRNO (E2BIG);
              return 0;
            }

          if (ref < (u8 *)out_data)
            {
              SET_ERRNO (EINVAL);
              return 0;
            }

#ifdef lzf_movsb
          len += 2;
          lzf_movsb (op, ref, len);
#else
          *op++ = *ref++;
          *op++ = *ref++;

          do
            *op++ = *ref++;
          while (--len);
#endif
        }
    }
  while (ip < in_end);

  return op - (u8 *)out_data;
}





/*
 * Decompress data compressed with some version of the lzf_compress
 * function and stored at location in_data and length in_len. The result
 * will be stored at out_data up to a maximum of out_len characters.
 *
 * If the output buffer is not large enough to hold the decompressed
 * data, a 0 is returned and errno is set to E2BIG. Otherwise the number
 * of decompressed bytes (i.e. the original length of the data) is
 * returned.
 *
 * If an error in the compressed data is detected, a zero is returned and
 * errno is set to EINVAL.
 *
 * This function is very fast, about as fast as a copying loop.
 */
//unsigned int 
//lzf_decompress (const void *const in_data,  unsigned int in_len,
//                void             *out_data, unsigned int out_len);



/**
 * Some note about the compressed data header.
 * Header[0] : 000C LLLL, 
 *   where C is the compression flag (0-> not compressed, 1-> compressed)
 *   and L is the size data length (1, or 2, or 4)
 *    -> Next data's length depends on L.
 * In the case C=1, next data is decompressed size.
**/
unsigned long _comp_header_get_data(unsigned char* header, int data_size, int index)
{
    unsigned long ret = 0;

    if(data_size == 4)
    {
        ret = ((header[1 + index * data_size + 0]) << 24)
            | ((header[1 + index * data_size + 1]) << 16)
            | ((header[1 + index * data_size + 2]) <<  8)
            | ((header[1 + index * data_size + 3]) <<  0);
    }
    else if(data_size == 2)
    {
        ret = ((header[1 + index * data_size + 0]) <<  8)
            | ((header[1 + index * data_size + 1]) <<  0);
    }
    else// if(data_size == 1)
    {
        ret = ((header[1 + index * data_size + 0]) <<  0);
    }

    return ret;
}
void _comp_header_set_data(unsigned char* header, int data_size, int index, unsigned long val)
{
    if(data_size == 4)
    {
        header[1 + index * data_size + 0] = (val >> 24) & 0xFF;
        header[1 + index * data_size + 1] = (val >> 16) & 0xFF;
        header[1 + index * data_size + 2] = (val >>  8) & 0xFF;
        header[1 + index * data_size + 3] = (val >>  0) & 0xFF;
    }
    else if(data_size == 2)
    {
        header[1 + index * data_size + 0] = (val >>  8) & 0xFF;
        header[1 + index * data_size + 1] = (val >>  0) & 0xFF;
    }
    else// if(data_size == 1)
    {
        header[1 + index * data_size + 0] = (val >>  0) & 0xFF;
    }
}




unsigned long _decomp_exec_internal(unsigned char* ind, unsigned long inl, unsigned char* outd)
{
    unsigned long ret;
    int header_len;
    unsigned long outl;

    int data_size, compress_flag;

    /* Retrieve header data. */
    if(inl == 0) return 0;
    compress_flag = ind[0] >> 4;
    data_size     = ind[0] & 0xF;
    /* Exclude wrong headers. */
    if((compress_flag > 1) || ((data_size != 1) && (data_size != 2) && (data_size != 4))) return 0;

    /* Set compression-related parameters. */
    if(compress_flag)
    { /* Compressed. */
        header_len = 1+data_size;
        outl = _comp_header_get_data(ind, data_size, 0/*index*/);
        if(outd != NULL)
        { /* Decompress ? */
            //unsigned int 
            //lzf_decompress (const void *const in_data,  unsigned int in_len,
            //                void             *out_data, unsigned int out_len);
            ret = lzf_decompress(ind+header_len, inl-header_len, outd, outl);
        }
        else
        {
            ret = outl;
        }
    }
    else
    { /* Not compressed. */
        header_len = 1;
        outl = inl-header_len;
        if(outd != NULL)
        { /* Decompress ? */
            memcpy(outd, ind+header_len, outl);
        }
        ret = outl;
    }

    return ret;
}
/**
 * Decompress data.
 * It is supposed there is enough space to decompress the data.
 * Note : output data length is automatically computed from input data.
 *  ind : input data.
 *  inl : input data length.
 *  outd: output data.
 * Return decompressed data size.
**/
unsigned long decomp_exec(unsigned char* ind, unsigned long inl, unsigned char* outd)
{
    return _decomp_exec_internal(ind, inl, outd);
}

/**
 * Get data size when decompressed.
 *  ind : input data.
 *  inl : input data length.
 * Return decompressed data size.
**/
unsigned long decomp_getsize(unsigned char* ind, unsigned long inl)
{
    return _decomp_exec_internal(ind, inl, NULL/*No decompress*/);
}




#ifdef _BCB
#ifdef  __cplusplus
}
#endif
#endif // _BCB



