Commit | Line | Data |
---|---|---|
126bb03b AC |
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 | { | |
f3d5049c | 185 | DWORD err; |
126bb03b | 186 | |
f3d5049c JMC |
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; | |
126bb03b AC |
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 |