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_FILE_VERSION(__FILE__, "$Revision$")
  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                 (f->frametype != AST_FRAME_VOICE) ||
 177                 !(ast_format_is_slinear(&f->subclass.format))) {
 178                 return -1;
 179         }
 180
 181         if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
 182                 return -1;
 183         }
 184
 185         shift = datastore->data;
 186
 187         if (direction == AST_AUDIOHOOK_DIRECTION_WRITE) {
 188                 pitch_shift(f, shift->tx.shift_amount, &shift->tx);
 189         } else {
 190                 pitch_shift(f, shift->rx.shift_amount, &shift->rx);
 191         }
 192
 193         return 0;
 194 }
 195
 196 static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
 197 {
 198         struct ast_datastore *datastore = NULL;
 199         struct pitchshift_data *shift = NULL;
 200         int new = 0;
 201         float amount = 0;
 202
 203         ast_channel_lock(chan);
 204         if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
 205                 ast_channel_unlock(chan);
 206
 207                 if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
 208                         return 0;
 209                 }
 210                 if (!(shift = ast_calloc(1, sizeof(*shift)))) {
 211                         ast_datastore_free(datastore);
 212                         return 0;
 213                 }
 214
 215                 ast_audiohook_init(&shift->audiohook, AST_AUDIOHOOK_TYPE_MANIPULATE, "pitch_shift", AST_AUDIOHOOK_MANIPULATE_ALL_RATES);
 216                 shift->audiohook.manipulate_callback = pitchshift_cb;
 217                 datastore->data = shift;
 218                 new = 1;
 219         } else {
 220                 ast_channel_unlock(chan);
 221                 shift = datastore->data;
 222         }
 223
 224
 225         if (!strcasecmp(value, "highest")) {
 226                 amount = HIGHEST;
 227         } else if (!strcasecmp(value, "higher")) {
 228                 amount = HIGHER;
 229         } else if (!strcasecmp(value, "high")) {
 230                 amount = HIGH;
 231         } else if (!strcasecmp(value, "lowest")) {
 232                 amount = LOWEST;
 233         } else if (!strcasecmp(value, "lower")) {
 234                 amount = LOWER;
 235         } else if (!strcasecmp(value, "low")) {
 236                 amount = LOW;
 237         } else {
 238                 if (!sscanf(value, "%30f", &amount) || (amount <= 0) || (amount > 4)) {
 239                         goto cleanup_error;
 240                 }
 241         }
 242
 243         if (!strcasecmp(data, "rx")) {
 244                 shift->rx.shift_amount = amount;
 245         } else if (!strcasecmp(data, "tx")) {
 246                 shift->tx.shift_amount = amount;
 247         } else if (!strcasecmp(data, "both")) {
 248                 shift->rx.shift_amount = amount;
 249                 shift->tx.shift_amount = amount;
 250         } else {
 251                 goto cleanup_error;
 252         }
 253
 254         if (new) {
 255                 ast_channel_lock(chan);
 256                 ast_channel_datastore_add(chan, datastore);
 257                 ast_channel_unlock(chan);
 258                 ast_audiohook_attach(chan, &shift->audiohook);
 259         }
 260
 261         return 0;
 262
 263 cleanup_error:
 264
 265         ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
 266         if (new) {
 267                 ast_datastore_free(datastore);
 268         }
 269         return -1;
 270 }
 271
 272 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
 273 {
 274         float wr, wi, arg, *p1, *p2, temp;
 275         float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
 276         long i, bitm, j, le, le2, k;
 277
 278         for (i = 2; i < 2 * fft_frame_size - 2; i += 2) {
 279                 for (bitm = 2, j = 0; bitm < 2 * fft_frame_size; bitm <<= 1) {
 280                         if (i & bitm) {
 281                                 j++;
 282                         }
 283                         j <<= 1;
 284                 }
 285                 if (i < j) {
 286                         p1 = fft_buffer + i; p2 = fft_buffer + j;
 287                         temp = *p1; *(p1++) = *p2;
 288                         *(p2++) = temp; temp = *p1;
 289                         *p1 = *p2; *p2 = temp;
 290                 }
 291         }
 292         for (k = 0, le = 2; k < (long) (log(fft_frame_size) / log(2.) + .5); k++) {
 293                 le <<= 1;
 294                 le2 = le>>1;
 295                 ur = 1.0;
 296                 ui = 0.0;
 297                 arg = M_PI / (le2>>1);
 298                 wr = cos(arg);
 299                 wi = sign * sin(arg);
 300                 for (j = 0; j < le2; j += 2) {
 301                         p1r = fft_buffer+j; p1i = p1r + 1;
 302                         p2r = p1r + le2; p2i = p2r + 1;
 303                         for (i = j; i < 2 * fft_frame_size; i += le) {
 304                                 tr = *p2r * ur - *p2i * ui;
 305                                 ti = *p2r * ui + *p2i * ur;
 306                                 *p2r = *p1r - tr; *p2i = *p1i - ti;
 307                                 *p1r += tr; *p1i += ti;
 308                                 p1r += le; p1i += le;
 309                                 p2r += le; p2i += le;
 310                         }
 311                         tr = ur * wr - ui * wi;
 312                         ui = ur * wi + ui * wr;
 313                         ur = tr;
 314                 }
 315         }
 316 }
 317
 318 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)
 319 {
 320         float *in_fifo = fft_data->in_fifo;
 321         float *out_fifo = fft_data->out_fifo;
 322         float *fft_worksp = fft_data->fft_worksp;
 323         float *last_phase = fft_data->last_phase;
 324         float *sum_phase = fft_data->sum_phase;
 325         float *output_accum = fft_data->output_accum;
 326         float *ana_freq = fft_data->ana_freq;
 327         float *ana_magn = fft_data->ana_magn;
 328         float *syn_freq = fft_data->syn_freq;
 329         float *sys_magn = fft_data->sys_magn;
 330
 331         double magn, phase, tmp, window, real, imag;
 332         double freq_per_bin, expct;
 333         long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
 334
 335         /* set up some handy variables */
 336         fft_frame_size2 = fft_frame_size / 2;
 337         step_size = fft_frame_size / osamp;
 338         freq_per_bin = sample_rate / (double) fft_frame_size;
 339         expct = 2. * M_PI * (double) step_size / (double) fft_frame_size;
 340         in_fifo_latency = fft_frame_size-step_size;
 341
 342         if (fft_data->gRover == 0) {
 343                 fft_data->gRover = in_fifo_latency;
 344         }
 345
 346         /* main processing loop */
 347         for (i = 0; i < num_samps_to_process; i++){
 348
 349                 /* As long as we have not yet collected enough data just read in */
 350                 in_fifo[fft_data->gRover] = indata[i];
 351                 outdata[i] = out_fifo[fft_data->gRover - in_fifo_latency];
 352                 fft_data->gRover++;
 353
 354                 /* now we have enough data for processing */
 355                 if (fft_data->gRover >= fft_frame_size) {
 356                         fft_data->gRover = in_fifo_latency;
 357
 358                         /* do windowing and re,im interleave */
 359                         for (k = 0; k < fft_frame_size;k++) {
 360                                 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
 361                                 fft_worksp[2*k] = in_fifo[k] * window;
 362                                 fft_worksp[2*k+1] = 0.;
 363                         }
 364
 365                         /* ***************** ANALYSIS ******************* */
 366                         /* do transform */
 367                         smb_fft(fft_worksp, fft_frame_size, -1);
 368
 369                         /* this is the analysis step */
 370                         for (k = 0; k <= fft_frame_size2; k++) {
 371
 372                                 /* de-interlace FFT buffer */
 373                                 real = fft_worksp[2*k];
 374                                 imag = fft_worksp[2*k+1];
 375
 376                                 /* compute magnitude and phase */
 377                                 magn = 2. * sqrt(real * real + imag * imag);
 378                                 phase = atan2(imag, real);
 379
 380                                 /* compute phase difference */
 381                                 tmp = phase - last_phase[k];
 382                                 last_phase[k] = phase;
 383
 384                                 /* subtract expected phase difference */
 385                                 tmp -= (double) k * expct;
 386
 387                                 /* map delta phase into +/- Pi interval */
 388                                 qpd = tmp / M_PI;
 389                                 if (qpd >= 0) {
 390                                         qpd += qpd & 1;
 391                                 } else {
 392                                         qpd -= qpd & 1;
 393                                 }
 394                                 tmp -= M_PI * (double) qpd;
 395
 396                                 /* get deviation from bin frequency from the +/- Pi interval */
 397                                 tmp = osamp * tmp / (2. * M_PI);
 398
 399                                 /* compute the k-th partials' true frequency */
 400                                 tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
 401
 402                                 /* store magnitude and true frequency in analysis arrays */
 403                                 ana_magn[k] = magn;
 404                                 ana_freq[k] = tmp;
 405
 406                         }
 407
 408                         /* ***************** PROCESSING ******************* */
 409                         /* this does the actual pitch shifting */
 410                         memset(sys_magn, 0, fft_frame_size * sizeof(float));
 411                         memset(syn_freq, 0, fft_frame_size * sizeof(float));
 412                         for (k = 0; k <= fft_frame_size2; k++) {
 413                                 index = k * pitchShift;
 414                                 if (index <= fft_frame_size2) {
 415                                         sys_magn[index] += ana_magn[k];
 416                                         syn_freq[index] = ana_freq[k] * pitchShift;
 417                                 }
 418                         }
 419
 420                         /* ***************** SYNTHESIS ******************* */
 421                         /* this is the synthesis step */
 422                         for (k = 0; k <= fft_frame_size2; k++) {
 423
 424                                 /* get magnitude and true frequency from synthesis arrays */
 425                                 magn = sys_magn[k];
 426                                 tmp = syn_freq[k];
 427
 428                                 /* subtract bin mid frequency */
 429                                 tmp -= (double) k * freq_per_bin;
 430
 431                                 /* get bin deviation from freq deviation */
 432                                 tmp /= freq_per_bin;
 433
 434                                 /* take osamp into account */
 435                                 tmp = 2. * M_PI * tmp / osamp;
 436
 437                                 /* add the overlap phase advance back in */
 438                                 tmp += (double) k * expct;
 439
 440                                 /* accumulate delta phase to get bin phase */
 441                                 sum_phase[k] += tmp;
 442                                 phase = sum_phase[k];
 443
 444                                 /* get real and imag part and re-interleave */
 445                                 fft_worksp[2*k] = magn * cos(phase);
 446                                 fft_worksp[2*k+1] = magn * sin(phase);
 447                         }
 448
 449                         /* zero negative frequencies */
 450                         for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
 451                                 fft_worksp[k] = 0.;
 452                         }
 453
 454                         /* do inverse transform */
 455                         smb_fft(fft_worksp, fft_frame_size, 1);
 456
 457                         /* do windowing and add to output accumulator */
 458                         for (k = 0; k < fft_frame_size; k++) {
 459                                 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
 460                                 output_accum[k] += 2. * window * fft_worksp[2*k] / (fft_frame_size2 * osamp);
 461                         }
 462                         for (k = 0; k < step_size; k++) {
 463                                 out_fifo[k] = output_accum[k];
 464                         }
 465
 466                         /* shift accumulator */
 467                         memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
 468
 469                         /* move input FIFO */
 470                         for (k = 0; k < in_fifo_latency; k++) {
 471                                 in_fifo[k] = in_fifo[k+step_size];
 472                         }
 473                 }
 474         }
 475 }
 476
 477 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
 478 {
 479         int16_t *fun = (int16_t *) f->data.ptr;
 480         int samples;
 481
 482         /* an amount of 1 has no effect */
 483         if (!amount || amount == 1 || !fun || (f->samples % 32)) {
 484                 return 0;
 485         }
 486         for (samples = 0; samples < f->samples; samples += 32) {
 487                 smb_pitch_shift(amount, 32, MAX_FRAME_LENGTH, 32, ast_format_rate(&f->subclass.format), fun+samples, fun+samples, fft);
 488         }
 489
 490         return 0;
 491 }
 492
 493 static struct ast_custom_function pitch_shift_function = {
 494         .name = "PITCH_SHIFT",
 495         .write = pitchshift_helper,
 496 };
 497
 498 static int unload_module(void)
 499 {
 500         return ast_custom_function_unregister(&pitch_shift_function);
 501 }
 502
 503 static int load_module(void)
 504 {
 505         int res = ast_custom_function_register(&pitch_shift_function);
 506         return res ? AST_MODULE_LOAD_DECLINE : AST_MODULE_LOAD_SUCCESS;
 507 }
 508
 509 AST_MODULE_INFO_STANDARD(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");