00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 #include "asterisk.h"
00062
00063 ASTERISK_FILE_VERSION(__FILE__, "$Revision: 251262 $")
00064
00065 #include "asterisk/module.h"
00066 #include "asterisk/channel.h"
00067 #include "asterisk/pbx.h"
00068 #include "asterisk/utils.h"
00069 #include "asterisk/audiohook.h"
00070 #include <math.h>
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111 #ifndef M_PI
00112 #define M_PI 3.14159265358979323846
00113 #endif
00114 #define MAX_FRAME_LENGTH 256
00115
00116 #define HIGHEST 2
00117 #define HIGHER 1.5
00118 #define HIGH 1.25
00119 #define LOW .85
00120 #define LOWER .7
00121 #define LOWEST .5
00122
00123 struct fft_data {
00124 float in_fifo[MAX_FRAME_LENGTH];
00125 float out_fifo[MAX_FRAME_LENGTH];
00126 float fft_worksp[2*MAX_FRAME_LENGTH];
00127 float last_phase[MAX_FRAME_LENGTH/2+1];
00128 float sum_phase[MAX_FRAME_LENGTH/2+1];
00129 float output_accum[2*MAX_FRAME_LENGTH];
00130 float ana_freq[MAX_FRAME_LENGTH];
00131 float ana_magn[MAX_FRAME_LENGTH];
00132 float syn_freq[MAX_FRAME_LENGTH];
00133 float sys_magn[MAX_FRAME_LENGTH];
00134 long gRover;
00135 float shift_amount;
00136 };
00137
00138 struct pitchshift_data {
00139 struct ast_audiohook audiohook;
00140
00141 struct fft_data rx;
00142 struct fft_data tx;
00143 };
00144
00145 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign);
00146 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);
00147 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft_data);
00148
00149 static void destroy_callback(void *data)
00150 {
00151 struct pitchshift_data *shift = data;
00152
00153 ast_audiohook_destroy(&shift->audiohook);
00154 ast_free(shift);
00155 };
00156
00157 static const struct ast_datastore_info pitchshift_datastore = {
00158 .type = "pitchshift",
00159 .destroy = destroy_callback
00160 };
00161
00162 static int pitchshift_cb(struct ast_audiohook *audiohook, struct ast_channel *chan, struct ast_frame *f, enum ast_audiohook_direction direction)
00163 {
00164 struct ast_datastore *datastore = NULL;
00165 struct pitchshift_data *shift = NULL;
00166
00167
00168 if (!f) {
00169 return 0;
00170 }
00171 if ((audiohook->status == AST_AUDIOHOOK_STATUS_DONE) ||
00172 (f->frametype != AST_FRAME_VOICE) ||
00173 ((f->subclass.codec != AST_FORMAT_SLINEAR) &&
00174 (f->subclass.codec != AST_FORMAT_SLINEAR16))) {
00175 return -1;
00176 }
00177
00178 if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
00179 return -1;
00180 }
00181
00182 shift = datastore->data;
00183
00184 if (direction == AST_AUDIOHOOK_DIRECTION_WRITE) {
00185 pitch_shift(f, shift->tx.shift_amount, &shift->tx);
00186 } else {
00187 pitch_shift(f, shift->rx.shift_amount, &shift->rx);
00188 }
00189
00190 return 0;
00191 }
00192
00193 static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
00194 {
00195 struct ast_datastore *datastore = NULL;
00196 struct pitchshift_data *shift = NULL;
00197 int new = 0;
00198 float amount = 0;
00199
00200 ast_channel_lock(chan);
00201 if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
00202 ast_channel_unlock(chan);
00203
00204 if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
00205 return 0;
00206 }
00207 if (!(shift = ast_calloc(1, sizeof(*shift)))) {
00208 ast_datastore_free(datastore);
00209 return 0;
00210 }
00211
00212 ast_audiohook_init(&shift->audiohook, AST_AUDIOHOOK_TYPE_MANIPULATE, "pitch_shift");
00213 shift->audiohook.manipulate_callback = pitchshift_cb;
00214 datastore->data = shift;
00215 new = 1;
00216 } else {
00217 ast_channel_unlock(chan);
00218 shift = datastore->data;
00219 }
00220
00221
00222 if (!strcasecmp(value, "highest")) {
00223 amount = HIGHEST;
00224 } else if (!strcasecmp(value, "higher")) {
00225 amount = HIGHER;
00226 } else if (!strcasecmp(value, "high")) {
00227 amount = HIGH;
00228 } else if (!strcasecmp(value, "lowest")) {
00229 amount = LOWEST;
00230 } else if (!strcasecmp(value, "lower")) {
00231 amount = LOWER;
00232 } else if (!strcasecmp(value, "low")) {
00233 amount = LOW;
00234 } else {
00235 if (!sscanf(value, "%30f", &amount) || (amount <= 0) || (amount > 4)) {
00236 goto cleanup_error;
00237 }
00238 }
00239
00240 if (!strcasecmp(data, "rx")) {
00241 shift->rx.shift_amount = amount;
00242 } else if (!strcasecmp(data, "tx")) {
00243 shift->tx.shift_amount = amount;
00244 } else if (!strcasecmp(data, "both")) {
00245 shift->rx.shift_amount = amount;
00246 shift->tx.shift_amount = amount;
00247 } else {
00248 goto cleanup_error;
00249 }
00250
00251 if (new) {
00252 ast_channel_lock(chan);
00253 ast_channel_datastore_add(chan, datastore);
00254 ast_channel_unlock(chan);
00255 ast_audiohook_attach(chan, &shift->audiohook);
00256 }
00257
00258 return 0;
00259
00260 cleanup_error:
00261
00262 ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
00263 if (new) {
00264 ast_datastore_free(datastore);
00265 }
00266 return -1;
00267 }
00268
00269 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
00270 {
00271 float wr, wi, arg, *p1, *p2, temp;
00272 float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
00273 long i, bitm, j, le, le2, k;
00274
00275 for (i = 2; i < 2 * fft_frame_size - 2; i += 2) {
00276 for (bitm = 2, j = 0; bitm < 2 * fft_frame_size; bitm <<= 1) {
00277 if (i & bitm) {
00278 j++;
00279 }
00280 j <<= 1;
00281 }
00282 if (i < j) {
00283 p1 = fft_buffer + i; p2 = fft_buffer + j;
00284 temp = *p1; *(p1++) = *p2;
00285 *(p2++) = temp; temp = *p1;
00286 *p1 = *p2; *p2 = temp;
00287 }
00288 }
00289 for (k = 0, le = 2; k < (long) (log(fft_frame_size) / log(2.) + .5); k++) {
00290 le <<= 1;
00291 le2 = le>>1;
00292 ur = 1.0;
00293 ui = 0.0;
00294 arg = M_PI / (le2>>1);
00295 wr = cos(arg);
00296 wi = sign * sin(arg);
00297 for (j = 0; j < le2; j += 2) {
00298 p1r = fft_buffer+j; p1i = p1r + 1;
00299 p2r = p1r + le2; p2i = p2r + 1;
00300 for (i = j; i < 2 * fft_frame_size; i += le) {
00301 tr = *p2r * ur - *p2i * ui;
00302 ti = *p2r * ui + *p2i * ur;
00303 *p2r = *p1r - tr; *p2i = *p1i - ti;
00304 *p1r += tr; *p1i += ti;
00305 p1r += le; p1i += le;
00306 p2r += le; p2i += le;
00307 }
00308 tr = ur * wr - ui * wi;
00309 ui = ur * wi + ui * wr;
00310 ur = tr;
00311 }
00312 }
00313 }
00314
00315 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)
00316 {
00317 float *in_fifo = fft_data->in_fifo;
00318 float *out_fifo = fft_data->out_fifo;
00319 float *fft_worksp = fft_data->fft_worksp;
00320 float *last_phase = fft_data->last_phase;
00321 float *sum_phase = fft_data->sum_phase;
00322 float *output_accum = fft_data->output_accum;
00323 float *ana_freq = fft_data->ana_freq;
00324 float *ana_magn = fft_data->ana_magn;
00325 float *syn_freq = fft_data->syn_freq;
00326 float *sys_magn = fft_data->sys_magn;
00327
00328 double magn, phase, tmp, window, real, imag;
00329 double freq_per_bin, expct;
00330 long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
00331
00332
00333 fft_frame_size2 = fft_frame_size / 2;
00334 step_size = fft_frame_size / osamp;
00335 freq_per_bin = sample_rate / (double) fft_frame_size;
00336 expct = 2. * M_PI * (double) step_size / (double) fft_frame_size;
00337 in_fifo_latency = fft_frame_size-step_size;
00338
00339 if (fft_data->gRover == 0) {
00340 fft_data->gRover = in_fifo_latency;
00341 }
00342
00343
00344 for (i = 0; i < num_samps_to_process; i++){
00345
00346
00347 in_fifo[fft_data->gRover] = indata[i];
00348 outdata[i] = out_fifo[fft_data->gRover - in_fifo_latency];
00349 fft_data->gRover++;
00350
00351
00352 if (fft_data->gRover >= fft_frame_size) {
00353 fft_data->gRover = in_fifo_latency;
00354
00355
00356 for (k = 0; k < fft_frame_size;k++) {
00357 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
00358 fft_worksp[2*k] = in_fifo[k] * window;
00359 fft_worksp[2*k+1] = 0.;
00360 }
00361
00362
00363
00364 smb_fft(fft_worksp, fft_frame_size, -1);
00365
00366
00367 for (k = 0; k <= fft_frame_size2; k++) {
00368
00369
00370 real = fft_worksp[2*k];
00371 imag = fft_worksp[2*k+1];
00372
00373
00374 magn = 2. * sqrt(real * real + imag * imag);
00375 phase = atan2(imag, real);
00376
00377
00378 tmp = phase - last_phase[k];
00379 last_phase[k] = phase;
00380
00381
00382 tmp -= (double) k * expct;
00383
00384
00385 qpd = tmp / M_PI;
00386 if (qpd >= 0) {
00387 qpd += qpd & 1;
00388 } else {
00389 qpd -= qpd & 1;
00390 }
00391 tmp -= M_PI * (double) qpd;
00392
00393
00394 tmp = osamp * tmp / (2. * M_PI);
00395
00396
00397 tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
00398
00399
00400 ana_magn[k] = magn;
00401 ana_freq[k] = tmp;
00402
00403 }
00404
00405
00406
00407 memset(sys_magn, 0, fft_frame_size * sizeof(float));
00408 memset(syn_freq, 0, fft_frame_size * sizeof(float));
00409 for (k = 0; k <= fft_frame_size2; k++) {
00410 index = k * pitchShift;
00411 if (index <= fft_frame_size2) {
00412 sys_magn[index] += ana_magn[k];
00413 syn_freq[index] = ana_freq[k] * pitchShift;
00414 }
00415 }
00416
00417
00418
00419 for (k = 0; k <= fft_frame_size2; k++) {
00420
00421
00422 magn = sys_magn[k];
00423 tmp = syn_freq[k];
00424
00425
00426 tmp -= (double) k * freq_per_bin;
00427
00428
00429 tmp /= freq_per_bin;
00430
00431
00432 tmp = 2. * M_PI * tmp / osamp;
00433
00434
00435 tmp += (double) k * expct;
00436
00437
00438 sum_phase[k] += tmp;
00439 phase = sum_phase[k];
00440
00441
00442 fft_worksp[2*k] = magn * cos(phase);
00443 fft_worksp[2*k+1] = magn * sin(phase);
00444 }
00445
00446
00447 for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
00448 fft_worksp[k] = 0.;
00449 }
00450
00451
00452 smb_fft(fft_worksp, fft_frame_size, 1);
00453
00454
00455 for (k = 0; k < fft_frame_size; k++) {
00456 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
00457 output_accum[k] += 2. * window * fft_worksp[2*k] / (fft_frame_size2 * osamp);
00458 }
00459 for (k = 0; k < step_size; k++) {
00460 out_fifo[k] = output_accum[k];
00461 }
00462
00463
00464 memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
00465
00466
00467 for (k = 0; k < in_fifo_latency; k++) {
00468 in_fifo[k] = in_fifo[k+step_size];
00469 }
00470 }
00471 }
00472 }
00473
00474 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
00475 {
00476 int16_t *fun = (int16_t *) f->data.ptr;
00477 int samples;
00478
00479
00480 if (!amount || amount == 1 || !fun || (f->samples % 32)) {
00481 return 0;
00482 }
00483 for (samples = 0; samples < f->samples; samples += 32) {
00484 smb_pitch_shift(amount, 32, MAX_FRAME_LENGTH, 32, ast_format_rate(f->subclass.codec), fun+samples, fun+samples, fft);
00485 }
00486
00487 return 0;
00488 }
00489
00490 static struct ast_custom_function pitch_shift_function = {
00491 .name = "PITCH_SHIFT",
00492 .write = pitchshift_helper,
00493 };
00494
00495 static int unload_module(void)
00496 {
00497 return ast_custom_function_unregister(&pitch_shift_function);
00498 }
00499
00500 static int load_module(void)
00501 {
00502 int res = ast_custom_function_register(&pitch_shift_function);
00503 return res ? AST_MODULE_LOAD_DECLINE : AST_MODULE_LOAD_SUCCESS;
00504 }
00505
00506 AST_MODULE_INFO_STANDARD(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");