Wed Jan 8 2020 09:49:47

Asterisk developer's documentation


func_pitchshift.c
Go to the documentation of this file.
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_FILE_VERSION(__FILE__, "$Revision: 411313 $")
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 {
138  long gRover;
140 };
141 
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 
158  ast_free(shift);
159 };
160 
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  (f->frametype != AST_FRAME_VOICE) ||
177  ((f->subclass.codec != AST_FORMAT_SLINEAR) &&
179  return -1;
180  }
181 
182  if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
183  return -1;
184  }
185 
186  shift = datastore->data;
187 
188  if (direction == AST_AUDIOHOOK_DIRECTION_WRITE) {
189  pitch_shift(f, shift->tx.shift_amount, &shift->tx);
190  } else {
191  pitch_shift(f, shift->rx.shift_amount, &shift->rx);
192  }
193 
194  return 0;
195 }
196 
197 static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
198 {
199  struct ast_datastore *datastore = NULL;
200  struct pitchshift_data *shift = NULL;
201  int new = 0;
202  float amount = 0;
203 
204  if (!chan) {
205  ast_log(LOG_WARNING, "No channel was provided to %s function.\n", cmd);
206  return -1;
207  }
208 
209  ast_channel_lock(chan);
210  if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
211  ast_channel_unlock(chan);
212 
213  if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
214  return 0;
215  }
216  if (!(shift = ast_calloc(1, sizeof(*shift)))) {
217  ast_datastore_free(datastore);
218  return 0;
219  }
220 
223  datastore->data = shift;
224  new = 1;
225  } else {
226  ast_channel_unlock(chan);
227  shift = datastore->data;
228  }
229 
230 
231  if (!strcasecmp(value, "highest")) {
232  amount = HIGHEST;
233  } else if (!strcasecmp(value, "higher")) {
234  amount = HIGHER;
235  } else if (!strcasecmp(value, "high")) {
236  amount = HIGH;
237  } else if (!strcasecmp(value, "lowest")) {
238  amount = LOWEST;
239  } else if (!strcasecmp(value, "lower")) {
240  amount = LOWER;
241  } else if (!strcasecmp(value, "low")) {
242  amount = LOW;
243  } else {
244  if (!sscanf(value, "%30f", &amount) || (amount <= 0) || (amount > 4)) {
245  goto cleanup_error;
246  }
247  }
248 
249  if (!strcasecmp(data, "rx")) {
250  shift->rx.shift_amount = amount;
251  } else if (!strcasecmp(data, "tx")) {
252  shift->tx.shift_amount = amount;
253  } else if (!strcasecmp(data, "both")) {
254  shift->rx.shift_amount = amount;
255  shift->tx.shift_amount = amount;
256  } else {
257  goto cleanup_error;
258  }
259 
260  if (new) {
261  ast_channel_lock(chan);
262  ast_channel_datastore_add(chan, datastore);
263  ast_channel_unlock(chan);
264  ast_audiohook_attach(chan, &shift->audiohook);
265  }
266 
267  return 0;
268 
269 cleanup_error:
270 
271  ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
272  if (new) {
273  ast_datastore_free(datastore);
274  }
275  return -1;
276 }
277 
278 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
279 {
280  float wr, wi, arg, *p1, *p2, temp;
281  float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
282  long i, bitm, j, le, le2, k;
283 
284  for (i = 2; i < 2 * fft_frame_size - 2; i += 2) {
285  for (bitm = 2, j = 0; bitm < 2 * fft_frame_size; bitm <<= 1) {
286  if (i & bitm) {
287  j++;
288  }
289  j <<= 1;
290  }
291  if (i < j) {
292  p1 = fft_buffer + i; p2 = fft_buffer + j;
293  temp = *p1; *(p1++) = *p2;
294  *(p2++) = temp; temp = *p1;
295  *p1 = *p2; *p2 = temp;
296  }
297  }
298  for (k = 0, le = 2; k < (long) (log(fft_frame_size) / log(2.) + .5); k++) {
299  le <<= 1;
300  le2 = le>>1;
301  ur = 1.0;
302  ui = 0.0;
303  arg = M_PI / (le2>>1);
304  wr = cos(arg);
305  wi = sign * sin(arg);
306  for (j = 0; j < le2; j += 2) {
307  p1r = fft_buffer+j; p1i = p1r + 1;
308  p2r = p1r + le2; p2i = p2r + 1;
309  for (i = j; i < 2 * fft_frame_size; i += le) {
310  tr = *p2r * ur - *p2i * ui;
311  ti = *p2r * ui + *p2i * ur;
312  *p2r = *p1r - tr; *p2i = *p1i - ti;
313  *p1r += tr; *p1i += ti;
314  p1r += le; p1i += le;
315  p2r += le; p2i += le;
316  }
317  tr = ur * wr - ui * wi;
318  ui = ur * wi + ui * wr;
319  ur = tr;
320  }
321  }
322 }
323 
324 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)
325 {
326  float *in_fifo = fft_data->in_fifo;
327  float *out_fifo = fft_data->out_fifo;
328  float *fft_worksp = fft_data->fft_worksp;
329  float *last_phase = fft_data->last_phase;
330  float *sum_phase = fft_data->sum_phase;
331  float *output_accum = fft_data->output_accum;
332  float *ana_freq = fft_data->ana_freq;
333  float *ana_magn = fft_data->ana_magn;
334  float *syn_freq = fft_data->syn_freq;
335  float *sys_magn = fft_data->sys_magn;
336 
337  double magn, phase, tmp, window, real, imag;
338  double freq_per_bin, expct;
339  long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
340 
341  /* set up some handy variables */
342  fft_frame_size2 = fft_frame_size / 2;
343  step_size = fft_frame_size / osamp;
344  freq_per_bin = sample_rate / (double) fft_frame_size;
345  expct = 2. * M_PI * (double) step_size / (double) fft_frame_size;
346  in_fifo_latency = fft_frame_size-step_size;
347 
348  if (fft_data->gRover == 0) {
349  fft_data->gRover = in_fifo_latency;
350  }
351 
352  /* main processing loop */
353  for (i = 0; i < num_samps_to_process; i++){
354 
355  /* As long as we have not yet collected enough data just read in */
356  in_fifo[fft_data->gRover] = indata[i];
357  outdata[i] = out_fifo[fft_data->gRover - in_fifo_latency];
358  fft_data->gRover++;
359 
360  /* now we have enough data for processing */
361  if (fft_data->gRover >= fft_frame_size) {
362  fft_data->gRover = in_fifo_latency;
363 
364  /* do windowing and re,im interleave */
365  for (k = 0; k < fft_frame_size;k++) {
366  window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
367  fft_worksp[2*k] = in_fifo[k] * window;
368  fft_worksp[2*k+1] = 0.;
369  }
370 
371  /* ***************** ANALYSIS ******************* */
372  /* do transform */
373  smb_fft(fft_worksp, fft_frame_size, -1);
374 
375  /* this is the analysis step */
376  for (k = 0; k <= fft_frame_size2; k++) {
377 
378  /* de-interlace FFT buffer */
379  real = fft_worksp[2*k];
380  imag = fft_worksp[2*k+1];
381 
382  /* compute magnitude and phase */
383  magn = 2. * sqrt(real * real + imag * imag);
384  phase = atan2(imag, real);
385 
386  /* compute phase difference */
387  tmp = phase - last_phase[k];
388  last_phase[k] = phase;
389 
390  /* subtract expected phase difference */
391  tmp -= (double) k * expct;
392 
393  /* map delta phase into +/- Pi interval */
394  qpd = tmp / M_PI;
395  if (qpd >= 0) {
396  qpd += qpd & 1;
397  } else {
398  qpd -= qpd & 1;
399  }
400  tmp -= M_PI * (double) qpd;
401 
402  /* get deviation from bin frequency from the +/- Pi interval */
403  tmp = osamp * tmp / (2. * M_PI);
404 
405  /* compute the k-th partials' true frequency */
406  tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
407 
408  /* store magnitude and true frequency in analysis arrays */
409  ana_magn[k] = magn;
410  ana_freq[k] = tmp;
411 
412  }
413 
414  /* ***************** PROCESSING ******************* */
415  /* this does the actual pitch shifting */
416  memset(sys_magn, 0, fft_frame_size * sizeof(float));
417  memset(syn_freq, 0, fft_frame_size * sizeof(float));
418  for (k = 0; k <= fft_frame_size2; k++) {
419  index = k * pitchShift;
420  if (index <= fft_frame_size2) {
421  sys_magn[index] += ana_magn[k];
422  syn_freq[index] = ana_freq[k] * pitchShift;
423  }
424  }
425 
426  /* ***************** SYNTHESIS ******************* */
427  /* this is the synthesis step */
428  for (k = 0; k <= fft_frame_size2; k++) {
429 
430  /* get magnitude and true frequency from synthesis arrays */
431  magn = sys_magn[k];
432  tmp = syn_freq[k];
433 
434  /* subtract bin mid frequency */
435  tmp -= (double) k * freq_per_bin;
436 
437  /* get bin deviation from freq deviation */
438  tmp /= freq_per_bin;
439 
440  /* take osamp into account */
441  tmp = 2. * M_PI * tmp / osamp;
442 
443  /* add the overlap phase advance back in */
444  tmp += (double) k * expct;
445 
446  /* accumulate delta phase to get bin phase */
447  sum_phase[k] += tmp;
448  phase = sum_phase[k];
449 
450  /* get real and imag part and re-interleave */
451  fft_worksp[2*k] = magn * cos(phase);
452  fft_worksp[2*k+1] = magn * sin(phase);
453  }
454 
455  /* zero negative frequencies */
456  for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
457  fft_worksp[k] = 0.;
458  }
459 
460  /* do inverse transform */
461  smb_fft(fft_worksp, fft_frame_size, 1);
462 
463  /* do windowing and add to output accumulator */
464  for (k = 0; k < fft_frame_size; k++) {
465  window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
466  output_accum[k] += 2. * window * fft_worksp[2*k] / (fft_frame_size2 * osamp);
467  }
468  for (k = 0; k < step_size; k++) {
469  out_fifo[k] = output_accum[k];
470  }
471 
472  /* shift accumulator */
473  memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
474 
475  /* move input FIFO */
476  for (k = 0; k < in_fifo_latency; k++) {
477  in_fifo[k] = in_fifo[k+step_size];
478  }
479  }
480  }
481 }
482 
483 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
484 {
485  int16_t *fun = (int16_t *) f->data.ptr;
486  int samples;
487 
488  /* an amount of 1 has no effect */
489  if (!amount || amount == 1 || !fun || (f->samples % 32)) {
490  return 0;
491  }
492  for (samples = 0; samples < f->samples; samples += 32) {
493  smb_pitch_shift(amount, 32, MAX_FRAME_LENGTH, 32, ast_format_rate(f->subclass.codec), fun+samples, fun+samples, fft);
494  }
495 
496  return 0;
497 }
498 
500  .name = "PITCH_SHIFT",
501  .write = pitchshift_helper,
502 };
503 
504 static int unload_module(void)
505 {
506  return ast_custom_function_unregister(&pitch_shift_function);
507 }
508 
509 static int load_module(void)
510 {
511  int res = ast_custom_function_register(&pitch_shift_function);
513 }
514 
515 AST_MODULE_INFO_STANDARD(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");
#define MAX_FRAME_LENGTH
const char * type
Definition: datastore.h:32
union ast_frame_subclass subclass
Definition: frame.h:146
#define ast_channel_lock(chan)
Definition: channel.h:2466
Main Channel structure associated with a channel.
Definition: channel.h:742
#define AST_MODULE_INFO_STANDARD(keystr, desc)
Definition: module.h:396
Asterisk main include file. File version handling, generic pbx functions.
struct ast_audiohook audiohook
float out_fifo[MAX_FRAME_LENGTH]
float ana_magn[MAX_FRAME_LENGTH]
void * ptr
Definition: frame.h:160
float output_accum[2 *MAX_FRAME_LENGTH]
int ast_audiohook_init(struct ast_audiohook *audiohook, enum ast_audiohook_type type, const char *source)
Initialize an audiohook structure.
Definition: audiohook.c:64
#define LOG_WARNING
Definition: logger.h:144
Audiohooks Architecture.
static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
float ana_freq[MAX_FRAME_LENGTH]
Structure for a data store type.
Definition: datastore.h:31
static int step_size(struct g726_state *state_ptr)
Definition: codec_g726.c:249
int ast_audiohook_attach(struct ast_channel *chan, struct ast_audiohook *audiohook)
Attach audiohook to channel.
Definition: audiohook.c:348
static int pitchshift_cb(struct ast_audiohook *audiohook, struct ast_channel *chan, struct ast_frame *f, enum ast_audiohook_direction direction)
static force_inline int ast_format_rate(format_t format)
Get the sample rate for a given format.
Definition: frame.h:809
Structure for a data store object.
Definition: datastore.h:54
float sum_phase[MAX_FRAME_LENGTH/2+1]
struct ast_datastore * ast_channel_datastore_find(struct ast_channel *chan, const struct ast_datastore_info *info, const char *uid)
Find a datastore on a channel.
Definition: channel.c:2604
float in_fifo[MAX_FRAME_LENGTH]
format_t codec
Definition: frame.h:137
int value
Definition: syslog.c:39
int ast_audiohook_destroy(struct ast_audiohook *audiohook)
Destroys an audiohook structure.
Definition: audiohook.c:96
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)
#define HIGH
int ast_custom_function_unregister(struct ast_custom_function *acf)
Unregister a custom function.
Definition: pbx.c:3814
int ast_datastore_free(struct ast_datastore *datastore)
Free a data store object.
Definition: datastore.c:65
Utility functions.
#define M_PI
ast_audiohook_manipulate_callback manipulate_callback
Definition: audiohook.h:116
float fft_worksp[2 *MAX_FRAME_LENGTH]
General Asterisk PBX channel definitions.
Data structure associated with a custom dialplan function.
Definition: pbx.h:95
float syn_freq[MAX_FRAME_LENGTH]
static void destroy_callback(void *data)
static struct ast_custom_function pitch_shift_function
float shift_amount
#define AST_FORMAT_SLINEAR16
Definition: frame.h:272
Core PBX routines and definitions.
#define LOW
#define HIGHER
#define HIGHEST
#define LOG_ERROR
Definition: logger.h:155
struct fft_data tx
#define LOWER
#define LOWEST
void ast_log(int level, const char *file, int line, const char *function, const char *fmt,...)
Used for sending a log message This is the standard logger function. Probably the only way you will i...
Definition: logger.c:1207
static int unload_module(void)
struct ast_datastore * ast_datastore_alloc(const struct ast_datastore_info *info, const char *uid)
Definition: datastore.c:98
static int load_module(void)
#define ast_channel_unlock(chan)
Definition: channel.h:2467
#define ast_free(a)
Definition: astmm.h:97
float sys_magn[MAX_FRAME_LENGTH]
static struct ast_format f[]
Definition: format_g726.c:181
if(yyss+yystacksize-1<=yyssp)
Definition: ast_expr2.c:1874
ast_audiohook_direction
Definition: audiohook.h:49
static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft_data)
static struct ast_datastore_info pitchshift_datastore
void * data
Definition: datastore.h:56
#define AST_FORMAT_SLINEAR
Definition: frame.h:254
#define ast_calloc(a, b)
Definition: astmm.h:82
Data structure associated with a single frame of data.
Definition: frame.h:142
const char * name
Definition: pbx.h:96
enum ast_audiohook_status status
Definition: audiohook.h:107
enum ast_frame_type frametype
Definition: frame.h:144
float last_phase[MAX_FRAME_LENGTH/2+1]
static unsigned int cos
Definition: chan_h323.c:147
struct fft_data rx
#define ASTERISK_GPL_KEY
The text the key() function should return.
Definition: module.h:38
union ast_frame::@172 data
int ast_channel_datastore_add(struct ast_channel *chan, struct ast_datastore *datastore)
Add a datastore to a channel.
Definition: channel.c:2590
static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
#define ast_custom_function_register(acf)
Register a custom function.
Definition: pbx.h:1164
#define ASTERISK_FILE_VERSION(file, version)
Register/unregister a source code file with the core.
Definition: asterisk.h:180
int samples
Definition: frame.h:150