Add FEC

lib/CMakeLists.txt

@@ -31,7 +31,8 @@ set(HEADER_FILES
 		include/chiaki/feedbacksender.h
 		include/chiaki/controller.h
 		include/chiaki/takionsendbuffer.h
-		include/chiaki/time.h)
+		include/chiaki/time.h
+		include/chiaki/fec.h)
 
 set(SOURCE_FILES
 		src/common.c
@@ -65,7 +66,8 @@ set(SOURCE_FILES
 		src/feedbacksender.c
 		src/controller.c
 		src/takionsendbuffer.c
-		src/time.c)
+		src/time.c
+		src/fec)
 
 add_subdirectory(protobuf)
 include_directories("${NANOPB_SOURCE_DIR}")
@@ -88,5 +90,6 @@ find_package(OpenSSL REQUIRED)
 target_link_libraries(chiaki-lib OpenSSL::Crypto)
 
 target_link_libraries(chiaki-lib protobuf-nanopb-static)
+target_link_libraries(chiaki-lib jerasure)
 
 target_link_libraries(chiaki-lib ${Opus_LIBRARIES})

lib/include/chiaki/common.h

@@ -42,7 +42,8 @@ typedef enum
 	CHIAKI_ERR_TIMEOUT,
 	CHIAKI_ERR_INVALID_RESPONSE,
 	CHIAKI_ERR_INVALID_MAC,
-	CHIAKI_ERR_UNINITIALIZED
+	CHIAKI_ERR_UNINITIALIZED,
+	CHIAKI_ERR_FEC_FAILED
 } ChiakiErrorCode;
 
 CHIAKI_EXPORT const char *chiaki_error_string(ChiakiErrorCode code);

lib/include/chiaki/fec.h

