1 /*******************************************************************************
2 *   Copyright 2013-2014 EPFL                                                   *
3 *   Copyright 2013-2014 Quentin Bonnard                                        *
4 *                                                                              *
5 *   This file is part of chilitags.                                            *
6 *                                                                              *
7 *   Chilitags is free software: you can redistribute it and/or modify          *
8 *   it under the terms of the Lesser GNU General Public License as             *
9 *   published by the Free Software Foundation, either version 3 of the         *
10 *   License, or (at your option) any later version.                            *
11 *                                                                              *
12 *   Chilitags is distributed in the hope that it will be useful,               *
13 *   but WITHOUT ANY WARRANTY; without even the implied warranty of             *
14 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
15 *   GNU Lesser General Public License for more details.                        *
16 *                                                                              *
17 *   You should have received a copy of the GNU Lesser General Public License   *
18 *   along with Chilitags.  If not, see <http://www.gnu.org/licenses/>.         *
19 *******************************************************************************/
20 
21 /*
22  * File:   Codec.cpp
23  * Author: zufferey
24  *
25  * Created on August 5, 2010, 10:05 PM
26  */
27 
28 #include "Codec.hpp"
29 
30 #include <stdio.h>
31 #include <string.h>
32 
33 namespace chilitags {
34 
Codec(int bitsId,int bitsCrc,int bitsFec,const char * xorMask,const char * crcPoly)35 Codec::Codec(int bitsId, int bitsCrc, int bitsFec, const char *xorMask, const char *crcPoly) :
36     m_bitsId(bitsId),
37     m_bitsCrc(bitsCrc),
38     m_bitsFec(bitsFec),
39     m_xorMask(binstr2int(xorMask)),
40     m_crcPoly(binstr2int(crcPoly)),
41     m_maxTagsNumber(1<<m_bitsId),
42     m_trackedTagsTable(new tag_info_t[m_maxTagsNumber]),
43     m_bitsBeforePuncturing((m_bitsId + m_bitsCrc + 2) * 2),
44     m_bitsAfterPuncturing(m_bitsId + m_bitsCrc + m_bitsFec),
45     m_puncturing(new unsigned char[m_bitsBeforePuncturing]),
46     m_dec_fec_id(new unsigned char[2 * m_bitsId]),
47     m_hamming_dist(new int[m_bitsId + 1]),
48     m_exploration_level(new int[m_bitsId + 1]),
49     m_fec_state(new int[m_bitsId + 1]),
50     m_fec_decoded_id(new unsigned char[m_bitsId])
51 {
52     for (int i = 0; i < m_bitsAfterPuncturing; ++i) {
53         m_puncturing[i] = 1;
54     }
55     for (int i = m_bitsAfterPuncturing; i < m_bitsBeforePuncturing; ++i) {
56         m_puncturing[i] = 0;
57     }
58 
59     m_fec_fsm[0].output[0] = 0;
60     m_fec_fsm[0].next_state[0] = 0;
61     m_fec_fsm[0].output[1] = 3;
62     m_fec_fsm[0].next_state[1] = 2;
63 
64     m_fec_fsm[1].output[0] = 1;
65     m_fec_fsm[1].next_state[0] = 0;
66     m_fec_fsm[1].output[1] = 2;
67     m_fec_fsm[1].next_state[1] = 2;
68 
69     m_fec_fsm[2].output[0] = 3;
70     m_fec_fsm[2].next_state[0] = 1;
71     m_fec_fsm[2].output[1] = 0;
72     m_fec_fsm[2].next_state[1] = 3;
73 
74     m_fec_fsm[3].output[0] = 2;
75     m_fec_fsm[3].next_state[0] = 1;
76     m_fec_fsm[3].output[1] = 1;
77     m_fec_fsm[3].next_state[1] = 3;
78     for (int i = 0; i<getMaxTagsNumber(); ++i) {
79         addTagToTrackingList(i);
80     }
81 }
82 
~Codec()83 Codec::~Codec() {
84     delete[] m_trackedTagsTable;
85     delete[] m_puncturing;
86     delete[] m_dec_fec_id;
87     delete[] m_hamming_dist;
88     delete[] m_exploration_level;
89     delete[] m_fec_state;
90     delete[] m_fec_decoded_id;
91 }
92 
getTagEncodedId(int tagId,unsigned char * data) const93 bool Codec::getTagEncodedId(int tagId, unsigned char* data) const {
94     if (tagId < 0 || tagId >= m_maxTagsNumber)
95         return false;
96 
97     memcpy(data, m_trackedTagsTable[tagId].fec, m_bitsAfterPuncturing);
98     return true;
99 }
100 
101 /**
102  * Add a tag to the tracking list: first encodes the tag to make decoding much faster.
103  */
addTagToTrackingList(int id)104 void Codec::addTagToTrackingList(int id) {
105     // allocate memory to store tag structure and initializes it
106     m_trackedTagsTable[id].id = id;
107 
108     // encode the tag, this makes the decoding way faster by short-cutting the process
109     encode(&m_trackedTagsTable[id]);
110 }
111 
encode(tag_info_t * tag)112 void Codec::encode(tag_info_t *tag) {
113     // XOR
114     tag->xor_id = tag->id ^ m_xorMask; // BITS_ID
115     // CRC
116     computeCRC(tag); // BITS_ID + BITS_CRC
117     // FEC
118     computeFEC(tag); // BITS_ID + BITS_CRC + BITS_FEC (puncturing is done immediately)
119 }
120 
121 /************ ENCODING *******************/
122 
computeCRC(tag_info_t * tag)123 int Codec::computeCRC(tag_info_t *tag) {
124     long id = tag->xor_id << m_bitsCrc; // multiply input by x16 to get a degree of 26
125     tag->crc = id;
126     long poly = m_crcPoly;
127     poly <<= m_bitsId; // give crc the same degree as input (26)
128     long current_bit = 1 << (m_bitsId + m_bitsCrc);
129     // euclidean division
130     for (int i = 0; i <= m_bitsId; i++) {
131         if (current_bit & tag->crc) {
132             tag->crc ^= poly;
133         }
134         current_bit >>= 1;
135         poly >>= 1;
136     }
137     tag->crc |= id;
138     return 0;
139 }
140 
computeFEC(tag_info_t * tag)141 int Codec::computeFEC(tag_info_t *tag) {
142     int state = 0;
143     int size = m_bitsCrc + m_bitsId + 2 - 1;
144     int bit_pointer = 1 << size;
145     long tmp_val = tag->crc << 2; // add two 0's at the end of the sequence to reset the registers
146     int punct_size = 0;
147     int punct_index = 0;
148     // process the input, starting a state 0 and moving through the FSM according to crc values
149     for (int i = size; i >= 0; i--) {
150         // get input value (bit)
151         int b = tmp_val & bit_pointer ? 1 : 0;
152         // get output corresponding to input value (0..3)
153         int o = m_fec_fsm[state].output[b];
154 
155         // add bits to input, ignoring punctured bits
156         if (m_puncturing[punct_index++]) { // first bit
157             tag->fec[punct_size] = o & 0x2 ? 1 : 0;
158             //printf("%d", tag->fec[punct_size]);
159             punct_size++;
160         }
161         if (m_puncturing[punct_index++]) { // second bit
162             tag->fec[punct_size] = o & 0x1;
163             //printf("%d", tag->fec[punct_size]);
164             punct_size++;
165         }
166         // update state
167         state = m_fec_fsm[state].next_state[b];
168         bit_pointer >>= 1;
169     }
170     return 0;
171 }
172 
173 /***************** DECODING *********************/
174 
175 /**
176  * Decode tag data - extract only the 20 first bits which correspond to the 10 id bits of the tag.
177  * Decode only those 10 bits and then check validity of potential ids by checking the actual
178  * encoded values. Greatly reduces the search space by using both backward search and forward testing.
179  */
decode(const unsigned char * data,int & id) const180 bool Codec::decode(const unsigned char *data, int & id) const {
181     // depuncture: expand data by adding 0 at places where bits were removed because of puncturing
182     // only 20 bits as discussed above
183     int data_index = 0;
184     for (int i = 0; i < 2 * m_bitsId; i++) {
185         if (m_puncturing[i]) {
186             m_dec_fec_id[i] = data[data_index++];
187         } else {
188             m_dec_fec_id[i] = 0;
189         }
190     }
191     tag_info_t *tag = 0;
192     if (viterbi(m_dec_fec_id, data, &tag)) {
193         id = tag->id;
194         return true;
195     }
196     return false;
197 }
198 
199 // param encoded_id: first 20 bits of the tag to decode
200 
viterbi(const unsigned char * encoded_id,const unsigned char * tag_data,tag_info_t ** tag) const201 bool Codec::viterbi(const unsigned char *encoded_id,
202                     const unsigned char *tag_data, tag_info_t **tag) const {
203     *tag = NULL;
204     m_hamming_dist[0] = 0;
205     for (int i = 0; i < m_bitsId + 1; i++) {
206         m_exploration_level[i] = 0;
207     }
208     m_hamming_dist[0] = 0;
209     m_fec_state[0] = 0;
210     int index = 1;
211     while (index > 0) {
212         if (m_exploration_level[index] > 1) {
213             m_exploration_level[index] = 0; // reset exploration level for next trial with different previous path
214             index--;
215             continue;
216         }
217         int input = m_exploration_level[index];
218         m_exploration_level[index]++;
219         m_hamming_dist[index] = m_hamming_dist[index - 1];
220         int output = m_fec_fsm[m_fec_state[index - 1]].output[input]; // expected output for input 0 or 1
221         int diff = output ^ ((encoded_id[(index - 1) * 2] << 1)
222                              + encoded_id[(index - 1) * 2 + 1]); // diff between expected and actual data
223         // compute hamming distance for this step, ignoring punctured bits
224         int new_dist = 0;
225         if (m_puncturing[(index - 1) * 2])
226             new_dist += (diff & 0x02) ? 1 : 0;
227         if (m_puncturing[(index - 1) * 2 + 1])
228             new_dist += diff & 0x01;
229         m_hamming_dist[index] += new_dist;
230         if (m_hamming_dist[index] <= 2) { // still valid path, go to next level
231             m_fec_decoded_id[m_bitsId - index] = input;
232             if (index == m_bitsId) {
233                 int potential_id;
234                 bin2int(m_fec_decoded_id, &potential_id, m_bitsId);
235                 potential_id ^= m_xorMask;
236                 int errors = m_hamming_dist[index];
237                 for (int i = m_bitsId * 2; i < m_bitsAfterPuncturing; i++) {
238                     if (m_trackedTagsTable[potential_id].fec[i] != tag_data[i]) {
239                         errors++;
240                         if (errors > 2)
241                             break;
242                     }
243                 }
244                 if (errors <= 2) { // tag found
245                     *tag = &m_trackedTagsTable[potential_id];
246                     return true;
247                 }
248             } else {
249                 m_fec_state[index]
250                     = m_fec_fsm[m_fec_state[index - 1]].next_state[input];             // update next state
251                 index++;
252             }
253         }
254     }
255     return false;
256 }
257 
bin2int(const unsigned char * bin,int * out,int size)258 void Codec::bin2int(const unsigned char *bin, int *out, int size) {
259     *out = 0;
260     for (int i = size - 1; i >= 0; i--) {
261         *out <<= 1;
262         *out += bin[i];
263     }
264 }
265 
binstr2int(const char * bin)266 unsigned long Codec::binstr2int(const char *bin) {
267     if (!bin) return 0;
268     unsigned long result = (bin[0]!='0');
269     for (int i = 1; bin[i] && i < 64; ++i) {
270         result <<= 1;
271         result += (bin[i]!='0');
272     }
273     return result;
274 }
275 
276 } /* namespace chilitags */
277