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