pi_ecc.c
8.1 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
#include <stdio.h>
#include <string.h>
#include <malloc.h>
#define BIT7 0x80
#define BIT6 0x40
#define BIT2 0x04
#define BIT0 0x01
#define BIT1BIT0 0x03
#define BIT23 0x00800000L
#define MASK_CPS 0x3f
#define CORRECTABLE 0x00555554L
static unsigned char table[] = {
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
};
/*
Transfer result
LP14,12,10,... & LP15,13,11,... -> LP15,14,13,... & LP7,6,5,..
*/
trans_result(reg2,reg3,ecc1,ecc2)
unsigned char reg2; /* LP14,LP12,LP10,... */
unsigned char reg3; /* LP15,LP13,LP11,... */
unsigned char *ecc1; /* LP15,LP14,LP13,... */
unsigned char *ecc2; /* LP07,LP06,LP05,... */
{
unsigned char a; /* Working for reg2,reg3 */
unsigned char b; /* Working for ecc1,ecc2 */
unsigned char i; /* For counting */
a=BIT7; b=BIT7;
*ecc1=*ecc2=0; /* Clear ecc1,ecc2 */
for(i=0; i<4; ++i) {
if ((reg3&a)!=0) *ecc1|=b; /* LP15,13,11,9 -> ecc1 */
b>>=1; /* Right shift */
if ((reg2&a)!=0) *ecc1|=b; /* LP14,12,10,8 -> ecc1 */
b>>=1; /* Right shift */
a>>=1; /* Right shift */
}
b=BIT7;
for(i=0; i<4; ++i) {
if ((reg3&a)!=0) *ecc2|=b; /* LP7,5,3,1 -> ecc2 */
b>>=1; /* Right shift */
if ((reg2&a)!=0) *ecc2|=b; /* LP6,4,2,0 -> ecc2 */
b>>=1; /* Right shift */
a>>=1; /* Right shift */
}
}
/*
Calculating ECC
data[0-255]: input data
ecc[0-2]: ecc values in SM order
*/
void calculate_ecc(
unsigned char *data, /* data */
unsigned char *ecc /* ecc[0]: LP07,LP06,LP05,... */
/* ecc[1]: LP15,LP14,LP13,... */
/* ecc[2]: CP5,CP4,CP3,...,"1","1" */
)
{
unsigned int i; /* For counting */
unsigned char a; /* Working for table */
unsigned char reg1; /* D-all,CP5,CP4,CP3,... */
unsigned char reg2; /* LP14,LP12,L10,... */
unsigned char 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^=(unsigned char)i; /* XOR with counter */
reg2^=~((unsigned char)i); /* XOR with inv. of counter */
}
}
/* Trans LP14,12,10,... & LP15,13,11,... -> LP15,14,13,... & LP7,6,5,.. */
trans_result(reg2,reg3,&ecc[1],&ecc[0]);
ecc[0]=~(ecc[0]); ecc[1]=~(ecc[1]); /* Inv. ecc2 & ecc3 */
ecc[2]=((~reg1)<<2)|BIT1BIT0; /* Make TEL format */
}
/* ecc[0]: LP07,LP06,LP05,... */
/* ecc[1]: LP15,LP14,LP13,... */
/* ecc[2]: CP5,CP4,CP3,...,"1","1" */
int correct_data(
unsigned char *data, /* 256B data */
unsigned char *ecc, /* input: ecc with data. output: corrected ecc */
unsigned char syndrome1, /* input: computed syndrome on data */
unsigned char syndrome2, /* same ordering as ecc[0,1,2] */
unsigned char syndrome3)
{
unsigned long l; /* Working to check d */
unsigned long d; /* Result of comparison */
unsigned int i; /* For counting */
unsigned char d1,d2,d3; /* Result of comparison */
unsigned char a; /* Working for add */
unsigned char add; /* Byte address of cor. DATA */
unsigned char b; /* Working for bit */
unsigned char bit; /* Bit address of cor. DATA */
d2=(ecc[0])^syndrome1; /* Compare LP's (d1 and d2) */
d1=(ecc[1])^syndrome2;
d3=(ecc[2])^syndrome3; /* Comapre CP's */
d=((unsigned long)d1<<16) /* Result of comparison */
+((unsigned long)d2<<8)
+(unsigned long)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<<bit); /* Put corrected data */
return(1);
}
i=0; /* Clear count */
d&=0x00ffffffL; /* Masking */
while(d) { /* If d=0 finish counting */
if (d&BIT0) ++i; /* Count number of 1 bit */
d>>=1; /* Right shift */
}
if (i==1) { /* If ECC error */
ecc[0]=syndrome1; /* put correct ecc codes */
ecc[1]=syndrome2;
ecc[2]=syndrome3;
return(2);
}
return(3); /* Uncorrectable error */
}
/*
return codes:
0: no error
1: corrected in data
2: corrected in ecc term
3: uncorrectable
*/
int correct_ecc(unsigned char *data,
unsigned char *ecc
)
{
unsigned char syndrome[3];
calculate_ecc(data,syndrome); /* compute syndrome */
return correct_data(data,ecc,syndrome[0],syndrome[1],syndrome[2]);
}