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
00062
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
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
00112
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
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
00353 for (i = 0; i < num_samps_to_process; i++){
00354
00355
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
00361 if (fft_data->gRover >= fft_frame_size) {
00362 fft_data->gRover = in_fifo_latency;
00363
00364
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
00372
00373 smb_fft(fft_worksp, fft_frame_size, -1);
00374
00375
00376 for (k = 0; k <= fft_frame_size2; k++) {
00377
00378
00379 real = fft_worksp[2*k];
00380 imag = fft_worksp[2*k+1];
00381
00382
00383 magn = 2. * sqrt(real * real + imag * imag);
00384 phase = atan2(imag, real);
00385
00386
00387 tmp = phase - last_phase[k];
00388 last_phase[k] = phase;
00389
00390
00391 tmp -= (double) k * expct;
00392
00393
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
00403 tmp = osamp * tmp / (2. * M_PI);
00404
00405
00406 tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
00407
00408
00409 ana_magn[k] = magn;
00410 ana_freq[k] = tmp;
00411
00412 }
00413
00414
00415
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
00427
00428 for (k = 0; k <= fft_frame_size2; k++) {
00429
00430
00431 magn = sys_magn[k];
00432 tmp = syn_freq[k];
00433
00434
00435 tmp -= (double) k * freq_per_bin;
00436
00437
00438 tmp /= freq_per_bin;
00439
00440
00441 tmp = 2. * M_PI * tmp / osamp;
00442
00443
00444 tmp += (double) k * expct;
00445
00446
00447 sum_phase[k] += tmp;
00448 phase = sum_phase[k];
00449
00450
00451 fft_worksp[2*k] = magn * cos(phase);
00452 fft_worksp[2*k+1] = magn * sin(phase);
00453 }
00454
00455
00456 for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
00457 fft_worksp[k] = 0.;
00458 }
00459
00460
00461 smb_fft(fft_worksp, fft_frame_size, 1);
00462
00463
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
00473 memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
00474
00475
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
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");