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