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: 328209 $")
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 ast_channel_lock(chan);
00205 if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
00206 ast_channel_unlock(chan);
00207
00208 if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
00209 return 0;
00210 }
00211 if (!(shift = ast_calloc(1, sizeof(*shift)))) {
00212 ast_datastore_free(datastore);
00213 return 0;
00214 }
00215
00216 ast_audiohook_init(&shift->audiohook, AST_AUDIOHOOK_TYPE_MANIPULATE, "pitch_shift");
00217 shift->audiohook.manipulate_callback = pitchshift_cb;
00218 datastore->data = shift;
00219 new = 1;
00220 } else {
00221 ast_channel_unlock(chan);
00222 shift = datastore->data;
00223 }
00224
00225
00226 if (!strcasecmp(value, "highest")) {
00227 amount = HIGHEST;
00228 } else if (!strcasecmp(value, "higher")) {
00229 amount = HIGHER;
00230 } else if (!strcasecmp(value, "high")) {
00231 amount = HIGH;
00232 } else if (!strcasecmp(value, "lowest")) {
00233 amount = LOWEST;
00234 } else if (!strcasecmp(value, "lower")) {
00235 amount = LOWER;
00236 } else if (!strcasecmp(value, "low")) {
00237 amount = LOW;
00238 } else {
00239 if (!sscanf(value, "%30f", &amount) || (amount <= 0) || (amount > 4)) {
00240 goto cleanup_error;
00241 }
00242 }
00243
00244 if (!strcasecmp(data, "rx")) {
00245 shift->rx.shift_amount = amount;
00246 } else if (!strcasecmp(data, "tx")) {
00247 shift->tx.shift_amount = amount;
00248 } else if (!strcasecmp(data, "both")) {
00249 shift->rx.shift_amount = amount;
00250 shift->tx.shift_amount = amount;
00251 } else {
00252 goto cleanup_error;
00253 }
00254
00255 if (new) {
00256 ast_channel_lock(chan);
00257 ast_channel_datastore_add(chan, datastore);
00258 ast_channel_unlock(chan);
00259 ast_audiohook_attach(chan, &shift->audiohook);
00260 }
00261
00262 return 0;
00263
00264 cleanup_error:
00265
00266 ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
00267 if (new) {
00268 ast_datastore_free(datastore);
00269 }
00270 return -1;
00271 }
00272
00273 static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
00274 {
00275 float wr, wi, arg, *p1, *p2, temp;
00276 float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
00277 long i, bitm, j, le, le2, k;
00278
00279 for (i = 2; i < 2 * fft_frame_size - 2; i += 2) {
00280 for (bitm = 2, j = 0; bitm < 2 * fft_frame_size; bitm <<= 1) {
00281 if (i & bitm) {
00282 j++;
00283 }
00284 j <<= 1;
00285 }
00286 if (i < j) {
00287 p1 = fft_buffer + i; p2 = fft_buffer + j;
00288 temp = *p1; *(p1++) = *p2;
00289 *(p2++) = temp; temp = *p1;
00290 *p1 = *p2; *p2 = temp;
00291 }
00292 }
00293 for (k = 0, le = 2; k < (long) (log(fft_frame_size) / log(2.) + .5); k++) {
00294 le <<= 1;
00295 le2 = le>>1;
00296 ur = 1.0;
00297 ui = 0.0;
00298 arg = M_PI / (le2>>1);
00299 wr = cos(arg);
00300 wi = sign * sin(arg);
00301 for (j = 0; j < le2; j += 2) {
00302 p1r = fft_buffer+j; p1i = p1r + 1;
00303 p2r = p1r + le2; p2i = p2r + 1;
00304 for (i = j; i < 2 * fft_frame_size; i += le) {
00305 tr = *p2r * ur - *p2i * ui;
00306 ti = *p2r * ui + *p2i * ur;
00307 *p2r = *p1r - tr; *p2i = *p1i - ti;
00308 *p1r += tr; *p1i += ti;
00309 p1r += le; p1i += le;
00310 p2r += le; p2i += le;
00311 }
00312 tr = ur * wr - ui * wi;
00313 ui = ur * wi + ui * wr;
00314 ur = tr;
00315 }
00316 }
00317 }
00318
00319 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)
00320 {
00321 float *in_fifo = fft_data->in_fifo;
00322 float *out_fifo = fft_data->out_fifo;
00323 float *fft_worksp = fft_data->fft_worksp;
00324 float *last_phase = fft_data->last_phase;
00325 float *sum_phase = fft_data->sum_phase;
00326 float *output_accum = fft_data->output_accum;
00327 float *ana_freq = fft_data->ana_freq;
00328 float *ana_magn = fft_data->ana_magn;
00329 float *syn_freq = fft_data->syn_freq;
00330 float *sys_magn = fft_data->sys_magn;
00331
00332 double magn, phase, tmp, window, real, imag;
00333 double freq_per_bin, expct;
00334 long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
00335
00336
00337 fft_frame_size2 = fft_frame_size / 2;
00338 step_size = fft_frame_size / osamp;
00339 freq_per_bin = sample_rate / (double) fft_frame_size;
00340 expct = 2. * M_PI * (double) step_size / (double) fft_frame_size;
00341 in_fifo_latency = fft_frame_size-step_size;
00342
00343 if (fft_data->gRover == 0) {
00344 fft_data->gRover = in_fifo_latency;
00345 }
00346
00347
00348 for (i = 0; i < num_samps_to_process; i++){
00349
00350
00351 in_fifo[fft_data->gRover] = indata[i];
00352 outdata[i] = out_fifo[fft_data->gRover - in_fifo_latency];
00353 fft_data->gRover++;
00354
00355
00356 if (fft_data->gRover >= fft_frame_size) {
00357 fft_data->gRover = in_fifo_latency;
00358
00359
00360 for (k = 0; k < fft_frame_size;k++) {
00361 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
00362 fft_worksp[2*k] = in_fifo[k] * window;
00363 fft_worksp[2*k+1] = 0.;
00364 }
00365
00366
00367
00368 smb_fft(fft_worksp, fft_frame_size, -1);
00369
00370
00371 for (k = 0; k <= fft_frame_size2; k++) {
00372
00373
00374 real = fft_worksp[2*k];
00375 imag = fft_worksp[2*k+1];
00376
00377
00378 magn = 2. * sqrt(real * real + imag * imag);
00379 phase = atan2(imag, real);
00380
00381
00382 tmp = phase - last_phase[k];
00383 last_phase[k] = phase;
00384
00385
00386 tmp -= (double) k * expct;
00387
00388
00389 qpd = tmp / M_PI;
00390 if (qpd >= 0) {
00391 qpd += qpd & 1;
00392 } else {
00393 qpd -= qpd & 1;
00394 }
00395 tmp -= M_PI * (double) qpd;
00396
00397
00398 tmp = osamp * tmp / (2. * M_PI);
00399
00400
00401 tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
00402
00403
00404 ana_magn[k] = magn;
00405 ana_freq[k] = tmp;
00406
00407 }
00408
00409
00410
00411 memset(sys_magn, 0, fft_frame_size * sizeof(float));
00412 memset(syn_freq, 0, fft_frame_size * sizeof(float));
00413 for (k = 0; k <= fft_frame_size2; k++) {
00414 index = k * pitchShift;
00415 if (index <= fft_frame_size2) {
00416 sys_magn[index] += ana_magn[k];
00417 syn_freq[index] = ana_freq[k] * pitchShift;
00418 }
00419 }
00420
00421
00422
00423 for (k = 0; k <= fft_frame_size2; k++) {
00424
00425
00426 magn = sys_magn[k];
00427 tmp = syn_freq[k];
00428
00429
00430 tmp -= (double) k * freq_per_bin;
00431
00432
00433 tmp /= freq_per_bin;
00434
00435
00436 tmp = 2. * M_PI * tmp / osamp;
00437
00438
00439 tmp += (double) k * expct;
00440
00441
00442 sum_phase[k] += tmp;
00443 phase = sum_phase[k];
00444
00445
00446 fft_worksp[2*k] = magn * cos(phase);
00447 fft_worksp[2*k+1] = magn * sin(phase);
00448 }
00449
00450
00451 for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
00452 fft_worksp[k] = 0.;
00453 }
00454
00455
00456 smb_fft(fft_worksp, fft_frame_size, 1);
00457
00458
00459 for (k = 0; k < fft_frame_size; k++) {
00460 window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
00461 output_accum[k] += 2. * window * fft_worksp[2*k] / (fft_frame_size2 * osamp);
00462 }
00463 for (k = 0; k < step_size; k++) {
00464 out_fifo[k] = output_accum[k];
00465 }
00466
00467
00468 memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
00469
00470
00471 for (k = 0; k < in_fifo_latency; k++) {
00472 in_fifo[k] = in_fifo[k+step_size];
00473 }
00474 }
00475 }
00476 }
00477
00478 static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
00479 {
00480 int16_t *fun = (int16_t *) f->data.ptr;
00481 int samples;
00482
00483
00484 if (!amount || amount == 1 || !fun || (f->samples % 32)) {
00485 return 0;
00486 }
00487 for (samples = 0; samples < f->samples; samples += 32) {
00488 smb_pitch_shift(amount, 32, MAX_FRAME_LENGTH, 32, ast_format_rate(f->subclass.codec), fun+samples, fun+samples, fft);
00489 }
00490
00491 return 0;
00492 }
00493
00494 static struct ast_custom_function pitch_shift_function = {
00495 .name = "PITCH_SHIFT",
00496 .write = pitchshift_helper,
00497 };
00498
00499 static int unload_module(void)
00500 {
00501 return ast_custom_function_unregister(&pitch_shift_function);
00502 }
00503
00504 static int load_module(void)
00505 {
00506 int res = ast_custom_function_register(&pitch_shift_function);
00507 return res ? AST_MODULE_LOAD_DECLINE : AST_MODULE_LOAD_SUCCESS;
00508 }
00509
00510 AST_MODULE_INFO_STANDARD(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");