Wed Jan 27 20:02:11 2016

Asterisk developer's documentation


func_pitchshift.c

Go to the documentation of this file.
00001 /*
00002  * Asterisk -- An open source telephony toolkit.
00003  *
00004  * Copyright (C) 2010, Digium, Inc.
00005  *
00006  * David Vossel <dvossel@digium.com>
00007  *
00008  * See http://www.asterisk.org for more information about
00009  * the Asterisk project. Please do not directly contact
00010  * any of the maintainers of this project for assistance;
00011  * the project provides a web site, mailing lists and IRC
00012  * channels for your use.
00013  *
00014  * This program is free software, distributed under the terms of
00015  * the GNU General Public License Version 2. See the LICENSE file
00016  * at the top of the source tree.
00017  */
00018 
00019 /*! \file
00020  *
00021  * \brief Pitch Shift Audio Effect
00022  *
00023  * \author David Vossel <dvossel@digium.com>
00024  *
00025  * \ingroup functions
00026  */
00027 
00028 /************************* SMB FUNCTION LICENSE *********************************
00029 *
00030 * SYNOPSIS: Routine for doing pitch shifting while maintaining
00031 * duration using the Short Time Fourier Transform.
00032 *
00033 * DESCRIPTION: The routine takes a pitchShift factor value which is between 0.5
00034 * (one octave down) and 2. (one octave up). A value of exactly 1 does not change
00035 * the pitch. num_samps_to_process tells the routine how many samples in indata[0...
00036 * num_samps_to_process-1] should be pitch shifted and moved to outdata[0 ...
00037 * num_samps_to_process-1]. The two buffers can be identical (ie. it can process the
00038 * data in-place). fft_frame_size defines the FFT frame size used for the
00039 * processing. Typical values are 1024, 2048 and 4096. It may be any value <=
00040 * MAX_FRAME_LENGTH but it MUST be a power of 2. osamp is the STFT
00041 * oversampling factor which also determines the overlap between adjacent STFT
00042 * frames. It should at least be 4 for moderate scaling ratios. A value of 32 is
00043 * recommended for best quality. sampleRate takes the sample rate for the signal
00044 * in unit Hz, ie. 44100 for 44.1 kHz audio. The data passed to the routine in
00045 * indata[] should be in the range [-1.0, 1.0), which is also the output range
00046 * for the data, make sure you scale the data accordingly (for 16bit signed integers
00047 * you would have to divide (and multiply) by 32768).
00048 *
00049 * COPYRIGHT 1999-2009 Stephan M. Bernsee <smb [AT] dspdimension [DOT] com>
00050 *
00051 *                        The Wide Open License (WOL)
00052 *
00053 * Permission to use, copy, modify, distribute and sell this software and its
00054 * documentation for any purpose is hereby granted without fee, provided that
00055 * the above copyright notice and this license appear in all source copies.
00056 * THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY OF
00057 * ANY KIND. See http://www.dspguru.com/wol.htm for more information.
00058 *
00059 *****************************************************************************/
00060 
00061 /*** MODULEINFO
00062    <support_level>extended</support_level>
00063  ***/
00064 
00065 #include "asterisk.h"
00066 
00067 ASTERISK_FILE_VERSION(__FILE__, "$Revision: 411313 $")
00068 
00069 #include "asterisk/module.h"
00070 #include "asterisk/channel.h"
00071 #include "asterisk/pbx.h"
00072 #include "asterisk/utils.h"
00073 #include "asterisk/audiohook.h"
00074 #include <math.h>
00075 
00076 /*** DOCUMENTATION
00077    <function name="PITCH_SHIFT" language="en_US">
00078       <synopsis>
00079          Pitch shift both tx and rx audio streams on a channel.
00080       </synopsis>
00081       <syntax>
00082          <parameter name="channel direction" required="true">
00083             <para>Direction can be either <literal>rx</literal>, <literal>tx</literal>, or
00084             <literal>both</literal>.  The direction can either be set to a valid floating
00085             point number between 0.1 and 4.0 or one of the enum values listed below. A value
00086             of 1.0 has no effect.  Greater than 1 raises the pitch. Lower than 1 lowers
00087             the pitch.</para>
00088 
00089             <para>The pitch amount can also be set by the following values</para>
00090             <enumlist>
00091                <enum name = "highest" />
00092                <enum name = "higher" />
00093                <enum name = "high" />
00094                <enum name = "low" />
00095                <enum name = "lower" />
00096                <enum name = "lowest" />
00097             </enumlist>
00098          </parameter>
00099       </syntax>
00100       <description>
00101          <para>Examples:</para>
00102          <para>exten => 1,1,Set(PITCH_SHIFT(tx)=highest); raises pitch an octave </para>
00103          <para>exten => 1,1,Set(PITCH_SHIFT(rx)=higher) ; raises pitch more </para>
00104          <para>exten => 1,1,Set(PITCH_SHIFT(both)=high)   ; raises pitch </para>
00105          <para>exten => 1,1,Set(PITCH_SHIFT(rx)=low)    ; lowers pitch </para>
00106          <para>exten => 1,1,Set(PITCH_SHIFT(tx)=lower)  ; lowers pitch more </para>
00107          <para>exten => 1,1,Set(PITCH_SHIFT(both)=lowest) ; lowers pitch an octave </para>
00108 
00109          <para>exten => 1,1,Set(PITCH_SHIFT(rx)=0.8)    ; lowers pitch </para>
00110          <para>exten => 1,1,Set(PITCH_SHIFT(tx)=1.5)    ; raises pitch </para>
00111       </description>
00112    </function>
00113  ***/
00114 
00115 #ifndef M_PI
00116 #define M_PI 3.14159265358979323846
00117 #endif
00118 #define MAX_FRAME_LENGTH 256
00119 
00120 #define HIGHEST 2
00121 #define HIGHER 1.5
00122 #define HIGH 1.25
00123 #define LOW .85
00124 #define LOWER .7
00125 #define LOWEST .5
00126 
00127 struct fft_data {
00128    float in_fifo[MAX_FRAME_LENGTH];
00129    float out_fifo[MAX_FRAME_LENGTH];
00130    float fft_worksp[2*MAX_FRAME_LENGTH];
00131    float last_phase[MAX_FRAME_LENGTH/2+1];
00132    float sum_phase[MAX_FRAME_LENGTH/2+1];
00133    float output_accum[2*MAX_FRAME_LENGTH];
00134    float ana_freq[MAX_FRAME_LENGTH];
00135    float ana_magn[MAX_FRAME_LENGTH];
00136    float syn_freq[MAX_FRAME_LENGTH];
00137    float sys_magn[MAX_FRAME_LENGTH];
00138    long gRover;
00139    float shift_amount;
00140 };
00141 
00142 struct pitchshift_data {
00143    struct ast_audiohook audiohook;
00144 
00145    struct fft_data rx;
00146    struct fft_data tx;
00147 };
00148 
00149 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign);
00150 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);
00151 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft_data);
00152 
00153 static void destroy_callback(void *data)
00154 {
00155    struct pitchshift_data *shift = data;
00156 
00157    ast_audiohook_destroy(&shift->audiohook);
00158    ast_free(shift);
00159 };
00160 
00161 static const struct ast_datastore_info pitchshift_datastore = {
00162    .type = "pitchshift",
00163    .destroy = destroy_callback
00164 };
00165 
00166 static int pitchshift_cb(struct ast_audiohook *audiohook, struct ast_channel *chan, struct ast_frame *f, enum ast_audiohook_direction direction)
00167 {
00168    struct ast_datastore *datastore = NULL;
00169    struct pitchshift_data *shift = NULL;
00170 
00171 
00172    if (!f) {
00173       return 0;
00174    }
00175    if ((audiohook->status == AST_AUDIOHOOK_STATUS_DONE) ||
00176       (f->frametype != AST_FRAME_VOICE) ||
00177       ((f->subclass.codec != AST_FORMAT_SLINEAR) &&
00178       (f->subclass.codec != AST_FORMAT_SLINEAR16))) {
00179       return -1;
00180    }
00181 
00182    if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
00183       return -1;
00184    }
00185 
00186    shift = datastore->data;
00187 
00188    if (direction == AST_AUDIOHOOK_DIRECTION_WRITE) {
00189       pitch_shift(f, shift->tx.shift_amount, &shift->tx);
00190    } else {
00191       pitch_shift(f, shift->rx.shift_amount, &shift->rx);
00192    }
00193 
00194    return 0;
00195 }
00196 
00197 static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
00198 {
00199    struct ast_datastore *datastore = NULL;
00200    struct pitchshift_data *shift = NULL;
00201    int new = 0;
00202    float amount = 0;
00203 
00204    if (!chan) {
00205       ast_log(LOG_WARNING, "No channel was provided to %s function.\n", cmd);
00206       return -1;
00207    }
00208 
00209    ast_channel_lock(chan);
00210    if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
00211       ast_channel_unlock(chan);
00212 
00213       if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
00214          return 0;
00215       }
00216       if (!(shift = ast_calloc(1, sizeof(*shift)))) {
00217          ast_datastore_free(datastore);
00218          return 0;
00219       }
00220 
00221       ast_audiohook_init(&shift->audiohook, AST_AUDIOHOOK_TYPE_MANIPULATE, "pitch_shift");
00222       shift->audiohook.manipulate_callback = pitchshift_cb;
00223       datastore->data = shift;
00224       new = 1;
00225    } else {
00226       ast_channel_unlock(chan);
00227       shift = datastore->data;
00228    }
00229 
00230 
00231    if (!strcasecmp(value, "highest")) {
00232       amount = HIGHEST;
00233    } else if (!strcasecmp(value, "higher")) {
00234       amount = HIGHER;
00235    } else if (!strcasecmp(value, "high")) {
00236       amount = HIGH;
00237    } else if (!strcasecmp(value, "lowest")) {
00238       amount = LOWEST;
00239    } else if (!strcasecmp(value, "lower")) {
00240       amount = LOWER;
00241    } else if (!strcasecmp(value, "low")) {
00242       amount = LOW;
00243    } else {
00244       if (!sscanf(value, "%30f", &amount) || (amount <= 0) || (amount > 4)) {
00245          goto cleanup_error;
00246       }
00247    }
00248 
00249    if (!strcasecmp(data, "rx")) {
00250       shift->rx.shift_amount = amount;
00251    } else if (!strcasecmp(data, "tx")) {
00252       shift->tx.shift_amount = amount;
00253    } else if (!strcasecmp(data, "both")) {
00254       shift->rx.shift_amount = amount;
00255       shift->tx.shift_amount = amount;
00256    } else {
00257       goto cleanup_error;
00258    }
00259 
00260    if (new) {
00261       ast_channel_lock(chan);
00262       ast_channel_datastore_add(chan, datastore);
00263       ast_channel_unlock(chan);
00264       ast_audiohook_attach(chan, &shift->audiohook);
00265    }
00266 
00267    return 0;
00268 
00269 cleanup_error:
00270 
00271    ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
00272    if (new) {
00273       ast_datastore_free(datastore);
00274    }
00275    return -1;
00276 }
00277 
00278 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
00279 {
00280    float wr, wi, arg, *p1, *p2, temp;
00281    float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
00282    long i, bitm, j, le, le2, k;
00283 
00284    for (i = 2; i < 2 * fft_frame_size - 2; i += 2) {
00285       for (bitm = 2, j = 0; bitm < 2 * fft_frame_size; bitm <<= 1) {
00286          if (i & bitm) {
00287             j++;
00288          }
00289          j <<= 1;
00290       }
00291       if (i < j) {
00292          p1 = fft_buffer + i; p2 = fft_buffer + j;
00293          temp = *p1; *(p1++) = *p2;
00294          *(p2++) = temp; temp = *p1;
00295          *p1 = *p2; *p2 = temp;
00296       }
00297    }
00298    for (k = 0, le = 2; k < (long) (log(fft_frame_size) / log(2.) + .5); k++) {
00299       le <<= 1;
00300       le2 = le>>1;
00301       ur = 1.0;
00302       ui = 0.0;
00303       arg = M_PI / (le2>>1);
00304       wr = cos(arg);
00305       wi = sign * sin(arg);
00306       for (j = 0; j < le2; j += 2) {
00307          p1r = fft_buffer+j; p1i = p1r + 1;
00308          p2r = p1r + le2; p2i = p2r + 1;
00309          for (i = j; i < 2 * fft_frame_size; i += le) {
00310             tr = *p2r * ur - *p2i * ui;
00311             ti = *p2r * ui + *p2i * ur;
00312             *p2r = *p1r - tr; *p2i = *p1i - ti;
00313             *p1r += tr; *p1i += ti;
00314             p1r += le; p1i += le;
00315             p2r += le; p2i += le;
00316          }
00317          tr = ur * wr - ui * wi;
00318          ui = ur * wi + ui * wr;
00319          ur = tr;
00320       }
00321    }
00322 }
00323 
00324 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)
00325 {
00326    float *in_fifo = fft_data->in_fifo;
00327    float *out_fifo = fft_data->out_fifo;
00328    float *fft_worksp = fft_data->fft_worksp;
00329    float *last_phase = fft_data->last_phase;
00330    float *sum_phase = fft_data->sum_phase;
00331    float *output_accum = fft_data->output_accum;
00332    float *ana_freq = fft_data->ana_freq;
00333    float *ana_magn = fft_data->ana_magn;
00334    float *syn_freq = fft_data->syn_freq;
00335    float *sys_magn = fft_data->sys_magn;
00336 
00337    double magn, phase, tmp, window, real, imag;
00338    double freq_per_bin, expct;
00339    long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
00340 
00341    /* set up some handy variables */
00342    fft_frame_size2 = fft_frame_size / 2;
00343    step_size = fft_frame_size / osamp;
00344    freq_per_bin = sample_rate / (double) fft_frame_size;
00345    expct = 2. * M_PI * (double) step_size / (double) fft_frame_size;
00346    in_fifo_latency = fft_frame_size-step_size;
00347 
00348    if (fft_data->gRover == 0) {
00349       fft_data->gRover = in_fifo_latency;
00350    }
00351 
00352    /* main processing loop */
00353    for (i = 0; i < num_samps_to_process; i++){
00354 
00355       /* As long as we have not yet collected enough data just read in */
00356       in_fifo[fft_data->gRover] = indata[i];
00357       outdata[i] = out_fifo[fft_data->gRover - in_fifo_latency];
00358       fft_data->gRover++;
00359 
00360       /* now we have enough data for processing */
00361       if (fft_data->gRover >= fft_frame_size) {
00362          fft_data->gRover = in_fifo_latency;
00363 
00364          /* do windowing and re,im interleave */
00365          for (k = 0; k < fft_frame_size;k++) {
00366             window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
00367             fft_worksp[2*k] = in_fifo[k] * window;
00368             fft_worksp[2*k+1] = 0.;
00369          }
00370 
00371          /* ***************** ANALYSIS ******************* */
00372          /* do transform */
00373          smb_fft(fft_worksp, fft_frame_size, -1);
00374 
00375          /* this is the analysis step */
00376          for (k = 0; k <= fft_frame_size2; k++) {
00377 
00378             /* de-interlace FFT buffer */
00379             real = fft_worksp[2*k];
00380             imag = fft_worksp[2*k+1];
00381 
00382             /* compute magnitude and phase */
00383             magn = 2. * sqrt(real * real + imag * imag);
00384             phase = atan2(imag, real);
00385 
00386             /* compute phase difference */
00387             tmp = phase - last_phase[k];
00388             last_phase[k] = phase;
00389 
00390             /* subtract expected phase difference */
00391             tmp -= (double) k * expct;
00392 
00393             /* map delta phase into +/- Pi interval */
00394             qpd = tmp / M_PI;
00395             if (qpd >= 0) {
00396                qpd += qpd & 1;
00397             } else {
00398                qpd -= qpd & 1;
00399             }
00400             tmp -= M_PI * (double) qpd;
00401 
00402             /* get deviation from bin frequency from the +/- Pi interval */
00403             tmp = osamp * tmp / (2. * M_PI);
00404 
00405             /* compute the k-th partials' true frequency */
00406             tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
00407 
00408             /* store magnitude and true frequency in analysis arrays */
00409             ana_magn[k] = magn;
00410             ana_freq[k] = tmp;
00411 
00412          }
00413 
00414          /* ***************** PROCESSING ******************* */
00415          /* this does the actual pitch shifting */
00416          memset(sys_magn, 0, fft_frame_size * sizeof(float));
00417          memset(syn_freq, 0, fft_frame_size * sizeof(float));
00418          for (k = 0; k <= fft_frame_size2; k++) {
00419             index = k * pitchShift;
00420             if (index <= fft_frame_size2) {
00421                sys_magn[index] += ana_magn[k];
00422                syn_freq[index] = ana_freq[k] * pitchShift;
00423             }
00424          }
00425 
00426          /* ***************** SYNTHESIS ******************* */
00427          /* this is the synthesis step */
00428          for (k = 0; k <= fft_frame_size2; k++) {
00429 
00430             /* get magnitude and true frequency from synthesis arrays */
00431             magn = sys_magn[k];
00432             tmp = syn_freq[k];
00433 
00434             /* subtract bin mid frequency */
00435             tmp -= (double) k * freq_per_bin;
00436 
00437             /* get bin deviation from freq deviation */
00438             tmp /= freq_per_bin;
00439 
00440             /* take osamp into account */
00441             tmp = 2. * M_PI * tmp / osamp;
00442 
00443             /* add the overlap phase advance back in */
00444             tmp += (double) k * expct;
00445 
00446             /* accumulate delta phase to get bin phase */
00447             sum_phase[k] += tmp;
00448             phase = sum_phase[k];
00449 
00450             /* get real and imag part and re-interleave */
00451             fft_worksp[2*k] = magn * cos(phase);
00452             fft_worksp[2*k+1] = magn * sin(phase);
00453          }
00454 
00455          /* zero negative frequencies */
00456          for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
00457             fft_worksp[k] = 0.;
00458          }
00459 
00460          /* do inverse transform */
00461          smb_fft(fft_worksp, fft_frame_size, 1);
00462 
00463          /* do windowing and add to output accumulator */
00464          for (k = 0; k < fft_frame_size; k++) {
00465             window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
00466             output_accum[k] += 2. * window * fft_worksp[2*k] / (fft_frame_size2 * osamp);
00467          }
00468          for (k = 0; k < step_size; k++) {
00469             out_fifo[k] = output_accum[k];
00470          }
00471 
00472          /* shift accumulator */
00473          memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
00474 
00475          /* move input FIFO */
00476          for (k = 0; k < in_fifo_latency; k++) {
00477             in_fifo[k] = in_fifo[k+step_size];
00478          }
00479       }
00480    }
00481 }
00482 
00483 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
00484 {
00485    int16_t *fun = (int16_t *) f->data.ptr;
00486    int samples;
00487 
00488    /* an amount of 1 has no effect */
00489    if (!amount || amount == 1 || !fun || (f->samples % 32)) {
00490       return 0;
00491    }
00492    for (samples = 0; samples < f->samples; samples += 32) {
00493       smb_pitch_shift(amount, 32, MAX_FRAME_LENGTH, 32, ast_format_rate(f->subclass.codec), fun+samples, fun+samples, fft);
00494    }
00495 
00496    return 0;
00497 }
00498 
00499 static struct ast_custom_function pitch_shift_function = {
00500    .name = "PITCH_SHIFT",
00501    .write = pitchshift_helper,
00502 };
00503 
00504 static int unload_module(void)
00505 {
00506    return ast_custom_function_unregister(&pitch_shift_function);
00507 }
00508 
00509 static int load_module(void)
00510 {
00511    int res = ast_custom_function_register(&pitch_shift_function);
00512    return res ? AST_MODULE_LOAD_DECLINE : AST_MODULE_LOAD_SUCCESS;
00513 }
00514 
00515 AST_MODULE_INFO_STANDARD(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");

Generated on 27 Jan 2016 for Asterisk - The Open Source Telephony Project by  doxygen 1.6.1