blob: 6b8f7d7a7436f5b633c8e63345056fa954d833d1 [file] [log] [blame]
Al Cho126bb032010-09-08 00:42:32 -07001#include "usb.h"
2#include "scsiglue.h"
3#include "transport.h"
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +08004/* #include "stdlib.h" */
5/* #include "EUCR6SK.h" */
Al Cho126bb032010-09-08 00:42:32 -07006#include "smcommon.h"
7#include "smil.h"
8
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +08009/* #include <stdio.h> */
10/* #include <stdlib.h> */
11/* #include <string.h> */
12/* #include <dos.h> */
13/* #include "EMCRIOS.h" */
Al Cho126bb032010-09-08 00:42:32 -070014
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080015/* CP0-CP5 code table */
Al Cho126bb032010-09-08 00:42:32 -070016static BYTE ecctable[256] = {
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800170x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F, 0x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03,
180x56, 0x55, 0x00, 0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A,
190x69, 0x3C, 0x66, 0x33, 0x30, 0x65, 0x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69,
200x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65, 0x30, 0x33, 0x66, 0x03, 0x56, 0x55, 0x00,
210x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03, 0x69,
220x3C, 0x3F, 0x6A, 0x30, 0x65, 0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F,
230x3C, 0x69, 0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56, 0x03, 0x00,
240x55, 0x0F, 0x5A, 0x59, 0x0C, 0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55,
250x55, 0x00, 0x03, 0x56, 0x0C, 0x59, 0x5A, 0x0F, 0x6A, 0x3F, 0x3C, 0x69, 0x33,
260x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F, 0x6A, 0x6A, 0x3F,
270x3C, 0x69, 0x33, 0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F,
280x6A, 0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55, 0x55, 0x00, 0x03, 0x56,
290x0C, 0x59, 0x5A, 0x0F, 0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56,
300x03, 0x00, 0x55, 0x0F, 0x5A, 0x59, 0x0C, 0x69, 0x3C, 0x3F, 0x6A, 0x30, 0x65,
310x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F, 0x3C, 0x69, 0x03, 0x56, 0x55,
320x00, 0x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03,
330x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69, 0x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65,
340x30, 0x33, 0x66, 0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A,
350x69, 0x3C, 0x66, 0x33, 0x30, 0x65, 0x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F,
360x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03, 0x56, 0x55, 0x00
Al Cho126bb032010-09-08 00:42:32 -070037};
38
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080039static void trans_result(BYTE, BYTE, BYTE *, BYTE *);
Al Cho126bb032010-09-08 00:42:32 -070040
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
Randy Dunlapd8aba9d2011-05-09 10:46:24 -070054/*
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080055 * reg2; * LP14,LP12,LP10,...
56 * reg3; * LP15,LP13,LP11,...
57 * *ecc1; * LP15,LP14,LP13,...
58 * *ecc2; * LP07,LP06,LP05,...
Randy Dunlapd8aba9d2011-05-09 10:46:24 -070059 */
60static void trans_result(BYTE reg2, BYTE reg3, BYTE *ecc1, BYTE *ecc2)
Al Cho126bb032010-09-08 00:42:32 -070061{
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080062 BYTE a; /* Working for reg2,reg3 */
63 BYTE b; /* Working for ecc1,ecc2 */
64 BYTE i; /* For counting */
Al Cho126bb032010-09-08 00:42:32 -070065
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080066 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 }
Al Cho126bb032010-09-08 00:42:32 -070077
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080078 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 }
Al Cho126bb032010-09-08 00:42:32 -070088}
89
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080090/*static void calculate_ecc(table,data,ecc1,ecc2,ecc3) */
Randy Dunlapd8aba9d2011-05-09 10:46:24 -070091/*
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +080092 * *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"
Randy Dunlapd8aba9d2011-05-09 10:46:24 -070097 */
98void calculate_ecc(BYTE *table, BYTE *data, BYTE *ecc1, BYTE *ecc2, BYTE *ecc3)
Al Cho126bb032010-09-08 00:42:32 -070099{
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800100 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,... */
Al Cho126bb032010-09-08 00:42:32 -0700105
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800106 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 }
Al Cho126bb032010-09-08 00:42:32 -0700115
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800116 /* 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 */
Al Cho126bb032010-09-08 00:42:32 -0700121}
122
Randy Dunlapd8aba9d2011-05-09 10:46:24 -0700123/*
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800124 * *data; * DATA
125 * *eccdata; * ECC DATA
126 * ecc1; * LP15,LP14,LP13,...
127 * ecc2; * LP07,LP06,LP05,...
128 * ecc3; * CP5,CP4,CP3,...,"1","1"
Randy Dunlapd8aba9d2011-05-09 10:46:24 -0700129 */
130BYTE correct_data(BYTE *data, BYTE *eccdata, BYTE ecc1, BYTE ecc2, BYTE ecc3)
Al Cho126bb032010-09-08 00:42:32 -0700131{
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800132 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 */
Al Cho126bb032010-09-08 00:42:32 -0700140
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800141 d1 = ecc1^eccdata[1]; d2 = ecc2^eccdata[0]; /* Compare LP's */
Masanari Iidab1639702013-12-12 22:22:59 +0900142 d3 = ecc3^eccdata[2]; /* Compare CP's */
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800143 d = ((DWORD)d1<<16) /* Result of comparison */
144 +((DWORD)d2<<8)
145 +(DWORD)d3;
Al Cho126bb032010-09-08 00:42:32 -0700146
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800147 if (d == 0)
148 return 0; /* If No error, return */
Al Cho126bb032010-09-08 00:42:32 -0700149
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800150 if (((d^(d>>1))&CORRECTABLE) == CORRECTABLE) { /* If correctable */
151 l = BIT23;
152 add = 0; /* Clear parameter */
153 a = BIT7;
Al Cho126bb032010-09-08 00:42:32 -0700154
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800155 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 }
Al Cho126bb032010-09-08 00:42:32 -0700160
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800161 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 }
Al Cho126bb032010-09-08 00:42:32 -0700168
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800169 b = BIT0;
170 data[add] ^= (b<<bit); /* Put corrected data */
171 return 1;
172 }
Al Cho126bb032010-09-08 00:42:32 -0700173
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800174 i = 0; /* Clear count */
175 d &= 0x00ffffffL; /* Masking */
Al Cho126bb032010-09-08 00:42:32 -0700176
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800177 while (d) { /* If d=0 finish counting */
178 if (d&BIT0)
179 ++i; /* Count number of 1 bit */
180 d >>= 1; /* Right shift */
181 }
Al Cho126bb032010-09-08 00:42:32 -0700182
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800183 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 */
Al Cho126bb032010-09-08 00:42:32 -0700189}
190
Randy Dunlapd8aba9d2011-05-09 10:46:24 -0700191int _Correct_D_SwECC(BYTE *buf, BYTE *redundant_ecc, BYTE *calculate_ecc)
Al Cho126bb032010-09-08 00:42:32 -0700192{
Javier Martinez Canillasf3d50492011-01-02 23:14:27 +0100193 DWORD err;
Al Cho126bb032010-09-08 00:42:32 -0700194
Javier Martinez Canillasf3d50492011-01-02 23:14:27 +0100195 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;
Al Cho126bb032010-09-08 00:42:32 -0700204}
205
Randy Dunlapd8aba9d2011-05-09 10:46:24 -0700206void _Calculate_D_SwECC(BYTE *buf, BYTE *ecc)
Al Cho126bb032010-09-08 00:42:32 -0700207{
Cho, Yu-Chen65cdcf32011-05-18 18:40:12 +0800208 calculate_ecc(ecctable, buf, ecc+1, ecc+0, ecc+2);
Al Cho126bb032010-09-08 00:42:32 -0700209}
210
211