@@ -0,0 +1,38 @@
+/*
+ * This file is part of Chiaki.
+ *
+ * Chiaki is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Chiaki is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Chiaki.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef CHIAKI_FEC_H
+#define CHIAKI_FEC_H
+
+#include "common.h"
+
+#include <stdint.h>
+#include <unistd.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define CHIAKI_FEC_WORDSIZE 8
+
+CHIAKI_EXPORT ChiakiErrorCode chiaki_fec_decode(uint8_t *frame_buf, size_t unit_size, unsigned int k, unsigned int m, const unsigned int *erasures, size_t erasures_count);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //CHIAKI_FEC_H

lib/include/chiaki/frameprocessor.h

@@ -46,8 +46,8 @@ typedef struct chiaki_frame_processor_t
 
 typedef enum chiaki_frame_flush_result_t {
 	CHIAKI_FRAME_PROCESSOR_FLUSH_RESULT_SUCCESS = 0,
-	CHIAKI_FRAME_PROCESSOR_FLUSH_RESULT_FAILED = 1
-	// TODO: FEC_SUCCESS, FEC_FAILED, ...
+	CHIAKI_FRAME_PROCESSOR_FLUSH_RESULT_FEC_SUCCESS = 1,
+	CHIAKI_FRAME_PROCESSOR_FLUSH_RESULT_FAILED = 2
 } ChiakiFrameProcessorFlushResult;
 
 CHIAKI_EXPORT void chiaki_frame_processor_init(ChiakiFrameProcessor *frame_processor, ChiakiLog *log);

lib/src/fec.c

@@ -0,0 +1,87 @@
+/*
+ * This file is part of Chiaki.
+ *
+ * Chiaki is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Chiaki is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Chiaki.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include <chiaki/fec.h>
+
+#include <jerasure.h>
+#include <cauchy.h>
+
+#include <string.h>
+#include <stdlib.h>
+
+int *create_matrix(unsigned int k, unsigned int m)
+{
+	return cauchy_original_coding_matrix(k, m, CHIAKI_FEC_WORDSIZE);
+}
+
+CHIAKI_EXPORT ChiakiErrorCode chiaki_fec_decode(uint8_t *frame_buf, size_t unit_size, unsigned int k, unsigned int m, const unsigned int *erasures, size_t erasures_count)
+{
+	int *matrix = create_matrix(k, m);
+	if(!matrix)
+		return CHIAKI_ERR_MEMORY;
+
+	ChiakiErrorCode err = CHIAKI_ERR_SUCCESS;
+
+	int *jerasures = calloc(erasures_count + 1, sizeof(int));
+	if(!jerasures)
+	{
+		err = CHIAKI_ERR_MEMORY;
+		goto error_matrix;
+	}
+	memcpy(jerasures, erasures, erasures_count * sizeof(int));
+	jerasures[erasures_count] = -1;
+
+	uint8_t **data_ptrs = calloc(k, sizeof(uint8_t *));
+	if(!data_ptrs)
+	{
+		err = CHIAKI_ERR_MEMORY;
+		goto error_jerasures;
+	}
+
+	uint8_t **coding_ptrs = calloc(m, sizeof(uint8_t *));
+	if(!coding_ptrs)
+	{
+		err = CHIAKI_ERR_MEMORY;
+		goto error_data_ptrs;
+	}
+
+	for(size_t i=0; i<k+m; i++)
+	{
+		uint8_t *buf_ptr = frame_buf + unit_size * i;
+		if(i < k)
+			data_ptrs[i] = buf_ptr;
+		else
+			coding_ptrs[i - k] = buf_ptr;
+	}
+
+	int res = jerasure_matrix_decode(k, m, CHIAKI_FEC_WORDSIZE, matrix, 0, jerasures,
+									 (char **)data_ptrs, (char **)coding_ptrs, unit_size);
+
+	if(res < 0)
+		err = CHIAKI_ERR_FEC_FAILED;
+	else
+		err = CHIAKI_ERR_SUCCESS;
+
+	free(coding_ptrs);
+error_data_ptrs:
+	free(data_ptrs);
+error_jerasures:
+	free(jerasures);
+error_matrix:
+	free(matrix);
+	return err;
+}

lib/src/frameprocessor.c

@@ -16,8 +16,10 @@
  */
 
 #include <chiaki/frameprocessor.h>
+#include <chiaki/fec.h>
 
 #include <string.h>
+#include <assert.h>
 
 
 #define UNIT_SLOTS_MAX 256
@@ -167,14 +169,86 @@ CHIAKI_EXPORT ChiakiErrorCode chiaki_frame_processor_put_unit(ChiakiFrameProcess
 	return CHIAKI_ERR_SUCCESS;
 }
 
