drivers/staging/keucr/smilecc.c

   1 #include "usb.h"
   2 #include "scsiglue.h"
   3 #include "transport.h"
   4 //#include "stdlib.h"
   5 //#include "EUCR6SK.h"
   6 #include "smcommon.h"
   7 #include "smil.h"
   8
   9 //#include <stdio.h>
  10 //#include <stdlib.h>
  11 //#include <string.h>
  12 //#include <dos.h>
  13 //
  14 //#include "EMCRIOS.h"
  15
  16 // CP0-CP5 code table
  17 static BYTE ecctable[256] = {
  18 0x00,0x55,0x56,0x03,0x59,0x0C,0x0F,0x5A,0x5A,0x0F,0x0C,0x59,0x03,0x56,0x55,0x00,
  19 0x65,0x30,0x33,0x66,0x3C,0x69,0x6A,0x3F,0x3F,0x6A,0x69,0x3C,0x66,0x33,0x30,0x65,
  20 0x66,0x33,0x30,0x65,0x3F,0x6A,0x69,0x3C,0x3C,0x69,0x6A,0x3F,0x65,0x30,0x33,0x66,
  21 0x03,0x56,0x55,0x00,0x5A,0x0F,0x0C,0x59,0x59,0x0C,0x0F,0x5A,0x00,0x55,0x56,0x03,
  22 0x69,0x3C,0x3F,0x6A,0x30,0x65,0x66,0x33,0x33,0x66,0x65,0x30,0x6A,0x3F,0x3C,0x69,
  23 0x0C,0x59,0x5A,0x0F,0x55,0x00,0x03,0x56,0x56,0x03,0x00,0x55,0x0F,0x5A,0x59,0x0C,
  24 0x0F,0x5A,0x59,0x0C,0x56,0x03,0x00,0x55,0x55,0x00,0x03,0x56,0x0C,0x59,0x5A,0x0F,
  25 0x6A,0x3F,0x3C,0x69,0x33,0x66,0x65,0x30,0x30,0x65,0x66,0x33,0x69,0x3C,0x3F,0x6A,
  26 0x6A,0x3F,0x3C,0x69,0x33,0x66,0x65,0x30,0x30,0x65,0x66,0x33,0x69,0x3C,0x3F,0x6A,
  27 0x0F,0x5A,0x59,0x0C,0x56,0x03,0x00,0x55,0x55,0x00,0x03,0x56,0x0C,0x59,0x5A,0x0F,
  28 0x0C,0x59,0x5A,0x0F,0x55,0x00,0x03,0x56,0x56,0x03,0x00,0x55,0x0F,0x5A,0x59,0x0C,
  29 0x69,0x3C,0x3F,0x6A,0x30,0x65,0x66,0x33,0x33,0x66,0x65,0x30,0x6A,0x3F,0x3C,0x69,
  30 0x03,0x56,0x55,0x00,0x5A,0x0F,0x0C,0x59,0x59,0x0C,0x0F,0x5A,0x00,0x55,0x56,0x03,
  31 0x66,0x33,0x30,0x65,0x3F,0x6A,0x69,0x3C,0x3C,0x69,0x6A,0x3F,0x65,0x30,0x33,0x66,
  32 0x65,0x30,0x33,0x66,0x3C,0x69,0x6A,0x3F,0x3F,0x6A,0x69,0x3C,0x66,0x33,0x30,0x65,
  33 0x00,0x55,0x56,0x03,0x59,0x0C,0x0F,0x5A,0x5A,0x0F,0x0C,0x59,0x03,0x56,0x55,0x00
  34 };
  35
  36 static void   trans_result  (BYTE,   BYTE,   BYTE *, BYTE *);
  37
  38 #define BIT7        0x80
  39 #define BIT6        0x40
  40 #define BIT5        0x20
  41 #define BIT4        0x10
  42 #define BIT3        0x08
  43 #define BIT2        0x04
  44 #define BIT1        0x02
  45 #define BIT0        0x01
  46 #define BIT1BIT0    0x03
  47 #define BIT23       0x00800000L
  48 #define MASK_CPS    0x3f
  49 #define CORRECTABLE 0x00555554L
  50
  51 static void trans_result(reg2,reg3,ecc1,ecc2)
  52 BYTE reg2; // LP14,LP12,LP10,...
  53 BYTE reg3; // LP15,LP13,LP11,...
  54 BYTE *ecc1; // LP15,LP14,LP13,...
  55 BYTE *ecc2; // LP07,LP06,LP05,...
  56 {
  57     BYTE a; // Working for reg2,reg3
  58     BYTE b; // Working for ecc1,ecc2
  59     BYTE i; // For counting
  60
  61     a=BIT7; b=BIT7; // 80h=10000000b
  62     *ecc1=*ecc2=0; // Clear ecc1,ecc2
  63     for(i=0; i<4; ++i) {
  64         if ((reg3&a)!=0)
  65             *ecc1|=b; // LP15,13,11,9 -> ecc1
  66         b=b>>1; // Right shift
  67         if ((reg2&a)!=0)
  68             *ecc1|=b; // LP14,12,10,8 -> ecc1
  69         b=b>>1; // Right shift
  70         a=a>>1; // Right shift
  71     }
  72
  73     b=BIT7; // 80h=10000000b
  74     for(i=0; i<4; ++i) {
  75         if ((reg3&a)!=0)
  76             *ecc2|=b; // LP7,5,3,1 -> ecc2
  77         b=b>>1; // Right shift
  78         if ((reg2&a)!=0)
  79             *ecc2|=b; // LP6,4,2,0 -> ecc2
  80         b=b>>1; // Right shift
  81         a=a>>1; // Right shift
  82     }
  83 }
  84
  85 //static void calculate_ecc(table,data,ecc1,ecc2,ecc3)
  86 void calculate_ecc(table,data,ecc1,ecc2,ecc3)
  87 BYTE *table; // CP0-CP5 code table
  88 BYTE *data; // DATA
  89 BYTE *ecc1; // LP15,LP14,LP13,...
  90 BYTE *ecc2; // LP07,LP06,LP05,...
  91 BYTE *ecc3; // CP5,CP4,CP3,...,"1","1"
  92 {
  93     DWORD  i;    // For counting
  94     BYTE a;    // Working for table
  95     BYTE reg1; // D-all,CP5,CP4,CP3,...
  96     BYTE reg2; // LP14,LP12,L10,...
  97     BYTE reg3; // LP15,LP13,L11,...
  98
  99     reg1=reg2=reg3=0;   // Clear parameter
 100     for(i=0; i<256; ++i) {
 101         a=table[data[i]]; // Get CP0-CP5 code from table
 102         reg1^=(a&MASK_CPS); // XOR with a
 103         if ((a&BIT6)!=0)
 104         { // If D_all(all bit XOR) = 1
 105             reg3^=(BYTE)i; // XOR with counter
 106             reg2^=~((BYTE)i); // XOR with inv. of counter
 107         }
 108     }
 109
 110     // Trans LP14,12,10,... & LP15,13,11,... -> LP15,14,13,... & LP7,6,5,..
 111     trans_result(reg2,reg3,ecc1,ecc2);
 112     *ecc1=~(*ecc1); *ecc2=~(*ecc2); // Inv. ecc2 & ecc3
 113     *ecc3=((~reg1)<<2)|BIT1BIT0; // Make TEL format
 114 }
 115
 116 BYTE correct_data(data,eccdata,ecc1,ecc2,ecc3)
 117 BYTE *data; // DATA
 118 BYTE *eccdata; // ECC DATA
 119 BYTE ecc1; // LP15,LP14,LP13,...
 120 BYTE ecc2; // LP07,LP06,LP05,...
 121 BYTE ecc3; // CP5,CP4,CP3,...,"1","1"
 122 {
 123     DWORD l; // Working to check d
 124     DWORD d; // Result of comparison
 125     DWORD i; // For counting
 126     BYTE d1,d2,d3; // Result of comparison
 127     BYTE a; // Working for add
 128     BYTE add; // Byte address of cor. DATA
 129     BYTE b; // Working for bit
 130     BYTE bit; // Bit address of cor. DATA
 131
 132     d1=ecc1^eccdata[1]; d2=ecc2^eccdata[0]; // Compare LP's
 133     d3=ecc3^eccdata[2]; // Comapre CP's
 134     d=((DWORD)d1<<16) // Result of comparison
 135     +((DWORD)d2<<8)
 136     +(DWORD)d3;
 137
 138     if (d==0) return(0); // If No error, return
 139
 140     if (((d^(d>>1))&CORRECTABLE)==CORRECTABLE)
 141     { // If correctable
 142         l=BIT23;
 143         add=0; // Clear parameter
 144         a=BIT7;
 145
 146         for(i=0; i<8; ++i) { // Checking 8 bit
 147             if ((d&l)!=0) add|=a; // Make byte address from LP's
 148             l>>=2; a>>=1; // Right Shift
 149         }
 150
 151         bit=0; // Clear parameter
 152         b=BIT2;
 153         for(i=0; i<3; ++i) { // Checking 3 bit
 154             if ((d&l)!=0) bit|=b; // Make bit address from CP's
 155             l>>=2; b>>=1; // Right shift
 156         }
 157
 158         b=BIT0;
 159         data[add]^=(b<<bit); // Put corrected data
 160         return(1);
 161     }
 162
 163     i=0; // Clear count
 164     d&=0x00ffffffL; // Masking
 165
 166     while(d) { // If d=0 finish counting
 167         if (d&BIT0) ++i; // Count number of 1 bit
 168         d>>=1; // Right shift
 169     }
 170
 171     if (i==1)
 172     { // If ECC error
 173         eccdata[1]=ecc1; eccdata[0]=ecc2; // Put right ECC code
 174         eccdata[2]=ecc3;
 175         return(2);
 176     }
 177     return(3); // Uncorrectable error
 178 }
 179
 180 int _Correct_D_SwECC(buf,redundant_ecc,calculate_ecc)
 181 BYTE *buf;
 182 BYTE *redundant_ecc;
 183 BYTE *calculate_ecc;
 184 {
 185         DWORD err;
 186
 187         err = correct_data(buf, redundant_ecc, *(calculate_ecc + 1),
 188                            *(calculate_ecc), *(calculate_ecc + 2));
 189         if (err == 1)
 190                 memcpy(calculate_ecc, redundant_ecc, 3);
 191
 192         if (err == 0 || err == 1 || err == 2)
 193                 return 0;
 194
 195         return -1;
 196 }
 197
 198 void _Calculate_D_SwECC(buf,ecc)
 199 BYTE *buf;
 200 BYTE *ecc;
 201 {
 202     calculate_ecc(ecctable,buf,ecc+1,ecc+0,ecc+2);
 203 }
 204
 205