astobj2: Add backtrace to log_bad_ao2.
[asterisk/asterisk.git] / funcs / func_pitchshift.c
1 /*
2  * Asterisk -- An open source telephony toolkit.
3  *
4  * Copyright (C) 2010, Digium, Inc.
5  *
6  * David Vossel <dvossel@digium.com>
7  *
8  * See http://www.asterisk.org for more information about
9  * the Asterisk project. Please do not directly contact
10  * any of the maintainers of this project for assistance;
11  * the project provides a web site, mailing lists and IRC
12  * channels for your use.
13  *
14  * This program is free software, distributed under the terms of
15  * the GNU General Public License Version 2. See the LICENSE file
16  * at the top of the source tree.
17  */
18
19 /*! \file
20  *
21  * \brief Pitch Shift Audio Effect
22  *
23  * \author David Vossel <dvossel@digium.com>
24  *
25  * \ingroup functions
26  */
27
28 /************************* SMB FUNCTION LICENSE *********************************
29 *
30 * SYNOPSIS: Routine for doing pitch shifting while maintaining
31 * duration using the Short Time Fourier Transform.
32 *
33 * DESCRIPTION: The routine takes a pitchShift factor value which is between 0.5
34 * (one octave down) and 2. (one octave up). A value of exactly 1 does not change
35 * the pitch. num_samps_to_process tells the routine how many samples in indata[0...
36 * num_samps_to_process-1] should be pitch shifted and moved to outdata[0 ...
37 * num_samps_to_process-1]. The two buffers can be identical (ie. it can process the
38 * data in-place). fft_frame_size defines the FFT frame size used for the
39 * processing. Typical values are 1024, 2048 and 4096. It may be any value <=
40 * MAX_FRAME_LENGTH but it MUST be a power of 2. osamp is the STFT
41 * oversampling factor which also determines the overlap between adjacent STFT
42 * frames. It should at least be 4 for moderate scaling ratios. A value of 32 is
43 * recommended for best quality. sampleRate takes the sample rate for the signal
44 * in unit Hz, ie. 44100 for 44.1 kHz audio. The data passed to the routine in
45 * indata[] should be in the range [-1.0, 1.0), which is also the output range
46 * for the data, make sure you scale the data accordingly (for 16bit signed integers
47 * you would have to divide (and multiply) by 32768).
48 *
49 * COPYRIGHT 1999-2009 Stephan M. Bernsee <smb [AT] dspdimension [DOT] com>
50 *
51 *                        The Wide Open License (WOL)
52 *
53 * Permission to use, copy, modify, distribute and sell this software and its
54 * documentation for any purpose is hereby granted without fee, provided that
55 * the above copyright notice and this license appear in all source copies.
56 * THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY OF
57 * ANY KIND. See http://www.dspguru.com/wol.htm for more information.
58 *
59 *****************************************************************************/
60
61 /*** MODULEINFO
62         <support_level>extended</support_level>
63  ***/
64
65 #include "asterisk.h"
66
67 ASTERISK_REGISTER_FILE()
68
69 #include "asterisk/module.h"
70 #include "asterisk/channel.h"
71 #include "asterisk/pbx.h"
72 #include "asterisk/utils.h"
73 #include "asterisk/audiohook.h"
74 #include <math.h>
75
76 /*** DOCUMENTATION
77         <function name="PITCH_SHIFT" language="en_US">
78                 <synopsis>
79                         Pitch shift both tx and rx audio streams on a channel.
80                 </synopsis>
81                 <syntax>
82                         <parameter name="channel direction" required="true">
83                                 <para>Direction can be either <literal>rx</literal>, <literal>tx</literal>, or
84                                 <literal>both</literal>.  The direction can either be set to a valid floating
85                                 point number between 0.1 and 4.0 or one of the enum values listed below. A value
86                                 of 1.0 has no effect.  Greater than 1 raises the pitch. Lower than 1 lowers
87                                 the pitch.</para>
88
89                                 <para>The pitch amount can also be set by the following values</para>
90                                 <enumlist>
91                                         <enum name = "highest" />
92                                         <enum name = "higher" />
93                                         <enum name = "high" />
94                                         <enum name = "low" />
95                                         <enum name = "lower" />
96                                         <enum name = "lowest" />
97                                 </enumlist>
98                         </parameter>
99                 </syntax>
100                 <description>
101                         <para>Examples:</para>
102                         <para>exten => 1,1,Set(PITCH_SHIFT(tx)=highest); raises pitch an octave </para>
103                         <para>exten => 1,1,Set(PITCH_SHIFT(rx)=higher) ; raises pitch more </para>
104                         <para>exten => 1,1,Set(PITCH_SHIFT(both)=high)   ; raises pitch </para>
105                         <para>exten => 1,1,Set(PITCH_SHIFT(rx)=low)    ; lowers pitch </para>
106                         <para>exten => 1,1,Set(PITCH_SHIFT(tx)=lower)  ; lowers pitch more </para>
107                         <para>exten => 1,1,Set(PITCH_SHIFT(both)=lowest) ; lowers pitch an octave </para>
108
109                         <para>exten => 1,1,Set(PITCH_SHIFT(rx)=0.8)    ; lowers pitch </para>
110                         <para>exten => 1,1,Set(PITCH_SHIFT(tx)=1.5)    ; raises pitch </para>
111                 </description>
112         </function>
113  ***/
114
115 #ifndef M_PI
116 #define M_PI 3.14159265358979323846
117 #endif
118 #define MAX_FRAME_LENGTH 256
119
120 #define HIGHEST 2
121 #define HIGHER 1.5
122 #define HIGH 1.25
123 #define LOW .85
124 #define LOWER .7
125 #define LOWEST .5
126
127 struct fft_data {
128         float in_fifo[MAX_FRAME_LENGTH];
129         float out_fifo[MAX_FRAME_LENGTH];
130         float fft_worksp[2*MAX_FRAME_LENGTH];
131         float last_phase[MAX_FRAME_LENGTH/2+1];
132         float sum_phase[MAX_FRAME_LENGTH/2+1];
133         float output_accum[2*MAX_FRAME_LENGTH];
134         float ana_freq[MAX_FRAME_LENGTH];
135         float ana_magn[MAX_FRAME_LENGTH];
136         float syn_freq[MAX_FRAME_LENGTH];
137         float sys_magn[MAX_FRAME_LENGTH];
138         long gRover;
139         float shift_amount;
140 };
141
142 struct pitchshift_data {
143         struct ast_audiohook audiohook;
144
145         struct fft_data rx;
146         struct fft_data tx;
147 };
148
149 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign);
150 static void smb_pitch_shift(float pitchShift, long num_samps_to_process, long fft_frame_size, long osamp, float sample_rate, int16_t *indata, int16_t *outdata, struct fft_data *fft_data);
151 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft_data);
152
153 static void destroy_callback(void *data)
154 {
155         struct pitchshift_data *shift = data;
156
157         ast_audiohook_destroy(&shift->audiohook);
158         ast_free(shift);
159 };
160
161 static const struct ast_datastore_info pitchshift_datastore = {
162         .type = "pitchshift",
163         .destroy = destroy_callback
164 };
165
166 static int pitchshift_cb(struct ast_audiohook *audiohook, struct ast_channel *chan, struct ast_frame *f, enum ast_audiohook_direction direction)
167 {
168         struct ast_datastore *datastore = NULL;
169         struct pitchshift_data *shift = NULL;
170
171
172         if (!f) {
173                 return 0;
174         }
175         if (audiohook->status == AST_AUDIOHOOK_STATUS_DONE) {
176                 return -1;
177         }
178
179         if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
180                 return -1;
181         }
182
183         shift = datastore->data;
184
185         if (direction == AST_AUDIOHOOK_DIRECTION_WRITE) {
186                 pitch_shift(f, shift->tx.shift_amount, &shift->tx);
187         } else {
188                 pitch_shift(f, shift->rx.shift_amount, &shift->rx);
189         }
190
191         return 0;
192 }
193
194 static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
195 {
196         struct ast_datastore *datastore = NULL;
197         struct pitchshift_data *shift = NULL;
198         int new = 0;
199         float amount = 0;
200
201         if (!chan) {
202                 ast_log(LOG_WARNING, "No channel was provided to %s function.\n", cmd);
203                 return -1;
204         }
205
206         ast_channel_lock(chan);
207         if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
208                 ast_channel_unlock(chan);
209
210                 if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
211                         return 0;
212                 }
213                 if (!(shift = ast_calloc(1, sizeof(*shift)))) {
214                         ast_datastore_free(datastore);
215                         return 0;
216                 }
217
218                 ast_audiohook_init(&shift->audiohook, AST_AUDIOHOOK_TYPE_MANIPULATE, "pitch_shift", AST_AUDIOHOOK_MANIPULATE_ALL_RATES);
219                 shift->audiohook.manipulate_callback = pitchshift_cb;
220                 datastore->data = shift;
221                 new = 1;
222         } else {
223                 ast_channel_unlock(chan);
224                 shift = datastore->data;
225         }
226
227
228         if (!strcasecmp(value, "highest")) {
229                 amount = HIGHEST;
230         } else if (!strcasecmp(value, "higher")) {
231                 amount = HIGHER;
232         } else if (!strcasecmp(value, "high")) {
233                 amount = HIGH;
234         } else if (!strcasecmp(value, "lowest")) {
235                 amount = LOWEST;
236         } else if (!strcasecmp(value, "lower")) {
237                 amount = LOWER;
238         } else if (!strcasecmp(value, "low")) {
239                 amount = LOW;
240         } else {
241                 if (!sscanf(value, "%30f", &amount) || (amount <= 0) || (amount > 4)) {
242                         goto cleanup_error;
243                 }
244         }
245
246         if (!strcasecmp(data, "rx")) {
247                 shift->rx.shift_amount = amount;
248         } else if (!strcasecmp(data, "tx")) {
249                 shift->tx.shift_amount = amount;
250         } else if (!strcasecmp(data, "both")) {
251                 shift->rx.shift_amount = amount;
252                 shift->tx.shift_amount = amount;
253         } else {
254                 goto cleanup_error;
255         }
256
257         if (new) {
258                 ast_channel_lock(chan);
259                 ast_channel_datastore_add(chan, datastore);
260                 ast_channel_unlock(chan);
261                 ast_audiohook_attach(chan, &shift->audiohook);
262         }
263
264         return 0;
265
266 cleanup_error:
267
268         ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
269         if (new) {
270                 ast_datastore_free(datastore);
271         }
272         return -1;
273 }
274
275 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
276 {
277         float wr, wi, arg, *p1, *p2, temp;
278         float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
279         long i, bitm, j, le, le2, k;
280
281         for (i = 2; i < 2 * fft_frame_size - 2; i += 2) {
282                 for (bitm = 2, j = 0; bitm < 2 * fft_frame_size; bitm <<= 1) {
283                         if (i & bitm) {
284                                 j++;
285                         }
286                         j <<= 1;
287                 }
288                 if (i < j) {
289                         p1 = fft_buffer + i; p2 = fft_buffer + j;
290                         temp = *p1; *(p1++) = *p2;
291                         *(p2++) = temp; temp = *p1;
292                         *p1 = *p2; *p2 = temp;
293                 }
294         }
295         for (k = 0, le = 2; k < (long) (log(fft_frame_size) / log(2.) + .5); k++) {
296                 le <<= 1;
297                 le2 = le>>1;
298                 ur = 1.0;
299                 ui = 0.0;
300                 arg = M_PI / (le2>>1);
301                 wr = cos(arg);
302                 wi = sign * sin(arg);
303                 for (j = 0; j < le2; j += 2) {
304                         p1r = fft_buffer+j; p1i = p1r + 1;
305                         p2r = p1r + le2; p2i = p2r + 1;
306                         for (i = j; i < 2 * fft_frame_size; i += le) {
307                                 tr = *p2r * ur - *p2i * ui;
308                                 ti = *p2r * ui + *p2i * ur;
309                                 *p2r = *p1r - tr; *p2i = *p1i - ti;
310                                 *p1r += tr; *p1i += ti;
311                                 p1r += le; p1i += le;
312                                 p2r += le; p2i += le;
313                         }
314                         tr = ur * wr - ui * wi;
315                         ui = ur * wi + ui * wr;
316                         ur = tr;
317                 }
318         }
319 }
320
321 static void smb_pitch_shift(float pitchShift, long num_samps_to_process, long fft_frame_size, long osamp, float sample_rate, int16_t *indata, int16_t *outdata, struct fft_data *fft_data)
322 {
323         float *in_fifo = fft_data->in_fifo;
324         float *out_fifo = fft_data->out_fifo;
325         float *fft_worksp = fft_data->fft_worksp;
326         float *last_phase = fft_data->last_phase;
327         float *sum_phase = fft_data->sum_phase;
328         float *output_accum = fft_data->output_accum;
329         float *ana_freq = fft_data->ana_freq;
330         float *ana_magn = fft_data->ana_magn;
331         float *syn_freq = fft_data->syn_freq;
332         float *sys_magn = fft_data->sys_magn;
333
334         double magn, phase, tmp, window, real, imag;
335         double freq_per_bin, expct;
336         long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
337
338         /* set up some handy variables */
339         fft_frame_size2 = fft_frame_size / 2;
340         step_size = fft_frame_size / osamp;
341         freq_per_bin = sample_rate / (double) fft_frame_size;
342         expct = 2. * M_PI * (double) step_size / (double) fft_frame_size;
343         in_fifo_latency = fft_frame_size-step_size;
344
345         if (fft_data->gRover == 0) {
346                 fft_data->gRover = in_fifo_latency;
347         }
348
349         /* main processing loop */
350         for (i = 0; i < num_samps_to_process; i++){
351
352                 /* As long as we have not yet collected enough data just read in */
353                 in_fifo[fft_data->gRover] = indata[i];
354                 outdata[i] = out_fifo[fft_data->gRover - in_fifo_latency];
355                 fft_data->gRover++;
356
357                 /* now we have enough data for processing */
358                 if (fft_data->gRover >= fft_frame_size) {
359                         fft_data->gRover = in_fifo_latency;
360
361                         /* do windowing and re,im interleave */
362                         for (k = 0; k < fft_frame_size;k++) {
363                                 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
364                                 fft_worksp[2*k] = in_fifo[k] * window;
365                                 fft_worksp[2*k+1] = 0.;
366                         }
367
368                         /* ***************** ANALYSIS ******************* */
369                         /* do transform */
370                         smb_fft(fft_worksp, fft_frame_size, -1);
371
372                         /* this is the analysis step */
373                         for (k = 0; k <= fft_frame_size2; k++) {
374
375                                 /* de-interlace FFT buffer */
376                                 real = fft_worksp[2*k];
377                                 imag = fft_worksp[2*k+1];
378
379                                 /* compute magnitude and phase */
380                                 magn = 2. * sqrt(real * real + imag * imag);
381                                 phase = atan2(imag, real);
382
383                                 /* compute phase difference */
384                                 tmp = phase - last_phase[k];
385                                 last_phase[k] = phase;
386
387                                 /* subtract expected phase difference */
388                                 tmp -= (double) k * expct;
389
390                                 /* map delta phase into +/- Pi interval */
391                                 qpd = tmp / M_PI;
392                                 if (qpd >= 0) {
393                                         qpd += qpd & 1;
394                                 } else {
395                                         qpd -= qpd & 1;
396                                 }
397                                 tmp -= M_PI * (double) qpd;
398
399                                 /* get deviation from bin frequency from the +/- Pi interval */
400                                 tmp = osamp * tmp / (2. * M_PI);
401
402                                 /* compute the k-th partials' true frequency */
403                                 tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
404
405                                 /* store magnitude and true frequency in analysis arrays */
406                                 ana_magn[k] = magn;
407                                 ana_freq[k] = tmp;
408
409                         }
410
411                         /* ***************** PROCESSING ******************* */
412                         /* this does the actual pitch shifting */
413                         memset(sys_magn, 0, fft_frame_size * sizeof(float));
414                         memset(syn_freq, 0, fft_frame_size * sizeof(float));
415                         for (k = 0; k <= fft_frame_size2; k++) {
416                                 index = k * pitchShift;
417                                 if (index <= fft_frame_size2) {
418                                         sys_magn[index] += ana_magn[k];
419                                         syn_freq[index] = ana_freq[k] * pitchShift;
420                                 }
421                         }
422
423                         /* ***************** SYNTHESIS ******************* */
424                         /* this is the synthesis step */
425                         for (k = 0; k <= fft_frame_size2; k++) {
426
427                                 /* get magnitude and true frequency from synthesis arrays */
428                                 magn = sys_magn[k];
429                                 tmp = syn_freq[k];
430
431                                 /* subtract bin mid frequency */
432                                 tmp -= (double) k * freq_per_bin;
433
434                                 /* get bin deviation from freq deviation */
435                                 tmp /= freq_per_bin;
436
437                                 /* take osamp into account */
438                                 tmp = 2. * M_PI * tmp / osamp;
439
440                                 /* add the overlap phase advance back in */
441                                 tmp += (double) k * expct;
442
443                                 /* accumulate delta phase to get bin phase */
444                                 sum_phase[k] += tmp;
445                                 phase = sum_phase[k];
446
447                                 /* get real and imag part and re-interleave */
448                                 fft_worksp[2*k] = magn * cos(phase);
449                                 fft_worksp[2*k+1] = magn * sin(phase);
450                         }
451
452                         /* zero negative frequencies */
453                         for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
454                                 fft_worksp[k] = 0.;
455                         }
456
457                         /* do inverse transform */
458                         smb_fft(fft_worksp, fft_frame_size, 1);
459
460                         /* do windowing and add to output accumulator */
461                         for (k = 0; k < fft_frame_size; k++) {
462                                 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
463                                 output_accum[k] += 2. * window * fft_worksp[2*k] / (fft_frame_size2 * osamp);
464                         }
465                         for (k = 0; k < step_size; k++) {
466                                 out_fifo[k] = output_accum[k];
467                         }
468
469                         /* shift accumulator */
470                         memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
471
472                         /* move input FIFO */
473                         for (k = 0; k < in_fifo_latency; k++) {
474                                 in_fifo[k] = in_fifo[k+step_size];
475                         }
476                 }
477         }
478 }
479
480 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
481 {
482         int16_t *fun = (int16_t *) f->data.ptr;
483         int samples;
484
485         /* an amount of 1 has no effect */
486         if (!amount || amount == 1 || !fun || (f->samples % 32)) {
487                 return 0;
488         }
489         for (samples = 0; samples < f->samples; samples += 32) {
490                 smb_pitch_shift(amount, 32, MAX_FRAME_LENGTH, 32, ast_format_get_sample_rate(f->subclass.format), fun+samples, fun+samples, fft);
491         }
492
493         return 0;
494 }
495
496 static struct ast_custom_function pitch_shift_function = {
497         .name = "PITCH_SHIFT",
498         .write = pitchshift_helper,
499 };
500
501 static int unload_module(void)
502 {
503         return ast_custom_function_unregister(&pitch_shift_function);
504 }
505
506 static int load_module(void)
507 {
508         int res = ast_custom_function_register(&pitch_shift_function);
509         return res ? AST_MODULE_LOAD_DECLINE : AST_MODULE_LOAD_SUCCESS;
510 }
511
512 AST_MODULE_INFO_STANDARD_EXTENDED(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");
513