zita_convolver.cpp

/*
 * This file is part of dsp.
 *
 * Copyright (c) 2016-2024 Michael Barbour <barbour.michael.0@gmail.com>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/

#include <cstdio>
#include <cstdlib>
#include <zita-convolver.h>
#include "zita_convolver.h"

extern "C" {
	#include "util.h"
	#include "codec.h"
	#include "sampleconv.h"
}

struct zita_convolver_state {
	ssize_t filter_frames, len, pos, drain_frames, drain_pos;
	sample_t **output;
	Convproc *cproc;
	int has_output, is_draining;
};

sample_t * zita_convolver_effect_run(struct effect *e, ssize_t *frames, sample_t *ibuf, sample_t *obuf)
{
	struct zita_convolver_state *state = (struct zita_convolver_state *) e->data;
	ssize_t i, k, iframes = 0, oframes = 0;
	while (iframes < *frames) {
		while (state->pos < state->len && iframes < *frames) {
			for (i = k = 0; i < e->ostream.channels; ++i) {
				#ifdef SYMMETRIC_IO
					obuf[oframes * e->ostream.channels + i] = (state->has_output) ? state->output[i][state->pos] : 0;
				#else
					if (state->has_output)
						obuf[oframes * e->ostream.channels + i] = state->output[i][state->pos];
				#endif
				if (GET_BIT(e->channel_selector, i)) {
					state->cproc->inpdata(k)[state->pos] = (ibuf) ? ibuf[iframes * e->ostream.channels + i] : 0;
					++k;
				}
				else
					state->output[i][state->pos] = (ibuf) ? ibuf[iframes * e->ostream.channels + i] : 0;
			}
			#ifdef SYMMETRIC_IO
				++oframes;
			#else
				if (state->has_output)
					++oframes;
			#endif
			++iframes;
			++state->pos;
		}
		if (state->pos == state->len) {
			state->cproc->process(true);
			for (i = k = 0; i < e->ostream.channels; ++i) {
				if (GET_BIT(e->channel_selector, i)) {
					read_buf_float((char *) state->cproc->outdata(k), state->output[i], state->len);
					++k;
				}
			}
			state->pos = 0;
			state->has_output = 1;
		}
	}
	*frames = oframes;
	return obuf;
}

ssize_t zita_convolver_effect_delay(struct effect *e)
{
	struct zita_convolver_state *state = (struct zita_convolver_state *) e->data;
	return (state->has_output) ? state->len : state->pos;
}

void zita_convolver_effect_reset(struct effect *e)
{
	/* Note: This doesn't reset zita_convolver's internal state */
	struct zita_convolver_state *state = (struct zita_convolver_state *) e->data;
	state->pos = 0;
	state->has_output = 0;
}

void zita_convolver_effect_drain(struct effect *e, ssize_t *frames, sample_t *obuf)
{
	struct zita_convolver_state *state = (struct zita_convolver_state *) e->data;
	if (!state->has_output && state->pos == 0)
		*frames = -1;
	else {
		if (!state->is_draining) {
			state->drain_frames = state->filter_frames;
			#ifdef SYMMETRIC_IO
				state->drain_frames += state->len - state->pos;
			#else
				if (state->has_output)
					state->drain_frames += state->len - state->pos;
			#endif
			state->drain_frames += state->pos;
			state->is_draining = 1;
		}
		if (state->drain_pos < state->drain_frames) {
			zita_convolver_effect_run(e, frames, NULL, obuf);
			state->drain_pos += *frames;
			*frames -= (state->drain_pos > state->drain_frames) ? state->drain_pos - state->drain_frames : 0;
		}
		else
			*frames = -1;
	}
}

void zita_convolver_effect_destroy(struct effect *e)
{
	int i;
	struct zita_convolver_state *state = (struct zita_convolver_state *) e->data;
	if (!state->cproc->check_stop())
		state->cproc->stop_process();
	state->cproc->cleanup();
	delete state->cproc;
	for (i = 0; i < e->ostream.channels; ++i)
		free(state->output[i]);
	free(state->output);
	free(state);
	free(e->channel_selector);
}

static void write_buf_floatp(sample_t *in, float **out, int channels, ssize_t s)
{
	int c = channels;
	ssize_t in_s = s * channels;
	while (s-- > 0) {
		while(c-- > 0)
			out[c][s] = SAMPLE_TO_FLOAT(in[--in_s]);
		c = channels;
	}
}

struct effect * zita_convolver_effect_init(const struct effect_info *ei, const struct stream_info *istream, const char *channel_selector, const char *dir, int argc, const char *const *argv)
{
	int i, k, n_channels;
	unsigned int min_part_len = 0, max_part_len = 0;
	struct effect *e;
	struct zita_convolver_state *state;
	struct codec *c_filter;
	Convproc *cproc;
	sample_t *buf_interleaved;
	float **buf_planar;
	char *endptr, *fp;

