00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00038
00039
00040 #include <aversive.h>
00041
00042 #define P_MASK 0x0000808B
00043
00044 #define P0_MASK 0x15555554
00045 #define P1_MASK 0x06666664
00046 #define P2_MASK 0x18787870
00047 #define P3_MASK 0x1F807F00
00048 #define P4_MASK 0x1FFF0000
00049
00050 #define P0_EMPL 0
00051 #define P1_EMPL 1
00052 #define P2_EMPL 3
00053 #define P3_EMPL 7
00054 #define P4_EMPL 15
00055
00056 uint8_t tab_abcd[16]=
00057 {
00058 0,
00059 4,
00060 16,
00061 20,
00062 32,
00063 36,
00064 48,
00065 52,
00066 64,
00067 68,
00068 80,
00069 84,
00070 96,
00071 100,
00072 112,
00073 116
00074 };
00075
00076 uint8_t tab_dcba[32]=
00077 {
00078 0,
00079 1,
00080 0,
00081 1,
00082 2,
00083 3,
00084 2,
00085 3,
00086 4,
00087 5,
00088 4,
00089 5,
00090 6,
00091 7,
00092 6,
00093 7,
00094 8,
00095 9,
00096 8,
00097 9,
00098 10,
00099 11,
00100 10,
00101 11,
00102 12,
00103 13,
00104 12,
00105 13,
00106 14,
00107 15,
00108 14,
00109 15
00110 };
00111
00112 uint8_t tab_parity[256] =
00113 {
00114 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0
00115 };
00116
00117
00118
00119 uint32_t hamming_expand(uint32_t frame)
00120 {
00121 uint32_t tmp;
00122
00123 uint16_t frame_middle;
00124 uint8_t frame_last;
00125
00126 tmp = tab_abcd[(uint8_t)frame&0x0F];
00127
00128 tmp |= (frame & 0x7F0) << 4;
00129
00130 frame_middle = extr32_16_mid(frame);
00131 tmp |= (uint32_t)(frame_middle & 0x7F8) << 13;
00132
00133
00134 frame_last = extr32_08_2(frame);
00135 tmp |= (uint32_t)(frame_last >> 3) << 24 ;
00136
00137 return tmp;
00138 }
00139
00140
00141
00142 uint32_t hamming_shrink(uint32_t frame)
00143 {
00144 uint32_t ret;
00145 uint8_t tmp;
00146
00147 ret = tab_dcba[ (((uint8_t)frame) >> 2) & 0x1F ];
00148
00149 tmp = * ((uint8_t *)(&frame) + 1);
00150 ret += (uint16_t)(tmp&0x7F) << 4;
00151
00152 tmp = * ((uint8_t *)(&frame) + 2);
00153 ret += (uint32_t)tmp << 11;
00154
00155 tmp = * ((uint8_t *)(&frame) + 3);
00156 ret += (uint32_t)(tmp&0x1F) << 19;
00157
00158 return ret;
00159 }
00160
00161
00162
00163 uint32_t hamming_process(uint32_t frame)
00164 {
00165 uint32_t tmp;
00166 uint8_t xor;
00167
00168 tmp = frame&P0_MASK;
00169 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00170 (* (((uint8_t *)&tmp) + 1) ) ^
00171 (* (((uint8_t *)&tmp) + 2) ) ^
00172 (* (((uint8_t *)&tmp) + 3) ) );
00173
00174 if(tab_parity[xor])
00175 frame |= (1l<<P0_EMPL);
00176
00177 tmp = frame&P1_MASK;
00178 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00179 (* (((uint8_t *)&tmp) + 1) ) ^
00180 (* (((uint8_t *)&tmp) + 2) ) ^
00181 (* (((uint8_t *)&tmp) + 3) ) );
00182
00183 if(tab_parity[xor])
00184 frame |= (1l<<P1_EMPL);
00185
00186 tmp = frame&P2_MASK;
00187 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00188 (* (((uint8_t *)&tmp) + 1) ) ^
00189 (* (((uint8_t *)&tmp) + 2) ) ^
00190 (* (((uint8_t *)&tmp) + 3) ) );
00191
00192 if(tab_parity[xor])
00193 frame |= (1l<<P2_EMPL);
00194
00195 tmp = frame&P3_MASK;
00196 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00197 (* (((uint8_t *)&tmp) + 1) ) ^
00198 (* (((uint8_t *)&tmp) + 2) ) ^
00199 (* (((uint8_t *)&tmp) + 3) ) );
00200
00201 if(tab_parity[xor])
00202 frame |= (1l<<P3_EMPL);
00203
00204 tmp = frame&P4_MASK;
00205 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00206 (* (((uint8_t *)&tmp) + 1) ) ^
00207 (* (((uint8_t *)&tmp) + 2) ) ^
00208 (* (((uint8_t *)&tmp) + 3) ) );
00209
00210 if(tab_parity[xor])
00211 frame |= (1l<<P4_EMPL);
00212
00213 return frame;
00214 }
00215
00216
00217
00218 uint8_t hamming_unprocess(uint32_t frame)
00219 {
00220 uint32_t tmp;
00221 uint8_t xor;
00222 uint8_t indice=0;
00223
00224 tmp = frame&P0_MASK;
00225 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00226 (* (((uint8_t *)&tmp) + 1) ) ^
00227 (* (((uint8_t *)&tmp) + 2) ) ^
00228 (* (((uint8_t *)&tmp) + 3) ) );
00229
00230 if((1l<<P0_EMPL)&frame)
00231 xor ^= 1;
00232
00233 if(tab_parity[xor])
00234 indice |= 1;
00235
00236
00237 tmp = frame&P1_MASK;
00238 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00239 (* (((uint8_t *)&tmp) + 1) ) ^
00240 (* (((uint8_t *)&tmp) + 2) ) ^
00241 (* (((uint8_t *)&tmp) + 3) ) );
00242
00243 if((1l<<P1_EMPL)&frame)
00244 xor ^= 1;
00245
00246 if(tab_parity[xor])
00247 indice |= 2;
00248
00249 tmp = frame&P2_MASK;
00250 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00251 (* (((uint8_t *)&tmp) + 1) ) ^
00252 (* (((uint8_t *)&tmp) + 2) ) ^
00253 (* (((uint8_t *)&tmp) + 3) ) );
00254
00255 if((1l<<P2_EMPL)&frame)
00256 xor ^= 1;
00257
00258 if(tab_parity[xor])
00259 indice |=4;
00260
00261 tmp = frame&P3_MASK;
00262 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00263 (* (((uint8_t *)&tmp) + 1) ) ^
00264 (* (((uint8_t *)&tmp) + 2) ) ^
00265 (* (((uint8_t *)&tmp) + 3) ) );
00266
00267 if((1l<<P3_EMPL)&frame)
00268 xor ^= 1;
00269
00270 if(tab_parity[xor])
00271 indice |=8;
00272
00273 tmp = frame&P4_MASK;
00274 xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
00275 (* (((uint8_t *)&tmp) + 1) ) ^
00276 (* (((uint8_t *)&tmp) + 2) ) ^
00277 (* (((uint8_t *)&tmp) + 3) ) );
00278
00279 if((1l<<P4_EMPL)&frame)
00280 xor ^= 1;
00281
00282 if(tab_parity[xor])
00283 indice |=16;
00284
00285 return indice;
00286 }
00287
00288
00289
00290
00291 uint32_t hamming_correct(uint32_t frame, uint8_t i)
00292 {
00293 return frame ^ (1l<<(i-1));
00294 }
00295
00296
00297
00298
00299 void hamming_do(uint32_t *frame)
00300 {
00301 *frame= hamming_process(hamming_expand(*frame));
00302 }
00303
00304
00305
00306 uint8_t hamming_undo(uint32_t *frame)
00307 {
00308 uint8_t tmp;
00309
00310 tmp=hamming_unprocess(*frame);
00311
00312 if(tmp)
00313 *frame= hamming_shrink(hamming_correct(*frame,tmp));
00314 else
00315 *frame= hamming_shrink(*frame);
00316
00317
00318 return tmp;
00319 }
00320
00321
00322