func_pitchshift.c 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514
  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. /*! \file
  19. *
  20. * \brief Pitch Shift Audio Effect
  21. *
  22. * \author David Vossel <dvossel@digium.com>
  23. *
  24. * \ingroup functions
  25. */
  26. /************************* SMB FUNCTION LICENSE *********************************
  27. *
  28. * SYNOPSIS: Routine for doing pitch shifting while maintaining
  29. * duration using the Short Time Fourier Transform.
  30. *
  31. * DESCRIPTION: The routine takes a pitchShift factor value which is between 0.5
  32. * (one octave down) and 2. (one octave up). A value of exactly 1 does not change
  33. * the pitch. num_samps_to_process tells the routine how many samples in indata[0...
  34. * num_samps_to_process-1] should be pitch shifted and moved to outdata[0 ...
  35. * num_samps_to_process-1]. The two buffers can be identical (ie. it can process the
  36. * data in-place). fft_frame_size defines the FFT frame size used for the
  37. * processing. Typical values are 1024, 2048 and 4096. It may be any value <=
  38. * MAX_FRAME_LENGTH but it MUST be a power of 2. osamp is the STFT
  39. * oversampling factor which also determines the overlap between adjacent STFT
  40. * frames. It should at least be 4 for moderate scaling ratios. A value of 32 is
  41. * recommended for best quality. sampleRate takes the sample rate for the signal
  42. * in unit Hz, ie. 44100 for 44.1 kHz audio. The data passed to the routine in
  43. * indata[] should be in the range [-1.0, 1.0), which is also the output range
  44. * for the data, make sure you scale the data accordingly (for 16bit signed integers
  45. * you would have to divide (and multiply) by 32768).
  46. *
  47. * COPYRIGHT 1999-2009 Stephan M. Bernsee <smb [AT] dspdimension [DOT] com>
  48. *
  49. * The Wide Open License (WOL)
  50. *
  51. * Permission to use, copy, modify, distribute and sell this software and its
  52. * documentation for any purpose is hereby granted without fee, provided that
  53. * the above copyright notice and this license appear in all source copies.
  54. * THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY OF
  55. * ANY KIND. See http://www.dspguru.com/wol.htm for more information.
  56. *
  57. *****************************************************************************/
  58. /*** MODULEINFO
  59. <support_level>extended</support_level>
  60. ***/
  61. #include "asterisk.h"
  62. ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
  63. #include "asterisk/module.h"
  64. #include "asterisk/channel.h"
  65. #include "asterisk/pbx.h"
  66. #include "asterisk/utils.h"
  67. #include "asterisk/audiohook.h"
  68. #include <math.h>
  69. /*** DOCUMENTATION
  70. <function name="PITCH_SHIFT" language="en_US">
  71. <synopsis>
  72. Pitch shift both tx and rx audio streams on a channel.
  73. </synopsis>
  74. <syntax>
  75. <parameter name="channel direction" required="true">
  76. <para>Direction can be either <literal>rx</literal>, <literal>tx</literal>, or
  77. <literal>both</literal>. The direction can either be set to a valid floating
  78. point number between 0.1 and 4.0 or one of the enum values listed below. A value
  79. of 1.0 has no effect. Greater than 1 raises the pitch. Lower than 1 lowers
  80. the pitch.</para>
  81. <para>The pitch amount can also be set by the following values</para>
  82. <enumlist>
  83. <enum name = "highest" />
  84. <enum name = "higher" />
  85. <enum name = "high" />
  86. <enum name = "low" />
  87. <enum name = "lower" />
  88. <enum name = "lowest" />
  89. </enumlist>
  90. </parameter>
  91. </syntax>
  92. <description>
  93. <para>Examples:</para>
  94. <para>exten => 1,1,Set(PITCH_SHIFT(tx)=highest); raises pitch an octave </para>
  95. <para>exten => 1,1,Set(PITCH_SHIFT(rx)=higher) ; raises pitch more </para>
  96. <para>exten => 1,1,Set(PITCH_SHIFT(both)=high) ; raises pitch </para>
  97. <para>exten => 1,1,Set(PITCH_SHIFT(rx)=low) ; lowers pitch </para>
  98. <para>exten => 1,1,Set(PITCH_SHIFT(tx)=lower) ; lowers pitch more </para>
  99. <para>exten => 1,1,Set(PITCH_SHIFT(both)=lowest) ; lowers pitch an octave </para>
  100. <para>exten => 1,1,Set(PITCH_SHIFT(rx)=0.8) ; lowers pitch </para>
  101. <para>exten => 1,1,Set(PITCH_SHIFT(tx)=1.5) ; raises pitch </para>
  102. </description>
  103. </function>
  104. ***/
  105. #ifndef M_PI
  106. #define M_PI 3.14159265358979323846
  107. #endif
  108. #define MAX_FRAME_LENGTH 256
  109. #define HIGHEST 2
  110. #define HIGHER 1.5
  111. #define HIGH 1.25
  112. #define LOW .85
  113. #define LOWER .7
  114. #define LOWEST .5
  115. struct fft_data {
  116. float in_fifo[MAX_FRAME_LENGTH];
  117. float out_fifo[MAX_FRAME_LENGTH];
  118. float fft_worksp[2*MAX_FRAME_LENGTH];
  119. float last_phase[MAX_FRAME_LENGTH/2+1];
  120. float sum_phase[MAX_FRAME_LENGTH/2+1];
  121. float output_accum[2*MAX_FRAME_LENGTH];
  122. float ana_freq[MAX_FRAME_LENGTH];
  123. float ana_magn[MAX_FRAME_LENGTH];
  124. float syn_freq[MAX_FRAME_LENGTH];
  125. float sys_magn[MAX_FRAME_LENGTH];
  126. long gRover;
  127. float shift_amount;
  128. };
  129. struct pitchshift_data {
  130. struct ast_audiohook audiohook;
  131. struct fft_data rx;
  132. struct fft_data tx;
  133. };
  134. static void smb_fft(float *fft_buffer, long fft_frame_size, long sign);
  135. 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);
  136. static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft_data);
  137. static void destroy_callback(void *data)
  138. {
  139. struct pitchshift_data *shift = data;
  140. ast_audiohook_destroy(&shift->audiohook);
  141. ast_free(shift);
  142. };
  143. static const struct ast_datastore_info pitchshift_datastore = {
  144. .type = "pitchshift",
  145. .destroy = destroy_callback
  146. };
  147. static int pitchshift_cb(struct ast_audiohook *audiohook, struct ast_channel *chan, struct ast_frame *f, enum ast_audiohook_direction direction)
  148. {
  149. struct ast_datastore *datastore = NULL;
  150. struct pitchshift_data *shift = NULL;
  151. if (!f) {
  152. return 0;
  153. }
  154. if (audiohook->status == AST_AUDIOHOOK_STATUS_DONE) {
  155. return -1;
  156. }
  157. if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
  158. return -1;
  159. }
  160. shift = datastore->data;
  161. if (direction == AST_AUDIOHOOK_DIRECTION_WRITE) {
  162. pitch_shift(f, shift->tx.shift_amount, &shift->tx);
  163. } else {
  164. pitch_shift(f, shift->rx.shift_amount, &shift->rx);
  165. }
  166. return 0;
  167. }
  168. static int pitchshift_helper(struct ast_channel *chan, const char *cmd, char *data, const char *value)
  169. {
  170. struct ast_datastore *datastore = NULL;
  171. struct pitchshift_data *shift = NULL;
  172. int new = 0;
  173. float amount = 0;
  174. if (!chan) {
  175. ast_log(LOG_WARNING, "No channel was provided to %s function.\n", cmd);
  176. return -1;
  177. }
  178. ast_channel_lock(chan);
  179. if (!(datastore = ast_channel_datastore_find(chan, &pitchshift_datastore, NULL))) {
  180. ast_channel_unlock(chan);
  181. if (!(datastore = ast_datastore_alloc(&pitchshift_datastore, NULL))) {
  182. return 0;
  183. }
  184. if (!(shift = ast_calloc(1, sizeof(*shift)))) {
  185. ast_datastore_free(datastore);
  186. return 0;
  187. }
  188. ast_audiohook_init(&shift->audiohook, AST_AUDIOHOOK_TYPE_MANIPULATE, "pitch_shift", AST_AUDIOHOOK_MANIPULATE_ALL_RATES);
  189. shift->audiohook.manipulate_callback = pitchshift_cb;
  190. datastore->data = shift;
  191. new = 1;
  192. } else {
  193. ast_channel_unlock(chan);
  194. shift = datastore->data;
  195. }
  196. if (!strcasecmp(value, "highest")) {
  197. amount = HIGHEST;
  198. } else if (!strcasecmp(value, "higher")) {
  199. amount = HIGHER;
  200. } else if (!strcasecmp(value, "high")) {
  201. amount = HIGH;
  202. } else if (!strcasecmp(value, "lowest")) {
  203. amount = LOWEST;
  204. } else if (!strcasecmp(value, "lower")) {
  205. amount = LOWER;
  206. } else if (!strcasecmp(value, "low")) {
  207. amount = LOW;
  208. } else {
  209. if (!sscanf(value, "%30f", &amount) || (amount <= 0) || (amount > 4)) {
  210. goto cleanup_error;
  211. }
  212. }
  213. if (!strcasecmp(data, "rx")) {
  214. shift->rx.shift_amount = amount;
  215. } else if (!strcasecmp(data, "tx")) {
  216. shift->tx.shift_amount = amount;
  217. } else if (!strcasecmp(data, "both")) {
  218. shift->rx.shift_amount = amount;
  219. shift->tx.shift_amount = amount;
  220. } else {
  221. goto cleanup_error;
  222. }
  223. if (new) {
  224. ast_channel_lock(chan);
  225. ast_channel_datastore_add(chan, datastore);
  226. ast_channel_unlock(chan);
  227. ast_audiohook_attach(chan, &shift->audiohook);
  228. }
  229. return 0;
  230. cleanup_error:
  231. ast_log(LOG_ERROR, "Invalid argument provided to the %s function\n", cmd);
  232. if (new) {
  233. ast_datastore_free(datastore);
  234. }
  235. return -1;
  236. }
  237. static void smb_fft(float *fft_buffer, long fft_frame_size, long sign)
  238. {
  239. float wr, wi, arg, *p1, *p2, temp;
  240. float tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
  241. long i, bitm, j, le, le2, k;
  242. for (i = 2; i < 2 * fft_frame_size - 2; i += 2) {
  243. for (bitm = 2, j = 0; bitm < 2 * fft_frame_size; bitm <<= 1) {
  244. if (i & bitm) {
  245. j++;
  246. }
  247. j <<= 1;
  248. }
  249. if (i < j) {
  250. p1 = fft_buffer + i; p2 = fft_buffer + j;
  251. temp = *p1; *(p1++) = *p2;
  252. *(p2++) = temp; temp = *p1;
  253. *p1 = *p2; *p2 = temp;
  254. }
  255. }
  256. for (k = 0, le = 2; k < (long) (log(fft_frame_size) / log(2.) + .5); k++) {
  257. le <<= 1;
  258. le2 = le>>1;
  259. ur = 1.0;
  260. ui = 0.0;
  261. arg = M_PI / (le2>>1);
  262. wr = cos(arg);
  263. wi = sign * sin(arg);
  264. for (j = 0; j < le2; j += 2) {
  265. p1r = fft_buffer+j; p1i = p1r + 1;
  266. p2r = p1r + le2; p2i = p2r + 1;
  267. for (i = j; i < 2 * fft_frame_size; i += le) {
  268. tr = *p2r * ur - *p2i * ui;
  269. ti = *p2r * ui + *p2i * ur;
  270. *p2r = *p1r - tr; *p2i = *p1i - ti;
  271. *p1r += tr; *p1i += ti;
  272. p1r += le; p1i += le;
  273. p2r += le; p2i += le;
  274. }
  275. tr = ur * wr - ui * wi;
  276. ui = ur * wi + ui * wr;
  277. ur = tr;
  278. }
  279. }
  280. }
  281. 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)
  282. {
  283. float *in_fifo = fft_data->in_fifo;
  284. float *out_fifo = fft_data->out_fifo;
  285. float *fft_worksp = fft_data->fft_worksp;
  286. float *last_phase = fft_data->last_phase;
  287. float *sum_phase = fft_data->sum_phase;
  288. float *output_accum = fft_data->output_accum;
  289. float *ana_freq = fft_data->ana_freq;
  290. float *ana_magn = fft_data->ana_magn;
  291. float *syn_freq = fft_data->syn_freq;
  292. float *sys_magn = fft_data->sys_magn;
  293. double magn, phase, tmp, window, real, imag;
  294. double freq_per_bin, expct;
  295. long i,k, qpd, index, in_fifo_latency, step_size, fft_frame_size2;
  296. /* set up some handy variables */
  297. fft_frame_size2 = fft_frame_size / 2;
  298. step_size = fft_frame_size / osamp;
  299. freq_per_bin = sample_rate / (double) fft_frame_size;
  300. expct = 2. * M_PI * (double) step_size / (double) fft_frame_size;
  301. in_fifo_latency = fft_frame_size-step_size;
  302. if (fft_data->gRover == 0) {
  303. fft_data->gRover = in_fifo_latency;
  304. }
  305. /* main processing loop */
  306. for (i = 0; i < num_samps_to_process; i++){
  307. /* As long as we have not yet collected enough data just read in */
  308. in_fifo[fft_data->gRover] = indata[i];
  309. outdata[i] = out_fifo[fft_data->gRover - in_fifo_latency];
  310. fft_data->gRover++;
  311. /* now we have enough data for processing */
  312. if (fft_data->gRover >= fft_frame_size) {
  313. fft_data->gRover = in_fifo_latency;
  314. /* do windowing and re,im interleave */
  315. for (k = 0; k < fft_frame_size;k++) {
  316. window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
  317. fft_worksp[2*k] = in_fifo[k] * window;
  318. fft_worksp[2*k+1] = 0.;
  319. }
  320. /* ***************** ANALYSIS ******************* */
  321. /* do transform */
  322. smb_fft(fft_worksp, fft_frame_size, -1);
  323. /* this is the analysis step */
  324. for (k = 0; k <= fft_frame_size2; k++) {
  325. /* de-interlace FFT buffer */
  326. real = fft_worksp[2*k];
  327. imag = fft_worksp[2*k+1];
  328. /* compute magnitude and phase */
  329. magn = 2. * sqrt(real * real + imag * imag);
  330. phase = atan2(imag, real);
  331. /* compute phase difference */
  332. tmp = phase - last_phase[k];
  333. last_phase[k] = phase;
  334. /* subtract expected phase difference */
  335. tmp -= (double) k * expct;
  336. /* map delta phase into +/- Pi interval */
  337. qpd = tmp / M_PI;
  338. if (qpd >= 0) {
  339. qpd += qpd & 1;
  340. } else {
  341. qpd -= qpd & 1;
  342. }
  343. tmp -= M_PI * (double) qpd;
  344. /* get deviation from bin frequency from the +/- Pi interval */
  345. tmp = osamp * tmp / (2. * M_PI);
  346. /* compute the k-th partials' true frequency */
  347. tmp = (double) k * freq_per_bin + tmp * freq_per_bin;
  348. /* store magnitude and true frequency in analysis arrays */
  349. ana_magn[k] = magn;
  350. ana_freq[k] = tmp;
  351. }
  352. /* ***************** PROCESSING ******************* */
  353. /* this does the actual pitch shifting */
  354. memset(sys_magn, 0, fft_frame_size * sizeof(float));
  355. memset(syn_freq, 0, fft_frame_size * sizeof(float));
  356. for (k = 0; k <= fft_frame_size2; k++) {
  357. index = k * pitchShift;
  358. if (index <= fft_frame_size2) {
  359. sys_magn[index] += ana_magn[k];
  360. syn_freq[index] = ana_freq[k] * pitchShift;
  361. }
  362. }
  363. /* ***************** SYNTHESIS ******************* */
  364. /* this is the synthesis step */
  365. for (k = 0; k <= fft_frame_size2; k++) {
  366. /* get magnitude and true frequency from synthesis arrays */
  367. magn = sys_magn[k];
  368. tmp = syn_freq[k];
  369. /* subtract bin mid frequency */
  370. tmp -= (double) k * freq_per_bin;
  371. /* get bin deviation from freq deviation */
  372. tmp /= freq_per_bin;
  373. /* take osamp into account */
  374. tmp = 2. * M_PI * tmp / osamp;
  375. /* add the overlap phase advance back in */
  376. tmp += (double) k * expct;
  377. /* accumulate delta phase to get bin phase */
  378. sum_phase[k] += tmp;
  379. phase = sum_phase[k];
  380. /* get real and imag part and re-interleave */
  381. fft_worksp[2*k] = magn * cos(phase);
  382. fft_worksp[2*k+1] = magn * sin(phase);
  383. }
  384. /* zero negative frequencies */
  385. for (k = fft_frame_size + 2; k < 2 * fft_frame_size; k++) {
  386. fft_worksp[k] = 0.;
  387. }
  388. /* do inverse transform */
  389. smb_fft(fft_worksp, fft_frame_size, 1);
  390. /* do windowing and add to output accumulator */
  391. for (k = 0; k < fft_frame_size; k++) {
  392. window = -.5 * cos(2. * M_PI * (double) k / (double) fft_frame_size) + .5;
  393. output_accum[k] += 2. * window * fft_worksp[2*k] / (fft_frame_size2 * osamp);
  394. }
  395. for (k = 0; k < step_size; k++) {
  396. out_fifo[k] = output_accum[k];
  397. }
  398. /* shift accumulator */
  399. memmove(output_accum, output_accum+step_size, fft_frame_size * sizeof(float));
  400. /* move input FIFO */
  401. for (k = 0; k < in_fifo_latency; k++) {
  402. in_fifo[k] = in_fifo[k+step_size];
  403. }
  404. }
  405. }
  406. }
  407. static int pitch_shift(struct ast_frame *f, float amount, struct fft_data *fft)
  408. {
  409. int16_t *fun = (int16_t *) f->data.ptr;
  410. int samples;
  411. /* an amount of 1 has no effect */
  412. if (!amount || amount == 1 || !fun || (f->samples % 32)) {
  413. return 0;
  414. }
  415. for (samples = 0; samples < f->samples; samples += 32) {
  416. smb_pitch_shift(amount, 32, MAX_FRAME_LENGTH, 32, ast_format_get_sample_rate(f->subclass.format), fun+samples, fun+samples, fft);
  417. }
  418. return 0;
  419. }
  420. static struct ast_custom_function pitch_shift_function = {
  421. .name = "PITCH_SHIFT",
  422. .write = pitchshift_helper,
  423. };
  424. static int unload_module(void)
  425. {
  426. return ast_custom_function_unregister(&pitch_shift_function);
  427. }
  428. static int load_module(void)
  429. {
  430. int res = ast_custom_function_register(&pitch_shift_function);
  431. return res ? AST_MODULE_LOAD_DECLINE : AST_MODULE_LOAD_SUCCESS;
  432. }
  433. AST_MODULE_INFO_STANDARD_EXTENDED(ASTERISK_GPL_KEY, "Audio Effects Dialplan Functions");