ecc256.c
3.7 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
#include "ecc256.h"
/* column parity table + byte parity in bit 6 */
static const unsigned char cp_table[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
};
void
ecc256_calculate(const unsigned char buf[256], unsigned char ecc[3]) {
int i;
unsigned char x1, x2, a0, a1, b, e0, e1, e2;
e2 = x1 = x2 = 0;
for(i = 0; i < 256; i++) {
unsigned char z = cp_table[buf[i]];
e2 ^= (z&0x3f); /* column parity */
if (z&0x40) { /* odd byte parity */
x1 ^= ~i;
x2 ^= i;
}
}
a1 = b = 0x80; a0 = 0x8;
e0 = e1 = 0;
for(i = 0; i < 4; i++) { /* compute ecc[0,1] */
if (x2 & a0) e0 |= b; /* line parity 7, 5, 3, 1 */
if (x2 & a1) e1 |= b; /* line parity 15, 13, 11, 9 */
b >>= 1;
if (x1 & a0) e0 |= b; /* line parity 6, 4, 2, 0 */
if (x1 & a1) e1 |= b; /* line parity 14, 12, 10, 8 */
a0 >>= 1; a1 >>= 1; b >>= 1;
}
ecc[0] = ~e0; /* lp7..lp0 */
ecc[1] = ~e1; /* lp15..lp8*/
ecc[2] = ~(e2 << 2); /* cp5..cp0 0 0 */
}
int
ecc256_correct(unsigned char buf[256], unsigned char read_ecc[3], const unsigned char calc_ecc[3]) {
unsigned char a, b, c, d, x0, x1, x2;
x0 = calc_ecc[0] ^ read_ecc[0];
x1 = calc_ecc[1] ^ read_ecc[1];
x2 = calc_ecc[2] ^ read_ecc[2];
if ((x0 | x1 | x2) == 0) /* No errors */
return ECC256_ERROR_NONE;
a = (x0 ^ (x0 >> 1)) & 0x55;
b = (x1 ^ (x1 >> 1)) & 0x55;
c = (x2 ^ (x2 >> 1)) & 0x54;
/* Check for and correct single bit error in the data */
if (a == 0x55 && b == 0x55 && c == 0x54) {
unsigned char i, address = 0, bit = 0;
a = c = 0x80;
b = 0x8; d = 0x4;
for (i = 0; i < 4; i++) { /* compute bit address */
if (x0 & c) address |= b;
if (x1 & c) address |= a;
if (x2 & c) bit |= d;
a >>= 1; b >>= 1; d >>= 1;
c >>= 2;
}
buf[address] ^= 1 << bit;
return ECC256_ERROR_CORRECTED;
} else {
/* count bits */
c = 0;
while (x0 | x1 | x2) {
c += x0 & 1;
c += x1 & 1;
c += x2 & 1;
x0 >>= 1;
x1 >>= 1;
x2 >>= 1;
}
if (c == 1) { read_ecc[0] = calc_ecc[0]; read_ecc[1] = calc_ecc[1]; read_ecc[2] = calc_ecc[2]; }
/* Error in the ECC code : Uncorrectable error */
return (c == 1) ? ECC256_ERROR_ECC : ECC256_ERROR_UNCORRECTABLE;
}
}