[deliverable/linux.git] / drivers / staging / keucr / smilecc.c

#include "usb.h"
#include "scsiglue.h"
#include "transport.h"
/* #include "stdlib.h" */
/* #include "EUCR6SK.h" */
#include "smcommon.h"
#include "smil.h"

/* #include <stdio.h> */
/* #include <stdlib.h> */
/* #include <string.h> */
/* #include <dos.h> */
/* #include "EMCRIOS.h" */

/* 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
};

static void   trans_result(BYTE,   BYTE,   BYTE *, BYTE *);

#define BIT7        0x80
#define BIT6        0x40
#define BIT5        0x20
#define BIT4        0x10
#define BIT3        0x08
#define BIT2        0x04
#define BIT1        0x02
#define BIT0        0x01
#define BIT1BIT0    0x03
#define BIT23       0x00800000L
#define MASK_CPS    0x3f
#define CORRECTABLE 0x00555554L

/*
 * 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 */
	}
}

/*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"
 */
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 */
}

/*
 * *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<<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 */
		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)
{
	DWORD err;

	err = correct_data(buf, redundant_ecc, *(calculate_ecc + 1),
			   *(calculate_ecc), *(calculate_ecc + 2));
	if (err == 1)
		memcpy(calculate_ecc, redundant_ecc, 3);

	if (err == 0 || err == 1 || err == 2)
		return 0;

	return -1;
}

void _Calculate_D_SwECC(BYTE *buf, BYTE *ecc)
{
	calculate_ecc(ecctable, buf, ecc+1, ecc+0, ecc+2);
}
Commit	Line	Data
126bb03b AC	1	#include "usb.h"
	2	#include "scsiglue.h"
	3	#include "transport.h"
65cdcf36 CYC	4	/* #include "stdlib.h" */
65cdcf36 CYC	5	/* #include "EUCR6SK.h" */
126bb03b AC	6	#include "smcommon.h"
	7	#include "smil.h"
	8
65cdcf36 CYC	9	/* #include <stdio.h> */
	10	/* #include <stdlib.h> */
	11	/* #include <string.h> */
	12	/* #include <dos.h> */
	13	/* #include "EMCRIOS.h" */
126bb03b	14
65cdcf36	15	/* CP0-CP5 code table */
126bb03b	16	static BYTE ecctable[256] = {
65cdcf36 CYC	17	0x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F, 0x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03,
	18	0x56, 0x55, 0x00, 0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A,
	19	0x69, 0x3C, 0x66, 0x33, 0x30, 0x65, 0x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69,
	20	0x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65, 0x30, 0x33, 0x66, 0x03, 0x56, 0x55, 0x00,
	21	0x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03, 0x69,
	22	0x3C, 0x3F, 0x6A, 0x30, 0x65, 0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F,
	23	0x3C, 0x69, 0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56, 0x03, 0x00,
	24	0x55, 0x0F, 0x5A, 0x59, 0x0C, 0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55,
	25	0x55, 0x00, 0x03, 0x56, 0x0C, 0x59, 0x5A, 0x0F, 0x6A, 0x3F, 0x3C, 0x69, 0x33,
	26	0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F, 0x6A, 0x6A, 0x3F,
	27	0x3C, 0x69, 0x33, 0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F,
	28	0x6A, 0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55, 0x55, 0x00, 0x03, 0x56,
	29	0x0C, 0x59, 0x5A, 0x0F, 0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56,
	30	0x03, 0x00, 0x55, 0x0F, 0x5A, 0x59, 0x0C, 0x69, 0x3C, 0x3F, 0x6A, 0x30, 0x65,
	31	0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F, 0x3C, 0x69, 0x03, 0x56, 0x55,
	32	0x00, 0x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03,
	33	0x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69, 0x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65,
	34	0x30, 0x33, 0x66, 0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A,
	35	0x69, 0x3C, 0x66, 0x33, 0x30, 0x65, 0x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F,
	36	0x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03, 0x56, 0x55, 0x00
126bb03b AC	37	};
126bb03b AC	38
65cdcf36	39	static void trans_result(BYTE, BYTE, BYTE , BYTE );
126bb03b AC	40
	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
