path: root/fecdec_filter.c

/* SPDX-License-Identifier: GPL-2.0 */

/** \file fecdec_filter.c A filter that fec-decodes an audio stream. */

#include "para.h"
#include "error.h"
#include "list.h"
#include "sched.h"
#include "buffer_tree.h"
#include "filter.h"
#include "string.h"
#include "portable_io.h"
#include "fec.h"
#include "fd.h"

/**
 * How many FEC groups to store in memory.
 *
 * Packet reordering requires to keep more than one FEC group in memory because
 * slices belonging to the next FEC group may arrive before the current FEC group
 * is complete.
 */
#define NUM_FEC_GROUPS 3

/** Data read from the header of a slice. */
struct fec_header {
	/** Total number of slices in this group. */
	uint8_t slices_per_group;
	/** Number of slices needed to start decoding. */
	uint8_t data_slices_per_group;
	/** Size of the ogg vorbis/wma header (zero for mp3, aac). */
	uint32_t audio_header_size;
	/** Number of the FEC group this slice belongs to. */
	uint32_t group_num;
	/** Size of the data in this FEC group. */
	uint32_t group_bytes;
	/** Number of this slice in the group. */
	uint8_t slice_num;
	/** Used data bytes of this slice. */
	uint16_t slice_bytes;
	/** Non-zero if this group is the beginning of the stream. */
	uint8_t bos;
	/** Non-zero if this stream embeds audio headers into fec groups. */
	uint8_t header_stream;
};

/**
 * The status of one partially received FEC group.
 */
struct fecdec_group {
	/** The header read from the last slice. */
	struct fec_header h;
	/** How many slices received so far. */
	int num_received_slices;
	/** Bitmap of received slices. */
	uint8_t received_slices[32];
	/** The size of the \a idx and the \a data arrays below. */
	int num_slices;
	/** Array of indices of the received slices. */
	int *idx;
	/** Content of the received slices. */
	unsigned char **data;
};

/**
 * Data private to the fecdec filter.
 */
struct private_fecdec_data {
	/** Used by the fec core code. */
	struct fec_parms *fec;
	/** Keeps track of what was received so far. */
	struct fecdec_group groups[NUM_FEC_GROUPS];
	/** Whether an audio file header was already received. */
	bool have_header;
	/** Points to the first received group. */
	struct fecdec_group *first_complete_group;
	struct btr_pool *btrp;
};

/** Iterate over all fecdec groups. */
#define FOR_EACH_FECDEC_GROUP(g, d) for (g = (d)->groups; \
	(g) < (d)->groups + NUM_FEC_GROUPS; (g)++)

static int group_complete(struct fecdec_group *fg)
{
	return fg->num_received_slices >= fg->h.data_slices_per_group;
}

static int group_empty(struct fecdec_group *fg)
{
	return fg->num_received_slices == 0;
}

static void clear_group(struct fecdec_group *fg)
{
	int i;

	for (i = 0; i < fg->num_slices; i++)
		free(fg->data[i]);
	free(fg->data);
	free(fg->idx);
	memset(fg, 0, sizeof(*fg));
}

static int find_group(struct fec_header *h,
		struct private_fecdec_data *pfd, struct fecdec_group **result)
{
	struct fecdec_group *fg;

	FOR_EACH_FECDEC_GROUP(fg, pfd) {
		if (fg->h.group_num != h->group_num)
			continue;
		if (fg->num_received_slices == 0)
			goto success;
		if (fg->h.slices_per_group != h->slices_per_group)
			return -E_BAD_FEC_HEADER;
		if (fg->h.data_slices_per_group != h->data_slices_per_group)
			return -E_BAD_FEC_HEADER;
		if (fg->h.group_bytes != h->group_bytes)
			return -E_BAD_FEC_HEADER;
success:
		*result = fg;
		return 1;
	}
	return 0;
}

static struct fecdec_group *find_unused_group(struct private_fecdec_data *pfd)
{
	struct fecdec_group *fg;

	FOR_EACH_FECDEC_GROUP(fg, pfd) {
		if (fg->num_received_slices == 0)
			return fg;
	}
	return NULL;
}

