1 /*
2 * Copyright (c) 2024 A Labs GmbH
3 * Copyright (c) 2024 tado GmbH
4 * Copyright (c) 2022 Jiapeng Li
5 *
6 * Based on: https://github.com/JiapengLi/LoRaWANFragmentedDataBlockTransportAlgorithm
7 * Original algorithm: http://www.inference.org.uk/mackay/gallager/papers/ldpc.pdf
8 *
9 * SPDX-License-Identifier: Apache-2.0
10 */
11
12 #include "frag_decoder_lowmem.h"
13 #include "frag_flash.h"
14
15 #include <zephyr/logging/log.h>
16 #include <zephyr/sys/util.h>
17 #include <zephyr/sys/bitarray.h>
18
19 LOG_MODULE_REGISTER(lorawan_frag_dec, CONFIG_LORAWAN_SERVICES_LOG_LEVEL);
20
21 SYS_BITARRAY_DEFINE_STATIC(lost_frames, FRAG_MAX_NB);
22 SYS_BITARRAY_DEFINE_STATIC(lost_frames_matrix, (FRAG_TOLERANCE * (FRAG_TOLERANCE + 1) / 2));
23 SYS_BITARRAY_DEFINE_STATIC(matched_lost_frm_bm0, FRAG_TOLERANCE);
24 SYS_BITARRAY_DEFINE_STATIC(matched_lost_frm_bm1, FRAG_TOLERANCE);
25 SYS_BITARRAY_DEFINE_STATIC(matrix_line_bm, FRAG_MAX_NB);
26
27
matrix_location_to_index(size_t x,size_t y,size_t m)28 static inline size_t matrix_location_to_index(size_t x, size_t y, size_t m)
29 {
30 /* We only store the top half of the matrix because it is triangular,
31 * but that means when mapping the coordinates into the flat representation
32 * we need to account for that
33 */
34 return (y + 1) * (m + m - y) / 2 - (m - x);
35 }
36
triangular_matrix_get_entry(struct sys_bitarray * m2tbm,size_t x,size_t y,size_t m)37 static bool triangular_matrix_get_entry(struct sys_bitarray *m2tbm, size_t x, size_t y, size_t m)
38 {
39 /* We are dealing with triangular matrices, so we don't expect actions in the lower half */
40 __ASSERT(x >= y, "x: %d, y: %d, m: %d", x, y, m);
41 size_t bit;
42 int ret;
43
44 ret = sys_bitarray_test_bit(m2tbm, matrix_location_to_index(x, y, m), &bit);
45 __ASSERT_NO_MSG(ret == 0);
46
47 return bit != 0;
48 }
49
triangular_matrix_set_entry(struct sys_bitarray * m2tbm,size_t x,size_t y,size_t m)50 static void triangular_matrix_set_entry(struct sys_bitarray *m2tbm, size_t x, size_t y, size_t m)
51 {
52 /* We are dealing with triangular matrices, so we don't expect actions in the lower half */
53 __ASSERT(x >= y, "x: %d, y: %d, m: %d", x, y, m);
54 int ret;
55
56 ret = sys_bitarray_set_bit(m2tbm, matrix_location_to_index(x, y, m));
57 __ASSERT_NO_MSG(ret == 0);
58 }
59
triangular_matrix_clear_entry(struct sys_bitarray * m2tbm,size_t x,size_t y,size_t m)60 static void triangular_matrix_clear_entry(struct sys_bitarray *m2tbm, size_t x, size_t y, size_t m)
61 {
62 /* We are dealing with triangular matrices, so we don't expect actions in the lower half */
63 __ASSERT(x >= y, "x: %d, y: %d, m: %d", x, y, m);
64
65 int ret;
66
67 ret = sys_bitarray_clear_bit(m2tbm, matrix_location_to_index(x, y, m));
68 __ASSERT_NO_MSG(ret == 0);
69 }
70
bit_get(struct sys_bitarray * bitmap,size_t index)71 static inline bool bit_get(struct sys_bitarray *bitmap, size_t index)
72 {
73 int bit, ret;
74
75 ret = sys_bitarray_test_bit(bitmap, index, &bit);
76 __ASSERT_NO_MSG(ret == 0);
77 return bit != 0;
78 }
79
bit_set(struct sys_bitarray * bitmap,size_t index)80 static inline void bit_set(struct sys_bitarray *bitmap, size_t index)
81 {
82 int ret;
83
84 ret = sys_bitarray_set_bit(bitmap, index);
85 __ASSERT_NO_MSG(ret == 0);
86 }
87
bit_clear(struct sys_bitarray * bitmap,size_t index)88 static inline void bit_clear(struct sys_bitarray *bitmap, size_t index)
89 {
90 int ret;
91
92 ret = sys_bitarray_clear_bit(bitmap, index);
93 __ASSERT_NO_MSG(ret == 0);
94 }
95
bit_count_ones(struct sys_bitarray * bitmap,size_t index)96 static inline size_t bit_count_ones(struct sys_bitarray *bitmap, size_t index)
97 {
98 size_t count;
99 int ret;
100
101 ret = sys_bitarray_popcount_region(bitmap, index + 1, 0, &count);
102 __ASSERT_NO_MSG(ret == 0);
103 return count;
104 }
105
bit_xor(struct sys_bitarray * des,struct sys_bitarray * src,size_t size)106 static inline void bit_xor(struct sys_bitarray *des, struct sys_bitarray *src, size_t size)
107 {
108 int ret;
109
110 ret = sys_bitarray_xor(des, src, size, 0);
111 __ASSERT_NO_MSG(ret == 0);
112 }
113
bit_clear_all(struct sys_bitarray * bitmap,size_t size)114 static inline void bit_clear_all(struct sys_bitarray *bitmap, size_t size)
115 {
116 int ret;
117
118 ret = sys_bitarray_clear_region(bitmap, size, 0);
119 __ASSERT_NO_MSG(ret == 0);
120 }
121
122 /**
123 * Generate a 23bit Pseudorandom Binary Sequence (PRBS)
124 *
125 * @param previous Previous value in the sequence
126 *
127 * @returns Next value in the pseudorandom sequence
128 */
prbs23(int32_t previous)129 static int32_t prbs23(int32_t previous)
130 {
131 int32_t b0 = previous & 1;
132 int32_t b1 = (previous & 32) / 32;
133
134 return (previous / 2) + ((b0 ^ b1) << 22);
135 }
136
137 /**
138 * Generate vector for coded fragment n of the MxN parity matrix
139 *
140 * @param m Total number of uncoded fragments (M)
141 * @param n Coded fragment number (starting at 1 and not 0)
142 * @param vec Output vector (buffer size must be greater than m)
143 */
frag_dec_parity_matrix_vector(size_t m,size_t n,struct sys_bitarray * vec)144 static void frag_dec_parity_matrix_vector(size_t m, size_t n, struct sys_bitarray *vec)
145 {
146 size_t mm, r;
147 int32_t x;
148 int ret;
149
150 ret = sys_bitarray_clear_region(vec, m, 0);
151 __ASSERT_NO_MSG(ret == 0);
152
153 /*
154 * Powers of 2 must be treated differently to make sure matrix content is close
155 * to random. Powers of 2 tend to generate patterns.
156 */
157 if (is_power_of_two(m)) {
158 mm = m + 1;
159 } else {
160 mm = m;
161 }
162
163 x = 1 + (1001 * n);
164
165 for (size_t nb_coeff = 0; nb_coeff < (m / 2); nb_coeff++) {
166 r = (1 << 16);
167 while (r >= m) {
168 x = prbs23(x);
169 r = x % mm;
170 }
171 ret = sys_bitarray_set_bit(vec, r);
172 __ASSERT_NO_MSG(ret == 0);
173 }
174 }
175
frag_dec_init(struct frag_decoder * decoder,size_t nb_frag,size_t frag_size)176 void frag_dec_init(struct frag_decoder *decoder, size_t nb_frag, size_t frag_size)
177 {
178 decoder->nb_frag = nb_frag;
179 decoder->frag_size = frag_size;
180 /* Set all frames lost, from 0 to nb_frag-1 */
181 decoder->lost_frame_count = decoder->nb_frag;
182 sys_bitarray_set_region(&lost_frames, decoder->nb_frag, 0);
183
184 sys_bitarray_clear_region(&lost_frames_matrix, (FRAG_TOLERANCE * (FRAG_TOLERANCE + 1) / 2),
185 0);
186
187 decoder->filled_lost_frame_count = 0;
188 decoder->status = FRAG_DEC_STA_UNCODED;
189 }
190
frag_dec_frame_received(struct frag_decoder * decoder,uint16_t index)191 void frag_dec_frame_received(struct frag_decoder *decoder, uint16_t index)
192 {
193 int ret, was_set;
194
195 ret = sys_bitarray_test_and_clear_bit(&lost_frames, index, &was_set);
196 __ASSERT_NO_MSG(ret == 0);
197
198 if (was_set != 0) {
199 decoder->lost_frame_count--;
200 }
201 }
202
frag_dec_write_vector(struct sys_bitarray * matrix,uint16_t line_index,struct sys_bitarray * vector,size_t len)203 static void frag_dec_write_vector(struct sys_bitarray *matrix, uint16_t line_index,
204 struct sys_bitarray *vector, size_t len)
205 {
206 for (size_t i = line_index; i < len; i++) {
207 if (bit_get(vector, i)) {
208 triangular_matrix_set_entry(matrix, i, line_index, len);
209 } else {
210 triangular_matrix_clear_entry(matrix, i, line_index, len);
211 }
212 }
213 }
214
frag_dec_read_vector(struct sys_bitarray * matrix,uint16_t line_index,struct sys_bitarray * vector,size_t len)215 static void frag_dec_read_vector(struct sys_bitarray *matrix, uint16_t line_index,
216 struct sys_bitarray *vector, size_t len)
217 {
218 for (size_t i = 0; i < len; i++) {
219 if (i >= line_index && triangular_matrix_get_entry(matrix, i, line_index, len)) {
220 bit_set(vector, i);
221 } else {
222 bit_clear(vector, i);
223 }
224 }
225 }
226
frag_dec(struct frag_decoder * decoder,uint16_t frag_counter,const uint8_t * buf,size_t len)227 int frag_dec(struct frag_decoder *decoder, uint16_t frag_counter, const uint8_t *buf, size_t len)
228 {
229 int ret;
230 int i, j;
231 size_t unmatched_frame_count;
232 size_t lost_frame_index, frame_index;
233 static uint8_t row_data_buf[FRAG_MAX_SIZE];
234 static uint8_t xor_row_data_buf[FRAG_MAX_SIZE];
235
236 if (decoder->status == FRAG_DEC_STA_DONE) {
237 return decoder->lost_frame_count;
238 }
239
240 if (len != decoder->frag_size) {
241 return FRAG_DEC_ERR_INVALID_FRAME;
242 }
243
244 __ASSERT_NO_MSG(frag_counter > 0);
245
246 if (frag_counter <= decoder->nb_frag && decoder->status == FRAG_DEC_STA_UNCODED) {
247 /* Mark new received frame */
248 frag_dec_frame_received(decoder, frag_counter - 1);
249 /* Save data to flash */
250 frag_flash_write((frag_counter - 1) * decoder->frag_size, (uint8_t *)buf,
251 decoder->frag_size);
252 /* If no frame was lost, we are already done */
253 if (decoder->lost_frame_count == 0) {
254 decoder->status = FRAG_DEC_STA_DONE;
255 return decoder->lost_frame_count;
256 }
257 return FRAG_DEC_ONGOING;
258 }
259 /* At least one frame was lost, start recovering frames */
260 decoder->status = FRAG_DEC_STA_CODED;
261
262 /* Clear all temporary bm and buf */
263 bit_clear_all(&matched_lost_frm_bm0, decoder->lost_frame_count);
264 bit_clear_all(&matched_lost_frm_bm1, decoder->lost_frame_count);
265
266 /* Copy data buffer because we need to manipulate it */
267 memcpy(xor_row_data_buf, buf, decoder->frag_size);
268
269 if (decoder->lost_frame_count > FRAG_TOLERANCE) {
270 return FRAG_DEC_ERR_TOO_MANY_FRAMES_LOST;
271 }
272
273 unmatched_frame_count = 0;
274 /* Build parity matrix vector for current line */
275 frag_dec_parity_matrix_vector(decoder->nb_frag, frag_counter - decoder->nb_frag,
276 &matrix_line_bm);
277 for (i = 0; i < decoder->nb_frag; i++) {
278 if (!bit_get(&matrix_line_bm, i)) {
279 /* This frame is not part of the recovery matrix for the current fragment */
280 continue;
281 }
282 if (bit_get(&lost_frames, i)) {
283 /* No match for this coded frame in the uncoded frames.
284 * Check which lost frame we are processing by checking how many have been
285 * lost between the start and the current coded fragment.
286 */
287 bit_set(&matched_lost_frm_bm0, bit_count_ones(&lost_frames, i) - 1);
288 unmatched_frame_count++;
289 } else {
290 /* Restore frame by XORing with already received frame */
291 /* Load previously received data into buffer */
292 frag_flash_read(i * decoder->frag_size, row_data_buf, decoder->frag_size);
293 /* XOR previously received data with data for current frame */
294 mem_xor_n(xor_row_data_buf, xor_row_data_buf, row_data_buf,
295 decoder->frag_size);
296 }
297 }
298 if (unmatched_frame_count == 0) {
299 return FRAG_DEC_ONGOING;
300 }
301
302 /* &matched_lost_frm_bm0 now contains new coded frame which excludes all received
303 * frames content start to diagonal &matched_lost_frm_bm0
304 */
305 do {
306 ret = sys_bitarray_find_nth_set(&matched_lost_frm_bm0, 1, decoder->lost_frame_count,
307 0, &lost_frame_index);
308 if (ret == 1) {
309 /* Not found */
310 break;
311 }
312 if (ret != 0) {
313 return FRAG_DEC_ERR;
314 }
315 /* We know which one is the next lost frame, try to find it in the lost frame bitmap
316 */
317 ret = sys_bitarray_find_nth_set(&lost_frames, lost_frame_index + 1,
318 decoder->nb_frag, 0, &frame_index);
319 if (ret == 1) {
320 /* Not found */
321 break;
322 }
323 if (ret != 0) {
324 return FRAG_DEC_ERR;
325 }
326 /* If current frame contains new information, save it */
327 if (!triangular_matrix_get_entry(&lost_frames_matrix, lost_frame_index,
328 lost_frame_index, decoder->lost_frame_count)) {
329 frag_dec_write_vector(&lost_frames_matrix, lost_frame_index,
330 &matched_lost_frm_bm0, decoder->lost_frame_count);
331 frag_flash_write(frame_index * decoder->frag_size, xor_row_data_buf,
332 decoder->frag_size);
333 decoder->filled_lost_frame_count++;
334 break;
335 }
336
337 frag_dec_read_vector(&lost_frames_matrix, lost_frame_index, &matched_lost_frm_bm1,
338 decoder->lost_frame_count);
339 bit_xor(&matched_lost_frm_bm0, &matched_lost_frm_bm1, decoder->lost_frame_count);
340 frag_flash_read(frame_index * decoder->frag_size, row_data_buf, decoder->frag_size);
341 mem_xor_n(xor_row_data_buf, xor_row_data_buf, row_data_buf, decoder->frag_size);
342 } while (!sys_bitarray_is_region_cleared(&matched_lost_frm_bm0, decoder->lost_frame_count,
343 0));
344
345 if (decoder->filled_lost_frame_count != decoder->lost_frame_count) {
346 return FRAG_DEC_ONGOING;
347 }
348
349 if (decoder->lost_frame_count < 2) {
350 decoder->status = FRAG_DEC_STA_DONE;
351 return decoder->lost_frame_count;
352 }
353
354 /* All frame content is received, now to reconstruct the whole frame */
355 for (i = (decoder->lost_frame_count - 2); i >= 0; i--) {
356 ret = sys_bitarray_find_nth_set(&lost_frames, i + 1, decoder->nb_frag, 0,
357 &frame_index);
358 if (ret != 0) {
359 return FRAG_DEC_ERR;
360 }
361 frag_flash_read(frame_index * decoder->frag_size, xor_row_data_buf,
362 decoder->frag_size);
363 frag_dec_read_vector(&lost_frames_matrix, i, &matched_lost_frm_bm1,
364 decoder->lost_frame_count);
365 for (j = (decoder->lost_frame_count - 1); j > i; j--) {
366 if (!bit_get(&matched_lost_frm_bm1, j)) {
367 continue;
368 }
369 ret = sys_bitarray_find_nth_set(&lost_frames, j + 1, decoder->nb_frag, 0,
370 &lost_frame_index);
371 if (ret != 0) {
372 return FRAG_DEC_ERR;
373 }
374 frag_dec_read_vector(&lost_frames_matrix, j, &matched_lost_frm_bm0,
375 decoder->lost_frame_count);
376 bit_xor(&matched_lost_frm_bm1, &matched_lost_frm_bm0,
377 decoder->lost_frame_count);
378 frag_flash_read(lost_frame_index * decoder->frag_size, row_data_buf,
379 decoder->frag_size);
380 mem_xor_n(xor_row_data_buf, xor_row_data_buf, row_data_buf,
381 decoder->frag_size);
382 frag_dec_write_vector(&lost_frames_matrix, i, &matched_lost_frm_bm1,
383 decoder->lost_frame_count);
384 }
385 frag_flash_write(frame_index * decoder->frag_size, xor_row_data_buf,
386 decoder->frag_size);
387 }
388 decoder->status = FRAG_DEC_STA_DONE;
389 return decoder->lost_frame_count;
390 }
391
