LXR linux/drivers/staging/keucr/smilecc.c

   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
  17static BYTE ecctable[256] = {
  180x00,0x55,0x56,0x03,0x59,0x0C,0x0F,0x5A,0x5A,0x0F,0x0C,0x59,0x03,0x56,0x55,0x00,
  190x65,0x30,0x33,0x66,0x3C,0x69,0x6A,0x3F,0x3F,0x6A,0x69,0x3C,0x66,0x33,0x30,0x65,
  200x66,0x33,0x30,0x65,0x3F,0x6A,0x69,0x3C,0x3C,0x69,0x6A,0x3F,0x65,0x30,0x33,0x66,
  210x03,0x56,0x55,0x00,0x5A,0x0F,0x0C,0x59,0x59,0x0C,0x0F,0x5A,0x00,0x55,0x56,0x03,
  220x69,0x3C,0x3F,0x6A,0x30,0x65,0x66,0x33,0x33,0x66,0x65,0x30,0x6A,0x3F,0x3C,0x69,
  230x0C,0x59,0x5A,0x0F,0x55,0x00,0x03,0x56,0x56,0x03,0x00,0x55,0x0F,0x5A,0x59,0x0C,
  240x0F,0x5A,0x59,0x0C,0x56,0x03,0x00,0x55,0x55,0x00,0x03,0x56,0x0C,0x59,0x5A,0x0F,
  250x6A,0x3F,0x3C,0x69,0x33,0x66,0x65,0x30,0x30,0x65,0x66,0x33,0x69,0x3C,0x3F,0x6A,
  260x6A,0x3F,0x3C,0x69,0x33,0x66,0x65,0x30,0x30,0x65,0x66,0x33,0x69,0x3C,0x3F,0x6A,
  270x0F,0x5A,0x59,0x0C,0x56,0x03,0x00,0x55,0x55,0x00,0x03,0x56,0x0C,0x59,0x5A,0x0F,
  280x0C,0x59,0x5A,0x0F,0x55,0x00,0x03,0x56,0x56,0x03,0x00,0x55,0x0F,0x5A,0x59,0x0C,
  290x69,0x3C,0x3F,0x6A,0x30,0x65,0x66,0x33,0x33,0x66,0x65,0x30,0x6A,0x3F,0x3C,0x69,
  300x03,0x56,0x55,0x00,0x5A,0x0F,0x0C,0x59,0x59,0x0C,0x0F,0x5A,0x00,0x55,0x56,0x03,
  310x66,0x33,0x30,0x65,0x3F,0x6A,0x69,0x3C,0x3C,0x69,0x6A,0x3F,0x65,0x30,0x33,0x66,
  320x65,0x30,0x33,0x66,0x3C,0x69,0x6A,0x3F,0x3F,0x6A,0x69,0x3C,0x66,0x33,0x30,0x65,
  330x00,0x55,0x56,0x03,0x59,0x0C,0x0F,0x5A,0x5A,0x0F,0x0C,0x59,0x03,0x56,0x55,0x00
  34};
  35
  36static 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
  51static void trans_result(reg2,reg3,ecc1,ecc2)
  52BYTE reg2; // LP14,LP12,LP10,...
  53BYTE reg3; // LP15,LP13,LP11,...
  54BYTE *ecc1; // LP15,LP14,LP13,...
  55BYTE *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)
  86void calculate_ecc(table,data,ecc1,ecc2,ecc3)
  87BYTE *table; // CP0-CP5 code table
  88BYTE *data; // DATA
  89BYTE *ecc1; // LP15,LP14,LP13,...
  90BYTE *ecc2; // LP07,LP06,LP05,...
  91BYTE *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
 116BYTE correct_data(data,eccdata,ecc1,ecc2,ecc3)
 117BYTE *data; // DATA
 118BYTE *eccdata; // ECC DATA
 119BYTE ecc1; // LP15,LP14,LP13,...
 120BYTE ecc2; // LP07,LP06,LP05,...
 121BYTE 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
 180int _Correct_D_SwECC(buf,redundant_ecc,calculate_ecc)
 181BYTE *buf;
 182BYTE *redundant_ecc;
 183BYTE *calculate_ecc;
 184{
 185        DWORD err;
 186
 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;
 196}
 197
 198void _Calculate_D_SwECC(buf,ecc)
 199BYTE *buf;
 200BYTE *ecc;
 201{
 202    calculate_ecc(ecctable,buf,ecc+1,ecc+0,ecc+2);
 203}
 204
 205
 206