merge
[aversive.git] / modules / encoding / hamming / hamming.c
1 /*  
2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
3  * 
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: hamming.c,v 1.2.4.4 2008-05-11 15:04:53 zer0 Exp $
19  *
20  */
21
22 /* Droids-corp, Eirbot, Microb Technology 2005 - Zer0
23  * Implementation for HAMMING
24  *
25  * Thanks and hello to Serpilliere
26  */
27
28 /** \file hamming.c
29  *  \brief Implementation for the Hamiing module.
30  *
31  *  \todo use progmem to store tables !
32  *
33  *  \test Seems to work
34  *
35  * This module provides functions for error detection/correction
36  */
37
38 /**********************************************************/
39
40 #include <aversive.h>
41
42 #define P_MASK  0x0000808B
43
44 #define P0_MASK 0x15555554
45 #define P1_MASK 0x06666664
46 #define P2_MASK 0x18787870
47 #define P3_MASK 0x1F807F00
48 #define P4_MASK 0x1FFF0000
49
50 #define P0_EMPL 0
51 #define P1_EMPL 1
52 #define P2_EMPL 3
53 #define P3_EMPL 7
54 #define P4_EMPL 15
55
56 uint8_t tab_abcd[16]=
57   {
58     0,
59     4,
60     16,
61     20,
62     32,
63     36,
64     48,
65     52,
66     64,
67     68,
68     80,
69     84,
70     96,
71     100,
72     112,
73     116
74   };
75
76 uint8_t tab_dcba[32]=
77   {
78     0,
79     1,
80     0,
81     1,
82     2,
83     3,
84     2,
85     3,
86     4,
87     5,
88     4,
89     5,
90     6,
91     7,
92     6,
93     7,
94     8,
95     9,
96     8,
97     9,
98     10,
99     11,
100     10,
101     11,
102     12,
103     13,
104     12,
105     13,
106     14,
107     15,
108     14,
109     15
110   };
111
112 uint8_t tab_parity[256] =
113   {
114   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 
115   };
116
117 /*******************************************************/
118
119 uint32_t hamming_expand(uint32_t frame)
120 {
121   uint32_t tmp;
122
123   uint16_t frame_middle; // octet 2 & 3
124   uint8_t frame_last; // octet 3
125
126   tmp = tab_abcd[(uint8_t)frame&0x0F];
127   
128   tmp |= (frame & 0x7F0) << 4;
129   
130   frame_middle =  extr32_16_mid(frame);
131   tmp |= (uint32_t)(frame_middle & 0x7F8) << 13;
132
133   
134   frame_last = extr32_08_2(frame);
135   tmp |= (uint32_t)(frame_last >> 3) << 24 ;
136
137   return tmp;
138 }
139
140 /*******************************************************/
141
142 uint32_t hamming_shrink(uint32_t frame)
143 {
144   uint32_t ret;
145   uint8_t tmp;
146
147   ret = tab_dcba[ (((uint8_t)frame) >> 2) & 0x1F ];
148   
149   tmp = * ((uint8_t *)(&frame) + 1);
150   ret += (uint16_t)(tmp&0x7F) << 4;
151
152   tmp = * ((uint8_t *)(&frame) + 2);
153   ret += (uint32_t)tmp << 11;
154
155   tmp = * ((uint8_t *)(&frame) + 3);
156   ret += (uint32_t)(tmp&0x1F) << 19;
157   
158   return ret;
159 }
160
161 /*******************************************************/
162
163 uint32_t hamming_process(uint32_t frame)
164 {
165   uint32_t tmp;
166   uint8_t xor;
167
168   tmp = frame&P0_MASK;
169   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
170           (* (((uint8_t *)&tmp) + 1) ) ^
171           (* (((uint8_t *)&tmp) + 2) ) ^
172           (* (((uint8_t *)&tmp) + 3) ) );
173   
174   if(tab_parity[xor])
175     frame |= (1l<<P0_EMPL);
176           
177   tmp = frame&P1_MASK;
178   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
179           (* (((uint8_t *)&tmp) + 1) ) ^
180           (* (((uint8_t *)&tmp) + 2) ) ^
181           (* (((uint8_t *)&tmp) + 3) ) );
182   
183   if(tab_parity[xor])
184     frame |= (1l<<P1_EMPL);
185
186   tmp = frame&P2_MASK;
187   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
188           (* (((uint8_t *)&tmp) + 1) ) ^
189           (* (((uint8_t *)&tmp) + 2) ) ^
190           (* (((uint8_t *)&tmp) + 3) ) );
191   
192   if(tab_parity[xor])
193     frame |= (1l<<P2_EMPL);
194
195   tmp = frame&P3_MASK;
196   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
197           (* (((uint8_t *)&tmp) + 1) ) ^
198           (* (((uint8_t *)&tmp) + 2) ) ^
199           (* (((uint8_t *)&tmp) + 3) ) );
200   
201   if(tab_parity[xor])
202     frame |= (1l<<P3_EMPL);
203
204   tmp = frame&P4_MASK;
205   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
206           (* (((uint8_t *)&tmp) + 1) ) ^
207           (* (((uint8_t *)&tmp) + 2) ) ^
208           (* (((uint8_t *)&tmp) + 3) ) );
209   
210   if(tab_parity[xor])
211     frame |= (1l<<P4_EMPL);
212
213   return frame;
214 }
215
216 /*******************************************************/
217
218 uint8_t hamming_unprocess(uint32_t frame)
219 {
220   uint32_t tmp;
221   uint8_t xor;
222   uint8_t indice=0;
223
224   tmp = frame&P0_MASK;
225   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
226           (* (((uint8_t *)&tmp) + 1) ) ^
227           (* (((uint8_t *)&tmp) + 2) ) ^
228           (* (((uint8_t *)&tmp) + 3) ) );
229   
230   if((1l<<P0_EMPL)&frame)
231     xor ^= 1;
232
233   if(tab_parity[xor])
234     indice |= 1;
235     
236           
237   tmp = frame&P1_MASK;
238   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
239           (* (((uint8_t *)&tmp) + 1) ) ^
240           (* (((uint8_t *)&tmp) + 2) ) ^
241           (* (((uint8_t *)&tmp) + 3) ) );
242   
243   if((1l<<P1_EMPL)&frame)
244     xor ^= 1;
245
246   if(tab_parity[xor])
247     indice |= 2;
248
249   tmp = frame&P2_MASK;
250   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
251           (* (((uint8_t *)&tmp) + 1) ) ^
252           (* (((uint8_t *)&tmp) + 2) ) ^
253           (* (((uint8_t *)&tmp) + 3) ) );
254   
255   if((1l<<P2_EMPL)&frame)
256     xor ^= 1;
257
258   if(tab_parity[xor])
259     indice |=4;
260
261   tmp = frame&P3_MASK;
262   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
263           (* (((uint8_t *)&tmp) + 1) ) ^
264           (* (((uint8_t *)&tmp) + 2) ) ^
265           (* (((uint8_t *)&tmp) + 3) ) );
266
267   if((1l<<P3_EMPL)&frame)
268     xor ^= 1;
269
270   if(tab_parity[xor])
271     indice |=8;
272
273   tmp = frame&P4_MASK;
274   xor = ( (* (((uint8_t *)&tmp) + 0) ) ^
275           (* (((uint8_t *)&tmp) + 1) ) ^
276           (* (((uint8_t *)&tmp) + 2) ) ^
277           (* (((uint8_t *)&tmp) + 3) ) );
278   
279   if((1l<<P4_EMPL)&frame)
280     xor ^= 1;
281
282   if(tab_parity[xor])
283     indice |=16;
284
285   return indice;
286 }
287
288
289 /*****************************/
290
291 uint32_t hamming_correct(uint32_t frame, uint8_t i)
292 {
293   return frame ^ (1l<<(i-1));
294 }
295
296 /*****************************/
297
298
299 void hamming_do(uint32_t *frame)
300 {
301   *frame= hamming_process(hamming_expand(*frame));
302 }
303
304 /*******************************************************/
305
306 uint8_t hamming_undo(uint32_t *frame)
307 {
308   uint8_t tmp;
309
310   tmp=hamming_unprocess(*frame);
311
312   if(tmp)
313     *frame= hamming_shrink(hamming_correct(*frame,tmp));
314   else
315     *frame= hamming_shrink(*frame);
316
317
318   return tmp;
319 }
320
321
322