static struct fecdec_group *try_to_free_group(struct private_fecdec_data *pfd)
{
	struct fecdec_group *fg;

	FOR_EACH_FECDEC_GROUP(fg, pfd) {
		if (!group_complete(fg))
			continue;
		/*
		 * Don't clear the first complete group if it has not yet been
		 * decoded.
		 */
		if (fg == pfd->first_complete_group)
			continue;
		clear_group(fg);
		return fg;
	}
	return NULL;
}

static struct fecdec_group *free_oldest_group(struct private_fecdec_data *pfd)
{
	struct fecdec_group *fg, *oldest = NULL;

	FOR_EACH_FECDEC_GROUP(fg, pfd) {
		if (!oldest || oldest->h.group_num > fg->h.group_num)
			oldest = fg;
	}
	if (!group_complete(oldest) && !group_empty(oldest))
		PARA_WARNING_LOG("Clearing incomplete group %u "
			"(contains %d slices)\n", oldest->h.group_num,
			oldest->num_received_slices);
	if (oldest == pfd->first_complete_group)
		pfd->first_complete_group = NULL;
	clear_group(oldest);
	return oldest;
}

/* returns 1 if the group was found, 0 if not, negative on errors */
static int get_group(struct fec_header *h, struct private_fecdec_data *pfd,
		struct fecdec_group **result)
{
	struct fecdec_group *fg;
	int ret = find_group(h, pfd, &fg);

	if (ret < 0)
		return ret;
	if (ret > 0) /* found group */
		goto success;
	/* group not found */
	fg = find_unused_group(pfd);
	if (fg)
		goto success;
	fg = try_to_free_group(pfd);
	if (fg)
		goto success;
	fg = free_oldest_group(pfd);
	ret = 0;
success:
	fg->h = *h;
	*result = fg;
	return ret;
}

static bool test_and_set_slice_bit(struct fecdec_group *fg, uint8_t slice_num)
{
	uint8_t *p = fg->received_slices + slice_num / 8, old = *p;

	*p |= 1 << (slice_num % 8);
	return old == *p;
}

/*
 * returns 1 if slice was added, zero otherwise (because the group was already
 * complete or a slice has been received twice).
 */
static int add_slice(char *buf, struct fecdec_group *fg)
{
	int r;
	uint8_t slice_num = fg->h.slice_num;

	if (group_complete(fg)) {
		PARA_DEBUG_LOG("group %u complete, ignoring slice %d\n",
			fg->h.group_num, slice_num);
		return 0;
	}
	if (fg->num_slices == 0) {
		fg->num_slices = fg->h.slices_per_group;
		fg->idx = arr_alloc(fg->num_slices, sizeof(int));
		fg->data = arr_zalloc(fg->num_slices, sizeof(unsigned char *));
	}
	r = fg->num_received_slices;
	/* Check if we already have this slice. */
	if (test_and_set_slice_bit(fg, slice_num)) {
		PARA_INFO_LOG("ignoring duplicate slice %u:%d\n", fg->h.group_num,
			slice_num);
		return 0;
	}
	fg->idx[r] = slice_num;
	fg->data[r] = alloc(fg->h.slice_bytes);
	memcpy(fg->data[r], buf, fg->h.slice_bytes);
	fg->num_received_slices++;
	return 1;
}

/**
 * The different states of a complete FEC group.
 *
 * Even if a FEC group has been received successfully, it probably can not be
 * used right away because some streams (ogg, wma) need to receive an audio
 * file header before decoding can start.
 */
enum fec_group_usability {
	/** Drop the group (because we did not receive the header yet). */
	FEC_GROUP_UNUSABLE,
	/** Use all data in the group. */
	FEC_GROUP_USABLE,
	/** Use the group, but drop its audio file header. */
	FEC_GROUP_USABLE_SKIP_HEADER,
	/** Use the group, including its header. */
	FEC_GROUP_USABLE_WITH_HEADER
};

static enum fec_group_usability group_is_usable(struct fecdec_group *fg,
		struct private_fecdec_data *pfd)
{
	struct fec_header *h = &fg->h;