+
+#include <jerasure.h>
+
+static ChiakiErrorCode chiaki_frame_processor_fec(ChiakiFrameProcessor *frame_processor)
+{
+	CHIAKI_LOGI(frame_processor->log, "Frame Processor received %u+%u / %u+%u units, attempting FEC",
+				frame_processor->units_regular_received, frame_processor->units_additional_received,
+				frame_processor->units_regular_expected, frame_processor->units_additional_expected);
+
+
+	size_t erasures_count = (frame_processor->units_regular_expected + frame_processor->units_additional_expected)
+			- (frame_processor->units_regular_received + frame_processor->units_additional_received);
+	unsigned int *erasures = calloc(erasures_count, sizeof(unsigned int));
+	if(!erasures)
+		return CHIAKI_ERR_MEMORY;
+
+	size_t erasure_index = 0;
+	for(size_t i=0; i<frame_processor->units_regular_expected + frame_processor->units_additional_expected; i++)
+	{
+		ChiakiFrameUnit *slot = frame_processor->unit_slots + i;
+		if(!slot->data_size)
+		{
+			if(erasure_index >= erasures_count)
+			{
+				// should never happen by design, but too scary not to check
+				assert(false);
+				free(erasures);
+				return CHIAKI_ERR_UNKNOWN;
+			}
+			erasures[erasure_index++] = (unsigned int)i;
+		}
+	}
+	assert(erasure_index == erasures_count);
+
+	ChiakiErrorCode err = chiaki_fec_decode(frame_processor->frame_buf, frame_processor->buf_size_per_unit,
+			frame_processor->units_regular_expected, frame_processor->units_additional_received,
+			erasures, erasures_count);
+
+	if(err != CHIAKI_ERR_SUCCESS)
+	{
+		err = CHIAKI_ERR_FEC_FAILED;
+		CHIAKI_LOGE(frame_processor->log, "FEC failed");
+	}
+	else
+	{
+		err = CHIAKI_ERR_SUCCESS;
+		CHIAKI_LOGI(frame_processor->log, "FEC successful");
+
+		// restore unit sizes
+		for(size_t i=0; i<frame_processor->units_regular_expected; i++)
+		{
+			ChiakiFrameUnit *slot = frame_processor->unit_slots + i;
+			uint8_t *buf_ptr = frame_processor->frame_buf + frame_processor->buf_size_per_unit * i;
+			uint16_t padding = ntohs(*((uint16_t *)buf_ptr));
+			if(padding >= frame_processor->buf_size_per_unit)
+			{
+				CHIAKI_LOGE(frame_processor->log, "Padding in unit (%#x) is larger or equals to the whole unit size (%#llx)",
+							(unsigned int)padding, frame_processor->buf_size_per_unit);
+				chiaki_log_hexdump(frame_processor->log, CHIAKI_LOG_DEBUG, buf_ptr, 0x50);
+				continue;
+			}
+			slot->data_size = frame_processor->buf_size_per_unit - padding;
+		}
+	}
+
+	free(erasures);
+	return err;
+}
+
 CHIAKI_EXPORT ChiakiFrameProcessorFlushResult chiaki_frame_processor_flush(ChiakiFrameProcessor *frame_processor, uint8_t **frame, size_t *frame_size)
 {
 	if(frame_processor->units_regular_expected == 0)
 		return CHIAKI_FRAME_PROCESSOR_FLUSH_RESULT_FAILED;
 
-	// TODO: FEC
 	if(frame_processor->units_regular_received < frame_processor->units_regular_expected)
-		return CHIAKI_FRAME_PROCESSOR_FLUSH_RESULT_FAILED;
+	{
+		ChiakiErrorCode err = chiaki_frame_processor_fec(frame_processor);
+		if(err != CHIAKI_ERR_SUCCESS)
+			return CHIAKI_FRAME_PROCESSOR_FLUSH_RESULT_FAILED;
+	}
 
 	uint8_t *buf = malloc(frame_processor->frame_buf_size); // TODO: this should come from outside instead of mallocing all the time
 	if(!buf)
@@ -187,10 +261,13 @@ CHIAKI_EXPORT ChiakiFrameProcessorFlushResult chiaki_frame_processor_flush(Chiak
 		if(unit->data_size < 2)
 		{
 			CHIAKI_LOGE(frame_processor->log, "Saved unit has size < 2");
+			chiaki_log_hexdump(frame_processor->log, CHIAKI_LOG_DEBUG, frame_processor->frame_buf + i*frame_processor->buf_size_per_unit, 0x50);
 			continue;
 		}
 		size_t part_size = unit->data_size - 2;
-		memcpy(buf + buf_size, frame_processor->frame_buf + i*frame_processor->buf_size_per_unit + 2, part_size);
+		uint8_t *buf_ptr = frame_processor->frame_buf + i*frame_processor->buf_size_per_unit;
+		//CHIAKI_LOGD(frame_processor->log, "unit size: %#zx, in buf: %#x", unit->data_size, frame_processor->buf_size_per_unit - (unsigned int)ntohs(*((uint16_t *)buf_ptr)));
+		memcpy(buf + buf_size, buf_ptr + 2, part_size);
 		buf_size += part_size;
 	}