	if (argc > 4 || argc < 2) {
		LOG_FMT(LL_ERROR, "%s: usage: %s", argv[0], ei->usage);
		return NULL;
	}
	if (argc > 2) {
		min_part_len = strtol(argv[1], &endptr, 10);
		CHECK_ENDPTR(argv[1], endptr, "min_part_len", return NULL);
	}
	if (argc > 3) {
		max_part_len = strtol(argv[2], &endptr, 10);
		CHECK_ENDPTR(argv[2], endptr, "max_part_len", return NULL);
	}
	min_part_len = (min_part_len == 0) ? Convproc::MINPART : min_part_len;
	max_part_len = (max_part_len == 0) ? Convproc::MAXPART : max_part_len;
	if (min_part_len < Convproc::MINPART || min_part_len > Convproc::MAXPART || max_part_len < Convproc::MINPART || max_part_len > Convproc::MAXPART) {
		LOG_FMT(LL_ERROR, "%s: error: partition lengths must be within [%d,%d] or 0 for default", argv[0], Convproc::MINPART, Convproc::MAXPART);
		return NULL;
	}
	if (max_part_len < min_part_len) {
		LOG_FMT(LL_ERROR, "%s: warning: max_part_len < min_part_len", argv[0]);
		max_part_len = min_part_len;
	}

	for (i = n_channels = 0; i < istream->channels; ++i)
		if (GET_BIT(channel_selector, i))
			++n_channels;
	if (n_channels > MINIMUM(Convproc::MAXINP, Convproc::MAXOUT)) {
		LOG_FMT(LL_ERROR, "%s: error: number of channels must not exceed %d", argv[0], MINIMUM(Convproc::MAXINP, Convproc::MAXOUT));
		return NULL;
	}
	fp = construct_full_path(dir, argv[argc - 1]);
	struct codec_params c_params = CODEC_PARAMS_AUTO(fp, CODEC_MODE_READ);
	c_filter = init_codec(&c_params);
	if (c_filter == NULL) {
		LOG_FMT(LL_ERROR, "%s: error: failed to open filter file: %s", argv[0], fp);
		free(fp);
		return NULL;
	}
	free(fp);
	if (c_filter->channels != 1 && c_filter->channels != n_channels) {
		LOG_FMT(LL_ERROR, "%s: error: channel mismatch: channels=%d filter_channels=%d", argv[0], n_channels, c_filter->channels);
		destroy_codec(c_filter);
		return NULL;
	}
	if (c_filter->fs != istream->fs) {
		LOG_FMT(LL_ERROR, "%s: error: sample rate mismatch: fs=%d filter_fs=%d", argv[0], istream->fs, c_filter->fs);
		destroy_codec(c_filter);
		return NULL;
	}
	if (c_filter->frames < 1) {
		LOG_FMT(LL_ERROR, "%s: error: filter length must be >= 1", argv[0]);
		destroy_codec(c_filter);
		return NULL;
	}
	cproc = new Convproc;
#if ZITA_CONVOLVER_MAJOR_VERSION >= 4
	if (cproc->configure(n_channels, n_channels, c_filter->frames, min_part_len, min_part_len, max_part_len, 0.0f)) {
#else
	if (cproc->configure(n_channels, n_channels, c_filter->frames, min_part_len, min_part_len, max_part_len)) {
#endif
		LOG_FMT(LL_ERROR, "%s: error: failed to configure convolution engine", argv[0]);
		destroy_codec(c_filter);
		delete cproc;
		return NULL;
	}
	LOG_FMT(LL_VERBOSE, "%s: info: filter_frames=%zd min_part_len=%d max_part_len=%d", argv[0], c_filter->frames, min_part_len, max_part_len);

	e = (struct effect *) calloc(1, sizeof(struct effect));
	e->name = ei->name;
	e->istream.fs = e->ostream.fs = istream->fs;
	e->istream.channels = e->ostream.channels = istream->channels;
	e->channel_selector = (char *) NEW_SELECTOR(istream->channels);
	COPY_SELECTOR(e->channel_selector, channel_selector, istream->channels);
	e->run = zita_convolver_effect_run;
	e->delay = zita_convolver_effect_delay;
	e->reset = zita_convolver_effect_reset;
	e->drain = zita_convolver_effect_drain;
	e->destroy = zita_convolver_effect_destroy;

	state = (struct zita_convolver_state *) calloc(1, sizeof(struct zita_convolver_state));
	state->filter_frames = c_filter->frames;
	state->len = min_part_len;
	state->cproc = cproc;
	state->output = (sample_t **) calloc(istream->channels, sizeof(sample_t *));
	for (i = 0; i < istream->channels; ++i)
		state->output[i] = (sample_t *) calloc(state->len, sizeof(sample_t));
	e->data = (void *) state;

	buf_interleaved = (sample_t *) calloc(c_filter->frames * c_filter->channels, sizeof(sample_t));
	if (c_filter->read(c_filter, buf_interleaved, c_filter->frames) != c_filter->frames)
		LOG_FMT(LL_ERROR, "%s: warning: short read", argv[0]);
	buf_planar = (float **) calloc(c_filter->channels, sizeof(float *));
	for (i = 0; i < c_filter->channels; ++i)
		buf_planar[i] = (float *) calloc(c_filter->frames, sizeof(float));
	write_buf_floatp(buf_interleaved, buf_planar, c_filter->channels, c_filter->frames);
	free(buf_interleaved);
	for (i = k = 0; i < istream->channels; ++i) {
		if (GET_BIT(channel_selector, i)) {
			cproc->impdata_create(k, k, 1, buf_planar[(c_filter->channels == 1) ? 0 : k], 0, c_filter->frames);
			++k;
		}
	}
	for (i = 0; i < c_filter->channels; ++i)
		free(buf_planar[i]);
	free(buf_planar);
	destroy_codec(c_filter);
	cproc->start_process(0, 0);

	return e;
}