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