	if (!h->header_stream)
		return FEC_GROUP_USABLE;
	if (pfd->have_header) {
		if (h->audio_header_size)
			return FEC_GROUP_USABLE_SKIP_HEADER;
		return FEC_GROUP_USABLE;
	}
	if (fg->h.bos)
		return FEC_GROUP_USABLE;
	if (fg->h.audio_header_size)
		return FEC_GROUP_USABLE_WITH_HEADER;
	return FEC_GROUP_UNUSABLE;
}

static int decode_group(struct fecdec_group *fg, struct filter_node *fn)
{
	int i, ret, sb = fg->h.slice_bytes;
	size_t written, need;
	struct private_fecdec_data *pfd = fn->private_data;
	enum fec_group_usability u = group_is_usable(fg, pfd);
	char *buf = NULL;

	if (u == FEC_GROUP_UNUSABLE) {
		PARA_INFO_LOG("dropping unusable group %u\n", fg->h.group_num);
		return 0;
	}
	PARA_DEBUG_LOG("decoding group %u (%d slices)\n", fg->h.group_num,
		fg->h.data_slices_per_group);
	ret = fec_decode(pfd->fec, fg->data, fg->idx, sb);
	if (ret < 0)
		return ret;
	pfd->have_header = true;
	i = 0;
	if (u == FEC_GROUP_USABLE_SKIP_HEADER) {
		i = DIV_ROUND_UP(fg->h.audio_header_size, fg->h.slice_bytes);
		PARA_DEBUG_LOG("skipping %d header slices\n", i);
	}
	PARA_DEBUG_LOG("writing group %u (%u/%d decoded data bytes)\n",
		fg->h.group_num, fg->h.group_bytes,
		fg->h.data_slices_per_group * sb);
	need = (fg->h.data_slices_per_group - i) * sb;
	if (need > btr_pool_unused(pfd->btrp))
		return -E_FECDEC_OVERRUN;
	btr_pool_get_buffer(pfd->btrp, &buf);
	if (u == FEC_GROUP_USABLE_WITH_HEADER) {
		PARA_INFO_LOG("writing audio file header\n");
		written = 0;
		for (i = 0; i < fg->h.data_slices_per_group; i++) {
			size_t n = sb;
			if (written >= fg->h.audio_header_size)
				break;
			if (sb + written > fg->h.audio_header_size)
				n = fg->h.audio_header_size - written;
			btr_copy(fg->data[i], n, pfd->btrp, fn->btrn);
			written += n;
		}
	}
	written = 0;
	for (; i < fg->h.data_slices_per_group; i++) {
		size_t n = sb;
		if (n + written > fg->h.group_bytes)
			n = fg->h.group_bytes - written;
		btr_copy(fg->data[i], n, pfd->btrp, fn->btrn);
		written += n;
	}
	return 0;
}

/**
 * Read a fec header from a buffer.
 *
 * \param buf The buffer to write to.
 * \param h The fec header to write.
 */
static int read_fec_header(char *buf, size_t len, struct fec_header *h)
{
	uint32_t magic;

	if (len < FEC_HEADER_SIZE)
		return 0;
	magic = read_u32(buf);
	if (magic != FEC_MAGIC)
		return -E_BAD_FEC_HEADER;
	h->slices_per_group = read_u8(buf + 4);
	h->data_slices_per_group = read_u8(buf + 5);
	h->audio_header_size = read_u32(buf + 6);

	h->group_num = read_u32(buf + 10);
	h->group_bytes = read_u32(buf + 14);

	h->slice_num = read_u8(buf + 18);
	h->slice_bytes = read_u16(buf + 20);
	h->bos = read_u8(buf + 22);
	h->header_stream = read_u8(buf + 23);
	if (!memcmp(buf, FEC_EOF_PACKET, FEC_EOF_PACKET_LEN))
		return -E_EOF;
//	PARA_DEBUG_LOG("group %u, slize %u, slices per group: %u\n",
//		h->group_num, h->slice_num, h->slices_per_group);
	return 1;
}

