Line data    Source code

       1             : #include "FEC.h"
       2             : #include <string.h>
       3             : 
       4           1 : RadioLibBCH::RadioLibBCH() {
       5             :   
       6           1 : }
       7             : 
       8           1 : RadioLibBCH::~RadioLibBCH() {
       9             :   #if !RADIOLIB_STATIC_ONLY
      10           1 :     delete[] this->alphaTo;
      11           1 :     delete[] this->indexOf;
      12           1 :     delete[] this->generator;
      13             :   #endif
      14           1 : }
      15             : 
      16             : /*
      17             :   BCH Encoder based on https://www.codeproject.com/articles/13189/pocsag-encoder
      18             : 
      19             :   Significantly cleaned up and slightly fixed.
      20             : */
      21           0 : void RadioLibBCH::begin(uint8_t n, uint8_t k, uint32_t poly) {
      22           0 :   this->n = n;
      23           0 :   this->k = k;
      24           0 :   this->poly = poly;
      25             :   #if !RADIOLIB_STATIC_ONLY
      26           0 :   this->alphaTo = new int32_t[n + 1];
      27           0 :   this->indexOf = new int32_t[n + 1];
      28           0 :   this->generator = new int32_t[n - k + 1];
      29             :   #endif
      30             : 
      31             :   // find the maximum power of the polynomial
      32           0 :   for(this->m = 0; this->m < 31; this->m++) {
      33           0 :     if((poly >> this->m) == 1) {
      34           0 :       break;
      35             :     }
      36             :   }
      37             : 
      38             :   /*
      39             :   * generate GF(2**m) from the irreducible polynomial p(X) in p[0]..p[m]
      40             :   * lookup tables:  index->polynomial form   this->alphaTo[] contains j=alpha**i;
      41             :   * polynomial form -> index form  this->indexOf[j=alpha**i] = i alpha=2 is the
      42             :   * primitive element of GF(2**m)
      43             :   */
      44             : 
      45           0 :         int32_t mask = 1;
      46           0 :         this->alphaTo[this->m] = 0;
      47             : 
      48           0 :         for(uint8_t i = 0; i < this->m; i++) {
      49           0 :                 this->alphaTo[i] = mask;
      50             : 
      51           0 :                 this->indexOf[this->alphaTo[i]] = i;
      52             : 
      53           0 :     if(this->poly & ((uint32_t)0x01 << i)) {
      54           0 :       this->alphaTo[this->m] ^= mask;
      55             :     }
      56             : 
      57           0 :                 mask <<= 1;
      58             :         }
      59             : 
      60           0 :         this->indexOf[this->alphaTo[this->m]] = this->m;
      61           0 :         mask >>= 1;
      62             : 
      63           0 :         for(uint8_t i = this->m + 1; i < this->n; i++) {
      64           0 :                 if(this->alphaTo[i - 1] >= mask) {
      65           0 :       this->alphaTo[i] = this->alphaTo[this->m] ^ ((this->alphaTo[i - 1] ^ mask) << 1);
      66             :     } else {
      67           0 :       this->alphaTo[i] = this->alphaTo[i - 1] << 1;
      68             :     }
      69             : 
      70           0 :                 this->indexOf[this->alphaTo[i]] = i;
      71             :         }
      72             : 
      73           0 :         this->indexOf[0] = -1;
      74             : 
      75             :   /*
      76             :         * Compute generator polynomial of BCH code of length = 31, redundancy = 10
      77             :         * (OK, this is not very efficient, but we only do it once, right? :)
      78             :         */
      79             : 
      80           0 :         int32_t ii = 0;
      81           0 :   int32_t jj = 1;
      82           0 :   int32_t ll = 0;
      83           0 :   int32_t kaux = 0;
      84           0 :   bool test = false;
      85           0 :         int32_t aux = 0;
      86           0 :         int32_t cycle[15][6] = { { 0 } };
      87           0 :   int32_t size[15] = { 0 };
      88             : 
      89             :         // Generate cycle sets modulo 31
      90           0 :         cycle[0][0] = 0; size[0] = 1;
      91           0 :         cycle[1][0] = 1; size[1] = 1;
      92             : 
      93             :         do {
      94             :                 // Generate the jj-th cycle set
      95           0 :                 ii = 0;
      96             :                 do {
      97           0 :                         ii++;
      98           0 :                         cycle[jj][ii] = (cycle[jj][ii - 1] * 2) % this->n;
      99           0 :                         size[jj]++;
     100           0 :                         aux = (cycle[jj][ii] * 2) % this->n;
     101           0 :                 } while(aux != cycle[jj][0]);
     102             : 
     103             :                 // Next cycle set representative
     104           0 :                 ll = 0;
     105             :                 do {
     106           0 :                         ll++;
     107           0 :                         test = false;
     108           0 :                         for(ii = 1; ((ii <= jj) && !test); ii++) {
     109             :         // Examine previous cycle sets
     110           0 :                           for(kaux = 0; ((kaux < size[ii]) && !test); kaux++) {
     111           0 :           test = (ll == cycle[ii][kaux]);
     112             :         }
     113             :       }
     114           0 :                 } while(test && (ll < (this->n - 1)));
     115             : 
     116           0 :                 if(!test) {
     117           0 :                         jj++;   // next cycle set index
     118           0 :                         cycle[jj][0] = ll;
     119           0 :                         size[jj] = 1;
     120             :                 }
     121             : 
     122           0 :         } while(ll < (this->n - 1));
     123             : 
     124             :         // Search for roots 1, 2, ..., m-1 in cycle sets
     125           0 :         int32_t rdncy = 0;
     126             :   #if RADIOLIB_STATIC_ONLY
     127             :     int32_t min[RADIOLIB_BCH_MAX_N - RADIOLIB_BCH_MAX_K + 1] = { 0 };
     128             :   #else
     129           0 :     int32_t* min = new int32_t[this->n - this->k + 1];
     130             :   #endif
     131           0 :         kaux = 0;
     132             : 
     133             :   // ensure the first element is always initializer
     134           0 :   min[0] = 0;
     135             : 
     136           0 :         for(ii = 1; ii <= jj; ii++) {
     137           0 :                 min[kaux] = 0;
     138           0 :                 for(jj = 0; jj < size[ii]; jj++) {
     139           0 :       for(uint8_t root = 1; root < this->m; root++) {
     140           0 :         if(root == cycle[ii][jj]) {
     141           0 :           min[kaux] = ii;
     142             :         }
     143             :       }
     144             :     }
     145             : 
     146           0 :                 if(min[kaux]) {
     147           0 :                         rdncy += size[min[kaux]];
     148           0 :                         kaux++;
     149             :                 }
     150             :         }
     151             : 
     152           0 :         int32_t noterms = kaux;
     153             :   #if RADIOLIB_STATIC_ONLY
     154             :     int32_t zeros[RADIOLIB_BCH_MAX_N - RADIOLIB_BCH_MAX_K + 1] = { 0 };
     155             :   #else
     156           0 :     int32_t* zeros = new int32_t[this->n - this->k + 1];
     157             :   #endif
     158           0 :         kaux = 1;
     159             : 
     160             :   // ensure the first element is always initializer
     161           0 :   zeros[1] = 0;
     162             : 
     163           0 :         for(ii = 0; ii < noterms; ii++) {
     164           0 :     for(jj = 0; jj < size[min[ii]]; jj++) {
     165           0 :                         zeros[kaux] = cycle[min[ii]][jj];
     166           0 :                         kaux++;
     167             :                 }
     168             :   }
     169             : 
     170             :   #if !RADIOLIB_STATIC_ONLY
     171           0 :   delete[] min;
     172             :   #endif
     173             : 
     174             :         // Compute generator polynomial
     175           0 :         this->generator[0] = this->alphaTo[zeros[1]];
     176           0 :         this->generator[1] = 1;              // g(x) = (X + zeros[1]) initially
     177             : 
     178           0 :         for(ii = 2; ii <= rdncy; ii++) {
     179           0 :           this->generator[ii] = 1;
     180           0 :           for(jj = ii - 1; jj > 0; jj--) {
     181           0 :       if(this->generator[jj] != 0) {
     182           0 :         this->generator[jj] = this->generator[jj - 1] ^ this->alphaTo[(this->indexOf[this->generator[jj]] + zeros[ii]) % this->n];
     183             :       } else {
     184           0 :         this->generator[jj] = this->generator[jj - 1];
     185             :       }
     186             :     }
     187           0 :                 this->generator[0] = this->alphaTo[(this->indexOf[this->generator[0]] + zeros[ii]) % this->n];
     188             :         }
     189             : 
     190             :   #if !RADIOLIB_STATIC_ONLY
     191           0 :   delete[] zeros;
     192             :   #endif
     193           0 : }
     194             : 
     195             : /*
     196             :   BCH Encoder based on https://www.codeproject.com/articles/13189/pocsag-encoder
     197             : 
     198             :   Significantly cleaned up and slightly fixed.
     199             : */
     200           0 : uint32_t RadioLibBCH::encode(uint32_t dataword) {
     201             :   // we only use the "k" most significant bits
     202             :   #if RADIOLIB_STATIC_ONLY
     203             :     int32_t data[RADIOLIB_BCH_MAX_K] = { 0 };
     204             :   #else
     205           0 :     int32_t* data = new int32_t[this->k];
     206           0 :     memset(data, 0, this->k*sizeof(int32_t));
     207             :   #endif
     208           0 :         int32_t j1 = 0;
     209           0 :         for(int32_t i = this->n; i > (this->n - this->k); i--) {
     210           0 :                 if(dataword & ((uint32_t)1<<i)) {
     211           0 :       data[j1++]=1;
     212             :     } else {
     213           0 :       data[j1++]=0;
     214             :     }
     215             :         }
     216             : 
     217             :   // reset the M(x)+r array elements
     218             :   #if RADIOLIB_STATIC_ONLY
     219             :     int32_t Mr[RADIOLIB_BCH_MAX_N] = { 0 };
     220             :   #else
     221           0 :     int32_t* Mr = new int32_t[this->n];
     222           0 :     memset(Mr, 0x00, this->n*sizeof(int32_t));
     223             :   #endif
     224             : 
     225             :   // copy the contents of data into Mr and add the zeros
     226           0 :   memcpy(Mr, data, this->k*sizeof(int32_t));
     227             : 
     228           0 :   int32_t j = 0;
     229           0 :   int32_t start = 0;
     230           0 :   int32_t end = this->n - this->k;
     231           0 :   while(end < this->n) {
     232           0 :     for(int32_t i = end; i > start-2; --i) {
     233           0 :       if(Mr[start]) {
     234           0 :         Mr[i] ^= this->generator[j];
     235           0 :         ++j;
     236             :       } else {
     237           0 :         ++start;
     238           0 :         j = 0;
     239           0 :         end = start + this->n - this->k;
     240           0 :         break;
     241             :       }
     242             :     }
     243             :   }
     244             : 
     245             :   #if RADIOLIB_STATIC_ONLY
     246             :     int32_t bb[RADIOLIB_BCH_MAX_N - RADIOLIB_BCH_MAX_K + 1] = { 0 };
     247             :   #else
     248           0 :     int32_t* bb = new int32_t[this->n - this->k + 1];
     249           0 :     memset(bb, 0, (this->n - this->k + 1)*sizeof(int32_t));
     250             :   #endif
     251           0 :   j = 0;
     252           0 :   for(int32_t i = start; i < end; ++i) {
     253           0 :     bb[j] = Mr[i];
     254           0 :     ++j;
     255             :   }
     256             : 
     257             :   #if !RADIOLIB_STATIC_ONLY
     258           0 :   delete[] Mr;
     259             :   #endif
     260             : 
     261           0 :         int32_t iEvenParity = 0;
     262             :   #if RADIOLIB_STATIC_ONLY
     263             :     int32_t recd[RADIOLIB_BCH_MAX_N + 1];
     264             :   #else
     265           0 :     int32_t* recd = new int32_t[this->n + 1];
     266             :   #endif
     267           0 :         for(uint8_t i = 0; i < this->k; i++) {
     268           0 :                 recd[this->n - i] = data[i];
     269           0 :                 if(data[i] == 1) {
     270           0 :       iEvenParity++;
     271             :     }
     272             :         }
     273             :   
     274             :   #if !RADIOLIB_STATIC_ONLY
     275           0 :   delete[] data;
     276             :   #endif
     277             : 
     278           0 :         for(uint8_t i = 0; i < this->n - this->k + 1; i++) {
     279           0 :                 recd[this->n - this->k - i] = bb[i];
     280           0 :                 if(bb[i] == 1) {
     281           0 :       iEvenParity++;
     282             :     }
     283             :         }
     284             :   
     285             :   #if !RADIOLIB_STATIC_ONLY
     286           0 :   delete[] bb;
     287             :   #endif
     288             : 
     289           0 :         if((iEvenParity % 2) == 0) {
     290           0 :     recd[0] = 0;
     291             :   } else {
     292           0 :     recd[0] = 1;
     293             :   }
     294             : 
     295           0 :   int32_t res = 0;
     296           0 :         for(int32_t i = 0; i < this->n + 1; i++) {
     297           0 :                 if(recd[i]) {
     298           0 :       res |= ((uint32_t)1<<i);
     299             :     }
     300             :         }
     301             : 
     302             :   #if !RADIOLIB_STATIC_ONLY
     303           0 :   delete[] recd;
     304             :   #endif
     305             : 
     306           0 :         return(res);
     307             : }
     308             : 
     309             : RadioLibBCH RadioLibBCHInstance;
     310             : 
     311           1 : RadioLibConvCode::RadioLibConvCode() {
     312             : 
     313           1 : }
     314             : 
     315           0 : void RadioLibConvCode::begin(uint8_t rt) {
     316           0 :   this->enc_state = 0;
     317           0 :   this->rate = rt;
     318           0 : }
     319             : 
     320           0 : int16_t RadioLibConvCode::encode(const uint8_t* in, size_t in_bits, uint8_t* out, size_t* out_bits) {
     321           0 :   if(!in || !out) {
     322           0 :     return(RADIOLIB_ERR_UNKNOWN);
     323             :   }
     324             : 
     325             :   size_t ind_bit;
     326           0 :   uint16_t data_out_bitcount = 0;
     327           0 :   uint32_t bin_out_word = 0;
     328             : 
     329             :   // iterate over the provided bits
     330           0 :   for(ind_bit = 0; ind_bit < in_bits; ind_bit++) {
     331           0 :     uint8_t cur_bit = GET_BIT_IN_ARRAY_LSB(in, ind_bit);
     332           0 :     const uint32_t* lut_ptr = (this->rate == 2) ? ConvCodeTable1_2 : ConvCodeTable1_3;
     333           0 :     uint8_t word_pos = this->enc_state / 4;
     334           0 :     uint8_t byte_pos = (3 - (this->enc_state % 4)) * 8;
     335           0 :     uint8_t nibble_pos = (1 - cur_bit) * 4;
     336           0 :     uint8_t g1g0 = (lut_ptr[word_pos] >> (byte_pos + nibble_pos)) & 0x0F;
     337             : 
     338           0 :     uint8_t mod = this->rate == 2 ? 16 : 64;
     339           0 :     this->enc_state = (this->enc_state * 2 + cur_bit) % mod;
     340           0 :     bin_out_word |= (g1g0 << ((7 - (ind_bit % 8)) * this->rate));
     341           0 :     if(ind_bit % 8 == 7) {
     342           0 :       if(this->rate == 3) {
     343           0 :         *out++ = (uint8_t)(bin_out_word >> 16);
     344             :       }
     345           0 :       *out++ = (uint8_t)(bin_out_word >> 8);
     346           0 :       *out++ = (uint8_t)bin_out_word;
     347           0 :       bin_out_word  = 0;
     348             :     }
     349           0 :     data_out_bitcount += this->rate;
     350             :   }
     351             : 
     352           0 :   if(ind_bit % 8) {
     353           0 :     if(this->rate == 3) {
     354           0 :       *out++ = (uint8_t)(bin_out_word >> 16);
     355             :     }
     356           0 :     *out++ = (uint8_t)(bin_out_word >> 8);
     357           0 :     *out++ = (uint8_t)bin_out_word;
     358             :   }
     359             : 
     360           0 :   if(out_bits) { *out_bits = data_out_bitcount; }
     361             : 
     362           0 :   return(RADIOLIB_ERR_NONE);
     363             : }
     364             : 
     365             : RadioLibConvCode RadioLibConvCodeInstance;