//****************************************************************************
//
//  Copyright (C) SEIKO EPSON CORP. 2007
//
//  File name: rs.c
//    This is RS eror corrctor calls
//
//  Revision history
//      2008.05.06        1st version
//
//****************************************************************************
#include "rs.h"

extern void sys_Printf(char *fmt, ...);

//-------------------------------------------------------
// variable declaration
unsigned int gexp[1024];
unsigned int glog[1024];

//-------------------------------------------------------
// static function declaration
static void One_Error_Correction(unsigned int ulCnt, unsigned int *Syndrome, 
                                 unsigned int *location, unsigned char *codeword);
static void Two_Error_Correction(unsigned int ulCnt, unsigned int *Syndrome, 
                                 unsigned int *location, unsigned char *codeword);
static void Three_Error_Correction(unsigned int ulCnt, unsigned int *Syndrome, 
                                 unsigned int *location, unsigned char *codeword);
static void Four_Error_Correction(unsigned int ulCnt, unsigned int *Syndrome, 
                                 unsigned int *location, unsigned char *codeword);
static unsigned int Find_Location (unsigned int ulCnt, unsigned int *Sigma, unsigned int *Location);

//---------------------------------------------------------------------------------
//      Function name  :   rs_ErrCorrect
//      Description    :   rs error corrector
//      Parameter      :   unsigned int ulCnt
//                     :   unsigned char *pucSrc
//                     :   unsigned char *pucSyndrome
//      Return value   :   int
//--------------------------------------------------------------------------------- 
int rs_ErrCorrect(unsigned int ulCnt, unsigned char *pucSrc, unsigned char *pucSyndrome)
{
  unsigned int  i;

  unsigned int  Syndrome[10];
  unsigned int  number;
  unsigned int  location[4];

  unsigned int  det0[4];
  unsigned int  det1[4];
  unsigned int  ma0;
  unsigned int  ma1;
  unsigned int  ma[6];
  unsigned int  mb[4];
  unsigned int  dets[16];
  unsigned int  mul[6];
  unsigned int  sigma[4];

  init_gtbl();

  Syndrome[9] = ((pucSyndrome[9] << 2) & 0x3ff) | (pucSyndrome[8] >> 6);
  Syndrome[8] = ((pucSyndrome[8] << 4) & 0x3ff) | (pucSyndrome[7] >> 4);
  Syndrome[7] = ((pucSyndrome[7] << 6) & 0x3ff) | (pucSyndrome[6] >> 2);
  Syndrome[6] = ((pucSyndrome[6] << 8) & 0x3ff) | pucSyndrome[5];
  Syndrome[5] = ((pucSyndrome[4] << 2) & 0x3ff) | (pucSyndrome[3] >> 6);
  Syndrome[4] = ((pucSyndrome[3] << 4) & 0x3ff) | (pucSyndrome[2] >> 4);
  Syndrome[3] = ((pucSyndrome[2] << 6) & 0x3ff) | (pucSyndrome[1] >> 2);
  Syndrome[2] = ((pucSyndrome[1] << 8) & 0x3ff) | pucSyndrome[0];

  for (i=0; i<8; i++) Syndrome[i] = Syndrome[i+2];

  i = Syndrome[0] | Syndrome[1] | Syndrome[2] | Syndrome[3] |
      Syndrome[4] | Syndrome[5] | Syndrome[6] | Syndrome[7];
  
  if (i==0)
    return 0;

  // Starting Error Correction
  number   = 1;
  ma[0] = DetMA4(Syndrome);
  if (ma[0] != 0) {
    mb[0] = DetMB40(Syndrome);
    mb[1] = DetMB41(Syndrome);
    mb[2] = DetMB42(Syndrome);
    mb[3] = DetMB43(Syndrome);
 
    sigma[3] = gdiv(ma[0], mb[0]);
    sigma[2] = gdiv(ma[0], mb[1]);
    sigma[1] = gdiv(ma[0], mb[2]);
    sigma[0] = gdiv(ma[0], mb[3]);
    
    number = Find_Location(ulCnt, sigma, location);
//    if (number != 4) printf("Error\n");
  }else {
    ma[0] = DetMA30(Syndrome);
    ma[1] = DetMA31(Syndrome);
    ma[2] = DetMA32(Syndrome);
    ma[3] = DetMA33(Syndrome);
     
    if ((ma[0] != 0) | (ma[1] != 0) |
        (ma[2] != 0) | (ma[3] != 0)) {
      sigma[3] = 0;
      if (ma[0] != 0) {
        mb[0] = DetMB300(Syndrome);
        mb[1] = DetMB301(Syndrome);
        mb[2] = DetMB302(Syndrome);
       
        sigma[2] = gdiv(ma[0], mb[0]);
        sigma[1] = gdiv(ma[0], mb[1]);
        sigma[0] = gdiv(ma[0], mb[2]);
      } else if (ma[1] != 0) {
        mb[0] = DetMB310(Syndrome);
        mb[1] = DetMB311(Syndrome);
        mb[2] = DetMB312(Syndrome);
       
        sigma[2] = gdiv(ma[1], mb[0]);
        sigma[1] = gdiv(ma[1], mb[1]);
        sigma[0] = gdiv(ma[1], mb[2]);
      } else if (ma[2] != 0) {
        mb[0] = DetMB320(Syndrome);
        mb[1] = DetMB321(Syndrome);
        mb[2] = DetMB322(Syndrome);
       
        sigma[2] = gdiv(ma[2], mb[0]);
        sigma[1] = gdiv(ma[2], mb[1]);
        sigma[0] = gdiv(ma[2], mb[2]);
      } else if (ma[3] != 0) {
        mb[0] = DetMB330(Syndrome);
        mb[1] = DetMB331(Syndrome);
        mb[2] = DetMB332(Syndrome);
       
        sigma[2] = gdiv(ma[3], mb[0]);
        sigma[1] = gdiv(ma[3], mb[1]);
        sigma[0] = gdiv(ma[3], mb[2]);
      }
      
      number = Find_Location(ulCnt, sigma, location);
//      if (number != 3) printf("Error\n");
    } else {
      ma[0] = DetMA20(Syndrome);
      ma[1] = DetMA21(Syndrome);
      ma[2] = DetMA22(Syndrome);
      ma[3] = DetMA23(Syndrome);
      ma[4] = DetMA24(Syndrome);
      ma[5] = DetMA25(Syndrome);
 
      if ((ma[0] != 0) || (ma[1] != 0) || (ma[2] != 0) ||
          (ma[3] != 0) || (ma[4] != 0) || (ma[5] != 0)) {
        sigma[3] = 0;
        sigma[2] = 0;
        if (ma[0] != 0) {
          mb[0] = DetMB200(Syndrome);
          mb[1] = DetMB201(Syndrome);
          sigma[1] = gdiv(ma[0], mb[0]);
          sigma[0] = gdiv(ma[0], mb[1]);
        }else if (ma[1] != 0) {
          mb[0] = DetMB210(Syndrome);
          mb[1] = DetMB211(Syndrome);
          sigma[1] = gdiv(ma[1], mb[0]);
          sigma[0] = gdiv(ma[1], mb[1]);
        }else if (ma[2] != 0) {
          mb[0] = DetMB220(Syndrome);
          mb[1] = DetMB221(Syndrome);
          sigma[1] = gdiv(ma[2], mb[0]);
          sigma[0] = gdiv(ma[2], mb[1]);
        }else if (ma[3] != 0) {
          mb[0] = DetMB230(Syndrome);
          mb[1] = DetMB231(Syndrome);
          sigma[1] = gdiv(ma[3], mb[0]);
          sigma[0] = gdiv(ma[3], mb[1]);
        }else if (ma[4] != 0) {
 
          mb[0] = DetMB240(Syndrome);
          mb[1] = DetMB241(Syndrome);
          sigma[1] = gdiv(ma[4], mb[0]);
          sigma[0] = gdiv(ma[4], mb[1]);
        }else if (ma[5] != 0) {
          mb[0] = DetMB250(Syndrome);
          mb[1] = DetMB251(Syndrome);
          sigma[1] = gdiv(ma[5], mb[0]);
          sigma[0] = gdiv(ma[5], mb[1]);
        }
        number = Find_Location(ulCnt, sigma, location);
//        if (number != 2) printf("Error\n");
      } else{
        number = 1;
        location[0] = ulCnt+8-glog[gdiv(Syndrome[3], Syndrome[4])];
      }
    }
  }

  switch (number) {
    case 1:{
      sys_Printf("\nError Data[0] = 0x%2x, Location = %d", pucSrc[location[0]],location[0]); 

      One_Error_Correction(ulCnt, Syndrome, location, pucSrc);

      sys_Printf("\nCorrect Data      = 0x%x", pucSrc[location[0]]); 
      break;
    }
 
    case 2:{
      sys_Printf("\nError Data[0] = 0x%2x, Location = %d", pucSrc[location[0]],location[0]); 
      sys_Printf("\nError Data[1] = 0x%2x, Location = %d", pucSrc[location[1]],location[1]); 

      Two_Error_Correction(ulCnt, Syndrome, location, pucSrc);
      if (location[0] < ulCnt) 
        sys_Printf("\nCorrect Data[0]   = 0x%x", pucSrc[location[0]]); 
      if (location[1] < ulCnt) 
        sys_Printf("\nCorrect Data[1]   = 0x%x", pucSrc[location[1]]); 
     break;
    }
 
    case 3:{
      sys_Printf("\nError Data[0] = 0x%2x, Location = %d", pucSrc[location[0]],location[0]); 
      sys_Printf("\nError Data[1] = 0x%2x, Location = %d", pucSrc[location[1]],location[1]); 
      sys_Printf("\nError Data[2] = 0x%2x, Location = %d", pucSrc[location[2]],location[2]); 
        
      Three_Error_Correction(ulCnt, Syndrome, location, pucSrc);
      
      if (location[0] < ulCnt) 
        sys_Printf("\nCorrect Data[0]   = 0x%x", pucSrc[location[0]]); 
      if (location[1] < ulCnt) 
        sys_Printf("\nCorrect Data[1]   = 0x%x", pucSrc[location[1]]); 
      if (location[2] < ulCnt) 
        sys_Printf("\nCorrect Data[2]   = 0x%x", pucSrc[location[2]]); 
      break;
    }
 
    case 4:{
      sys_Printf("\nError Data[0] = 0x%2x, Location = %d", pucSrc[location[0]],location[0]); 
      sys_Printf("\nError Data[1] = 0x%2x, Location = %d", pucSrc[location[1]],location[1]); 
      sys_Printf("\nError Data[2] = 0x%2x, Location = %d", pucSrc[location[2]],location[2]); 
      sys_Printf("\nError Data[3] = 0x%2x, Location = %d", pucSrc[location[3]],location[3]); 
 
      Four_Error_Correction(ulCnt, Syndrome, location, pucSrc);

      if (location[0] < ulCnt) 
        sys_Printf("\nCorrect Data[0]   = 0x%x", pucSrc[location[0]]); 
      if (location[1] < ulCnt) 
        sys_Printf("\nCorrect Data[1]   = 0x%x", pucSrc[location[1]]); 
      if (location[2] < ulCnt) 
        sys_Printf("\nCorrect Data[2]   = 0x%x", pucSrc[location[2]]); 
      if (location[3] < ulCnt) 
        sys_Printf("\nCorrect Data[3]   = 0x%x", pucSrc[location[3]]); 
      break;
    }
  }
}


//---------------------------------------------------------------------------------
//      Function name  :   init_gtbl
//      Description    :   initialize table
//      Parameter      :   NULL
//      Return value   :   NULL
//--------------------------------------------------------------------------------- 
void init_gtbl(void)
{
  unsigned int i;
  unsigned int p0, p1, p2, p3, p4, p5, p6, p7, p8, p9;
  unsigned int p9_tmp;

  gexp[0] = 0;
  gexp[1] = 1;

  /* x^10 + x^3 + 1 */
  p0 = 1;
  p1 = p2 = p3 = p4 = p5 = p6 = p7 = p8 = p9 = 0;