diff --git a/Golay24.hpp b/Golay24.hpp new file mode 100644 index 0000000..89f9a47 --- /dev/null +++ b/Golay24.hpp @@ -0,0 +1,323 @@ +/** + * Based on the work of Mr Hank Wallace. + * + * Code adapted from http://www.aqdi.com/golay.htm + * + * Tried before with the implementation from ITPP library but couldn't make it produce the same outputs + * expected from the P25 transmissions that I have tested. This implementation does work. + */ + +#include + + +#define POLY 0xAE3 + +class Golay24 +{ +private: + unsigned int golay(unsigned int cw) + /* This function calculates [23,12] Golay codewords. + The format of the returned int is + [checkbits(11),data(12)]. */ + { + int i; + unsigned int c; + cw&=0xfffl; + c=cw; /* save original codeword */ + for (i=1; i<=12; i++) /* examine each data bit */ + { + if (cw & 1) /* test data bit */ + cw^=POLY; /* XOR polynomial */ + cw>>=1; /* shift intermediate result */ + } + return((cw<<12)|c); /* assemble codeword */ + } + + int parity(unsigned int cw) + /* This function checks the overall parity of codeword cw. + If parity is even, 0 is returned, else 1. */ + { + unsigned char p; + + /* XOR the bytes of the codeword */ + p=*(unsigned char*)&cw; + p^=*((unsigned char*)&cw+1); + p^=*((unsigned char*)&cw+2); + + /* XOR the halves of the intermediate result */ + p=p ^ (p>>4); + p=p ^ (p>>2); + p=p ^ (p>>1); + + /* return the parity result */ + return(p & 1); + } + + unsigned int syndrome(unsigned int cw) + /* This function calculates and returns the syndrome + of a [23,12] Golay codeword. */ + { + int i; + cw&=0x7fffffl; + for (i=1; i<=12; i++) /* examine each data bit */ + { + if (cw & 1) /* test data bit */ + cw^=POLY; /* XOR polynomial */ + cw>>=1; /* shift intermediate result */ + } + return(cw<<12); /* value pairs with upper bits of cw */ + } + + int weight(unsigned int cw) + /* This function calculates the weight of + 23 bit codeword cw. */ + { + int bits,k; + + /* nibble weight table */ + const char wgt[16] = {0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4}; + + bits=0; /* bit counter */ + k=0; + /* do all bits, six nibbles max */ + while ((k<6) && (cw)) + { + bits=bits+wgt[cw & 0xf]; + cw>>=4; + k++; + } + + return(bits); + } + + unsigned int rotate_left(unsigned int cw, int n) + /* This function rotates 23 bit codeword cw left by n bits. */ + { + int i; + + if (n != 0) + { + for (i=1; i<=n; i++) + { + if ((cw & 0x400000l) != 0) + cw=(cw << 1) | 1; + else + cw<<=1; + } + } + + return(cw & 0x7fffffl); + } + + unsigned int rotate_right(unsigned int cw, int n) + /* This function rotates 23 bit codeword cw right by n bits. */ + { + int i; + + if (n != 0) + { + for (i=1; i<=n; i++) + { + if ((cw & 1) != 0) + cw=(cw >> 1) | 0x400000l; + else + cw>>=1; + } + } + + return(cw & 0x7fffffl); + } + +public: + unsigned int correct(unsigned int cw, int *errs) + /* This function corrects Golay [23,12] codeword cw, returning the + corrected codeword. This function will produce the corrected codeword + for three or fewer errors. It will produce some other valid Golay + codeword for four or more errors, possibly not the intended + one. *errs is set to the number of bit errors corrected. */ + { + unsigned char + w; /* current syndrome limit weight, 2 or 3 */ + unsigned int + mask; /* mask for bit flipping */ + int + i,j; /* index */ + unsigned int + s, /* calculated syndrome */ + cwsaver; /* saves initial value of cw */ + + cwsaver=cw; /* save */ + *errs=0; + w=3; /* initial syndrome weight threshold */ + j=-1; /* -1 = no trial bit flipping on first pass */ + mask=1; + while (j<23) /* flip each trial bit */ + { + if (j != -1) /* toggle a trial bit */ + { + if (j>0) /* restore last trial bit */ + { + cw=cwsaver ^ mask; + mask+=mask; /* point to next bit */ + } + cw=cwsaver ^ mask; /* flip next trial bit */ + w=2; /* lower the threshold while bit diddling */ + } + + s=syndrome(cw); /* look for errors */ + if (s) /* errors exist */ + { + for (i=0; i<23; i++) /* check syndrome of each cyclic shift */ + { + if ((*errs=weight(s)) <= w) /* syndrome matches error pattern */ + { + cw=cw ^ s; /* remove errors */ + cw=rotate_right(cw,i); /* unrotate data */ + return(s=cw); + } + else + { + cw=rotate_left(cw,1); /* rotate to next pattern */ + s=syndrome(cw); /* calc new syndrome */ + } + } + j++; /* toggle next trial bit */ + } + else + return(cw); /* return corrected codeword */ + } + + return(cwsaver); /* return original if no corrections */ + } /* correct */ + + unsigned int encode(unsigned int data) + { + unsigned int codeword =golay(data); /* make a test codeword */ + if (parity(codeword)) { + codeword^=0x800000l; + } + + return codeword; + } + + int decode(int *errs, unsigned int *cw) + /* This function decodes codeword *cw. Here error correction is attempted, + with *errs set to the number of + bits corrected, and returning 0 if no errors exist, or 1 if parity errors + exist. */ + { + unsigned int parity_bit; + + parity_bit=*cw & 0x800000l; /* save parity bit */ + *cw&=~0x800000l; /* remove parity bit for correction */ + + *cw=correct(*cw, errs); /* correct up to three bits */ + *cw|=parity_bit; /* restore parity bit */ + + /* check for 4 bit errors */ + if (parity(*cw)) /* odd parity is an error */ + return(1); + + return(0); /* no errors */ + } /* decode */ + + int detect(int *errs, unsigned int cw) + /* This function decodes codeword cw. Here error detection is performed on cw, + returning 0 if no errors exist, 1 if an overall parity error exists, and + 2 if a codeword error exists. */ + { + unsigned int parity_bit; + + *errs=0; + if (parity(cw)) /* odd parity is an error */ + { + *errs=1; + return(1); + } + if (syndrome(cw)) + { + *errs=1; + return(2); + } + else + return(0); /* no errors */ + + } /* decode */ +}; + +/** + * Convenience class that adapts Mr Wallace's implementation to the input/output format of DSD. + * DSD works with data stored in char arrays where each element represents a bit (0 and 1 values). + * The original implementation works with codewords that pack the 24 bits of information (12 data and + * another 12 of parity) in an integer. + */ +class DSDGolay24 : public Golay24 +{ +public: + unsigned int adapt_to_codeword(const char* hex, const char* parity) + { + unsigned int codeword = 0; + + // Data needs to be packed with the 12 bits of parity as the most significant + // bits and 12 bits of data as the less significant. All these discovered by trial and error. + for (unsigned int i=0; i<12; i++) { + codeword |= parity[11-i]; + codeword <<= 1; + } + for (unsigned int i=0; i<6; i++) { + codeword |= hex[5-i]; + codeword <<= 1; + } + // We only have 6 bits of data. We fill up the less 6 significant bits of codeword with zeros. + codeword <<= 5; + + return codeword; + } + + void adapt_to_hex(unsigned int codeword, char* hex) + { + // put back in hex the bits from 6 to 11 of codeword + for (unsigned int i=0, mask = 1<<6; i<6; i++, mask<<=1) { + hex[i] = (codeword & mask) != 0? 1 : 0; + } + } + + /** + * Important method that takes an hex word, a 12 bit parity word and uses the Golay24 implementation + * to error correct it. + * \param hex The data to error correct, packed in an array of 6 chars, representing a bit each. + * \param parity The 12 bits of parity packed in an array of chars. + * \param fixed_errors Output argument, returns the number of errors detected and fixed. + * \return 1 if we were unable to correct the hex word. Too many errors detected. + * in this case the original data remains unchanged. + * 0 if the data was successfully error corrected. + */ + int decode(char* hex, const char* parity, int* fixed_errors) + { + unsigned int codeword = adapt_to_codeword(hex, parity); + // codeword contains: + // bits 0- 5: zeros + // bits 6-11: hex bits + // bits 12-23: golay (24,12) parity bits + + // bits are ordered from left to right, so bit 0 is the most significant and bit 23 is the less. + + int uncorrectable_errors = Golay24::decode(fixed_errors, &codeword); + + // codeword is now hopefully fixed + + // If there are uncorrectable errors and the fixed proposal includes ones + // in bits 0-5 then probably the fix is not good and we discard it. + // Bits 0-5 should always be zero, if there is a problem it's on the other bits + // from 6 to 24. + if (uncorrectable_errors == 1 && (codeword & 0x3f) != 0) { + // discard, don't touch hex + } else { + // put it back into our hex format + adapt_to_hex(codeword, hex); + uncorrectable_errors = 0; + } + + return uncorrectable_errors; + } +}; diff --git a/ReedSolomon.hpp b/ReedSolomon.hpp new file mode 100644 index 0000000..40c7895 --- /dev/null +++ b/ReedSolomon.hpp @@ -0,0 +1,454 @@ +#ifndef __REED_SOLOMON_HPP__ +#define __REED_SOLOMON_HPP__ + +/** + * Code taken and adapted from www.eccpage.com/rs.c + * Credit goes to Mr Simon Rockliff. + * + * Tried before with the implementation from ITPP library but couldn't make it produce the same outputs + * expected from the P25 transmissions that I have tested. This implementation does work. + */ + +/* This program is an encoder/decoder for Reed-Solomon codes. Encoding is in + systematic form, decoding via the Berlekamp iterative algorithm. + In the present form , the constants mm, nn, tt, and kk=nn-2tt must be + specified (the double letters are used simply to avoid clashes with + other n,k,t used in other programs into which this was incorporated!) + Also, the irreducible polynomial used to generate GF(2**mm) must also be + entered -- these can be found in Lin and Costello, and also Clark and Cain. + + The representation of the elements of GF(2**m) is either in index form, + where the number is the power of the primitive element alpha, which is + convenient for multiplication (add the powers modulo 2**m-1) or in + polynomial form, where the bits represent the coefficients of the + polynomial representation of the number, which is the most convenient form + for addition. The two forms are swapped between via lookup tables. + This leads to fairly messy looking expressions, but unfortunately, there + is no easy alternative when working with Galois arithmetic. + + The code is not written in the most elegant way, but to the best + of my knowledge, (no absolute guarantees!), it works. + However, when including it into a simulation program, you may want to do + some conversion of global variables (used here because I am lazy!) to + local variables where appropriate, and passing parameters (eg array + addresses) to the functions may be a sensible move to reduce the number + of global variables and thus decrease the chance of a bug being introduced. + + This program does not handle erasures at present, but should not be hard + to adapt to do this, as it is just an adjustment to the Berlekamp-Massey + algorithm. It also does not attempt to decode past the BCH bound -- see + Blahut "Theory and practice of error control codes" for how to do this. + + Simon Rockliff, University of Adelaide 21/9/89 + + 26/6/91 Slight modifications to remove a compiler dependent bug which hadn't + previously surfaced. A few extra comments added for clarity. + Appears to all work fine, ready for posting to net! + + Notice + -------- + This program may be freely modified and/or given to whoever wants it. + A condition of such distribution is that the author's contribution be + acknowledged by his name being left in the comments heading the program, + however no responsibility is accepted for any financial or other loss which + may result from some unforseen errors or malfunctioning of the program + during use. + Simon Rockliff, 26th June 1991 + */ + +#include +#include + +// Constants to adapt the code to DSD/P25 needs + +#define mm 6 /* RS code over GF(2**4) - change to suit */ +#define nn 63 /* nn=2**mm -1 length of codeword */ +#define tt 8 /* number of errors that can be corrected */ +#define kk 47 /* kk = nn-2*tt */ +// distance = nn-kk+1 = 63-47+1 = 17 + +class ReedSolomon_63_47_17 +{ +private: + int alpha_to[nn + 1], index_of[nn + 1], gg[nn - kk + 1]; + + void generate_gf(int* generator_polinomial) + /* generate GF(2**mm) from the irreducible polynomial p(X) in pp[0]..pp[mm] + lookup tables: index->polynomial form alpha_to[] contains j=alpha**i; + polynomial form -> index form index_of[j=alpha**i] = i + alpha=2 is the primitive element of GF(2**mm) + */ + { + register int i, mask; + + mask = 1; + alpha_to[mm] = 0; + for (i = 0; i < mm; i++) { + alpha_to[i] = mask; + index_of[alpha_to[i]] = i; + if (generator_polinomial[i] != 0) + alpha_to[mm] ^= mask; + mask <<= 1; + } + index_of[alpha_to[mm]] = mm; + mask >>= 1; + for (i = mm + 1; i < nn; i++) { + if (alpha_to[i - 1] >= mask) + alpha_to[i] = alpha_to[mm] ^ ((alpha_to[i - 1] ^ mask) << 1); + else + alpha_to[i] = alpha_to[i - 1] << 1; + index_of[alpha_to[i]] = i; + } + index_of[0] = -1; + } + + void gen_poly() + /* Obtain the generator polynomial of the tt-error correcting, length + nn=(2**mm -1) Reed Solomon code from the product of (X+alpha**i), i=1..2*tt + */ + { + register int i, j; + + gg[0] = 2; /* primitive element alpha = 2 for GF(2**mm) */ + gg[1] = 1; /* g(x) = (X+alpha) initially */ + for (i = 2; i <= nn - kk; i++) { + gg[i] = 1; + for (j = i - 1; j > 0; j--) + if (gg[j] != 0) + gg[j] = gg[j - 1] ^ alpha_to[(index_of[gg[j]] + i) % nn]; + else + gg[j] = gg[j - 1]; + gg[0] = alpha_to[(index_of[gg[0]] + i) % nn]; /* gg[0] can never be zero */ + } + /* convert gg[] to index form for quicker encoding */ + for (i = 0; i <= nn - kk; i++) + gg[i] = index_of[gg[i]]; + } + +public: + ReedSolomon_63_47_17() + { + // Polynom used in P25 is alpha**6+alpha+1 + int generator_polinomial[mm + 1] = { 1, 1, 0, 0, 0, 0, 1 }; /* specify irreducible polynomial coeffts */ + generate_gf(generator_polinomial); + + gen_poly(); + } + + void encode(int* data, int* bb) + /* take the string of symbols in data[i], i=0..(k-1) and encode systematically + to produce 2*tt parity symbols in bb[0]..bb[2*tt-1] + data[] is input and bb[] is output in polynomial form. + Encoding is done by using a feedback shift register with appropriate + connections specified by the elements of gg[], which was generated above. + Codeword is c(X) = data(X)*X**(nn-kk)+ b(X) */ + { + register int i, j; + int feedback; + + for (i = 0; i < nn - kk; i++) + bb[i] = 0; + for (i = kk - 1; i >= 0; i--) { + feedback = index_of[data[i] ^ bb[nn - kk - 1]]; + if (feedback != -1) { + for (j = nn - kk - 1; j > 0; j--) + if (gg[j] != -1) + bb[j] = bb[j - 1] ^ alpha_to[(gg[j] + feedback) % nn]; + else + bb[j] = bb[j - 1]; + bb[0] = alpha_to[(gg[0] + feedback) % nn]; + } else { + for (j = nn - kk - 1; j > 0; j--) + bb[j] = bb[j - 1]; + bb[0] = 0; + } + } + } + + void decode(const int* input, int* recd) + /* assume we have received bits grouped into mm-bit symbols in recd[i], + i=0..(nn-1), and recd[i] is polynomial form. + We first compute the 2*tt syndromes by substituting alpha**i into rec(X) and + evaluating, storing the syndromes in s[i], i=1..2tt (leave s[0] zero) . + Then we use the Berlekamp iteration to find the error location polynomial + elp[i]. If the degree of the elp is >tt, we cannot correct all the errors + and hence just put out the information symbols uncorrected. If the degree of + elp is <=tt, we substitute alpha**i , i=1..n into the elp to get the roots, + hence the inverse roots, the error location numbers. If the number of errors + located does not equal the degree of the elp, we have more than tt errors + and cannot correct them. Otherwise, we then solve for the error value at + the error location and correct the error. The procedure is that found in + Lin and Costello. For the cases where the number of errors is known to be too + large to correct, the information symbols as received are output (the + advantage of systematic encoding is that hopefully some of the information + symbols will be okay and that if we are in luck, the errors are in the + parity part of the transmitted codeword). Of course, these insoluble cases + can be returned as error flags to the calling routine if desired. */ + { + register int i, j, u, q; + int elp[nn - kk + 2][nn - kk], d[nn - kk + 2], l[nn - kk + 2], u_lu[nn - kk + + 2], s[nn - kk + 1]; + int count = 0, syn_error = 0, root[tt], loc[tt], z[tt + 1], err[nn], reg[tt + + 1]; + + for (int i = 0; i < nn; i++) + recd[i] = index_of[input[i]]; /* put recd[i] into index form (ie as powers of alpha) */ + + /* first form the syndromes */ + for (i = 1; i <= nn - kk; i++) { + s[i] = 0; + for (j = 0; j < nn; j++) + if (recd[j] != -1) + s[i] ^= alpha_to[(recd[j] + i * j) % nn]; /* recd[j] in index form */ + /* convert syndrome from polynomial form to index form */ + if (s[i] != 0) + syn_error = 1; /* set flag if non-zero syndrome => error */ + s[i] = index_of[s[i]]; + } + + if (syn_error) /* if errors, try and correct */ + { + /* compute the error location polynomial via the Berlekamp iterative algorithm, + following the terminology of Lin and Costello : d[u] is the 'mu'th + discrepancy, where u='mu'+1 and 'mu' (the Greek letter!) is the step number + ranging from -1 to 2*tt (see L&C), l[u] is the + degree of the elp at that step, and u_l[u] is the difference between the + step number and the degree of the elp. + */ + /* initialise table entries */ + d[0] = 0; /* index form */ + d[1] = s[1]; /* index form */ + elp[0][0] = 0; /* index form */ + elp[1][0] = 1; /* polynomial form */ + for (i = 1; i < nn - kk; i++) { + elp[0][i] = -1; /* index form */ + elp[1][i] = 0; /* polynomial form */ + } + l[0] = 0; + l[1] = 0; + u_lu[0] = -1; + u_lu[1] = 0; + u = 0; + + do { + u++; + if (d[u] == -1) { + l[u + 1] = l[u]; + for (i = 0; i <= l[u]; i++) { + elp[u + 1][i] = elp[u][i]; + elp[u][i] = index_of[elp[u][i]]; + } + } else + /* search for words with greatest u_lu[q] for which d[q]!=0 */ + { + q = u - 1; + while ((d[q] == -1) && (q > 0)) + q--; + /* have found first non-zero d[q] */ + if (q > 0) { + j = q; + do { + j--; + if ((d[j] != -1) && (u_lu[q] < u_lu[j])) + q = j; + } while (j > 0); + }; + + /* have now found q such that d[u]!=0 and u_lu[q] is maximum */ + /* store degree of new elp polynomial */ + if (l[u] > l[q] + u - q) + l[u + 1] = l[u]; + else + l[u + 1] = l[q] + u - q; + + /* form new elp(x) */ + for (i = 0; i < nn - kk; i++) + elp[u + 1][i] = 0; + for (i = 0; i <= l[q]; i++) + if (elp[q][i] != -1) + elp[u + 1][i + u - q] = alpha_to[(d[u] + nn - d[q] + + elp[q][i]) % nn]; + for (i = 0; i <= l[u]; i++) { + elp[u + 1][i] ^= elp[u][i]; + elp[u][i] = index_of[elp[u][i]]; /*convert old elp value to index*/ + } + } + u_lu[u + 1] = u - l[u + 1]; + + /* form (u+1)th discrepancy */ + if (u < nn - kk) /* no discrepancy computed on last iteration */ + { + if (s[u + 1] != -1) + d[u + 1] = alpha_to[s[u + 1]]; + else + d[u + 1] = 0; + for (i = 1; i <= l[u + 1]; i++) + if ((s[u + 1 - i] != -1) && (elp[u + 1][i] != 0)) + d[u + 1] ^= alpha_to[(s[u + 1 - i] + + index_of[elp[u + 1][i]]) % nn]; + d[u + 1] = index_of[d[u + 1]]; /* put d[u+1] into index form */ + } + } while ((u < nn - kk) && (l[u + 1] <= tt)); + + u++; + if (l[u] <= tt) /* can correct error */ + { + /* put elp into index form */ + for (i = 0; i <= l[u]; i++) + elp[u][i] = index_of[elp[u][i]]; + + /* find roots of the error location polynomial */ + for (i = 1; i <= l[u]; i++) + reg[i] = elp[u][i]; + count = 0; + for (i = 1; i <= nn; i++) { + q = 1; + for (j = 1; j <= l[u]; j++) + if (reg[j] != -1) { + reg[j] = (reg[j] + j) % nn; + q ^= alpha_to[reg[j]]; + }; + if (!q) /* store root and error location number indices */ + { + root[count] = i; + loc[count] = nn - i; + count++; + }; + }; + if (count == l[u]) /* no. roots = degree of elp hence <= tt errors */ + { + /* form polynomial z(x) */ + for (i = 1; i <= l[u]; i++) /* Z[0] = 1 always - do not need */ + { + if ((s[i] != -1) && (elp[u][i] != -1)) + z[i] = alpha_to[s[i]] ^ alpha_to[elp[u][i]]; + else if ((s[i] != -1) && (elp[u][i] == -1)) + z[i] = alpha_to[s[i]]; + else if ((s[i] == -1) && (elp[u][i] != -1)) + z[i] = alpha_to[elp[u][i]]; + else + z[i] = 0; + for (j = 1; j < i; j++) + if ((s[j] != -1) && (elp[u][i - j] != -1)) + z[i] ^= alpha_to[(elp[u][i - j] + s[j]) % nn]; + z[i] = index_of[z[i]]; /* put into index form */ + }; + + /* evaluate errors at locations given by error location numbers loc[i] */ + for (i = 0; i < nn; i++) { + err[i] = 0; + if (recd[i] != -1) /* convert recd[] to polynomial form */ + recd[i] = alpha_to[recd[i]]; + else + recd[i] = 0; + } + for (i = 0; i < l[u]; i++) /* compute numerator of error term first */ + { + err[loc[i]] = 1; /* accounts for z[0] */ + for (j = 1; j <= l[u]; j++) + if (z[j] != -1) + err[loc[i]] ^= alpha_to[(z[j] + j * root[i]) % nn]; + if (err[loc[i]] != 0) { + err[loc[i]] = index_of[err[loc[i]]]; + q = 0; /* form denominator of error term */ + for (j = 0; j < l[u]; j++) + if (j != i) + q += index_of[1 + ^ alpha_to[(loc[j] + root[i]) % nn]]; + q = q % nn; + err[loc[i]] = alpha_to[(err[loc[i]] - q + nn) % nn]; + recd[loc[i]] ^= err[loc[i]]; /*recd[i] must be in polynomial form */ + } + } + } else + /* no. roots != degree of elp => >tt errors and cannot solve */ + for (i = 0; i < nn; i++) /* could return error flag if desired */ + if (recd[i] != -1) /* convert recd[] to polynomial form */ + recd[i] = alpha_to[recd[i]]; + else + recd[i] = 0; /* just output received codeword as is */ + } else + /* elp has degree has degree >tt hence cannot solve */ + for (i = 0; i < nn; i++) /* could return error flag if desired */ + if (recd[i] != -1) /* convert recd[] to polynomial form */ + recd[i] = alpha_to[recd[i]]; + else + recd[i] = 0; /* just output received codeword as is */ + } else + /* no non-zero syndromes => no errors: output received codeword */ + for (i = 0; i < nn; i++) + if (recd[i] != -1) /* convert recd[] to polynomial form */ + recd[i] = alpha_to[recd[i]]; + else + recd[i] = 0; + } +}; + +/** + * Convenience class that does a Reed-Solomon (63,20,17) error correction adapting input and output to + * the DSD data format: hex words packed as char arrays. + */ +class DSDReedSolomon_63_20_17 : public ReedSolomon_63_47_17 +{ +private: + int bin_to_hex(char* input) + { + int output = ((input[0] != 0)? 32 : 0) | + ((input[1] != 0)? 16 : 0) | + ((input[2] != 0)? 8 : 0) | + ((input[3] != 0)? 4 : 0) | + ((input[4] != 0)? 2 : 0) | + ((input[5] != 0)? 1 : 0); + + return output; + } + + void hex_to_bin(int input, char* output) + { + output[0] = ((input & 32) != 0)? 1 : 0; + output[1] = ((input & 16) != 0)? 1 : 0; + output[2] = ((input & 8) != 0)? 1 : 0; + output[3] = ((input & 4) != 0)? 1 : 0; + output[4] = ((input & 2) != 0)? 1 : 0; + output[5] = ((input & 1) != 0)? 1 : 0; + } + +public: + /** + * Does a Reed-Solomon decode adapting the input and output to the expected DSD data format. + * \param hex_data Data packed bits, originally a char[20][6], so containing 20 hex works, each char + * is a bit. + * \param hex_parity Parity packed bits, originally a char[16][6], 16 hex words. + */ + void decode(char* hex_data, char* hex_parity) + { + int input[63]; + int output[63]; + + // First put the parity data, 16 + for(unsigned int i=0; i<16; i++) { + input[i] = bin_to_hex(hex_parity + i*6); + } + + // Then the 20 hex words of data + for(unsigned int i=16; i<16+20; i++) { + input[i] = bin_to_hex(hex_data + (i-16)*6); + } + + // Fill up with zeros to complete the 47 expected hex words of data + for(unsigned int i=36; i<63; i++) { + input[i] = 0; + } + + // Now we can call decode on the base class + ReedSolomon_63_47_17::decode(input, output); + + // Convert it back to binary and put it into hex_data. If decode failed we should have + // the input unchanged. + for(unsigned int i=16; i<16+20; i++) { + hex_to_bin(output[i], hex_data + (i-16)*6); + } + } +}; + +#endif // __REED_SOLOMON_HPP__ diff --git a/p25p1_check_hdu.cpp b/p25p1_check_hdu.cpp new file mode 100644 index 0000000..e4fc452 --- /dev/null +++ b/p25p1_check_hdu.cpp @@ -0,0 +1,74 @@ + +#include "p25p1_check_hdu.h" +#include "ReedSolomon.hpp" +#include "Golay24.hpp" + +//#define __CHECK_HDU_DEBUG__ + +int check_and_fix_golay24(char* hex, char* parity, int* fixed_errors) +{ + static DSDGolay24 golay24; + +#ifdef __CHECK_HDU_DEBUG__ + printf("["); + for(unsigned int i=0; i<6; i++) { + printf("%c", (hex[i] != 0)? 'X': ' '); + } + printf("] ["); + for(unsigned int i=12; i<24; i++) { + printf("%c", (parity[i-12] != 0)? 'X': ' '); + } + printf("]"); +#endif + + int uncorrectable_errors = golay24.decode(hex, parity, fixed_errors); + +#ifdef __CHECK_HDU_DEBUG__ + printf(" -> ["); + for(unsigned int i=0; i<6; i++) { + printf("%c", (hex[i] != 0)? 'X': ' '); + } + printf("]"); + if (uncorrectable_errors) { + printf(" Errors: +4"); + } else { + printf(" Errors: %i", *fixed_errors); + } + printf("\n"); +#endif + + return uncorrectable_errors; +} + +void check_and_fix_redsolomon63(char* data, char* parity) +{ + static DSDReedSolomon_63_20_17 reed_solomon63; + +#ifdef __CHECK_HDU_DEBUG__ + char original[20][6]; + for (int i = 0; i < 20; i++) { + for (int j = 0; j < 6; j++) { + original[i][j] = data[i*6+j]; + } + } +#endif + + reed_solomon63.decode(data, parity); + +#ifdef __CHECK_HDU_DEBUG__ + printf("Results for Reed-Solomon code (n=%3d, k=%3d, t= %3d)\n\n", nn, kk, tt); + printf(" i original fixed\n"); + for (int i = 0; i < 20; i++) { + printf("%3d [", i); + for (int j = 0; j < 6; j++) { + printf("%c", (original[i][j] == 1)? 'X' : ' '); + } + printf("] ["); + for (int j = 0; j < 6; j++) { + printf("%c", (data[i*6+j] == 1)? 'X' : ' '); + } + printf("]\n"); + } + printf("\n"); +#endif +} diff --git a/p25p1_check_hdu.h b/p25p1_check_hdu.h new file mode 100644 index 0000000..d15e0e1 --- /dev/null +++ b/p25p1_check_hdu.h @@ -0,0 +1,28 @@ +#ifndef __P25P1_CHECK_HDU_H__ +#define __P25P1_CHECK_HDU_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Attempts to correct an hex (6 bit) word using the Golay(24,12,8) FEC. + * \param hex The 6 bit word to error correct, given as a char array of 6 elements, each one bit. + * \param parity A 12 bit word with the Golay parity data, given as a char array of 12 elements. + * \param fixed_error Output. Filled with the number of errors fixed in the input. + * \return 1 if there were uncorrectable errors in the input, 0 otherwise. + */ +int check_and_fix_golay24(char* hex, char* parity, int* fixed_errors); + +/** + * Attempts to correct 20 hex words using the Reed-Solomon(63,20,17) FEC. + * \param data The packed hex words, each of 6 chars, one after the other. + * \param parity The corresponding 16 hex words with the parity information. + */ +void check_and_fix_redsolomon63(char* data, char* parity); + +#ifdef __cplusplus +} +#endif + +#endif // __P25P1_CHECK_HDU_H__ diff --git a/p25p1_check_nid.h b/p25p1_check_nid.h index 70fe08f..99b8f66 100644 --- a/p25p1_check_nid.h +++ b/p25p1_check_nid.h @@ -1,5 +1,5 @@ -#ifndef __CHECK_NID_H__ -#define __CHECK_NID_H__ +#ifndef __P25P1_CHECK_NID_H__ +#define __P25P1_CHECK_NID_H__ #ifdef __cplusplus extern "C" { @@ -21,4 +21,4 @@ int check_NID(char* bch_code, int* new_nac, char* new_duid, unsigned char parity } #endif -#endif // __CHECK_NID_H__ +#endif // __P25P1_CHECK_NID_H__ diff --git a/p25p1_hdu.c b/p25p1_hdu.c index 0842331..c8f7433 100644 --- a/p25p1_hdu.c +++ b/p25p1_hdu.c @@ -17,6 +17,97 @@ #include "dsd.h" + +#include "p25p1_check_hdu.h" + +/** + * Read two bits from the stream of data. Takes into account the periodic occurrence of status bits. + * Those are discarded. + */ +static void +read_dibit (dsd_opts* opts, dsd_state* state, char* output, int* status_count) +{ + int dibit; + int status; + + if (*status_count == 35) { + // Status bits now + status = getDibit (opts, state); + // TODO: do something useful with the status bits... + *status_count = 1; + } else { + (*status_count)++; + } + + dibit = getDibit (opts, state); + output[0] = (1 & (dibit >> 1)); // bit 1 + output[1] = (1 & dibit); // bit 0 +} + +/** + * Read an hex word from the stream of data. + */ +static void +read_hex (dsd_opts* opts, dsd_state* state, char* hex, int* status_count) +{ + unsigned int i; + + // Read 3 times a dibit = read 6 bits. + for (i=0; i<=4; i+=2) + { + // We read two bits on each call + read_dibit(opts, state, hex + i, status_count); + } +} + +/** + * Read 12 bits from the stream of data. Those are the parity bits that follow each hex word. + */ +static void +read_golay24_parity (dsd_opts* opts, dsd_state* state, char* parity, int* status_count) +{ + unsigned int i; + + // Read 6 times a dibit = read 12 bits. + for (i=0; i<=10; i+=2) + { + // We read two bits on each call + read_dibit(opts, state, parity + i, status_count); + } +} + +static void +correct_hex_word (dsd_state* state, char* hex, char* parity) +{ + int fixed_errors; + int unrecoverable_errors; + + unrecoverable_errors = check_and_fix_golay24(hex, parity, &fixed_errors); + + state->debug_header_errors += fixed_errors; + if (unrecoverable_errors != 0) + { + state->debug_header_critical_errors++; + } +} + +/** + * Read an hex word, its parity bits and attempts to error correct it using the Golay24 algorithm. + */ +static void +read_and_correct_hex_word (dsd_opts* opts, dsd_state* state, char* hex, int* status_count) +{ + char parity[12]; + unsigned int i; + + // Read the hex word + read_hex (opts, state, hex, status_count); + // Read the parity + read_golay24_parity (opts, state, parity, status_count); + // Use the Golay24 FEC to correct it. This call modifies the content of hex to fix it, hopefully + correct_hex_word (state, hex, parity); +} + void processHDU (dsd_opts * opts, dsd_state * state) { @@ -25,6 +116,41 @@ processHDU (dsd_opts * opts, dsd_state * state) int dibit, i, j; long talkgroup; int algidhex, kidhex; + char hex[6]; + char parity[12]; + int status_count; + int status; + + char hex_data[20][6]; // Data in hex-words (6 bit words). A total of 20 hex words. + char hex_parity[16][6]; // Parity of the data, again in hex-word format. A total of 16 parity hex words. + + status_count = 35 - 14; + + // Read 20 hex words, correct them using their Golay24 parity data. + for (i=19; i>=0; i--) + { + read_and_correct_hex_word (opts, state, hex, &status_count); + // Store the corrected hex word into the hex_data store: + for (j=0; j<6; j++) + { + hex_data[i][j] = hex[j]; + } + } + + + // Read the 16 parity hex word. These are used to FEC the 20 hex words using Reed-Solomon. + for (i=15; i>=0; i--) + { + read_and_correct_hex_word (opts, state, hex, &status_count); + // Store the corrected hex word into the hex_parity store: + for (j=0; j<6; j++) + { + hex_parity[i][j] = hex[j]; + } + } + + // Use the Reed-Solomon algorithm to correct the data. hex_data is modified in place + check_and_fix_redsolomon63((char*)hex_data, (char*)hex_parity); mi[72] = 0; mfid[8] = 0; @@ -32,233 +158,154 @@ processHDU (dsd_opts * opts, dsd_state * state) kid[16] = 0; tgid[16] = 0; - // Now processing NID + // Now put the corrected data on the DSD structures - dibit = getDibit (opts, state); - mi[0] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[1] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[2] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[3] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[4] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[5] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[ 0] = hex_data[19][0] + '0'; + mi[ 1] = hex_data[19][1] + '0'; + mi[ 2] = hex_data[19][2] + '0'; + mi[ 3] = hex_data[19][3] + '0'; + mi[ 4] = hex_data[19][4] + '0'; + mi[ 5] = hex_data[19][5] + '0'; - dibit = getDibit (opts, state); - mi[6] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[7] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[8] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[9] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[10] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[11] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 7); + mi[ 6] = hex_data[18][0] + '0'; + mi[ 7] = hex_data[18][1] + '0'; + mi[ 8] = hex_data[18][2] + '0'; + mi[ 9] = hex_data[18][3] + '0'; + mi[10] = hex_data[18][4] + '0'; + mi[11] = hex_data[18][5] + '0'; - dibit = getDibit (opts, state); - mi[12] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[13] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[14] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[15] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[16] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[17] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[12] = hex_data[17][0] + '0'; + mi[13] = hex_data[17][1] + '0'; + mi[14] = hex_data[17][2] + '0'; + mi[15] = hex_data[17][3] + '0'; + mi[16] = hex_data[17][4] + '0'; + mi[17] = hex_data[17][5] + '0'; - dibit = getDibit (opts, state); - mi[18] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[19] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[20] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[21] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[22] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[23] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[18] = hex_data[16][0] + '0'; + mi[19] = hex_data[16][1] + '0'; + mi[20] = hex_data[16][2] + '0'; + mi[21] = hex_data[16][3] + '0'; + mi[22] = hex_data[16][4] + '0'; + mi[23] = hex_data[16][5] + '0'; - dibit = getDibit (opts, state); - mi[24] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[25] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[26] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[27] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[28] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[29] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[24] = hex_data[15][0] + '0'; + mi[25] = hex_data[15][1] + '0'; + mi[26] = hex_data[15][2] + '0'; + mi[27] = hex_data[15][3] + '0'; + mi[28] = hex_data[15][4] + '0'; + mi[29] = hex_data[15][5] + '0'; - dibit = getDibit (opts, state); - mi[30] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[31] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[32] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[33] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[34] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[35] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 7); + mi[30] = hex_data[14][0] + '0'; + mi[31] = hex_data[14][1] + '0'; + mi[32] = hex_data[14][2] + '0'; + mi[33] = hex_data[14][3] + '0'; + mi[34] = hex_data[14][4] + '0'; + mi[35] = hex_data[14][5] + '0'; - dibit = getDibit (opts, state); - mi[36] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[37] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[38] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[39] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[40] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[41] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[36] = hex_data[13][0] + '0'; + mi[37] = hex_data[13][1] + '0'; + mi[38] = hex_data[13][2] + '0'; + mi[39] = hex_data[13][3] + '0'; + mi[40] = hex_data[13][4] + '0'; + mi[41] = hex_data[13][5] + '0'; - dibit = getDibit (opts, state); - mi[42] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[43] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[44] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[45] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[46] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[47] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[42] = hex_data[12][0] + '0'; + mi[43] = hex_data[12][1] + '0'; + mi[44] = hex_data[12][2] + '0'; + mi[45] = hex_data[12][3] + '0'; + mi[46] = hex_data[12][4] + '0'; + mi[47] = hex_data[12][5] + '0'; - dibit = getDibit (opts, state); - mi[48] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[49] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[50] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[51] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[52] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[53] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[48] = hex_data[11][0] + '0'; + mi[49] = hex_data[11][1] + '0'; + mi[50] = hex_data[11][2] + '0'; + mi[51] = hex_data[11][3] + '0'; + mi[52] = hex_data[11][4] + '0'; + mi[53] = hex_data[11][5] + '0'; - dibit = getDibit (opts, state); - mi[54] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[55] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[56] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[57] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[58] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[59] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 7); + mi[54] = hex_data[10][0] + '0'; + mi[55] = hex_data[10][1] + '0'; + mi[56] = hex_data[10][2] + '0'; + mi[57] = hex_data[10][3] + '0'; + mi[58] = hex_data[10][4] + '0'; + mi[59] = hex_data[10][5] + '0'; - dibit = getDibit (opts, state); - mi[60] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[61] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[62] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[63] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[64] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[65] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[60] = hex_data[9][0] + '0'; + mi[61] = hex_data[9][1] + '0'; + mi[62] = hex_data[9][2] + '0'; + mi[63] = hex_data[9][3] + '0'; + mi[64] = hex_data[9][4] + '0'; + mi[65] = hex_data[9][5] + '0'; - dibit = getDibit (opts, state); - mi[66] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[67] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[68] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[69] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mi[70] = (1 & (dibit >> 1)) + 48; // bit 1 - mi[71] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mi[66] = hex_data[8][0] + '0'; + mi[67] = hex_data[8][1] + '0'; + mi[68] = hex_data[8][2] + '0'; + mi[69] = hex_data[8][3] + '0'; + mi[70] = hex_data[8][4] + '0'; + mi[71] = hex_data[8][5] + '0'; - dibit = getDibit (opts, state); - mfid[0] = (1 & (dibit >> 1)) + 48; // bit 1 - mfid[1] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mfid[2] = (1 & (dibit >> 1)) + 48; // bit 1 - mfid[3] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - mfid[4] = (1 & (dibit >> 1)) + 48; // bit 1 - mfid[5] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mfid[0] = hex_data[7][0] + '0'; + mfid[1] = hex_data[7][1] + '0'; + mfid[2] = hex_data[7][2] + '0'; + mfid[3] = hex_data[7][3] + '0'; + mfid[4] = hex_data[7][4] + '0'; + mfid[5] = hex_data[7][5] + '0'; - dibit = getDibit (opts, state); - mfid[6] = (1 & (dibit >> 1)) + 48; // bit 1 - mfid[7] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - algid[0] = (1 & (dibit >> 1)) + 48; // bit 1 - algid[1] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 1); - dibit = getDibit (opts, state); - algid[2] = (1 & (dibit >> 1)) + 48; // bit 1 - algid[3] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + mfid[6] = hex_data[6][0] + '0'; + mfid[7] = hex_data[6][1] + '0'; + algid[0] = hex_data[6][2] + '0'; // The important algorithm ID. This indicates whether the data is + algid[1] = hex_data[6][3] + '0'; // encrypted and if so what is the encryption algorithm used. + algid[2] = hex_data[6][4] + '0'; // A code 0x80 here means that the data is unencrypted. + algid[3] = hex_data[6][5] + '0'; - dibit = getDibit (opts, state); - algid[4] = (1 & (dibit >> 1)) + 48; // bit 1 - algid[5] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - algid[6] = (1 & (dibit >> 1)) + 48; // bit 1 - algid[7] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - kid[0] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[1] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + algid[4] = hex_data[5][0] + '0'; + algid[5] = hex_data[5][1] + '0'; + algid[6] = hex_data[5][2] + '0'; + algid[7] = hex_data[5][3] + '0'; + kid[ 0] = hex_data[5][4] + '0'; + kid[ 1] = hex_data[5][5] + '0'; - dibit = getDibit (opts, state); - kid[2] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[3] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - kid[4] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[5] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - kid[6] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[7] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + kid[ 2] = hex_data[4][0] + '0'; // The encryption key ID + kid[ 3] = hex_data[4][1] + '0'; + kid[ 4] = hex_data[4][2] + '0'; + kid[ 5] = hex_data[4][3] + '0'; + kid[ 6] = hex_data[4][4] + '0'; + kid[ 7] = hex_data[4][5] + '0'; - dibit = getDibit (opts, state); - kid[8] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[9] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - kid[10] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[11] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - kid[12] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[13] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + kid[ 8] = hex_data[3][0] + '0'; + kid[ 9] = hex_data[3][1] + '0'; + kid[10] = hex_data[3][2] + '0'; + kid[11] = hex_data[3][3] + '0'; + kid[12] = hex_data[3][4] + '0'; + kid[13] = hex_data[3][5] + '0'; - dibit = getDibit (opts, state); - kid[14] = (1 & (dibit >> 1)) + 48; // bit 1 - kid[15] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 1); - dibit = getDibit (opts, state); - tgid[0] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[1] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - tgid[2] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[3] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + kid[14] = hex_data[2][0] + '0'; + kid[15] = hex_data[2][1] + '0'; + tgid[ 0] = hex_data[2][2] + '0'; // Talk group ID + tgid[ 1] = hex_data[2][3] + '0'; + tgid[ 2] = hex_data[2][4] + '0'; + tgid[ 3] = hex_data[2][5] + '0'; - dibit = getDibit (opts, state); - tgid[4] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[5] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - tgid[6] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[7] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - tgid[8] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[9] = (1 & dibit) + 48; // bit 0 - skipDibit (opts, state, 6); + tgid[ 4] = hex_data[1][0] + '0'; + tgid[ 5] = hex_data[1][1] + '0'; + tgid[ 6] = hex_data[1][2] + '0'; + tgid[ 7] = hex_data[1][3] + '0'; + tgid[ 8] = hex_data[1][4] + '0'; + tgid[ 9] = hex_data[1][5] + '0'; - dibit = getDibit (opts, state); - tgid[10] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[11] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - tgid[12] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[13] = (1 & dibit) + 48; // bit 0 - dibit = getDibit (opts, state); - tgid[14] = (1 & (dibit >> 1)) + 48; // bit 1 - tgid[15] = (1 & dibit) + 48; // bit 0 - - skipDibit (opts, state, 160); + tgid[10] = hex_data[0][0] + '0'; + tgid[11] = hex_data[0][1] + '0'; + tgid[12] = hex_data[0][2] + '0'; + tgid[13] = hex_data[0][3] + '0'; + tgid[14] = hex_data[0][4] + '0'; + tgid[15] = hex_data[0][5] + '0'; state->p25kid = strtol(kid, NULL, 2); + skipDibit (opts, state, 5); + status = getDibit (opts, state); + //TODO: Do something useful with the status bits... + if (opts->p25enc == 1) { algidhex = strtol (algid, NULL, 2); diff --git a/test_Golay24.cpp b/test_Golay24.cpp new file mode 100644 index 0000000..029ab01 --- /dev/null +++ b/test_Golay24.cpp @@ -0,0 +1,142 @@ +/** + * Unit tests for the Golay24 implementation. + */ + +#include + +#include "Golay24.hpp" + +/** + * Converts an 24-bit word packed in an integer to a binary string. + */ +static const char *hex_to_bin24(unsigned long x) +{ + static char b[27]; + int z, i; + + for (z = 1, i = 0; i < 26; z <<= 1, i++) + { + b[i] = ((x & z) != 0)? 'X' : ' '; + if (i == 5 || i == 12) { + i++; + b[i] = '-'; + } + } + b[26] = '\0'; + + return b; +} + +/** + * Attempts to FEC an hex word in the DSD format writing some useful debugging information on the way. + * \param expected_errors Set it to 4 to indicate that we expect uncorrectable errors + */ +static void fix_golay24(char* hex, char* sgp, char* expected, int expected_errors) +{ + DSDGolay24 golay24; + + unsigned int trashed_codeword = golay24.adapt_to_codeword(hex, sgp); + + printf("[%s]", hex_to_bin24(trashed_codeword)); + + int fixed_errors = 0; + int many_errors = golay24.decode(hex, sgp, &fixed_errors); // try to correct bit errors + + printf(" -> "); + printf("["); + for (unsigned int i=0; i<6; i++) { + printf("%c", hex[i]? 'X' : ' '); + } + printf("] "); + if (many_errors) { + // Signal that there were errors that couldn't get fixed, probably more than 4. + printf(" +4 errors"); + } else { + printf(" %i errors", fixed_errors); + } + + // check + if (expected_errors == 4) { + assert (many_errors == 1); + } else { + assert (many_errors == 0); + assert (fixed_errors == expected_errors); + } + + printf("\n"); +} + +/** + * This function tests the Golay routines for detection and correction + * of a few representative cases that were manually validated. + */ +static void test_golay24() +{ + Golay24 golay24; + + { + char hex[6] = { 1, 0, 0, 0, 1, 1}; + char parity[12] = { 0, 1, 1, 1, 0, 1, 0, 1, 1, 0, 0, 0}; + char expected[6] = { 1, 0, 0, 0, 1, 1}; + + fix_golay24(hex, parity, expected, 1); + } + + { + char hex[6] = { 1, 0, 1, 0, 1, 0}; + char parity[12] = { 0, 0, 1, 0, 0, 0, 1, 1, 0, 1, 0, 1}; + char expected[6] = { 1, 0, 1, 0, 1, 0}; + + fix_golay24(hex, parity, expected, 0); + } + + { + char hex[6] = { 1, 0, 1, 1, 1, 0}; + char parity[12] = { 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 1, 0}; + char expected[6] = { 1, 0, 1, 1, 1, 0}; + + fix_golay24(hex, parity, expected, 4); // should fail, many errors + } + + { + char hex[6] = { 0, 0, 1, 0, 0, 0}; + char parity[12] = { 1, 1, 0, 1, 1, 1, 0, 0, 0, 1, 1, 0}; + char expected[6] = { 0, 0, 1, 0, 0, 0}; + + fix_golay24(hex, parity, expected, 0); + } + + { + char hex[6] = { 1, 0, 0, 0, 0, 0}; + char parity[12] = { 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1}; + char expected[6] = { 1, 0, 0, 1, 0, 0}; + + fix_golay24(hex, parity, expected, 4); // should fail, many errors + } + + { + char hex[6] = { 0, 0, 0, 0, 0, 0}; + char parity[12] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0}; + char expected[6] = { 0, 0, 0, 0, 0, 0}; + + fix_golay24(hex, parity, expected, 4); // should fail, many errors + } + + { + char hex[6] = { 1, 0, 0, 0, 0, 0}; + char parity[12] = { 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 0}; + char expected[6] = { 0, 0, 1, 0, 0, 0}; + + fix_golay24(hex, parity, expected, 2); + } + +} + +/* +int main(int argc, char** argv) +{ + test_golay24(); + + return 0; +} +*/ diff --git a/test_ReedSolomon.cpp b/test_ReedSolomon.cpp new file mode 100644 index 0000000..b024ed1 --- /dev/null +++ b/test_ReedSolomon.cpp @@ -0,0 +1,138 @@ +/** + * Unit tests for the Reed-Solomon implementation. + */ + +#include + +#include "ReedSolomon.hpp" + +static void byte_to_binary(char* b, int x) +{ + int z, i; + + for (z = 32, i=0; z > 0; z >>= 1, i++) { + b[i] = ((x & z) != 0) ? 'X' : ' '; + } + b[6] = 0; +} + +static void test_reed_solomon63() +{ + ReedSolomon_63_47_17 rs; + + int data[kk]; + int bb[nn - kk]; + int recd[nn]; + int output[nn]; + + for (int i = 0; i < nn; i++) { + recd[i] = 0; + } + + // send this message + recd[16+0] = 1; + recd[16+6] = 8; + + recd[ 0] = 8+4; + recd[ 1] = 3; + recd[ 2] = 8+2; + recd[ 3] = 16+1; + recd[ 4] = 16+2; + recd[ 5] = 32+8+3; + recd[ 6] = 4; + recd[ 7] = 32+2; + recd[ 8] = 32+2; + recd[ 9] = 32+16+8+1; + recd[10] = 32+16+1; + recd[11] = 32+16+3; + recd[12] = 16+8; + recd[13] = 32+2; + recd[14] = 16+4; + recd[15] = 16+4+3; + + rs.decode(recd, output); /* recd[] is returned in polynomial form */ + + /* print out the relevant stuff */ + printf("Results for Reed-Solomon code (n=%3d, k=%3d, t= %3d)\n\n", nn, kk, tt); + printf(" i recd[i](decoded)\n"); + for (int i = 0; i < nn; i++) { + char b[9]; + byte_to_binary(b, output[i]); + printf("%3d [%s]\n", i, b); + } + printf("\n"); + + assert(output[ 0] == 8+4); + assert(output[ 1] == 3); + assert(output[ 2] == 8+2); + assert(output[ 3] == 16+1); + assert(output[ 4] == 16+2); + assert(output[ 5] == 32+3); + assert(output[ 6] == 4); + assert(output[ 7] == 32+2); + assert(output[ 8] == 32+8+2); + assert(output[ 9] == 32+16+8+1); + assert(output[10] == 32+16+1); + assert(output[11] == 32+16+3); + assert(output[12] == 16); + assert(output[13] == 32+8+2); + assert(output[14] == 16+4); + assert(output[15] == 16+4+3); + assert(output[16] == 1); + assert(output[17] == 0); + assert(output[18] == 0); + assert(output[19] == 0); + assert(output[20] == 0); + assert(output[21] == 0); + assert(output[22] == 8); + assert(output[23] == 0); + assert(output[24] == 0); + assert(output[25] == 0); + assert(output[26] == 0); + assert(output[27] == 0); + assert(output[28] == 0); + assert(output[29] == 0); + assert(output[30] == 0); + assert(output[31] == 0); + assert(output[32] == 0); + assert(output[33] == 0); + assert(output[34] == 0); + assert(output[35] == 0); + assert(output[36] == 0); + assert(output[37] == 0); + assert(output[38] == 0); + assert(output[39] == 0); + assert(output[40] == 0); + assert(output[41] == 0); + assert(output[42] == 0); + assert(output[43] == 0); + assert(output[44] == 0); + assert(output[45] == 0); + assert(output[46] == 0); + assert(output[47] == 0); + assert(output[48] == 0); + assert(output[49] == 0); + assert(output[50] == 0); + assert(output[51] == 0); + assert(output[52] == 0); + assert(output[53] == 0); + assert(output[54] == 0); + assert(output[55] == 0); + assert(output[56] == 0); + assert(output[57] == 0); + assert(output[58] == 0); + assert(output[59] == 0); + assert(output[60] == 0); + assert(output[61] == 0); + assert(output[62] == 0); + assert(output[63] == 0); +} + +/* +int main(int argc, char** argv) +{ + test_reed_solomon63(); + + return 0; +} +*/