d8aba9d5	54	/*
65cdcf36 CYC	55	* reg2; * LP14,LP12,LP10,...
	56	* reg3; * LP15,LP13,LP11,...
	57	* ecc1; LP15,LP14,LP13,...
	58	* ecc2; LP07,LP06,LP05,...
d8aba9d5 RD	59	*/
d8aba9d5 RD	60	static void trans_result(BYTE reg2, BYTE reg3, BYTE ecc1, BYTE ecc2)
126bb03b	61	{
65cdcf36 CYC	62	BYTE a; /* Working for reg2,reg3 */
	63	BYTE b; /* Working for ecc1,ecc2 */
	64	BYTE i; /* For counting */
	65
	66	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	}
	77
	78	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	}
126bb03b AC	88	}
126bb03b AC	89
65cdcf36	90	/static void calculate_ecc(table,data,ecc1,ecc2,ecc3) /
d8aba9d5	91	/*
65cdcf36 CYC	92	* 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"
d8aba9d5 RD	97	*/
d8aba9d5 RD	98	void calculate_ecc(BYTE table, BYTE data, BYTE ecc1, BYTE ecc2, BYTE *ecc3)
126bb03b	99	{
65cdcf36 CYC	100	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,... */
	105
	106	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	}
	115
	116	/* 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 */
126bb03b AC	121	}
126bb03b AC	122
d8aba9d5	123	/*
65cdcf36 CYC	124	* data; DATA
	125	* eccdata; ECC DATA
	126	* ecc1; * LP15,LP14,LP13,...
	127	* ecc2; * LP07,LP06,LP05,...
	128	* ecc3; * CP5,CP4,CP3,...,"1","1"
d8aba9d5 RD	129	*/
d8aba9d5 RD	130	BYTE correct_data(BYTE data, BYTE eccdata, BYTE ecc1, BYTE ecc2, BYTE ecc3)
126bb03b	131	{
65cdcf36 CYC	132	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 */
	140
	141	d1 = ecc1^eccdata[1]; d2 = ecc2^eccdata[0]; /* Compare LP's */
	142	d3 = ecc3^eccdata[2]; /* Comapre CP's */
	143	d = ((DWORD)d1<<16) /* Result of comparison */
	144	+((DWORD)d2<<8)
	145	+(DWORD)d3;
	146
	147	if (d == 0)
	148	return 0; /* If No error, return */
	149
	150	if (((d^(d>>1))&CORRECTABLE) == CORRECTABLE) { /* If correctable */
	151	l = BIT23;
	152	add = 0; /* Clear parameter */
	153	a = BIT7;
	154
	155	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	}
	160
	161	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	}
	168
	169	b = BIT0;
	170	data[add] ^= (b<<bit); /* Put corrected data */
	171	return 1;
	172	}
	173
	174	i = 0; /* Clear count */
	175	d &= 0x00ffffffL; /* Masking */
	176
	177	while (d) { /* If d=0 finish counting */
	178	if (d&BIT0)
	179	++i; /* Count number of 1 bit */
	180	d >>= 1; /* Right shift */
	181	}
	182
	183	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 */
126bb03b AC	189	}
126bb03b AC	190
d8aba9d5	191	int _Correct_D_SwECC(BYTE buf, BYTE redundant_ecc, BYTE *calculate_ecc)
126bb03b	192	{
f3d5049c	193	DWORD err;
126bb03b	194
f3d5049c JMC	195	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;
126bb03b AC	204	}
126bb03b AC	205
d8aba9d5	206	void _Calculate_D_SwECC(BYTE buf, BYTE ecc)
126bb03b	207	{
65cdcf36	208	calculate_ecc(ecctable, buf, ecc+1, ecc+0, ecc+2);
126bb03b AC	209	}
	210
	211