diff --git a/drivers/staging/keucr/smilecc.c b/drivers/staging/keucr/smilecc.c index ba25f158a1bc7de7ae39d9537579d56adf178d4e..3085f1d4a4ebe154be32040a43b183fbbe849055 100644 --- a/drivers/staging/keucr/smilecc.c +++ b/drivers/staging/keucr/smilecc.c @@ -1,39 +1,42 @@ #include "usb.h" #include "scsiglue.h" #include "transport.h" -//#include "stdlib.h" -//#include "EUCR6SK.h" +/* #include "stdlib.h" */ +/* #include "EUCR6SK.h" */ #include "smcommon.h" #include "smil.h" -//#include -//#include -//#include -//#include -// -//#include "EMCRIOS.h" +/* #include */ +/* #include */ +/* #include */ +/* #include */ +/* #include "EMCRIOS.h" */ -// CP0-CP5 code table +/* CP0-CP5 code table */ static BYTE ecctable[256] = { -0x00,0x55,0x56,0x03,0x59,0x0C,0x0F,0x5A,0x5A,0x0F,0x0C,0x59,0x03,0x56,0x55,0x00, -0x65,0x30,0x33,0x66,0x3C,0x69,0x6A,0x3F,0x3F,0x6A,0x69,0x3C,0x66,0x33,0x30,0x65, -0x66,0x33,0x30,0x65,0x3F,0x6A,0x69,0x3C,0x3C,0x69,0x6A,0x3F,0x65,0x30,0x33,0x66, -0x03,0x56,0x55,0x00,0x5A,0x0F,0x0C,0x59,0x59,0x0C,0x0F,0x5A,0x00,0x55,0x56,0x03, -0x69,0x3C,0x3F,0x6A,0x30,0x65,0x66,0x33,0x33,0x66,0x65,0x30,0x6A,0x3F,0x3C,0x69, -0x0C,0x59,0x5A,0x0F,0x55,0x00,0x03,0x56,0x56,0x03,0x00,0x55,0x0F,0x5A,0x59,0x0C, -0x0F,0x5A,0x59,0x0C,0x56,0x03,0x00,0x55,0x55,0x00,0x03,0x56,0x0C,0x59,0x5A,0x0F, -0x6A,0x3F,0x3C,0x69,0x33,0x66,0x65,0x30,0x30,0x65,0x66,0x33,0x69,0x3C,0x3F,0x6A, -0x6A,0x3F,0x3C,0x69,0x33,0x66,0x65,0x30,0x30,0x65,0x66,0x33,0x69,0x3C,0x3F,0x6A, -0x0F,0x5A,0x59,0x0C,0x56,0x03,0x00,0x55,0x55,0x00,0x03,0x56,0x0C,0x59,0x5A,0x0F, -0x0C,0x59,0x5A,0x0F,0x55,0x00,0x03,0x56,0x56,0x03,0x00,0x55,0x0F,0x5A,0x59,0x0C, -0x69,0x3C,0x3F,0x6A,0x30,0x65,0x66,0x33,0x33,0x66,0x65,0x30,0x6A,0x3F,0x3C,0x69, -0x03,0x56,0x55,0x00,0x5A,0x0F,0x0C,0x59,0x59,0x0C,0x0F,0x5A,0x00,0x55,0x56,0x03, -0x66,0x33,0x30,0x65,0x3F,0x6A,0x69,0x3C,0x3C,0x69,0x6A,0x3F,0x65,0x30,0x33,0x66, -0x65,0x30,0x33,0x66,0x3C,0x69,0x6A,0x3F,0x3F,0x6A,0x69,0x3C,0x66,0x33,0x30,0x65, -0x00,0x55,0x56,0x03,0x59,0x0C,0x0F,0x5A,0x5A,0x0F,0x0C,0x59,0x03,0x56,0x55,0x00 +0x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F, 0x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03, +0x56, 0x55, 0x00, 0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A, +0x69, 0x3C, 0x66, 0x33, 0x30, 0x65, 0x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69, +0x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65, 0x30, 0x33, 0x66, 0x03, 0x56, 0x55, 0x00, +0x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03, 0x69, +0x3C, 0x3F, 0x6A, 0x30, 0x65, 0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F, +0x3C, 0x69, 0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56, 0x03, 0x00, +0x55, 0x0F, 0x5A, 0x59, 0x0C, 0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55, +0x55, 0x00, 0x03, 0x56, 0x0C, 0x59, 0x5A, 0x0F, 0x6A, 0x3F, 0x3C, 0x69, 0x33, +0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F, 0x6A, 0x6A, 0x3F, +0x3C, 0x69, 0x33, 0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F, +0x6A, 0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55, 0x55, 0x00, 0x03, 0x56, +0x0C, 0x59, 0x5A, 0x0F, 0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56, +0x03, 0x00, 0x55, 0x0F, 0x5A, 0x59, 0x0C, 0x69, 0x3C, 0x3F, 0x6A, 0x30, 0x65, +0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F, 0x3C, 0x69, 0x03, 0x56, 0x55, +0x00, 0x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03, +0x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69, 0x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65, +0x30, 0x33, 0x66, 0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A, +0x69, 0x3C, 0x66, 0x33, 0x30, 0x65, 0x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F, +0x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03, 0x56, 0x55, 0x00 }; -static void trans_result (BYTE, BYTE, BYTE *, BYTE *); +static void trans_result(BYTE, BYTE, BYTE *, BYTE *); #define BIT7 0x80 #define BIT6 0x40 @@ -49,138 +52,140 @@ static void trans_result (BYTE, BYTE, BYTE *, BYTE *); #define CORRECTABLE 0x00555554L /* - * reg2; // LP14,LP12,LP10,... - * reg3; // LP15,LP13,LP11,... - * *ecc1; // LP15,LP14,LP13,... - * *ecc2; // LP07,LP06,LP05,... + * reg2; * LP14,LP12,LP10,... + * reg3; * LP15,LP13,LP11,... + * *ecc1; * LP15,LP14,LP13,... + * *ecc2; * LP07,LP06,LP05,... */ static void trans_result(BYTE reg2, BYTE reg3, BYTE *ecc1, BYTE *ecc2) { - BYTE a; // Working for reg2,reg3 - BYTE b; // Working for ecc1,ecc2 - BYTE i; // For counting - - a=BIT7; b=BIT7; // 80h=10000000b - *ecc1=*ecc2=0; // Clear ecc1,ecc2 - for(i=0; i<4; ++i) { - if ((reg3&a)!=0) - *ecc1|=b; // LP15,13,11,9 -> ecc1 - b=b>>1; // Right shift - if ((reg2&a)!=0) - *ecc1|=b; // LP14,12,10,8 -> ecc1 - b=b>>1; // Right shift - a=a>>1; // Right shift - } - - b=BIT7; // 80h=10000000b - for(i=0; i<4; ++i) { - if ((reg3&a)!=0) - *ecc2|=b; // LP7,5,3,1 -> ecc2 - b=b>>1; // Right shift - if ((reg2&a)!=0) - *ecc2|=b; // LP6,4,2,0 -> ecc2 - b=b>>1; // Right shift - a=a>>1; // Right shift - } + BYTE a; /* Working for reg2,reg3 */ + BYTE b; /* Working for ecc1,ecc2 */ + BYTE i; /* For counting */ + + a = BIT7; b = BIT7; /* 80h=10000000b */ + *ecc1 = *ecc2 = 0; /* Clear ecc1,ecc2 */ + for (i = 0; i < 4; ++i) { + if ((reg3&a) != 0) + *ecc1 |= b; /* LP15,13,11,9 -> ecc1 */ + b = b>>1; /* Right shift */ + if ((reg2&a) != 0) + *ecc1 |= b; /* LP14,12,10,8 -> ecc1 */ + b = b>>1; /* Right shift */ + a = a>>1; /* Right shift */ + } + + b = BIT7; /* 80h=10000000b */ + for (i = 0; i < 4; ++i) { + if ((reg3&a) != 0) + *ecc2 |= b; /* LP7,5,3,1 -> ecc2 */ + b = b>>1; /* Right shift */ + if ((reg2&a) != 0) + *ecc2 |= b; /* LP6,4,2,0 -> ecc2 */ + b = b>>1; /* Right shift */ + a = a>>1; /* Right shift */ + } } -//static void calculate_ecc(table,data,ecc1,ecc2,ecc3) +/*static void calculate_ecc(table,data,ecc1,ecc2,ecc3) */ /* - * *table; // CP0-CP5 code table - * *data; // DATA - * *ecc1; // LP15,LP14,LP13,... - * *ecc2; // LP07,LP06,LP05,... - * *ecc3; // CP5,CP4,CP3,...,"1","1" + * *table; * CP0-CP5 code table + * *data; * DATA + * *ecc1; * LP15,LP14,LP13,... + * *ecc2; * LP07,LP06,LP05,... + * *ecc3; * CP5,CP4,CP3,...,"1","1" */ void calculate_ecc(BYTE *table, BYTE *data, BYTE *ecc1, BYTE *ecc2, BYTE *ecc3) { - DWORD i; // For counting - BYTE a; // Working for table - BYTE reg1; // D-all,CP5,CP4,CP3,... - BYTE reg2; // LP14,LP12,L10,... - BYTE reg3; // LP15,LP13,L11,... - - reg1=reg2=reg3=0; // Clear parameter - for(i=0; i<256; ++i) { - a=table[data[i]]; // Get CP0-CP5 code from table - reg1^=(a&MASK_CPS); // XOR with a - if ((a&BIT6)!=0) - { // If D_all(all bit XOR) = 1 - reg3^=(BYTE)i; // XOR with counter - reg2^=~((BYTE)i); // XOR with inv. of counter - } - } - - // Trans LP14,12,10,... & LP15,13,11,... -> LP15,14,13,... & LP7,6,5,.. - trans_result(reg2,reg3,ecc1,ecc2); - *ecc1=~(*ecc1); *ecc2=~(*ecc2); // Inv. ecc2 & ecc3 - *ecc3=((~reg1)<<2)|BIT1BIT0; // Make TEL format + DWORD i; /* For counting */ + BYTE a; /* Working for table */ + BYTE reg1; /* D-all,CP5,CP4,CP3,... */ + BYTE reg2; /* LP14,LP12,L10,... */ + BYTE reg3; /* LP15,LP13,L11,... */ + + reg1 = reg2 = reg3 = 0; /* Clear parameter */ + for (i = 0; i < 256; ++i) { + a = table[data[i]]; /* Get CP0-CP5 code from table */ + reg1 ^= (a&MASK_CPS); /* XOR with a */ + if ((a&BIT6) != 0) { /* If D_all(all bit XOR) = 1 */ + reg3 ^= (BYTE)i; /* XOR with counter */ + reg2 ^= ~((BYTE)i); /* XOR with inv. of counter */ + } + } + + /* Trans LP14,12,10,... & LP15,13,11,... -> + LP15,14,13,... & LP7,6,5,.. */ + trans_result(reg2, reg3, ecc1, ecc2); + *ecc1 = ~(*ecc1); *ecc2 = ~(*ecc2); /* Inv. ecc2 & ecc3 */ + *ecc3 = ((~reg1)<<2)|BIT1BIT0; /* Make TEL format */ } /* - * *data; // DATA - * *eccdata; // ECC DATA - * ecc1; // LP15,LP14,LP13,... - * ecc2; // LP07,LP06,LP05,... - * ecc3; // CP5,CP4,CP3,...,"1","1" + * *data; * DATA + * *eccdata; * ECC DATA + * ecc1; * LP15,LP14,LP13,... + * ecc2; * LP07,LP06,LP05,... + * ecc3; * CP5,CP4,CP3,...,"1","1" */ BYTE correct_data(BYTE *data, BYTE *eccdata, BYTE ecc1, BYTE ecc2, BYTE ecc3) { - DWORD l; // Working to check d - DWORD d; // Result of comparison - DWORD i; // For counting - BYTE d1,d2,d3; // Result of comparison - BYTE a; // Working for add - BYTE add; // Byte address of cor. DATA - BYTE b; // Working for bit - BYTE bit; // Bit address of cor. DATA - - d1=ecc1^eccdata[1]; d2=ecc2^eccdata[0]; // Compare LP's - d3=ecc3^eccdata[2]; // Comapre CP's - d=((DWORD)d1<<16) // Result of comparison - +((DWORD)d2<<8) - +(DWORD)d3; - - if (d==0) return(0); // If No error, return - - if (((d^(d>>1))&CORRECTABLE)==CORRECTABLE) - { // If correctable - l=BIT23; - add=0; // Clear parameter - a=BIT7; - - for(i=0; i<8; ++i) { // Checking 8 bit - if ((d&l)!=0) add|=a; // Make byte address from LP's - l>>=2; a>>=1; // Right Shift - } - - bit=0; // Clear parameter - b=BIT2; - for(i=0; i<3; ++i) { // Checking 3 bit - if ((d&l)!=0) bit|=b; // Make bit address from CP's - l>>=2; b>>=1; // Right shift - } - - b=BIT0; - data[add]^=(b<>=1; // Right shift - } - - if (i==1) - { // If ECC error - eccdata[1]=ecc1; eccdata[0]=ecc2; // Put right ECC code - eccdata[2]=ecc3; - return(2); - } - return(3); // Uncorrectable error + DWORD l; /* Working to check d */ + DWORD d; /* Result of comparison */ + DWORD i; /* For counting */ + BYTE d1, d2, d3; /* Result of comparison */ + BYTE a; /* Working for add */ + BYTE add; /* Byte address of cor. DATA */ + BYTE b; /* Working for bit */ + BYTE bit; /* Bit address of cor. DATA */ + + d1 = ecc1^eccdata[1]; d2 = ecc2^eccdata[0]; /* Compare LP's */ + d3 = ecc3^eccdata[2]; /* Comapre CP's */ + d = ((DWORD)d1<<16) /* Result of comparison */ + +((DWORD)d2<<8) + +(DWORD)d3; + + if (d == 0) + return 0; /* If No error, return */ + + if (((d^(d>>1))&CORRECTABLE) == CORRECTABLE) { /* If correctable */ + l = BIT23; + add = 0; /* Clear parameter */ + a = BIT7; + + for (i = 0; i < 8; ++i) { /* Checking 8 bit */ + if ((d&l) != 0) + add |= a; /* Make byte address from LP's */ + l >>= 2; a >>= 1; /* Right Shift */ + } + + bit = 0; /* Clear parameter */ + b = BIT2; + for (i = 0; i < 3; ++i) { /* Checking 3 bit */ + if ((d&l) != 0) + bit |= b; /* Make bit address from CP's */ + l >>= 2; b >>= 1; /* Right shift */ + } + + b = BIT0; + data[add] ^= (b<>= 1; /* Right shift */ + } + + if (i == 1) { /* If ECC error */ + eccdata[1] = ecc1; eccdata[0] = ecc2; /* Put right ECC code */ + eccdata[2] = ecc3; + return 2; + } + return 3; /* Uncorrectable error */ } int _Correct_D_SwECC(BYTE *buf, BYTE *redundant_ecc, BYTE *calculate_ecc) @@ -200,7 +205,7 @@ int _Correct_D_SwECC(BYTE *buf, BYTE *redundant_ecc, BYTE *calculate_ecc) void _Calculate_D_SwECC(BYTE *buf, BYTE *ecc) { - calculate_ecc(ecctable,buf,ecc+1,ecc+0,ecc+2); + calculate_ecc(ecctable, buf, ecc+1, ecc+0, ecc+2); }