/* returns 1 if we used the buffer, 0 if we didn't, negative on errors */
static int dispatch_slice(char *buf, size_t len, struct fec_header *h,
		struct filter_node *fn)
{
	struct fecdec_group *fg;
	int ret, k, n;
	struct private_fecdec_data *pfd = fn->private_data;

	if (h->slice_bytes > len) { /* can not use the thing, try to read more */
		fn->min_iqs = h->slice_bytes + FEC_HEADER_SIZE;
		return 0;
	}
	ret = get_group(h, pfd, &fg);
	if (ret < 0)
		return ret;
	if (!add_slice(buf, fg)) /* group already complete */
		return 1;
	if (!group_complete(fg))
		return 1;
	/* this slice completed the group */
	if (pfd->fec)
		goto decode;
	/* it's either the first or the second complete group */
	if (!pfd->first_complete_group) { /* it's the first group */
		enum fec_group_usability u = group_is_usable(fg, pfd);
		assert(u != FEC_GROUP_USABLE_SKIP_HEADER);
		if (u == FEC_GROUP_UNUSABLE) /* forget it */
			return 1;
		pfd->first_complete_group = fg; /* remember it */
		return 1;
	}
	/* we have two complete groups, let's go */
	k = h->data_slices_per_group;
	n = h->slices_per_group;
	PARA_NOTICE_LOG("init fec (%d, %d)\n", k, n);
	ret = fec_new(k, n, &pfd->fec);
	if (ret < 0)
		return ret;
	pfd->btrp = btr_pool_new("fecdec", 128 * 1024);
	/* decode and clear the first group */
	ret = decode_group(pfd->first_complete_group, fn);
	if (ret < 0)
		return ret;
	clear_group(pfd->first_complete_group);
	pfd->first_complete_group = NULL;
decode:
	ret = decode_group(fg, fn);
	if (ret < 0)
		return ret;
	return 1;
}

static void fecdec_close(struct filter_node *fn)
{
	struct private_fecdec_data *pfd = fn->private_data;
	struct fecdec_group *fg;

	FOR_EACH_FECDEC_GROUP(fg, pfd)
		clear_group(fg);
	fec_free(pfd->fec);
	btr_pool_free(pfd->btrp);
	free(fn->private_data);
	fn->private_data = NULL;
}

static int fecdec_post_monitor(__a_unused struct sched *s, void *context)
{
	struct filter_node *fn = context;
	struct btr_node *btrn = fn->btrn;
	int ret;
	struct fec_header h;
	char *buf;
	size_t len;

next_buffer:
	ret = btr_node_status(btrn, fn->min_iqs, BTR_NT_INTERNAL);
	if (ret <= 0)
		goto out;
	btr_merge(btrn, fn->min_iqs);
	len = btr_next_buffer(btrn, &buf);
	ret = read_fec_header(buf, len, &h);
	if (ret <= 0)
		goto out;
	ret = -E_BAD_SLICE_SIZE;
	if (!h.slice_bytes)
		goto out;
	ret = -E_BAD_SLICE_NUM;
	if (h.slice_num > h.slices_per_group)
		goto out;
	ret = dispatch_slice(buf + FEC_HEADER_SIZE, len - FEC_HEADER_SIZE,
		&h, fn);
	//PARA_INFO_LOG("ret: %d, len: %d, slice_bytes: %d\n", ret, len, h.slice_bytes);
	if (ret <= 0)
		goto out;
	btr_consume(btrn, FEC_HEADER_SIZE + h.slice_bytes);
	goto next_buffer;
out:
	if (ret < 0)
		btr_remove_node(&fn->btrn);
	return ret;
}

static void fecdec_open(struct filter_node *fn)
{
	struct private_fecdec_data *pfd;
	pfd = zalloc(sizeof(*pfd));
	fn->private_data = pfd;
	fn->min_iqs = FEC_HEADER_SIZE;
}

/** \cond doxygen_ignore */
const struct filter lsg_filter_cmd_com_fecdec_user_data = {
	.open = fecdec_open,
	.pre_monitor = generic_filter_pre_monitor,
	.post_monitor = fecdec_post_monitor,
	.close = fecdec_close,
};
/** \endcond */