Draft implementation:HEADmain

- f must be divisors of 44100 and be positive
- 44100 hardcoded

1 files changed, 256 insertions, 0 deletions

diff --git a/lib/src/dsp.c b/lib/src/dsp.c
new file mode 100644
index 0000000..45192bf
--- /dev/null
+++ b/lib/src/dsp.c
@@ -0,0 +1,256 @@
+#include <libfshift.h>
+
+#include <fft/pffft/pffft.h>
+
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <malloc.h>
+
+//del
+#include <stdio.h>
+
+////////////////////////////////////////////////////////////////////////////////
+
+struct hilbert_ctx {
+	PFFFT_Setup* fft_ctx;
+	float *pcm_in;
+	float *fft_bins;
+	float *pcm_out1;
+	float *pcm_out2;
+	const float* half_win;
+};
+
+struct fshift_ctx {
+	struct hilbert_ctx hilbert_ctx;
+	float *carrier;
+	size_t carrier_sz;
+	size_t carrier_cos_offset;
+	size_t carrier_cur_pos;
+	size_t delay_sz;
+	float fshift;
+};
+
+static void fill_half_win(float* w, int sz)
+{
+	int i;
+	const float n = sz * 2.0 - 1;
+	for (i = 0; i < sz; i++)
+		w[i] = sinf(M_PI * (float)i / n);
+}
+
+static size_t fill_carrier(float* s, int sz, float shift)
+{
+//	fprintf(stderr, "fill car: %d %f\n", sz, shift);
+	shift /= 44100;
+	size_t cos_offset = 0;
+	s[0] = sinf(2 * M_PI * shift * (float)0);
+	float prev = s[0];
+	for (int i = 1; i < sz; i++) {
+		s[i] = sinf(2 * M_PI * shift * (float)i);
+		if (!cos_offset) {
+			if (s[i] < prev) {
+				cos_offset = i - 1;
+				//fprintf(stderr, "cos offset: %d\n", (int)cos_offset);
+			} else {
+				prev = s[i];
+			}
+		}
+		//fprintf(stderr, "carrier: %d	%f\n", i, s[i]);
+	}
+	return cos_offset;
+}
+
+static void do_hilbert_transform(struct hilbert_ctx* ctx, const float* const in, float* out1, float* out2, int sz)
+{
+	int i, j;
+	float* cur = ctx->pcm_in + sz;
+	const float scale = 1.0 / (sz * 2);
+
+	// Fill second half of buffer
+	for (i = 0, j = sz - 1; i < sz; i++, j--) {
+		//fprintf(stderr, "%d	in: %f , win %f\n", i, in[i], *(ctx->half_win + j));
+		*cur++ = in[i] * (*(ctx->half_win + j));
+	}
+
+//	for (i = 0; i < sz * 2; i++)
+//		fprintf(stderr, "in %d: %f\n", i, *(ctx->pcm_in + i));
+
+	pffft_transform_ordered(ctx->fft_ctx, ctx->pcm_in, ctx->fft_bins, NULL, PFFFT_FORWARD);
+
+	// Fill first half of buffer
+	cur = ctx->pcm_in;
+	for (i = 0; i < sz; i++)
+		*cur++ = in[i] * (*(ctx->half_win + i));
+
+	// Scale fft result
+	for (i = 0; i < sz * 2; i++)
+		*(ctx->fft_bins + i) *= scale;
+
+	// Zero DC
+	*(ctx->fft_bins) = 0.0;
+	*(ctx->fft_bins + 1) = 0.0;
+
+	for (i = 0; i < sz; i++)
+		out2[i] = *(ctx->pcm_out2 + i);
+
+	pffft_transform_ordered(ctx->fft_ctx, ctx->fft_bins, ctx->pcm_out2, NULL, PFFFT_BACKWARD);
+
+	for (i = 0; i < sz; i++) {
+		out2[i] += (*(ctx->pcm_out2 + i) * (*(ctx->half_win + i)));
+		*(ctx->pcm_out2 + i) = *(ctx->pcm_out2 + i + sz) * (*(ctx->half_win + sz - 1 - i));
+	}
+
+	// Multiply to -j
+	for (i = 2; i < sz * 2; i+=2) {
+		//fprintf(stderr, "fft %d: %f\n", i, *(ctx->fft_bins + i));
+		float t = *(ctx->fft_bins + i);
+		*(ctx->fft_bins + i) = *(ctx->fft_bins + i + 1);
+		*(ctx->fft_bins + i + 1) = -t;
+	}
+
+//	for (i = 0; i < sz * 2; i++)
+//		fprintf(stderr, "fft %d: %f\n", i, *(ctx->fft_bins + i));
+
+	for (i = 0; i < sz; i++)
+		out1[i] = *(ctx->pcm_out1 + i);
+
+	pffft_transform_ordered(ctx->fft_ctx, ctx->fft_bins, ctx->pcm_out1, NULL, PFFFT_BACKWARD);
+
+//	for (i = 0; i < sz * 2; i++)
+//		fprintf(stderr, "out %d: %f\n", i, *(ctx->pcm_out1 + i));
+
+	for (i = 0; i < sz; i++) {
+		out1[i] += (*(ctx->pcm_out1 + i) * (*(ctx->half_win + i)));
+		*(ctx->pcm_out1 + i) = *(ctx->pcm_out1 + i + sz) * (*(ctx->half_win + sz - 1 - i));
+	}
+
+//	for (i = 0; i < sz; i++)
+//		fprintf(stderr, "res out %d: %f\n", i, out1[i]);
+
+}
+
+static size_t calc_delay_sz(uint8_t sz)
+{
+	return 1 << (sz - 1);
+}
+
+static size_t calc_carrier_size(float shift)
+{
+	return 44100 / shift;
+}
+
+// n - number of samples to process
+static int init_hilbert_ctx(struct hilbert_ctx *ctx, int n)
+{
+	memset(ctx, 0, sizeof(struct hilbert_ctx));
+	int fft_sz = n * 2;
+
+	fprintf(stderr, "fft sz: %d\n", n);
+	ctx->fft_ctx = pffft_new_setup(fft_sz, PFFFT_REAL);
+	if (!ctx->fft_ctx)
+		goto err;
+
+	ctx->pcm_in = pffft_aligned_malloc(sizeof(float) * fft_sz);
+	if (!ctx->pcm_in)
+		goto err;
+
+	ctx->fft_bins = pffft_aligned_malloc(sizeof(float) * fft_sz);
+	if (!ctx->fft_bins)
+		goto err;
+
+	ctx->pcm_out1 = pffft_aligned_malloc(sizeof(float) * fft_sz);
+	if (!ctx->pcm_out1)
+		goto err;
+
+	ctx->pcm_out2 = pffft_aligned_malloc(sizeof(float) * fft_sz);
+	if (!ctx->pcm_out2)
+		goto err;
+
+
+	float *w = memalign(16, sizeof(float) * n);
+	if (!w)
+		goto err;
+
+	fill_half_win(w, n);
+	ctx->half_win = w;
+	memset(ctx->pcm_in, 0, sizeof(float) * fft_sz);
+	memset(ctx->pcm_out1, 0, sizeof(float) * fft_sz);
+	memset(ctx->pcm_out2, 0, sizeof(float) * fft_sz);
+
+	return 0;
+
+err:
+	return -1;
+}
+
+static void free_hilbert_ctx(struct hilbert_ctx *ctx)
+{
+	if (ctx->half_win)
+		free((void*)ctx->half_win);
+
+	if (ctx->pcm_out2)
+		pffft_aligned_free(ctx->pcm_out2);
+
+	if (ctx->pcm_out1)
+		pffft_aligned_free(ctx->pcm_out1);
+
+	if (ctx->fft_bins)
+		pffft_aligned_free(ctx->fft_bins);
+
+	if (ctx->pcm_in)
+		pffft_aligned_free(ctx->pcm_in);
+
+	if (ctx->fft_ctx)
+		pffft_destroy_setup(ctx->fft_ctx);
+}
+
+fshift_ctx_t fshift_create_ctx(float fshift, uint8_t sz)
+{
+	const size_t ctx_sz = sizeof(struct fshift_ctx);
+	const size_t delay_sz = calc_delay_sz(sz);
+	const size_t carrier_sz = calc_carrier_size(fshift);
+	size_t cos_offset = 0;
+	fshift_ctx_t ctx = malloc(ctx_sz + sizeof(float) * carrier_sz);
+	ctx->carrier = ((void*)ctx) + ctx_sz;
+	ctx->carrier_sz = carrier_sz;
+	ctx->delay_sz = delay_sz;
+	memset(ctx->carrier, 0, (sizeof(float) * carrier_sz));
+	cos_offset = fill_carrier(ctx->carrier, carrier_sz, fshift);
+	ctx->carrier_cos_offset = cos_offset;
+	ctx->carrier_cur_pos = 0;
+	ctx->fshift = fshift;
+	init_hilbert_ctx(&ctx->hilbert_ctx, delay_sz * 2);
+	return ctx;
+}
+
+void fshift_free_ctx(fshift_ctx_t ctx)
+{
+	free_hilbert_ctx(&ctx->hilbert_ctx);
+	free(ctx);
+}
+
+static void do_fshift(fshift_ctx_t ctx, float* out1, const float* out2)
+{
+	size_t cur = ctx->carrier_cur_pos;
+	size_t block_sz = ctx->delay_sz * 2;
+	for (int i = 0; i < block_sz; i++, cur++) {
+		size_t sin_pos = cur % ctx->carrier_sz;
+		size_t cos_pos = (cur + ctx->carrier_cos_offset ) % ctx->carrier_sz;
+		float s = *(ctx->carrier + sin_pos);
+		float c = *(ctx->carrier + cos_pos);
+		//fprintf(stderr, "dsp: %d, %f %f\n", sin_pos, s, c);
+		out1[i] = out1[i] * s - out2[i] * c;
+		//out1[i] = out1[i] * s + out2[i] * c;
+		cur = sin_pos;
+	}
+	ctx->carrier_cur_pos = cur;
+}
+
+void fshift_run(fshift_ctx_t ctx, const float* const in, float* out1, float* out2)
+{
+	do_hilbert_transform(&ctx->hilbert_ctx, in, out1, out2, ctx->delay_sz * 2);
+	do_fshift(ctx, out1, out2);
+}