dsp.c

   1 /*
   2  * Asterisk -- A telephony toolkit for Linux.
   3  *
   4  * Convenience Signal Processing routines
   5  *
   6  * Copyright (C) 2002, Digium
   7  *
   8  * Mark Spencer <markster@linux-support.net>
   9  *
  10  * This program is free software, distributed under the terms of
  11  * the GNU General Public License.
  12  *
  13  * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
  14  * DTMF detector.
  15  *
  16  */
  17
  18 /* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
  19 /*
  20         tone_detect.c - General telephony tone detection, and specific
  21                         detection of DTMF.
  22
  23         Copyright (C) 2001  Steve Underwood <steveu@coppice.org>
  24
  25         Despite my general liking of the GPL, I place this code in the
  26         public domain for the benefit of all mankind - even the slimy
  27         ones who might try to proprietize my work and use it to my
  28         detriment.
  29 */
  30
  31 #include <sys/types.h>
  32 #include <asterisk/frame.h>
  33 #include <asterisk/channel.h>
  34 #include <asterisk/channel_pvt.h>
  35 #include <asterisk/logger.h>
  36 #include <asterisk/dsp.h>
  37 #include <asterisk/ulaw.h>
  38 #include <asterisk/alaw.h>
  39 #include <stdlib.h>
  40 #include <unistd.h>
  41 #include <string.h>
  42 #include <math.h>
  43 #include <errno.h>
  44 #include <stdio.h>
  45
  46 #define DEFAULT_THRESHOLD 1024
  47
  48 #define BUSY_THRESHOLD  100             /* Max number of ms difference between max and min times in busy */
  49 #define BUSY_MIN                80              /* Busy must be at least 80 ms in half-cadence */
  50 #define BUSY_MAX                1100    /* Busy can't be longer than 1100 ms in half-cadence */
  51
  52 /* Remember last 3 units */
  53 #define DSP_HISTORY 5
  54
  55 /* Number of goertzels for progress detect */
  56 #define GSAMP_SIZE 183
  57
  58 #define HZ_350  0
  59 #define HZ_440  1
  60 #define HZ_480  2
  61 #define HZ_620  3
  62 #define HZ_950  4
  63 #define HZ_1400 5
  64 #define HZ_1800 6
  65
  66 #define TONE_THRESH 10.0        /* How much louder the tone should be than channel energy */
  67 #define TONE_MIN_THRESH 1e8     /* How much tone there should be at least to attempt */
  68 #define COUNT_THRESH  3         /* Need at least 50ms of stuff to count it */
  69
  70 #define TONE_STATE_SILENCE  0
  71 #define TONE_STATE_RINGING  1
  72 #define TONE_STATE_DIALTONE 2
  73 #define TONE_STATE_TALKING  3
  74 #define TONE_STATE_BUSY     4
  75 #define TONE_STATE_SPECIAL1     5
  76 #define TONE_STATE_SPECIAL2 6
  77 #define TONE_STATE_SPECIAL3 7
  78
  79 #define MAX_DTMF_DIGITS 128
  80
  81 /* Basic DTMF specs:
  82  *
  83  * Minimum tone on = 40ms
  84  * Minimum tone off = 50ms
  85  * Maximum digit rate = 10 per second
  86  * Normal twist <= 8dB accepted
  87  * Reverse twist <= 4dB accepted
  88  * S/N >= 15dB will detect OK
  89  * Attenuation <= 26dB will detect OK
  90  * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
  91  */
  92
  93 #define DTMF_THRESHOLD              8.0e7
  94 #define FAX_THRESHOLD              8.0e7
  95 #define FAX_2ND_HARMONIC                2.0     /* 4dB */
  96 #define DTMF_NORMAL_TWIST           6.3     /* 8dB */
  97 #define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5)     /* 4dB normal */
  98 #define DTMF_RELATIVE_PEAK_ROW      6.3     /* 8dB */
  99 #define DTMF_RELATIVE_PEAK_COL      6.3     /* 8dB */
 100 #define DTMF_2ND_HARMONIC_ROW       ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 1.7 : 2.5)     /* 4dB normal */
 101 #define DTMF_2ND_HARMONIC_COL       63.1    /* 18dB */
 102
 103 #define MF_THRESHOLD              8.0e7
 104 #define MF_NORMAL_TWIST           5.3     /* 8dB */
 105 #define MF_REVERSE_TWIST          4.0     /* was 2.5 */
 106 #define MF_RELATIVE_PEAK      5.3     /* 8dB */
 107 #define MF_2ND_HARMONIC       1.7 /* was 2.5  */
 108
 109 typedef struct {
 110         float v2;
 111         float v3;
 112         float fac;
 113 } goertzel_state_t;
 114
 115 typedef struct
 116 {
 117     int hit1;
 118     int hit2;
 119     int hit3;
 120     int hit4;
 121     int mhit;
 122
 123     goertzel_state_t row_out[4];
 124     goertzel_state_t col_out[4];
 125     goertzel_state_t row_out2nd[4];
 126     goertzel_state_t col_out2nd[4];
 127         goertzel_state_t fax_tone;
 128         goertzel_state_t fax_tone2nd;
 129     float energy;
 130
 131     int current_sample;
 132     char digits[MAX_DTMF_DIGITS + 1];
 133     int current_digits;
 134     int detected_digits;
 135     int lost_digits;
 136     int digit_hits[16];
 137         int fax_hits;
 138 } dtmf_detect_state_t;
 139
 140 typedef struct
 141 {
 142     int hit1;
 143     int hit2;
 144     int hit3;
 145     int hit4;
 146     int mhit;
 147
 148     goertzel_state_t tone_out[6];
 149     goertzel_state_t tone_out2nd[6];
 150     float energy;
 151
 152     int current_sample;
 153     char digits[MAX_DTMF_DIGITS + 1];
 154     int current_digits;
 155     int detected_digits;
 156     int lost_digits;
 157         int fax_hits;
 158 } mf_detect_state_t;
 159
 160 static float dtmf_row[] =
 161 {
 162      697.0,  770.0,  852.0,  941.0
 163 };
 164 static float dtmf_col[] =
 165 {
 166     1209.0, 1336.0, 1477.0, 1633.0
 167 };
 168
 169 static float mf_tones[] =
 170 {
 171         700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
 172 };
 173
 174 static float fax_freq = 1100.0;
 175
 176 static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";
 177
 178 static char mf_hit[6][6] = {
 179         /*  700 + */ {   0, '1', '2', '4', '7', 'C' },
 180         /*  900 + */ { '1',   0, '3', '5', '8', 'A' },
 181         /* 1100 + */ { '2', '3',   0, '6', '9', '*' },
 182         /* 1300 + */ { '4', '5', '6',   0, '0', 'B' },
 183         /* 1500 + */ { '7', '8', '9', '0',  0, '#' },
 184         /* 1700 + */ { 'C', 'A', '*', 'B', '#',  0  },
 185 };
 186
 187 static inline void goertzel_sample(goertzel_state_t *s, short sample)
 188 {
 189         float v1;
 190         float fsamp  = sample;
 191         v1 = s->v2;
 192         s->v2 = s->v3;
 193         s->v3 = s->fac * s->v2 - v1 + fsamp;
 194 }
 195
 196 static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
 197 {
 198         int i;
 199         for (i=0;i<count;i++)
 200                 goertzel_sample(s, samps[i]);
 201 }
 202
 203
 204 static inline float goertzel_result(goertzel_state_t *s)
 205 {
 206         return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
 207 }
 208
 209 static inline void goertzel_init(goertzel_state_t *s, float freq)
 210 {
 211         s->v2 = s->v3 = 0.0;
 212         s->fac = 2.0 * cos(2.0 * M_PI * (freq / 8000.0));
 213 }
 214
 215 static inline void goertzel_reset(goertzel_state_t *s)
 216 {
 217         s->v2 = s->v3 = 0.0;
 218 }
 219
 220 struct ast_dsp {
 221         struct ast_frame f;
 222         int threshold;
 223         int totalsilence;
 224         int totalnoise;
 225         int features;
 226         int busymaybe;
 227         int busycount;
 228         int historicnoise[DSP_HISTORY];
 229         int historicsilence[DSP_HISTORY];
 230         goertzel_state_t freqs[7];
 231         int gsamps;
 232         int tstate;
 233         int tcount;
 234         int digitmode;
 235         int thinkdigit;
 236         float genergy;
 237         union {
 238                 dtmf_detect_state_t dtmf;
 239                 mf_detect_state_t mf;
 240         } td;
 241 };
 242
 243 static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
 244 {
 245     int i;
 246
 247     s->hit1 =
 248     s->hit2 = 0;
 249
 250     for (i = 0;  i < 4;  i++)
 251     {
 252
 253                 goertzel_init (&s->row_out[i], dtmf_row[i]);
 254         goertzel_init (&s->col_out[i], dtmf_col[i]);
 255         goertzel_init (&s->row_out2nd[i], dtmf_row[i] * 2.0);
 256         goertzel_init (&s->col_out2nd[i], dtmf_col[i] * 2.0);
 257
 258                 s->energy = 0.0;
 259     }
 260
 261         /* Same for the fax dector */
 262     goertzel_init (&s->fax_tone, fax_freq);
 263
 264         /* Same for the fax dector 2nd harmonic */
 265     goertzel_init (&s->fax_tone2nd, fax_freq * 2.0);
 266
 267     s->current_sample = 0;
 268     s->detected_digits = 0;
 269         s->current_digits = 0;
 270         memset(&s->digits, 0, sizeof(s->digits));
 271     s->lost_digits = 0;
 272     s->digits[0] = '\0';
 273     s->mhit = 0;
 274 }
 275
 276 static void ast_mf_detect_init (mf_detect_state_t *s)
 277 {
 278     int i;
 279
 280     s->hit1 =
 281     s->hit2 = 0;
 282
 283     for (i = 0;  i < 6;  i++)
 284     {
 285
 286                 goertzel_init (&s->tone_out[i], mf_tones[i]);
 287         goertzel_init (&s->tone_out2nd[i], mf_tones[i] * 2.0);
 288
 289                 s->energy = 0.0;
 290     }
 291
 292         s->current_digits = 0;
 293         memset(&s->digits, 0, sizeof(s->digits));
 294     s->current_sample = 0;
 295     s->detected_digits = 0;
 296     s->lost_digits = 0;
 297     s->digits[0] = '\0';
 298     s->mhit = 0;
 299 }
 300
 301 static int dtmf_detect (dtmf_detect_state_t *s,
 302                  int16_t amp[],
 303                  int samples,
 304                  int digitmode, int *writeback)
 305 {
 306
 307     float row_energy[4];
 308     float col_energy[4];
 309     float fax_energy;
 310     float fax_energy_2nd;
 311     float famp;
 312     float v1;
 313     int i;
 314     int j;
 315     int sample;
 316     int best_row;
 317     int best_col;
 318     int hit;
 319     int limit;
 320
 321     hit = 0;
 322     for (sample = 0;  sample < samples;  sample = limit)
 323     {
 324         /* 102 is optimised to meet the DTMF specs. */
 325         if ((samples - sample) >= (102 - s->current_sample))
 326             limit = sample + (102 - s->current_sample);
 327         else
 328             limit = samples;
 329 #if defined(USE_3DNOW)
 330         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
 331         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
 332         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
 333         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
 334                 /* XXX Need to fax detect for 3dnow too XXX */
 335                 #warning "Fax Support Broken"
 336 #else
 337         /* The following unrolled loop takes only 35% (rough estimate) of the
 338            time of a rolled loop on the machine on which it was developed */
 339         for (j = sample;  j < limit;  j++)
 340         {
 341             famp = amp[j];
 342
 343             s->energy += famp*famp;
 344
 345             /* With GCC 2.95, the following unrolled code seems to take about 35%
 346                (rough estimate) as long as a neat little 0-3 loop */
 347             v1 = s->row_out[0].v2;
 348             s->row_out[0].v2 = s->row_out[0].v3;
 349             s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
 350
 351             v1 = s->col_out[0].v2;
 352             s->col_out[0].v2 = s->col_out[0].v3;
 353             s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
 354
 355             v1 = s->row_out[1].v2;
 356             s->row_out[1].v2 = s->row_out[1].v3;
 357             s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
 358
 359             v1 = s->col_out[1].v2;
 360             s->col_out[1].v2 = s->col_out[1].v3;
 361             s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
 362
 363             v1 = s->row_out[2].v2;
 364             s->row_out[2].v2 = s->row_out[2].v3;
 365             s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
 366
 367             v1 = s->col_out[2].v2;
 368             s->col_out[2].v2 = s->col_out[2].v3;
 369             s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
 370
 371             v1 = s->row_out[3].v2;
 372             s->row_out[3].v2 = s->row_out[3].v3;
 373             s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;
 374
 375             v1 = s->col_out[3].v2;
 376             s->col_out[3].v2 = s->col_out[3].v3;
 377             s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;
 378
 379             v1 = s->col_out2nd[0].v2;
 380             s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
 381             s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
 382
 383             v1 = s->row_out2nd[0].v2;
 384             s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
 385             s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
 386
 387             v1 = s->col_out2nd[1].v2;
 388             s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
 389             s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
 390
 391             v1 = s->row_out2nd[1].v2;
 392             s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
 393             s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
 394
 395             v1 = s->col_out2nd[2].v2;
 396             s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
 397             s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
 398
 399             v1 = s->row_out2nd[2].v2;
 400             s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
 401             s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
 402
 403             v1 = s->col_out2nd[3].v2;
 404             s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
 405             s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
 406
 407             v1 = s->row_out2nd[3].v2;
 408             s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
 409             s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;
 410
 411                         /* Update fax tone */
 412             v1 = s->fax_tone.v2;
 413             s->fax_tone.v2 = s->fax_tone.v3;
 414             s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;
 415
 416             v1 = s->fax_tone.v2;
 417             s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
 418             s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
 419         }
 420 #endif
 421         s->current_sample += (limit - sample);
 422         if (s->current_sample < 102) {
 423                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
 424                                 /* If we had a hit last time, go ahead and clear this out since likely it
 425                                    will be another hit */
 426                                 for (i=sample;i<limit;i++)
 427                                         amp[i] = 0;
 428                                 *writeback = 1;
 429                         }
 430             continue;
 431                 }
 432
 433                 /* Detect the fax energy, too */
 434                 fax_energy = goertzel_result(&s->fax_tone);
 435
 436         /* We are at the end of a DTMF detection block */
 437         /* Find the peak row and the peak column */
 438         row_energy[0] = goertzel_result (&s->row_out[0]);
 439         col_energy[0] = goertzel_result (&s->col_out[0]);
 440
 441         for (best_row = best_col = 0, i = 1;  i < 4;  i++)
 442         {
 443             row_energy[i] = goertzel_result (&s->row_out[i]);
 444             if (row_energy[i] > row_energy[best_row])
 445                 best_row = i;
 446             col_energy[i] = goertzel_result (&s->col_out[i]);
 447             if (col_energy[i] > col_energy[best_col])
 448                 best_col = i;
 449         }
 450         hit = 0;
 451         /* Basic signal level test and the twist test */
 452         if (row_energy[best_row] >= DTMF_THRESHOLD
 453             &&
 454             col_energy[best_col] >= DTMF_THRESHOLD
 455             &&
 456             col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST
 457             &&
 458             col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row])
 459         {
 460             /* Relative peak test */
 461             for (i = 0;  i < 4;  i++)
 462             {
 463                 if ((i != best_col  &&  col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col])
 464                     ||
 465                     (i != best_row  &&  row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row]))
 466                 {
 467                     break;
 468                 }
 469             }
 470             /* ... and second harmonic test */
 471             if (i >= 4
 472                 &&
 473                 (row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy
 474                 &&
 475                 goertzel_result (&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
 476                 &&
 477                 goertzel_result (&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row])
 478             {
 479                                 /* Got a hit */
 480                 hit = dtmf_positions[(best_row << 2) + best_col];
 481                                 if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
 482                                         /* Zero out frame data if this is part DTMF */
 483                                         for (i=sample;i<limit;i++)
 484                                                 amp[i] = 0;
 485                                         *writeback = 1;
 486                                 }
 487                 /* Look for two successive similar results */
 488                 /* The logic in the next test is:
 489                    We need two successive identical clean detects, with
 490                    something different preceeding it. This can work with
 491                    back to back differing digits. More importantly, it
 492                    can work with nasty phones that give a very wobbly start
 493                    to a digit. */
 494                 if (hit == s->hit3  &&  s->hit3 != s->hit2)
 495                 {
 496                     s->mhit = hit;
 497                     s->digit_hits[(best_row << 2) + best_col]++;
 498                     s->detected_digits++;
 499                     if (s->current_digits < MAX_DTMF_DIGITS)
 500                     {
 501                         s->digits[s->current_digits++] = hit;
 502                         s->digits[s->current_digits] = '\0';
 503                     }
 504                     else
 505                     {
 506                         s->lost_digits++;
 507                     }
 508                 }
 509             }
 510         }
 511                 if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy > s->energy * 21.0)) {
 512                                 fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
 513                                 if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
 514 #if 0
 515                                         printf("Fax energy/Second Harmonic: %f/%f\n", fax_energy, fax_energy_2nd);
 516 #endif
 517                                         /* XXX Probably need better checking than just this the energy XXX */
 518                                         hit = 'f';
 519                                         s->fax_hits++;
 520                                 } /* Don't reset fax hits counter */
 521                 } else {
 522                         if (s->fax_hits > 5) {
 523                                  hit = 'f';
 524                                  s->mhit = 'f';
 525                      s->detected_digits++;
 526                      if (s->current_digits < MAX_DTMF_DIGITS)
 527                      {
 528                           s->digits[s->current_digits++] = hit;
 529                           s->digits[s->current_digits] = '\0';
 530                      }
 531                      else
 532                      {
 533                            s->lost_digits++;
 534                      }
 535                         }
 536                         s->fax_hits = 0;
 537                 }
 538         s->hit1 = s->hit2;
 539         s->hit2 = s->hit3;
 540         s->hit3 = hit;
 541         /* Reinitialise the detector for the next block */
 542         for (i = 0;  i < 4;  i++)
 543         {
 544             goertzel_reset(&s->row_out[i]);
 545             goertzel_reset(&s->col_out[i]);
 546             goertzel_reset(&s->row_out2nd[i]);
 547             goertzel_reset(&s->col_out2nd[i]);
 548         }
 549         goertzel_reset (&s->fax_tone);
 550         goertzel_reset (&s->fax_tone2nd);
 551                 s->energy = 0.0;
 552         s->current_sample = 0;
 553     }
 554     if ((!s->mhit) || (s->mhit != hit))
 555     {
 556         s->mhit = 0;
 557         return(0);
 558     }
 559     return (hit);
 560 }
 561
 562 /* MF goertzel size */
 563 #define MF_GSIZE 160
 564
 565 static int mf_detect (mf_detect_state_t *s,
 566                  int16_t amp[],
 567                  int samples,
 568                  int digitmode, int *writeback)
 569 {
 570
 571     float tone_energy[6];
 572     float famp;
 573     float v1;
 574     int i;
 575     int j;
 576     int sample;
 577     int best1;
 578     int best2;
 579         float max;
 580     int hit;
 581     int limit;
 582         int sofarsogood;
 583
 584     hit = 0;
 585     for (sample = 0;  sample < samples;  sample = limit)
 586     {
 587         /* 80 is optimised to meet the MF specs. */
 588         if ((samples - sample) >= (MF_GSIZE - s->current_sample))
 589             limit = sample + (MF_GSIZE - s->current_sample);
 590         else
 591             limit = samples;
 592 #if defined(USE_3DNOW)
 593         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
 594         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
 595         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
 596         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
 597                 /* XXX Need to fax detect for 3dnow too XXX */
 598                 #warning "Fax Support Broken"
 599 #else
 600         /* The following unrolled loop takes only 35% (rough estimate) of the
 601            time of a rolled loop on the machine on which it was developed */
 602         for (j = sample;  j < limit;  j++)
 603         {
 604             famp = amp[j];
 605
 606             s->energy += famp*famp;
 607
 608             /* With GCC 2.95, the following unrolled code seems to take about 35%
 609                (rough estimate) as long as a neat little 0-3 loop */
 610             v1 = s->tone_out[0].v2;
 611             s->tone_out[0].v2 = s->tone_out[0].v3;
 612             s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
 613
 614             v1 = s->tone_out[1].v2;
 615             s->tone_out[1].v2 = s->tone_out[1].v3;
 616             s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
 617
 618             v1 = s->tone_out[2].v2;
 619             s->tone_out[2].v2 = s->tone_out[2].v3;
 620             s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
 621
 622             v1 = s->tone_out[3].v2;
 623             s->tone_out[3].v2 = s->tone_out[3].v3;
 624             s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
 625
 626             v1 = s->tone_out[4].v2;
 627             s->tone_out[4].v2 = s->tone_out[4].v3;
 628             s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
 629
 630             v1 = s->tone_out[5].v2;
 631             s->tone_out[5].v2 = s->tone_out[5].v3;
 632             s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
 633
 634             v1 = s->tone_out2nd[0].v2;
 635             s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
 636             s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
 637
 638             v1 = s->tone_out2nd[1].v2;
 639             s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
 640             s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
 641
 642             v1 = s->tone_out2nd[2].v2;
 643             s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
 644             s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
 645
 646             v1 = s->tone_out2nd[3].v2;
 647             s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
 648             s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
 649
 650             v1 = s->tone_out2nd[4].v2;
 651             s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
 652             s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
 653
 654             v1 = s->tone_out2nd[3].v2;
 655             s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
 656             s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
 657
 658         }
 659 #endif
 660         s->current_sample += (limit - sample);
 661         if (s->current_sample < MF_GSIZE) {
 662                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
 663                                 /* If we had a hit last time, go ahead and clear this out since likely it
 664                                    will be another hit */
 665                                 for (i=sample;i<limit;i++)
 666                                         amp[i] = 0;
 667                                 *writeback = 1;
 668                         }
 669             continue;
 670                 }
 671
 672                 /* We're at the end of an MF detection block.  Go ahead and calculate
 673                    all the energies. */
 674                 for (i=0;i<6;i++) {
 675                         tone_energy[i] = goertzel_result(&s->tone_out[i]);
 676                 }
 677
 678                 /* Find highest */
 679                 best1 = 0;
 680                 max = tone_energy[0];
 681                 for (i=1;i<6;i++) {
 682                         if (tone_energy[i] > max) {
 683                                 max = tone_energy[i];
 684                                 best1 = i;
 685                         }
 686                 }
 687
 688                 /* Find 2nd highest */
 689                 if (best1)
 690                         max = tone_energy[0];
 691                 else
 692                         max = tone_energy[1];
 693                 best2 = 0;
 694                 for (i=0;i<6;i++) {
 695                         if (i == best1) continue;
 696                         if (tone_energy[i] > max) {
 697                                 max = tone_energy[i];
 698                                 best2 = i;
 699                         }
 700                 }
 701
 702         hit = 0;
 703                 sofarsogood=1;
 704                 /* Check for relative energies */
 705                 for (i=0;i<6;i++) {
 706                         if (i == best1) continue;
 707                         if (i == best2) continue;
 708                         if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
 709                                 sofarsogood = 0;
 710                                 break;
 711                         }
 712                         if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
 713                                 sofarsogood = 0;
 714                                 break;
 715                         }
 716                 }
 717
 718                 if (sofarsogood) {
 719                         /* Check for 2nd harmonic */
 720                         if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1])
 721                                 sofarsogood = 0;
 722                         else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
 723                                 sofarsogood = 0;
 724                 }
 725                 if (sofarsogood) {
 726                         hit = mf_hit[best1][best2];
 727                         if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
 728                                 /* Zero out frame data if this is part DTMF */
 729                                 for (i=sample;i<limit;i++)
 730                                         amp[i] = 0;
 731                                 *writeback = 1;
 732                         }
 733                         /* Look for two consecutive clean hits */
 734                         if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
 735                                 s->mhit = hit;
 736                                 s->detected_digits++;
 737                                 if (s->current_digits < MAX_DTMF_DIGITS - 2) {
 738                                         s->digits[s->current_digits++] = hit;
 739                                         s->digits[s->current_digits] = '\0';
 740                                 } else {
 741                                         s->lost_digits++;
 742                                 }
 743                         }
 744                 }
 745
 746         s->hit1 = s->hit2;
 747         s->hit2 = s->hit3;
 748         s->hit3 = hit;
 749         /* Reinitialise the detector for the next block */
 750         for (i = 0;  i < 6;  i++)
 751         {
 752             goertzel_reset(&s->tone_out[i]);
 753             goertzel_reset(&s->tone_out2nd[i]);
 754         }
 755                 s->energy = 0.0;
 756         s->current_sample = 0;
 757     }
 758     if ((!s->mhit) || (s->mhit != hit))
 759     {
 760                 s->mhit = 0;
 761                 return(0);
 762     }
 763     return (hit);
 764 }
 765
 766 static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
 767 {
 768         int res;
 769         if (dsp->digitmode & DSP_DIGITMODE_MF)
 770                 res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
 771         else
 772                 res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
 773         return res;
 774 }
 775
 776 int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf)
 777 {
 778         short *s;
 779         int len;
 780         int ign=0;
 781         if (inf->frametype != AST_FRAME_VOICE) {
 782                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
 783                 return 0;
 784         }
 785         if (inf->subclass != AST_FORMAT_SLINEAR) {
 786                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
 787                 return 0;
 788         }
 789         s = inf->data;
 790         len = inf->datalen / 2;
 791         return __ast_dsp_digitdetect(dsp, s, len, &ign);
 792 }
 793
 794 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
 795 {
 796         /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
 797         /* Make sure absolute levels are high enough */
 798         if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
 799                 return 0;
 800         /* Amplify ignored stuff */
 801         i2 *= TONE_THRESH;
 802         i1 *= TONE_THRESH;
 803         e *= TONE_THRESH;
 804         /* Check first tone */
 805         if ((p1 < i1) || (p1 < i2) || (p1 < e))
 806                 return 0;
 807         /* And second */
 808         if ((p2 < i1) || (p2 < i2) || (p2 < e))
 809                 return 0;
 810         /* Guess it's there... */
 811         return 1;
 812 }
 813
 814 int ast_dsp_getdigits (struct ast_dsp *dsp,
 815               char *buf,
 816               int max)
 817 {
 818         if (dsp->digitmode & DSP_DIGITMODE_MF) {
 819             if (max > dsp->td.mf.current_digits)
 820                 max = dsp->td.mf.current_digits;
 821             if (max > 0)
 822             {
 823                 memcpy (buf, dsp->td.mf.digits, max);
 824                 memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
 825                 dsp->td.mf.current_digits -= max;
 826             }
 827             buf[max] = '\0';
 828             return  max;
 829         } else {
 830             if (max > dsp->td.dtmf.current_digits)
 831                 max = dsp->td.dtmf.current_digits;
 832             if (max > 0)
 833             {
 834                 memcpy (buf, dsp->td.dtmf.digits, max);
 835                 memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
 836                 dsp->td.dtmf.current_digits -= max;
 837             }
 838             buf[max] = '\0';
 839             return  max;
 840         }
 841 }
 842
 843 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
 844 {
 845         int x;
 846         int pass;
 847         int newstate = TONE_STATE_SILENCE;
 848         int res = 0;
 849         while(len) {
 850                 /* Take the lesser of the number of samples we need and what we have */
 851                 pass = len;
 852                 if (pass > GSAMP_SIZE - dsp->gsamps)
 853                         pass = GSAMP_SIZE - dsp->gsamps;
 854                 for (x=0;x<pass;x++) {
 855                         goertzel_sample(&dsp->freqs[HZ_350], s[x]);
 856                         goertzel_sample(&dsp->freqs[HZ_440], s[x]);
 857                         goertzel_sample(&dsp->freqs[HZ_480], s[x]);
 858                         goertzel_sample(&dsp->freqs[HZ_620], s[x]);
 859                         goertzel_sample(&dsp->freqs[HZ_950], s[x]);
 860                         goertzel_sample(&dsp->freqs[HZ_1400], s[x]);
 861                         goertzel_sample(&dsp->freqs[HZ_1800], s[x]);
 862                         dsp->genergy += s[x] * s[x];
 863                 }
 864                 s += pass;
 865                 dsp->gsamps += pass;
 866                 len -= pass;
 867                 if (dsp->gsamps == GSAMP_SIZE) {
 868                         float hz_350;
 869                         float hz_440;
 870                         float hz_480;
 871                         float hz_620;
 872                         float hz_950;
 873                         float hz_1400;
 874                         float hz_1800;
 875                         hz_350 = goertzel_result(&dsp->freqs[HZ_350]);
 876                         hz_440 = goertzel_result(&dsp->freqs[HZ_440]);
 877                         hz_480 = goertzel_result(&dsp->freqs[HZ_480]);
 878                         hz_620 = goertzel_result(&dsp->freqs[HZ_620]);
 879                         hz_950 = goertzel_result(&dsp->freqs[HZ_950]);
 880                         hz_1400 = goertzel_result(&dsp->freqs[HZ_1400]);
 881                         hz_1800 = goertzel_result(&dsp->freqs[HZ_1800]);
 882 #if 0
 883                         printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n",
 884                                 hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
 885 #endif
 886                         if (pair_there(hz_480, hz_620, hz_350, hz_440, dsp->genergy)) {
 887                                 newstate = TONE_STATE_BUSY;
 888                         } else if (pair_there(hz_440, hz_480, hz_350, hz_620, dsp->genergy)) {
 889                                 newstate = TONE_STATE_RINGING;
 890                         } else if (pair_there(hz_350, hz_440, hz_480, hz_620, dsp->genergy)) {
 891                                 newstate = TONE_STATE_DIALTONE;
 892                         } else if (hz_950 > TONE_MIN_THRESH * TONE_THRESH) {
 893                                 newstate = TONE_STATE_SPECIAL1;
 894                         } else if (hz_1400 > TONE_MIN_THRESH * TONE_THRESH) {
 895                                 if (dsp->tstate == TONE_STATE_SPECIAL1)
 896                                         newstate = TONE_STATE_SPECIAL2;
 897                         } else if (hz_1800 > TONE_MIN_THRESH * TONE_THRESH) {
 898                                 if (dsp->tstate == TONE_STATE_SPECIAL2)
 899                                         newstate = TONE_STATE_SPECIAL3;
 900                         } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
 901                                 newstate = TONE_STATE_TALKING;
 902                         } else
 903                                 newstate = TONE_STATE_SILENCE;
 904
 905                         if (newstate == dsp->tstate) {
 906                                 dsp->tcount++;
 907                                 if (dsp->tcount == COUNT_THRESH) {
 908                                         if (dsp->tstate == TONE_STATE_BUSY) {
 909                                                 res = AST_CONTROL_BUSY;
 910                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
 911                                         } else if (dsp->tstate == TONE_STATE_TALKING) {
 912                                                 res = AST_CONTROL_ANSWER;
 913                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
 914                                         } else if (dsp->tstate == TONE_STATE_RINGING)
 915                                                 res = AST_CONTROL_RINGING;
 916                                         else if (dsp->tstate == TONE_STATE_SPECIAL3) {
 917                                                 res = AST_CONTROL_CONGESTION;
 918                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
 919                                         }
 920
 921                                 }
 922                         } else {
 923 #if 0
 924                                 printf("Newstate: %d\n", newstate);
 925 #endif
 926                                 dsp->tstate = newstate;
 927                                 dsp->tcount = 1;
 928                         }
 929
 930                         /* Reset goertzel */
 931                         for (x=0;x<7;x++)
 932                                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
 933                         dsp->gsamps = 0;
 934                         dsp->genergy = 0.0;
 935                 }
 936         }
 937 #if 0
 938         if (res)
 939                 printf("Returning %d\n", res);
 940 #endif
 941         return res;
 942 }
 943
 944 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
 945 {
 946         if (inf->frametype != AST_FRAME_VOICE) {
 947                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
 948                 return 0;
 949         }
 950         if (inf->subclass != AST_FORMAT_SLINEAR) {
 951                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
 952                 return 0;
 953         }
 954         return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
 955 }
 956
 957 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
 958 {
 959         int accum;
 960         int x;
 961         int res = 0;
 962
 963         accum = 0;
 964         for (x=0;x<len; x++)
 965                 accum += abs(s[x]);
 966         accum /= x;
 967         if (accum < dsp->threshold) {
 968                 dsp->totalsilence += len/8;
 969                 if (dsp->totalnoise) {
 970                         /* Move and save history */
 971                         memmove(dsp->historicnoise, dsp->historicnoise + 1, sizeof(dsp->historicnoise) - sizeof(dsp->historicnoise[0]));
 972                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
 973                         dsp->busymaybe = 1;
 974                 }
 975                 dsp->totalnoise = 0;
 976                 res = 1;
 977         } else {
 978                 dsp->totalnoise += len/8;
 979                 if (dsp->totalsilence) {
 980                         /* Move and save history */
 981                         memmove(dsp->historicsilence, dsp->historicsilence + 1, sizeof(dsp->historicsilence) - sizeof(dsp->historicsilence[0]));
 982                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
 983                         dsp->busymaybe = 1;
 984                 }
 985                 dsp->totalsilence = 0;
 986         }
 987         if (totalsilence)
 988                 *totalsilence = dsp->totalsilence;
 989         return res;
 990 }
 991
 992 int ast_dsp_busydetect(struct ast_dsp *dsp)
 993 {
 994         int x;
 995         int res = 0;
 996         int max, min;
 997         if (dsp->busymaybe) {
 998 #if 0
 999                 printf("Maybe busy!\n");
1000 #endif
1001                 dsp->busymaybe = 0;
1002                 min = 9999;
1003                 max = 0;
1004                 for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1005 #if 0
1006                         printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
1007 #endif
1008                         if (dsp->historicsilence[x] < min)
1009                                 min = dsp->historicsilence[x];
1010                         if (dsp->historicnoise[x] < min)
1011                                 min = dsp->historicnoise[x];
1012                         if (dsp->historicsilence[x] > max)
1013                                 max = dsp->historicsilence[x];
1014                         if (dsp->historicnoise[x] > max)
1015                                 max = dsp->historicnoise[x];
1016                 }
1017                 if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
1018 #if 0
1019                         printf("Busy!\n");
1020 #endif
1021                         res = 1;
1022                 }
1023 #if 0
1024                 printf("Min: %d, max: %d\n", min, max);
1025 #endif
1026         }
1027         return res;
1028 }
1029
1030 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1031 {
1032         short *s;
1033         int len;
1034
1035         if (f->frametype != AST_FRAME_VOICE) {
1036                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1037                 return 0;
1038         }
1039         if (f->subclass != AST_FORMAT_SLINEAR) {
1040                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1041                 return 0;
1042         }
1043         s = f->data;
1044         len = f->datalen/2;
1045         return __ast_dsp_silence(dsp, s, len, totalsilence);
1046 }
1047
1048 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af, int needlock)
1049 {
1050         int silence;
1051         int res;
1052         int digit;
1053         int x;
1054         unsigned short *shortdata;
1055         unsigned char *odata;
1056         int len;
1057         int writeback = 0;
1058
1059 #define FIX_INF(inf) do { \
1060                 if (writeback) { \
1061                         switch(inf->subclass) { \
1062                         case AST_FORMAT_SLINEAR: \
1063                                 break; \
1064                         case AST_FORMAT_ULAW: \
1065                                 for (x=0;x<len;x++) \
1066                                         odata[x] = AST_LIN2MU(shortdata[x]); \
1067                                 break; \
1068                         case AST_FORMAT_ALAW: \
1069                                 for (x=0;x<len;x++) \
1070                                         odata[x] = AST_LIN2A(shortdata[x]); \
1071                                 break; \
1072                         } \
1073                 } \
1074         } while(0)
1075
1076         if (!af)
1077                 return NULL;
1078         if (af->frametype != AST_FRAME_VOICE)
1079                 return af;
1080         odata = af->data;
1081         len = af->datalen;
1082         /* Make sure we have short data */
1083         switch(af->subclass) {
1084         case AST_FORMAT_SLINEAR:
1085                 shortdata = af->data;
1086                 len = af->datalen / 2;
1087                 break;
1088         case AST_FORMAT_ULAW:
1089                 shortdata = alloca(af->datalen * 2);
1090                 if (!shortdata) {
1091                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1092                         return af;
1093                 }
1094                 for (x=0;x<len;x++)
1095                         shortdata[x] = AST_MULAW(odata[x]);
1096                 break;
1097         case AST_FORMAT_ALAW:
1098                 shortdata = alloca(af->datalen * 2);
1099                 if (!shortdata) {
1100                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1101                         return af;
1102                 }
1103                 for (x=0;x<len;x++)
1104                         shortdata[x] = AST_ALAW(odata[x]);
1105                 break;
1106         default:
1107                 ast_log(LOG_WARNING, "Unable to detect process %d frames\n", af->subclass);
1108                 return af;
1109         }
1110         silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
1111         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1112                 memset(&dsp->f, 0, sizeof(dsp->f));
1113                 dsp->f.frametype = AST_FRAME_NULL;
1114                 return &dsp->f;
1115         }
1116         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1117                 memset(&dsp->f, 0, sizeof(dsp->f));
1118                 dsp->f.frametype = AST_FRAME_CONTROL;
1119                 dsp->f.subclass = AST_CONTROL_BUSY;
1120                 return &dsp->f;
1121         }
1122         if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
1123                 digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
1124 #if 0
1125                 if (digit)
1126                         printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
1127 #endif
1128                 if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
1129                         if (!dsp->thinkdigit) {
1130                                 if (digit) {
1131                                         /* Looks like we might have something.  Request a conference mute for the moment */
1132                                         memset(&dsp->f, 0, sizeof(dsp->f));
1133                                         dsp->f.frametype = AST_FRAME_DTMF;
1134                                         dsp->f.subclass = 'm';
1135                                         dsp->thinkdigit = 'x';
1136                                         FIX_INF(af);
1137                                         if (chan)
1138                                                 ast_queue_frame(chan, af, needlock);
1139                                         ast_frfree(af);
1140                                         return &dsp->f;
1141                                 }
1142                         } else {
1143                                 if (digit) {
1144                                         /* Thought we saw one last time.  Pretty sure we really have now */
1145                                         if (dsp->thinkdigit) {
1146                                                 if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
1147                                                         /* If we found a digit, and we're changing digits, go
1148                                                            ahead and send this one, but DON'T stop confmute because
1149                                                            we're detecting something else, too... */
1150                                                         memset(&dsp->f, 0, sizeof(dsp->f));
1151                                                         dsp->f.frametype = AST_FRAME_DTMF;
1152                                                         dsp->f.subclass = dsp->thinkdigit;
1153                                                         FIX_INF(af);
1154                                                         if (chan)
1155                                                                 ast_queue_frame(chan, af, needlock);
1156                                                         ast_frfree(af);
1157                                                 }
1158                                                 dsp->thinkdigit = digit;
1159                                                 return &dsp->f;
1160                                         }
1161                                         dsp->thinkdigit = digit;
1162                                 } else {
1163                                         if (dsp->thinkdigit) {
1164                                                 memset(&dsp->f, 0, sizeof(dsp->f));
1165                                                 if (dsp->thinkdigit != 'x') {
1166                                                         /* If we found a digit, send it now */
1167                                                         dsp->f.frametype = AST_FRAME_DTMF;
1168                                                         dsp->f.subclass = dsp->thinkdigit;
1169                                                         if (chan)
1170                                                                 ast_queue_frame(chan, &dsp->f, needlock);
1171                                                 }
1172                                                 dsp->f.frametype = AST_FRAME_DTMF;
1173                                                 dsp->f.subclass = 'u';
1174                                                 dsp->thinkdigit = 0;
1175                                                 FIX_INF(af);
1176                                                 if (chan)
1177                                                         ast_queue_frame(chan, af, needlock);
1178                                                 ast_frfree(af);
1179                                                 return &dsp->f;
1180                                         }
1181                                 }
1182                         }
1183                 } else if (!digit) {
1184                         /* Only check when there is *not* a hit... */
1185                         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1186                                 if (dsp->td.mf.current_digits) {
1187                                         memset(&dsp->f, 0, sizeof(dsp->f));
1188                                         dsp->f.frametype = AST_FRAME_DTMF;
1189                                         dsp->f.subclass = dsp->td.mf.digits[0];
1190                                         memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
1191                                         dsp->td.mf.current_digits--;
1192                                         FIX_INF(af);
1193                                         if (chan)
1194                                                 ast_queue_frame(chan, af, needlock);
1195                                         ast_frfree(af);
1196                                         return &dsp->f;
1197                                 }
1198                         } else {
1199                                 if (dsp->td.dtmf.current_digits) {
1200                                         memset(&dsp->f, 0, sizeof(dsp->f));
1201                                         dsp->f.frametype = AST_FRAME_DTMF;
1202                                         dsp->f.subclass = dsp->td.dtmf.digits[0];
1203                                         memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
1204                                         dsp->td.dtmf.current_digits--;
1205                                         FIX_INF(af);
1206                                         if (chan)
1207                                                 ast_queue_frame(chan, af, needlock);
1208                                         ast_frfree(af);
1209                                         return &dsp->f;
1210                                 }
1211                         }
1212                 }
1213         }
1214         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1215                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1216                 memset(&dsp->f, 0, sizeof(dsp->f));
1217                 dsp->f.frametype = AST_FRAME_CONTROL;
1218                 if (res) {
1219                         switch(res) {
1220                         case AST_CONTROL_ANSWER:
1221                         case AST_CONTROL_BUSY:
1222                         case AST_CONTROL_RINGING:
1223                         case AST_CONTROL_CONGESTION:
1224                                 dsp->f.subclass = res;
1225                                 if (chan)
1226                                         ast_queue_frame(chan, &dsp->f, needlock);
1227                                 break;
1228                         default:
1229                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1230                         }
1231                 }
1232         }
1233         FIX_INF(af);
1234         return af;
1235 }
1236
1237 struct ast_dsp *ast_dsp_new(void)
1238 {
1239         struct ast_dsp *dsp;
1240         dsp = malloc(sizeof(struct ast_dsp));
1241         if (dsp) {
1242                 memset(dsp, 0, sizeof(struct ast_dsp));
1243                 dsp->threshold = DEFAULT_THRESHOLD;
1244                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1245                 dsp->busycount = 3;
1246                 /* Initialize goertzels */
1247                 goertzel_init(&dsp->freqs[HZ_350], 350.0);
1248                 goertzel_init(&dsp->freqs[HZ_440], 440.0);
1249                 goertzel_init(&dsp->freqs[HZ_480], 480.0);
1250                 goertzel_init(&dsp->freqs[HZ_620], 620.0);
1251                 goertzel_init(&dsp->freqs[HZ_950], 950.0);
1252                 goertzel_init(&dsp->freqs[HZ_1400], 1400.0);
1253                 goertzel_init(&dsp->freqs[HZ_1800], 1800.0);
1254                 /* Initialize DTMF detector */
1255                 ast_dtmf_detect_init(&dsp->td.dtmf);
1256         }
1257         return dsp;
1258 }
1259
1260 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1261 {
1262         dsp->features = features;
1263 }
1264
1265 void ast_dsp_free(struct ast_dsp *dsp)
1266 {
1267         free(dsp);
1268 }
1269
1270 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
1271 {
1272         dsp->threshold = threshold;
1273 }
1274
1275 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1276 {
1277         if (cadences < 1)
1278                 cadences = 1;
1279         if (cadences > DSP_HISTORY)
1280                 cadences = DSP_HISTORY;
1281         dsp->busycount = cadences;
1282 }
1283
1284 void ast_dsp_digitreset(struct ast_dsp *dsp)
1285 {
1286         int i;
1287         dsp->thinkdigit = 0;
1288         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1289                 memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
1290                 dsp->td.mf.current_digits = 0;
1291                 /* Reinitialise the detector for the next block */
1292                 for (i = 0;  i < 6;  i++) {
1293                 goertzel_reset(&dsp->td.mf.tone_out[i]);
1294                     goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
1295                 }
1296                 dsp->td.mf.energy = 0.0;
1297                 dsp->td.mf.current_sample = 0;
1298             dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
1299         } else {
1300                 memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
1301                 dsp->td.dtmf.current_digits = 0;
1302                 /* Reinitialise the detector for the next block */
1303                 for (i = 0;  i < 4;  i++) {
1304                 goertzel_reset(&dsp->td.dtmf.row_out[i]);
1305                     goertzel_reset(&dsp->td.dtmf.col_out[i]);
1306                 goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
1307                 goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
1308                 }
1309             goertzel_reset (&dsp->td.dtmf.fax_tone);
1310             goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
1311                 dsp->td.dtmf.energy = 0.0;
1312                 dsp->td.dtmf.current_sample = 0;
1313             dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
1314         }
1315 }
1316
1317 void ast_dsp_reset(struct ast_dsp *dsp)
1318 {
1319         int x;
1320         dsp->totalsilence = 0;
1321         dsp->gsamps = 0;
1322         for (x=0;x<4;x++)
1323                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1324         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1325         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
1326
1327 }
1328
1329 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
1330 {
1331         int new, old;
1332         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1333         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1334         if (old != new) {
1335                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1336                 if (new & DSP_DIGITMODE_MF)
1337                         ast_mf_detect_init(&dsp->td.mf);
1338                 else
1339                         ast_dtmf_detect_init(&dsp->td.dtmf);
1340         }
1341         dsp->digitmode = digitmode;
1342         return 0;
1343 }