[asterisk-commits] dvossel: branch dvossel/funk_effects r248105 - /team/dvossel/funk_effects/funcs/

SVN commits to the Asterisk project asterisk-commits at lists.digium.com
Fri Feb 19 22:50:14 CST 2010


Author: dvossel
Date: Fri Feb 19 22:50:11 2010
New Revision: 248105

URL: http://svnview.digium.com/svn/asterisk?view=rev&rev=248105
Log:
addition func_effects.c with PITCH_SHIFT dialplan function


Added:
    team/dvossel/funk_effects/funcs/func_effects.c   (with props)

Added: team/dvossel/funk_effects/funcs/func_effects.c
URL: http://svnview.digium.com/svn/asterisk/team/dvossel/funk_effects/funcs/func_effects.c?view=auto&rev=248105
==============================================================================
--- team/dvossel/funk_effects/funcs/func_effects.c (added)
+++ team/dvossel/funk_effects/funcs/func_effects.c Fri Feb 19 22:50:11 2010
@@ -1,0 +1,439 @@
+/*
+ * Asterisk -- An open source telephony toolkit.
+ *
+ * Copyright (C) 2010, Digium, Inc.
+ *
+ * David Vossel <dvossel at digium.com>
+ *
+ * See http://www.asterisk.org for more information about
+ * the Asterisk project. Please do not directly contact
+ * any of the maintainers of this project for assistance;
+ * the project provides a web site, mailing lists and IRC
+ * channels for your use.
+ *
+ * This program is free software, distributed under the terms of
+ * the GNU General Public License Version 2. See the LICENSE file
+ * at the top of the source tree.
+ */
+
+/*! \file
+ *
+ * \brief Audio Effects
+ *
+ * \author David Vossel <dvossel at digium.com>
+ *
+ * \ingroup functions
+ */
+
+/************************* SMB FUNCTION LICENSE *********************************
+*
+* SYNOPSIS: Routine for doing pitch shifting while maintaining
+* duration using the Short Time Fourier Transform.
+*
+* DESCRIPTION: The routine takes a pitchShift factor value which is between 0.5
+* (one octave down) and 2. (one octave up). A value of exactly 1 does not change
+* the pitch. numSampsToProcess tells the routine how many samples in indata[0...
+* numSampsToProcess-1] should be pitch shifted and moved to outdata[0 ...
+* numSampsToProcess-1]. The two buffers can be identical (ie. it can process the
+* data in-place). fftFrameSize defines the FFT frame size used for the
+* processing. Typical values are 1024, 2048 and 4096. It may be any value <=
+* MAX_FRAME_LENGTH but it MUST be a power of 2. osamp is the STFT
+* oversampling factor which also determines the overlap between adjacent STFT
+* frames. It should at least be 4 for moderate scaling ratios. A value of 32 is
+* recommended for best quality. sampleRate takes the sample rate for the signal 
+* in unit Hz, ie. 44100 for 44.1 kHz audio. The data passed to the routine in 
+* indata[] should be in the range [-1.0, 1.0), which is also the output range 
+* for the data, make sure you scale the data accordingly (for 16bit signed integers
+* you would have to divide (and multiply) by 32768). 
+*
+* COPYRIGHT 1999-2009 Stephan M. Bernsee <smb [AT] dspdimension [DOT] com>
+*
+* 						The Wide Open License (WOL)
+*
+* Permission to use, copy, modify, distribute and sell this software and its
+* documentation for any purpose is hereby granted without fee, provided that
+* the above copyright notice and this license appear in all source copies. 
+* THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY OF
+* ANY KIND. See http://www.dspguru.com/wol.htm for more information.
+*
+*****************************************************************************/
+
+#include "asterisk.h"
+
+ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
+
+#include "asterisk/module.h"
+#include "asterisk/channel.h"
+#include "asterisk/pbx.h"
+#include "asterisk/utils.h"
+#include "asterisk/audiohook.h"
+#include <math.h>
+
+/*** DOCUMENTATION
+	<function name="PITCH_SHIFT" language="en_US">
+		<synopsis>
+			Pitch shift both tx and rx audio streams on a channel.
+		</synopsis>
+		<syntax>
+			<parameter name="channel direction" required="true">
+				<para>This can be either <literal>rx</literal> or <literal>tx</literal> and
+				must be set to a valid floating point number between 0.1 and 4.0. A value
+				of 1.0 has no effect.  Greater than 1 raises the pitch. Lower than 1 lowers
+				the pitch.</para>
+			</parameter>
+		</syntax>
+		<description>
+			<para>Examples:</para>
+			<para>exten => 1,1,Set(PITCH_SHIFT(rx)=0.8) ; lowers pitch</para>
+			<para>exten => 2,1,Set(PITCH_SHIFT(tx)=1.5) ; raises pitch</para>
+		</description>
+
+
+	</function>
+ ***/
+
+#define M_PI 3.14159265358979323846
+#define MAX_FRAME_LENGTH 8192
+
+struct fft_data {
+	float in_fifo[MAX_FRAME_LENGTH];
+	float out_fifo[MAX_FRAME_LENGTH];
+	float fft_worksp[2*MAX_FRAME_LENGTH];
+	float last_phase[MAX_FRAME_LENGTH/2+1];
+	float sum_phase[MAX_FRAME_LENGTH/2+1];
+	float output_accum[2*MAX_FRAME_LENGTH];
+	float ana_freq[MAX_FRAME_LENGTH];
+	float ana_magn[MAX_FRAME_LENGTH];
+	float syn_freq[MAX_FRAME_LENGTH];
+	float sys_magn[MAX_FRAME_LENGTH];
+	long gRover;
+	float shift_amount;
+};
+
+struct pitchshift_data {
+	struct ast_audiohook audiohook;
+
+	struct fft_data rx;
+	struct fft_data tx;
+};
+
+static void smb_fft(float *fftBuffer, long fftFrameSize, long sign);
+static void smb_pitch_shift(float pitchShift, long numSampsToProcess, long fftFrameSize, long osamp, float sample_rate, int16_t *indata, int16_t *outdata, struct fft_data *fft_data);
+static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft_data);
+
+static void destroy_callback(void *data)
+{
+	struct pitchshift_data *shift = data;
+
+	ast_audiohook_destroy(&shift->audiohook);
+	ast_free(data);
+};
+
+static const struct ast_datastore_info pitchshift_datastore = {
+	.type = "pitchshift",
+	.destroy = destroy_callback
+};
+
+static int pitchshift_cb(struct ast_audiohook *audiohook, struct ast_channel *chan, struct ast_frame *f, enum ast_audiohook_direction direction)
+{
+	struct ast_datastore *datastore = NULL;
+	struct pitchshift_data *shift = NULL;
+
+	if ((audiohook->status == AST_AUDIOHOOK_STATUS_DONE) ||
+		(f->frametype != AST_FRAME_VOICE)) {
+		return -1;
+	}
+
+	if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
+		return -1;
+	}
+
+	shift = datastore->data;
+
+	/* READ for tx WRITE for rx */
+	if ((direction == AST_AUDIOHOOK_DIRECTION_READ) && shift->tx.shift_amount) {
+		pitch_shift(f, shift->tx.shift_amount, &shift->tx);
+	} else if ((direction == AST_AUDIOHOOK_DIRECTION_WRITE) && shift->rx.shift_amount) {
+		pitch_shift(f, shift->rx.shift_amount, &shift->rx);
+	}
+
+	return 0;
+}
+
+static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
+{
+	struct ast_datastore *datastore = NULL;
+	struct pitchshift_data *shift = NULL;
+	int new = 0;
+	float amount = 0;
+
+	ast_channel_lock(chan);
+	if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
+		ast_channel_unlock(chan);
+
+		if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
+			return 0;
+		}
+		if (!(shift = ast_calloc(1, sizeof(*shift)))) {
+			ast_datastore_free(datastore);
+			return 0;
+		}
+
+		ast_audiohook_init(&shift->audiohook, AST_AUDIOHOOK_TYPE_MANIPULATE, "pitch_shift");
+		shift->audiohook.manipulate_callback = pitchshift_cb;
+		new = 1;
+	} else {
+		ast_channel_unlock(chan);
+		shift = datastore->data;
+	}
+
+	if (!sscanf(value, "%30f", &amount)) {
+		goto cleanup_error;
+	}
+
+	if (!strcasecmp(data, "rx")) {
+		shift->rx.shift_amount = amount;
+	}
+
+	if (!strcasecmp(data, "tx")) {
+		shift->tx.shift_amount = amount;
+	}
+
+	if (new) {
+		datastore->data = shift;
+		ast_channel_lock(chan);
+		ast_channel_datastore_add(chan, datastore);
+		ast_channel_unlock(chan);
+		ast_audiohook_attach(chan, &shift->audiohook);
+	}
+
+	return 0;
+
+cleanup_error:
+
+	ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
+	if (new) {
+		ast_datastore_free(datastore);
+	}
+	return -1;
+}
+
+static void smb_fft(float *fftBuffer, long fftFrameSize, long sign)
+{
+	float wr, wi, arg, *p1, *p2, temp;
+	float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
+	long i, bitm, j, le, le2, k;
+
+	for (i = 2; i < 2*fftFrameSize-2; i += 2) {
+		for (bitm = 2, j = 0; bitm < 2*fftFrameSize; bitm <<= 1) {
+			if (i & bitm) j++;
+			j <<= 1;
+		}
+		if (i < j) {
+			p1 = fftBuffer+i; p2 = fftBuffer+j;
+			temp = *p1; *(p1++) = *p2;
+			*(p2++) = temp; temp = *p1;
+			*p1 = *p2; *p2 = temp;
+		}
+	}
+	for (k = 0, le = 2; k < (long)(log(fftFrameSize)/log(2.)+.5); k++) {
+		le <<= 1;
+		le2 = le>>1;
+		ur = 1.0;
+		ui = 0.0;
+		arg = M_PI / (le2>>1);
+		wr = cos(arg);
+		wi = sign*sin(arg);
+		for (j = 0; j < le2; j += 2) {
+			p1r = fftBuffer+j; p1i = p1r+1;
+			p2r = p1r+le2; p2i = p2r+1;
+			for (i = j; i < 2*fftFrameSize; i += le) {
+				tr = *p2r * ur - *p2i * ui;
+				ti = *p2r * ui + *p2i * ur;
+				*p2r = *p1r - tr; *p2i = *p1i - ti;
+				*p1r += tr; *p1i += ti;
+				p1r += le; p1i += le;
+				p2r += le; p2i += le;
+			}
+			tr = ur*wr - ui*wi;
+			ui = ur*wi + ui*wr;
+			ur = tr;
+		}
+	}
+}
+
+static void smb_pitch_shift(float pitchShift, long numSampsToProcess, long fftFrameSize, long osamp, float sample_rate, int16_t *indata, int16_t *outdata, struct fft_data *fft_data)
+{
+	float *in_fifo = fft_data->in_fifo;
+	float *out_fifo = fft_data->out_fifo;
+	float *fft_worksp = fft_data->fft_worksp;
+	float *last_phase = fft_data->last_phase;
+	float *sum_phase = fft_data->sum_phase;
+	float *output_accum = fft_data->output_accum;
+	float *ana_freq = fft_data->ana_freq;
+	float *ana_magn = fft_data->ana_magn;
+	float *syn_freq = fft_data->syn_freq;
+	float *sys_magn = fft_data->sys_magn;
+
+	double magn, phase, tmp, window, real, imag;
+	double freq_per_bin, expct;
+	long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
+
+	/* set up some handy variables */
+	fft_frame_size2 = fftFrameSize/2;
+	step_size = fftFrameSize/osamp;
+	freq_per_bin = sample_rate/(double)fftFrameSize;
+	expct = 2.*M_PI*(double)step_size/(double)fftFrameSize;
+	in_fifo_latency = fftFrameSize-step_size;
+	if (fft_data->gRover == 0) fft_data->gRover = in_fifo_latency;
+
+	/* main processing loop */
+	for (i = 0; i < numSampsToProcess; i++){
+
+		/* As long as we have not yet collected enough data just read in */
+		in_fifo[fft_data->gRover] = indata[i];
+		outdata[i] = out_fifo[fft_data->gRover-in_fifo_latency];
+		fft_data->gRover++;
+
+		/* now we have enough data for processing */
+		if (fft_data->gRover >= fftFrameSize) {
+			fft_data->gRover = in_fifo_latency;
+
+			/* do windowing and re,im interleave */
+			for (k = 0; k < fftFrameSize;k++) {
+				window = -.5*cos(2.*M_PI*(double)k/(double)fftFrameSize)+.5;
+				fft_worksp[2*k] = in_fifo[k] * window;
+				fft_worksp[2*k+1] = 0.;
+			}
+
+			/* ***************** ANALYSIS ******************* */
+			/* do transform */
+			smb_fft(fft_worksp, fftFrameSize, -1);
+
+			/* this is the analysis step */
+			for (k = 0; k <= fft_frame_size2; k++) {
+
+				/* de-interlace FFT buffer */
+				real = fft_worksp[2*k];
+				imag = fft_worksp[2*k+1];
+
+				/* compute magnitude and phase */
+				magn = 2.*sqrt(real*real + imag*imag);
+				phase = atan2(imag,real);
+
+				/* compute phase difference */
+				tmp = phase - last_phase[k];
+				last_phase[k] = phase;
+
+				/* subtract expected phase difference */
+				tmp -= (double)k*expct;
+
+				/* map delta phase into +/- Pi interval */
+				qpd = tmp/M_PI;
+				if (qpd >= 0) qpd += qpd&1;
+				else qpd -= qpd&1;
+				tmp -= M_PI*(double)qpd;
+
+				/* get deviation from bin frequency from the +/- Pi interval */
+				tmp = osamp*tmp/(2.*M_PI);
+
+				/* compute the k-th partials' true frequency */
+				tmp = (double)k*freq_per_bin + tmp*freq_per_bin;
+
+				/* store magnitude and true frequency in analysis arrays */
+				ana_magn[k] = magn;
+				ana_freq[k] = tmp;
+
+			}
+
+			/* ***************** PROCESSING ******************* */
+			/* this does the actual pitch shifting */
+			memset(sys_magn, 0, fftFrameSize*sizeof(float));
+			memset(syn_freq, 0, fftFrameSize*sizeof(float));
+			for (k = 0; k <= fft_frame_size2; k++) {
+				index = k*pitchShift;
+				if (index <= fft_frame_size2) {
+					sys_magn[index] += ana_magn[k];
+					syn_freq[index] = ana_freq[k] * pitchShift;
+				}
+			}
+
+			/* ***************** SYNTHESIS ******************* */
+			/* this is the synthesis step */
+			for (k = 0; k <= fft_frame_size2; k++) {
+
+				/* get magnitude and true frequency from synthesis arrays */
+				magn = sys_magn[k];
+				tmp = syn_freq[k];
+
+				/* subtract bin mid frequency */
+				tmp -= (double)k*freq_per_bin;
+
+				/* get bin deviation from freq deviation */
+				tmp /= freq_per_bin;
+
+				/* take osamp into account */
+				tmp = 2.*M_PI*tmp/osamp;
+
+				/* add the overlap phase advance back in */
+				tmp += (double)k*expct;
+
+				/* accumulate delta phase to get bin phase */
+				sum_phase[k] += tmp;
+				phase = sum_phase[k];
+
+				/* get real and imag part and re-interleave */
+				fft_worksp[2*k] = magn*cos(phase);
+				fft_worksp[2*k+1] = magn*sin(phase);
+			}
+
+			/* zero negative frequencies */
+			for (k = fftFrameSize+2; k < 2*fftFrameSize; k++) fft_worksp[k] = 0.;
+
+			/* do inverse transform */
+			smb_fft(fft_worksp, fftFrameSize, 1);
+
+			/* do windowing and add to output accumulator */
+			for(k=0; k < fftFrameSize; k++) {
+				window = -.5*cos(2.*M_PI*(double)k/(double)fftFrameSize)+.5;
+				output_accum[k] += 2.*window*fft_worksp[2*k]/(fft_frame_size2*osamp);
+			}
+			for (k = 0; k < step_size; k++) out_fifo[k] = output_accum[k];
+
+			/* shift accumulator */
+			memmove(output_accum, output_accum+step_size, fftFrameSize*sizeof(float));
+
+			/* move input FIFO */
+			for (k = 0; k < in_fifo_latency; k++) in_fifo[k] = in_fifo[k+step_size];
+		}
+	}
+}
+
+static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
+{
+	int16_t *fun = (int16_t *) f->data.ptr;
+	int samples;
+		for (samples = 0; samples < f->samples; samples += 32) {
+		smb_pitch_shift(amount, 32, 128, 32, 8000, fun+samples, fun+samples, fft);
+	}
+
+	return 0;
+}
+
+static struct ast_custom_function pitch_shift_function = {
+	.name = "PITCH_SHIFT",
+	.write = pitchshift_helper,
+};
+
+static int unload_module(void)
+{
+	return ast_custom_function_unregister(&pitch_shift_function);
+}
+
+static int load_module(void)
+{
+	int res = ast_custom_function_register(&pitch_shift_function);
+	return res ? AST_MODULE_LOAD_DECLINE : AST_MODULE_LOAD_SUCCESS;
+}
+
+AST_MODULE_INFO_STANDARD(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");

Propchange: team/dvossel/funk_effects/funcs/func_effects.c
------------------------------------------------------------------------------
    svn:eol-style = native

Propchange: team/dvossel/funk_effects/funcs/func_effects.c
------------------------------------------------------------------------------
    svn:keywords = Author Date Id Revision

Propchange: team/dvossel/funk_effects/funcs/func_effects.c
------------------------------------------------------------------------------
    svn:mime-type = text/plain




More information about the asterisk-commits mailing list