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_PERCENT            10      /* The percentage diffrence between the two last silence periods */
  49 #define BUSY_THRESHOLD          100     /* Max number of ms difference between max and min times in busy */
  50 #define BUSY_MIN                75      /* Busy must be at least 80 ms in half-cadence */
  51 #define BUSY_MAX                1100    /* Busy can't be longer than 1100 ms in half-cadence */
  52
  53 /* Remember last 15 units */
  54 #define DSP_HISTORY 15
  55
  56 /* Number of goertzels for progress detect */
  57 #define GSAMP_SIZE 183
  58
  59 #define HZ_350  0
  60 #define HZ_440  1
  61 #define HZ_480  2
  62 #define HZ_620  3
  63 #define HZ_950  4
  64 #define HZ_1400 5
  65 #define HZ_1800 6
  66
  67 #define TONE_THRESH 10.0        /* How much louder the tone should be than channel energy */
  68 #define TONE_MIN_THRESH 1e8     /* How much tone there should be at least to attempt */
  69 #define COUNT_THRESH  3         /* Need at least 50ms of stuff to count it */
  70
  71 #define TONE_STATE_SILENCE  0
  72 #define TONE_STATE_RINGING  1
  73 #define TONE_STATE_DIALTONE 2
  74 #define TONE_STATE_TALKING  3
  75 #define TONE_STATE_BUSY     4
  76 #define TONE_STATE_SPECIAL1     5
  77 #define TONE_STATE_SPECIAL2 6
  78 #define TONE_STATE_SPECIAL3 7
  79
  80 #define MAX_DTMF_DIGITS 128
  81
  82 /*
  83  * Comment out the following line to use the new DSP routines.
  84  */
  85 #define OLD_DSP_ROUTINES
  86
  87 /* Basic DTMF specs:
  88  *
  89  * Minimum tone on = 40ms
  90  * Minimum tone off = 50ms
  91  * Maximum digit rate = 10 per second
  92  * Normal twist <= 8dB accepted
  93  * Reverse twist <= 4dB accepted
  94  * S/N >= 15dB will detect OK
  95  * Attenuation <= 26dB will detect OK
  96  * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
  97  */
  98
  99 #define DTMF_THRESHOLD              8.0e7
 100 #define FAX_THRESHOLD              8.0e7
 101 #define FAX_2ND_HARMONIC                2.0     /* 4dB */
 102 #define DTMF_NORMAL_TWIST           6.3     /* 8dB */
 103 #define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5)     /* 4dB normal */
 104 #define DTMF_RELATIVE_PEAK_ROW      6.3     /* 8dB */
 105 #define DTMF_RELATIVE_PEAK_COL      6.3     /* 8dB */
 106 #define DTMF_2ND_HARMONIC_ROW       ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 1.7 : 2.5)     /* 4dB normal */
 107 #define DTMF_2ND_HARMONIC_COL       63.1    /* 18dB */
 108 #ifndef OLD_DSP_ROUTINES
 109 #define DTMF_TO_TOTAL_ENERGY        42.0
 110 #endif
 111
 112 #ifdef OLD_DSP_ROUTINES
 113 #define MF_THRESHOLD              8.0e7
 114 #define MF_NORMAL_TWIST           5.3     /* 8dB */
 115 #define MF_REVERSE_TWIST          4.0     /* was 2.5 */
 116 #define MF_RELATIVE_PEAK      5.3     /* 8dB */
 117 #define MF_2ND_HARMONIC       1.7 /* was 2.5  */
 118 #else
 119 #define BELL_MF_THRESHOLD           1.6e9
 120 #define BELL_MF_TWIST               4.0     /* 6dB */
 121 #define BELL_MF_RELATIVE_PEAK       12.6    /* 11dB */
 122 #endif
 123
 124 typedef struct {
 125         float v2;
 126         float v3;
 127         float fac;
 128 #ifndef OLD_DSP_ROUTINES
 129         int samples;
 130 #endif
 131 } goertzel_state_t;
 132
 133 typedef struct
 134 {
 135
 136     goertzel_state_t row_out[4];
 137     goertzel_state_t col_out[4];
 138         goertzel_state_t fax_tone;
 139 #ifdef OLD_DSP_ROUTINES
 140     goertzel_state_t row_out2nd[4];
 141     goertzel_state_t col_out2nd[4];
 142         goertzel_state_t fax_tone2nd;
 143     int hit1;
 144     int hit2;
 145     int hit3;
 146     int hit4;
 147 #else
 148     int hits[3];
 149 #endif
 150     int mhit;
 151     float energy;
 152     int current_sample;
 153
 154     char digits[MAX_DTMF_DIGITS + 1];
 155     int current_digits;
 156     int detected_digits;
 157     int lost_digits;
 158     int digit_hits[16];
 159
 160
 161         int fax_hits;
 162 } dtmf_detect_state_t;
 163
 164 typedef struct
 165 {
 166     goertzel_state_t tone_out[6];
 167     int mhit;
 168 #ifdef OLD_DSP_ROUTINES
 169     int hit1;
 170     int hit2;
 171     int hit3;
 172     int hit4;
 173     goertzel_state_t tone_out2nd[6];
 174     float energy;
 175 #else
 176     int hits[5];
 177 #endif
 178
 179     int current_sample;
 180     char digits[MAX_DTMF_DIGITS + 1];
 181     int current_digits;
 182     int detected_digits;
 183     int lost_digits;
 184         int fax_hits;
 185 } mf_detect_state_t;
 186
 187 static float dtmf_row[] =
 188 {
 189      697.0,  770.0,  852.0,  941.0
 190 };
 191 static float dtmf_col[] =
 192 {
 193     1209.0, 1336.0, 1477.0, 1633.0
 194 };
 195
 196 static float mf_tones[] =
 197 {
 198         700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
 199 };
 200
 201 static float fax_freq = 1100.0;
 202
 203 static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";
 204
 205 #ifdef OLD_DSP_ROUTINES
 206 static char mf_hit[6][6] = {
 207         /*  700 + */ {   0, '1', '2', '4', '7', 'C' },
 208         /*  900 + */ { '1',   0, '3', '5', '8', 'A' },
 209         /* 1100 + */ { '2', '3',   0, '6', '9', '*' },
 210         /* 1300 + */ { '4', '5', '6',   0, '0', 'B' },
 211         /* 1500 + */ { '7', '8', '9', '0',  0, '#' },
 212         /* 1700 + */ { 'C', 'A', '*', 'B', '#',  0  },
 213 };
 214 #else
 215 static char bell_mf_positions[] = "1247C-358A--69*---0B----#";
 216 #endif
 217
 218 static inline void goertzel_sample(goertzel_state_t *s, short sample)
 219 {
 220         float v1;
 221         float fsamp  = sample;
 222         v1 = s->v2;
 223         s->v2 = s->v3;
 224         s->v3 = s->fac * s->v2 - v1 + fsamp;
 225 }
 226
 227 static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
 228 {
 229         int i;
 230         for (i=0;i<count;i++)
 231                 goertzel_sample(s, samps[i]);
 232 }
 233
 234
 235 static inline float goertzel_result(goertzel_state_t *s)
 236 {
 237         return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
 238 }
 239
 240 static inline void goertzel_init(goertzel_state_t *s, float freq, int samples)
 241 {
 242         s->v2 = s->v3 = 0.0;
 243         s->fac = 2.0 * cos(2.0 * M_PI * (freq / 8000.0));
 244 #ifndef OLD_DSP_ROUTINES
 245         s->samples = samples;
 246 #endif
 247 }
 248
 249 static inline void goertzel_reset(goertzel_state_t *s)
 250 {
 251         s->v2 = s->v3 = 0.0;
 252 }
 253
 254 struct ast_dsp {
 255         struct ast_frame f;
 256         int threshold;
 257         int totalsilence;
 258         int totalnoise;
 259         int features;
 260         int busymaybe;
 261         int busycount;
 262         int historicnoise[DSP_HISTORY];
 263         int historicsilence[DSP_HISTORY];
 264         goertzel_state_t freqs[7];
 265         int gsamps;
 266         int tstate;
 267         int tcount;
 268         int digitmode;
 269         int thinkdigit;
 270         float genergy;
 271         union {
 272                 dtmf_detect_state_t dtmf;
 273                 mf_detect_state_t mf;
 274         } td;
 275 };
 276
 277 static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
 278 {
 279     int i;
 280
 281 #ifdef OLD_DSP_ROUTINES
 282     s->hit1 =
 283     s->mhit =
 284         s->hit3 =
 285         s->hit4 =
 286     s->hit2 = 0;
 287 #else
 288         s->hits[0] = s->hits[1] = s->hits[2] = 0;
 289 #endif
 290     for (i = 0;  i < 4;  i++)
 291     {
 292
 293                 goertzel_init (&s->row_out[i], dtmf_row[i], 102);
 294         goertzel_init (&s->col_out[i], dtmf_col[i], 102);
 295 #ifdef OLD_DSP_ROUTINES
 296         goertzel_init (&s->row_out2nd[i], dtmf_row[i] * 2.0, 102);
 297         goertzel_init (&s->col_out2nd[i], dtmf_col[i] * 2.0, 102);
 298 #endif
 299                 s->energy = 0.0;
 300     }
 301
 302         /* Same for the fax dector */
 303     goertzel_init (&s->fax_tone, fax_freq, 102);
 304
 305 #ifdef OLD_DSP_ROUTINES
 306         /* Same for the fax dector 2nd harmonic */
 307     goertzel_init (&s->fax_tone2nd, fax_freq * 2.0, 102);
 308 #endif
 309
 310     s->current_sample = 0;
 311     s->detected_digits = 0;
 312         s->current_digits = 0;
 313         memset(&s->digits, 0, sizeof(s->digits));
 314     s->lost_digits = 0;
 315     s->digits[0] = '\0';
 316 }
 317
 318 static void ast_mf_detect_init (mf_detect_state_t *s)
 319 {
 320     int i;
 321
 322 #ifdef OLD_DSP_ROUTINES
 323     s->hit1 =
 324     s->hit2 = 0;
 325 #else
 326         s->hits[0] = s->hits[1] = s->hits[2] = s->hits[3] = s->hits[4] = 0;
 327 #endif
 328     for (i = 0;  i < 6;  i++)
 329     {
 330
 331                 goertzel_init (&s->tone_out[i], mf_tones[i], 160);
 332 #ifdef OLD_DSP_ROUTINES
 333         goertzel_init (&s->tone_out2nd[i], mf_tones[i] * 2.0, 160);
 334                 s->energy = 0.0;
 335 #endif
 336
 337     }
 338
 339         s->current_digits = 0;
 340         memset(&s->digits, 0, sizeof(s->digits));
 341     s->current_sample = 0;
 342     s->detected_digits = 0;
 343     s->lost_digits = 0;
 344     s->digits[0] = '\0';
 345     s->mhit = 0;
 346 }
 347
 348 static int dtmf_detect (dtmf_detect_state_t *s,
 349                  int16_t amp[],
 350                  int samples,
 351                  int digitmode, int *writeback)
 352 {
 353
 354     float row_energy[4];
 355     float col_energy[4];
 356     float fax_energy;
 357 #ifdef OLD_DSP_ROUTINES
 358     float fax_energy_2nd;
 359 #endif
 360     float famp;
 361     float v1;
 362     int i;
 363     int j;
 364     int sample;
 365     int best_row;
 366     int best_col;
 367     int hit;
 368     int limit;
 369
 370     hit = 0;
 371     for (sample = 0;  sample < samples;  sample = limit)
 372     {
 373         /* 102 is optimised to meet the DTMF specs. */
 374         if ((samples - sample) >= (102 - s->current_sample))
 375             limit = sample + (102 - s->current_sample);
 376         else
 377             limit = samples;
 378 #if defined(USE_3DNOW)
 379         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
 380         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
 381 #ifdef OLD_DSP_ROUTINES
 382         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
 383         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
 384 #endif
 385                 /* XXX Need to fax detect for 3dnow too XXX */
 386                 #warning "Fax Support Broken"
 387 #else
 388         /* The following unrolled loop takes only 35% (rough estimate) of the
 389            time of a rolled loop on the machine on which it was developed */
 390         for (j = sample;  j < limit;  j++)
 391         {
 392             famp = amp[j];
 393
 394             s->energy += famp*famp;
 395
 396             /* With GCC 2.95, the following unrolled code seems to take about 35%
 397                (rough estimate) as long as a neat little 0-3 loop */
 398             v1 = s->row_out[0].v2;
 399             s->row_out[0].v2 = s->row_out[0].v3;
 400             s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
 401
 402             v1 = s->col_out[0].v2;
 403             s->col_out[0].v2 = s->col_out[0].v3;
 404             s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
 405
 406             v1 = s->row_out[1].v2;
 407             s->row_out[1].v2 = s->row_out[1].v3;
 408             s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
 409
 410             v1 = s->col_out[1].v2;
 411             s->col_out[1].v2 = s->col_out[1].v3;
 412             s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
 413
 414             v1 = s->row_out[2].v2;
 415             s->row_out[2].v2 = s->row_out[2].v3;
 416             s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
 417
 418             v1 = s->col_out[2].v2;
 419             s->col_out[2].v2 = s->col_out[2].v3;
 420             s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
 421
 422             v1 = s->row_out[3].v2;
 423             s->row_out[3].v2 = s->row_out[3].v3;
 424             s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;
 425
 426             v1 = s->col_out[3].v2;
 427             s->col_out[3].v2 = s->col_out[3].v3;
 428             s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;
 429
 430                         /* Update fax tone */
 431             v1 = s->fax_tone.v2;
 432             s->fax_tone.v2 = s->fax_tone.v3;
 433             s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;
 434
 435 #ifdef OLD_DSP_ROUTINES
 436             v1 = s->col_out2nd[0].v2;
 437             s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
 438             s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
 439
 440             v1 = s->row_out2nd[0].v2;
 441             s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
 442             s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
 443
 444             v1 = s->col_out2nd[1].v2;
 445             s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
 446             s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
 447
 448             v1 = s->row_out2nd[1].v2;
 449             s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
 450             s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
 451
 452             v1 = s->col_out2nd[2].v2;
 453             s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
 454             s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
 455
 456             v1 = s->row_out2nd[2].v2;
 457             s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
 458             s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
 459
 460             v1 = s->col_out2nd[3].v2;
 461             s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
 462             s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
 463
 464             v1 = s->row_out2nd[3].v2;
 465             s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
 466             s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;
 467
 468             v1 = s->fax_tone.v2;
 469             s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
 470             s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
 471 #endif
 472         }
 473 #endif
 474         s->current_sample += (limit - sample);
 475         if (s->current_sample < 102) {
 476                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
 477                                 /* If we had a hit last time, go ahead and clear this out since likely it
 478                                    will be another hit */
 479                                 for (i=sample;i<limit;i++)
 480                                         amp[i] = 0;
 481                                 *writeback = 1;
 482                         }
 483             continue;
 484                 }
 485
 486                 /* Detect the fax energy, too */
 487                 fax_energy = goertzel_result(&s->fax_tone);
 488
 489         /* We are at the end of a DTMF detection block */
 490         /* Find the peak row and the peak column */
 491         row_energy[0] = goertzel_result (&s->row_out[0]);
 492         col_energy[0] = goertzel_result (&s->col_out[0]);
 493
 494         for (best_row = best_col = 0, i = 1;  i < 4;  i++)
 495         {
 496             row_energy[i] = goertzel_result (&s->row_out[i]);
 497             if (row_energy[i] > row_energy[best_row])
 498                 best_row = i;
 499             col_energy[i] = goertzel_result (&s->col_out[i]);
 500             if (col_energy[i] > col_energy[best_col])
 501                 best_col = i;
 502         }
 503         hit = 0;
 504         /* Basic signal level test and the twist test */
 505         if (row_energy[best_row] >= DTMF_THRESHOLD
 506             &&
 507             col_energy[best_col] >= DTMF_THRESHOLD
 508             &&
 509             col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST
 510             &&
 511             col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row])
 512         {
 513             /* Relative peak test */
 514             for (i = 0;  i < 4;  i++)
 515             {
 516                 if ((i != best_col  &&  col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col])
 517                     ||
 518                     (i != best_row  &&  row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row]))
 519                 {
 520                     break;
 521                 }
 522             }
 523 #ifdef OLD_DSP_ROUTINES
 524             /* ... and second harmonic test */
 525             if (i >= 4
 526                 &&
 527                 (row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy
 528                 &&
 529                 goertzel_result (&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
 530                 &&
 531                 goertzel_result (&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row])
 532 #else
 533             /* ... and fraction of total energy test */
 534             if (i >= 4
 535                 &&
 536                 (row_energy[best_row] + col_energy[best_col]) > DTMF_TO_TOTAL_ENERGY*s->energy)
 537 #endif
 538             {
 539                                 /* Got a hit */
 540                 hit = dtmf_positions[(best_row << 2) + best_col];
 541                                 if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
 542                                         /* Zero out frame data if this is part DTMF */
 543                                         for (i=sample;i<limit;i++)
 544                                                 amp[i] = 0;
 545                                         *writeback = 1;
 546                                 }
 547                 /* Look for two successive similar results */
 548                 /* The logic in the next test is:
 549                    We need two successive identical clean detects, with
 550                    something different preceeding it. This can work with
 551                    back to back differing digits. More importantly, it
 552                    can work with nasty phones that give a very wobbly start
 553                    to a digit. */
 554
 555 #ifdef OLD_DSP_ROUTINES
 556                 if (hit == s->hit3  &&  s->hit3 != s->hit2)
 557                 {
 558                                     s->mhit = hit;
 559                     s->digit_hits[(best_row << 2) + best_col]++;
 560                     s->detected_digits++;
 561                     if (s->current_digits < MAX_DTMF_DIGITS)
 562                     {
 563                         s->digits[s->current_digits++] = hit;
 564                         s->digits[s->current_digits] = '\0';
 565                     }
 566                     else
 567                     {
 568                         s->lost_digits++;
 569                     }
 570                 }
 571 #else
 572                 if (hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0])
 573                 {
 574                                     s->mhit = hit;
 575                     s->digit_hits[(best_row << 2) + best_col]++;
 576                     s->detected_digits++;
 577                     if (s->current_digits < MAX_DTMF_DIGITS)
 578                     {
 579                         s->digits[s->current_digits++] = hit;
 580                         s->digits[s->current_digits] = '\0';
 581                     }
 582                     else
 583                     {
 584                         s->lost_digits++;
 585                     }
 586                 }
 587 #endif
 588             }
 589         }
 590 #ifdef OLD_DSP_ROUTINES
 591                 if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy > s->energy * 21.0)) {
 592                                 fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
 593                                 fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
 594                                 if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
 595 #if 0
 596                                         printf("Fax energy/Second Harmonic: %f/%f\n", fax_energy, fax_energy_2nd);
 597 #endif
 598                                         /* XXX Probably need better checking than just this the energy XXX */
 599                                         hit = 'f';
 600                                         s->fax_hits++;
 601                                 } /* Don't reset fax hits counter */
 602                 }
 603 #else /* OLD_DSP_ROUTINES */
 604                 if (!hit && (fax_energy >= DTMF_TO_TOTAL_ENERGY*s->energy)) {
 605 #if 0
 606                                 printf("Fax energy/Second Harmonic: %f\n", fax_energy);
 607 #endif
 608                                         /* XXX Probably need better checking than just this the energy XXX */
 609                                 hit = 'f';
 610                                 s->fax_hits++;
 611                 }
 612 #endif /* OLD_DSP_ROUTINES */
 613                 else {
 614                         if (s->fax_hits > 5) {
 615                                  hit = 'f';
 616                                  s->mhit = 'f';
 617                      s->detected_digits++;
 618                      if (s->current_digits < MAX_DTMF_DIGITS)
 619                      {
 620                           s->digits[s->current_digits++] = hit;
 621                           s->digits[s->current_digits] = '\0';
 622                      }
 623                      else
 624                      {
 625                            s->lost_digits++;
 626                      }
 627                         }
 628                         s->fax_hits = 0;
 629                 }
 630 #ifdef OLD_DSP_ROUTINES
 631         s->hit1 = s->hit2;
 632         s->hit2 = s->hit3;
 633         s->hit3 = hit;
 634 #else
 635         s->hits[0] = s->hits[1];
 636         s->hits[1] = s->hits[2];
 637         s->hits[2] = hit;
 638 #endif
 639         /* Reinitialise the detector for the next block */
 640         for (i = 0;  i < 4;  i++)
 641         {
 642             goertzel_reset(&s->row_out[i]);
 643             goertzel_reset(&s->col_out[i]);
 644 #ifdef OLD_DSP_ROUTINES
 645             goertzel_reset(&s->row_out2nd[i]);
 646             goertzel_reset(&s->col_out2nd[i]);
 647 #endif
 648         }
 649         goertzel_reset (&s->fax_tone);
 650 #ifdef OLD_DSP_ROUTINES
 651         goertzel_reset (&s->fax_tone2nd);
 652 #endif
 653                 s->energy = 0.0;
 654         s->current_sample = 0;
 655     }
 656     if ((!s->mhit) || (s->mhit != hit))
 657     {
 658         s->mhit = 0;
 659         return(0);
 660     }
 661     return (hit);
 662 }
 663
 664 /* MF goertzel size */
 665 #ifdef OLD_DSP_ROUTINES
 666 #define MF_GSIZE 160
 667 #else
 668 #define MF_GSIZE 120
 669 #endif
 670
 671 static int mf_detect (mf_detect_state_t *s,
 672                  int16_t amp[],
 673                  int samples,
 674                  int digitmode, int *writeback)
 675 {
 676
 677 #ifdef OLD_DSP_ROUTINES
 678     float tone_energy[6];
 679     int best1;
 680     int best2;
 681         float max;
 682         int sofarsogood;
 683 #else
 684     float energy[6];
 685     int best;
 686     int second_best;
 687 #endif
 688     float famp;
 689     float v1;
 690     int i;
 691     int j;
 692     int sample;
 693     int hit;
 694     int limit;
 695
 696     hit = 0;
 697     for (sample = 0;  sample < samples;  sample = limit)
 698     {
 699         /* 80 is optimised to meet the MF specs. */
 700         if ((samples - sample) >= (MF_GSIZE - s->current_sample))
 701             limit = sample + (MF_GSIZE - s->current_sample);
 702         else
 703             limit = samples;
 704 #if defined(USE_3DNOW)
 705         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
 706         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
 707 #ifdef OLD_DSP_ROUTINES
 708         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
 709         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
 710 #endif
 711                 /* XXX Need to fax detect for 3dnow too XXX */
 712                 #warning "Fax Support Broken"
 713 #else
 714         /* The following unrolled loop takes only 35% (rough estimate) of the
 715            time of a rolled loop on the machine on which it was developed */
 716         for (j = sample;  j < limit;  j++)
 717         {
 718             famp = amp[j];
 719
 720 #ifdef OLD_DSP_ROUTINES
 721             s->energy += famp*famp;
 722 #endif
 723
 724             /* With GCC 2.95, the following unrolled code seems to take about 35%
 725                (rough estimate) as long as a neat little 0-3 loop */
 726             v1 = s->tone_out[0].v2;
 727             s->tone_out[0].v2 = s->tone_out[0].v3;
 728             s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
 729
 730             v1 = s->tone_out[1].v2;
 731             s->tone_out[1].v2 = s->tone_out[1].v3;
 732             s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
 733
 734             v1 = s->tone_out[2].v2;
 735             s->tone_out[2].v2 = s->tone_out[2].v3;
 736             s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
 737
 738             v1 = s->tone_out[3].v2;
 739             s->tone_out[3].v2 = s->tone_out[3].v3;
 740             s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
 741
 742             v1 = s->tone_out[4].v2;
 743             s->tone_out[4].v2 = s->tone_out[4].v3;
 744             s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
 745
 746             v1 = s->tone_out[5].v2;
 747             s->tone_out[5].v2 = s->tone_out[5].v3;
 748             s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
 749
 750 #ifdef OLD_DSP_ROUTINES
 751             v1 = s->tone_out2nd[0].v2;
 752             s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
 753             s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
 754
 755             v1 = s->tone_out2nd[1].v2;
 756             s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
 757             s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
 758
 759             v1 = s->tone_out2nd[2].v2;
 760             s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
 761             s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
 762
 763             v1 = s->tone_out2nd[3].v2;
 764             s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
 765             s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
 766
 767             v1 = s->tone_out2nd[4].v2;
 768             s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
 769             s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
 770
 771             v1 = s->tone_out2nd[3].v2;
 772             s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
 773             s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
 774 #endif
 775         }
 776 #endif
 777         s->current_sample += (limit - sample);
 778         if (s->current_sample < MF_GSIZE) {
 779                         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
 780                                 /* If we had a hit last time, go ahead and clear this out since likely it
 781                                    will be another hit */
 782                                 for (i=sample;i<limit;i++)
 783                                         amp[i] = 0;
 784                                 *writeback = 1;
 785                         }
 786             continue;
 787                 }
 788
 789
 790 #ifdef OLD_DSP_ROUTINES
 791                 /* We're at the end of an MF detection block.  Go ahead and calculate
 792                    all the energies. */
 793                 for (i=0;i<6;i++) {
 794                         tone_energy[i] = goertzel_result(&s->tone_out[i]);
 795                 }
 796                 /* Find highest */
 797                 best1 = 0;
 798                 max = tone_energy[0];
 799                 for (i=1;i<6;i++) {
 800                         if (tone_energy[i] > max) {
 801                                 max = tone_energy[i];
 802                                 best1 = i;
 803                         }
 804                 }
 805
 806                 /* Find 2nd highest */
 807                 if (best1) {
 808                         max = tone_energy[0];
 809                         best2 = 0;
 810                 } else {
 811                         max = tone_energy[1];
 812                         best2 = 1;
 813                 }
 814
 815                 for (i=0;i<6;i++) {
 816                         if (i == best1) continue;
 817                         if (tone_energy[i] > max) {
 818                                 max = tone_energy[i];
 819                                 best2 = i;
 820                         }
 821                 }
 822
 823         hit = 0;
 824                 if (best1 != best2) sofarsogood=1;
 825                 else sofarsogood=0;
 826                 /* Check for relative energies */
 827                 for (i=0;i<6;i++) {
 828                         if (i == best1) continue;
 829                         if (i == best2) continue;
 830                         if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
 831                                 sofarsogood = 0;
 832                                 break;
 833                         }
 834                         if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
 835                                 sofarsogood = 0;
 836                                 break;
 837                         }
 838                 }
 839
 840                 if (sofarsogood) {
 841                         /* Check for 2nd harmonic */
 842                         if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1])
 843                                 sofarsogood = 0;
 844                         else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
 845                                 sofarsogood = 0;
 846                 }
 847                 if (sofarsogood) {
 848                         hit = mf_hit[best1][best2];
 849                         if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
 850                                 /* Zero out frame data if this is part DTMF */
 851                                 for (i=sample;i<limit;i++)
 852                                         amp[i] = 0;
 853                                 *writeback = 1;
 854                         }
 855                         /* Look for two consecutive clean hits */
 856                         if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
 857                                 s->mhit = hit;
 858                                 s->detected_digits++;
 859                                 if (s->current_digits < MAX_DTMF_DIGITS - 2) {
 860                                         s->digits[s->current_digits++] = hit;
 861                                         s->digits[s->current_digits] = '\0';
 862                                 } else {
 863                                         s->lost_digits++;
 864                                 }
 865                         }
 866                 }
 867
 868         s->hit1 = s->hit2;
 869         s->hit2 = s->hit3;
 870         s->hit3 = hit;
 871         /* Reinitialise the detector for the next block */
 872         for (i = 0;  i < 6;  i++)
 873         {
 874             goertzel_reset(&s->tone_out[i]);
 875             goertzel_reset(&s->tone_out2nd[i]);
 876         }
 877                 s->energy = 0.0;
 878         s->current_sample = 0;
 879     }
 880 #else
 881                 /* We're at the end of an MF detection block.  */
 882         /* Find the two highest energies. The spec says to look for
 883            two tones and two tones only. Taking this literally -ie
 884            only two tones pass the minimum threshold - doesn't work
 885            well. The sinc function mess, due to rectangular windowing
 886            ensure that! Find the two highest energies and ensure they
 887            are considerably stronger than any of the others. */
 888         energy[0] = goertzel_result(&s->tone_out[0]);
 889         energy[1] = goertzel_result(&s->tone_out[1]);
 890         if (energy[0] > energy[1])
 891         {
 892             best = 0;
 893             second_best = 1;
 894         }
 895         else
 896         {
 897             best = 1;
 898             second_best = 0;
 899         }
 900         /*endif*/
 901         for (i = 2;  i < 6;  i++)
 902         {
 903             energy[i] = goertzel_result(&s->tone_out[i]);
 904             if (energy[i] >= energy[best])
 905             {
 906                 second_best = best;
 907                 best = i;
 908             }
 909             else if (energy[i] >= energy[second_best])
 910             {
 911                 second_best = i;
 912             }
 913         }
 914         /* Basic signal level and twist tests */
 915         hit = 0;
 916         if (energy[best] >= BELL_MF_THRESHOLD
 917             &&
 918             energy[second_best] >= BELL_MF_THRESHOLD
 919             &&
 920             energy[best] < energy[second_best]*BELL_MF_TWIST
 921             &&
 922             energy[best]*BELL_MF_TWIST > energy[second_best])
 923         {
 924             /* Relative peak test */
 925             hit = -1;
 926             for (i = 0;  i < 6;  i++)
 927             {
 928                 if (i != best  &&  i != second_best)
 929                 {
 930                     if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best])
 931                     {
 932                         /* The best two are not clearly the best */
 933                         hit = 0;
 934                         break;
 935                     }
 936                 }
 937             }
 938         }
 939         if (hit)
 940         {
 941             /* Get the values into ascending order */
 942             if (second_best < best)
 943             {
 944                 i = best;
 945                 best = second_best;
 946                 second_best = i;
 947             }
 948             best = best*5 + second_best - 1;
 949             hit = bell_mf_positions[best];
 950             /* Look for two successive similar results */
 951             /* The logic in the next test is:
 952                For KP we need 4 successive identical clean detects, with
 953                two blocks of something different preceeding it. For anything
 954                else we need two successive identical clean detects, with
 955                two blocks of something different preceeding it. */
 956             if (hit == s->hits[4]
 957                 &&
 958                 hit == s->hits[3]
 959                 &&
 960                    ((hit != '*'  &&  hit != s->hits[2]  &&  hit != s->hits[1])
 961                     ||
 962                     (hit == '*'  &&  hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0])))
 963             {
 964                 s->detected_digits++;
 965                 if (s->current_digits < MAX_DTMF_DIGITS)
 966                 {
 967                     s->digits[s->current_digits++] = hit;
 968                     s->digits[s->current_digits] = '\0';
 969                 }
 970                 else
 971                 {
 972                     s->lost_digits++;
 973                 }
 974             }
 975         }
 976         else
 977         {
 978             hit = 0;
 979         }
 980         s->hits[0] = s->hits[1];
 981         s->hits[1] = s->hits[2];
 982         s->hits[2] = s->hits[3];
 983         s->hits[3] = s->hits[4];
 984         s->hits[4] = hit;
 985         /* Reinitialise the detector for the next block */
 986         for (i = 0;  i < 6;  i++)
 987             goertzel_reset(&s->tone_out[i]);
 988         s->current_sample = 0;
 989     }
 990 #endif
 991     if ((!s->mhit) || (s->mhit != hit))
 992     {
 993                 s->mhit = 0;
 994                 return(0);
 995     }
 996     return (hit);
 997 }
 998
 999 static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
1000 {
1001         int res;
1002         if (dsp->digitmode & DSP_DIGITMODE_MF)
1003                 res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
1004         else
1005                 res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
1006         return res;
1007 }
1008
1009 int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf)
1010 {
1011         short *s;
1012         int len;
1013         int ign=0;
1014         if (inf->frametype != AST_FRAME_VOICE) {
1015                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
1016                 return 0;
1017         }
1018         if (inf->subclass != AST_FORMAT_SLINEAR) {
1019                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
1020                 return 0;
1021         }
1022         s = inf->data;
1023         len = inf->datalen / 2;
1024         return __ast_dsp_digitdetect(dsp, s, len, &ign);
1025 }
1026
1027 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
1028 {
1029         /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
1030         /* Make sure absolute levels are high enough */
1031         if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
1032                 return 0;
1033         /* Amplify ignored stuff */
1034         i2 *= TONE_THRESH;
1035         i1 *= TONE_THRESH;
1036         e *= TONE_THRESH;
1037         /* Check first tone */
1038         if ((p1 < i1) || (p1 < i2) || (p1 < e))
1039                 return 0;
1040         /* And second */
1041         if ((p2 < i1) || (p2 < i2) || (p2 < e))
1042                 return 0;
1043         /* Guess it's there... */
1044         return 1;
1045 }
1046
1047 int ast_dsp_getdigits (struct ast_dsp *dsp,
1048               char *buf,
1049               int max)
1050 {
1051         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1052             if (max > dsp->td.mf.current_digits)
1053                 max = dsp->td.mf.current_digits;
1054             if (max > 0)
1055             {
1056                 memcpy (buf, dsp->td.mf.digits, max);
1057                 memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
1058                 dsp->td.mf.current_digits -= max;
1059             }
1060             buf[max] = '\0';
1061             return  max;
1062         } else {
1063             if (max > dsp->td.dtmf.current_digits)
1064                 max = dsp->td.dtmf.current_digits;
1065             if (max > 0)
1066             {
1067                 memcpy (buf, dsp->td.dtmf.digits, max);
1068                 memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
1069                 dsp->td.dtmf.current_digits -= max;
1070             }
1071             buf[max] = '\0';
1072             return  max;
1073         }
1074 }
1075
1076 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
1077 {
1078         int x;
1079         int pass;
1080         int newstate = TONE_STATE_SILENCE;
1081         int res = 0;
1082         while(len) {
1083                 /* Take the lesser of the number of samples we need and what we have */
1084                 pass = len;
1085                 if (pass > GSAMP_SIZE - dsp->gsamps)
1086                         pass = GSAMP_SIZE - dsp->gsamps;
1087                 for (x=0;x<pass;x++) {
1088                         goertzel_sample(&dsp->freqs[HZ_350], s[x]);
1089                         goertzel_sample(&dsp->freqs[HZ_440], s[x]);
1090                         goertzel_sample(&dsp->freqs[HZ_480], s[x]);
1091                         goertzel_sample(&dsp->freqs[HZ_620], s[x]);
1092                         goertzel_sample(&dsp->freqs[HZ_950], s[x]);
1093                         goertzel_sample(&dsp->freqs[HZ_1400], s[x]);
1094                         goertzel_sample(&dsp->freqs[HZ_1800], s[x]);
1095                         dsp->genergy += s[x] * s[x];
1096                 }
1097                 s += pass;
1098                 dsp->gsamps += pass;
1099                 len -= pass;
1100                 if (dsp->gsamps == GSAMP_SIZE) {
1101                         float hz_350;
1102                         float hz_440;
1103                         float hz_480;
1104                         float hz_620;
1105                         float hz_950;
1106                         float hz_1400;
1107                         float hz_1800;
1108                         hz_350 = goertzel_result(&dsp->freqs[HZ_350]);
1109                         hz_440 = goertzel_result(&dsp->freqs[HZ_440]);
1110                         hz_480 = goertzel_result(&dsp->freqs[HZ_480]);
1111                         hz_620 = goertzel_result(&dsp->freqs[HZ_620]);
1112                         hz_950 = goertzel_result(&dsp->freqs[HZ_950]);
1113                         hz_1400 = goertzel_result(&dsp->freqs[HZ_1400]);
1114                         hz_1800 = goertzel_result(&dsp->freqs[HZ_1800]);
1115 #if 0
1116                         printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n",
1117                                 hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
1118 #endif
1119                         if (pair_there(hz_480, hz_620, hz_350, hz_440, dsp->genergy)) {
1120                                 newstate = TONE_STATE_BUSY;
1121                         } else if (pair_there(hz_440, hz_480, hz_350, hz_620, dsp->genergy)) {
1122                                 newstate = TONE_STATE_RINGING;
1123                         } else if (pair_there(hz_350, hz_440, hz_480, hz_620, dsp->genergy)) {
1124                                 newstate = TONE_STATE_DIALTONE;
1125                         } else if (hz_950 > TONE_MIN_THRESH * TONE_THRESH) {
1126                                 newstate = TONE_STATE_SPECIAL1;
1127                         } else if (hz_1400 > TONE_MIN_THRESH * TONE_THRESH) {
1128                                 if (dsp->tstate == TONE_STATE_SPECIAL1)
1129                                         newstate = TONE_STATE_SPECIAL2;
1130                         } else if (hz_1800 > TONE_MIN_THRESH * TONE_THRESH) {
1131                                 if (dsp->tstate == TONE_STATE_SPECIAL2)
1132                                         newstate = TONE_STATE_SPECIAL3;
1133                         } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
1134                                 newstate = TONE_STATE_TALKING;
1135                         } else
1136                                 newstate = TONE_STATE_SILENCE;
1137
1138                         if (newstate == dsp->tstate) {
1139                                 dsp->tcount++;
1140                                 if (dsp->tcount == COUNT_THRESH) {
1141                                         if (dsp->tstate == TONE_STATE_BUSY) {
1142                                                 res = AST_CONTROL_BUSY;
1143                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1144                                         } else if (dsp->tstate == TONE_STATE_TALKING) {
1145                                                 res = AST_CONTROL_ANSWER;
1146                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1147                                         } else if (dsp->tstate == TONE_STATE_RINGING)
1148                                                 res = AST_CONTROL_RINGING;
1149                                         else if (dsp->tstate == TONE_STATE_SPECIAL3) {
1150                                                 res = AST_CONTROL_CONGESTION;
1151                                                 dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
1152                                         }
1153
1154                                 }
1155                         } else {
1156 #if 0
1157                                 printf("Newstate: %d\n", newstate);
1158 #endif
1159                                 dsp->tstate = newstate;
1160                                 dsp->tcount = 1;
1161                         }
1162
1163                         /* Reset goertzel */
1164                         for (x=0;x<7;x++)
1165                                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1166                         dsp->gsamps = 0;
1167                         dsp->genergy = 0.0;
1168                 }
1169         }
1170 #if 0
1171         if (res)
1172                 printf("Returning %d\n", res);
1173 #endif
1174         return res;
1175 }
1176
1177 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
1178 {
1179         if (inf->frametype != AST_FRAME_VOICE) {
1180                 ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
1181                 return 0;
1182         }
1183         if (inf->subclass != AST_FORMAT_SLINEAR) {
1184                 ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
1185                 return 0;
1186         }
1187         return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
1188 }
1189
1190 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
1191 {
1192         int accum;
1193         int x;
1194         int res = 0;
1195
1196         accum = 0;
1197         for (x=0;x<len; x++)
1198                 accum += abs(s[x]);
1199         accum /= len;
1200         if (accum < dsp->threshold) {
1201                 dsp->totalsilence += len/8;
1202                 if (dsp->totalnoise) {
1203                         /* Move and save history */
1204                         memmove(dsp->historicnoise + DSP_HISTORY - dsp->busycount, dsp->historicnoise + DSP_HISTORY - dsp->busycount +1, dsp->busycount*sizeof(dsp->historicnoise[0]));
1205                         dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
1206 /* we don't want to check for busydetect that frequently */
1207 #if 0
1208                         dsp->busymaybe = 1;
1209 #endif
1210                 }
1211                 dsp->totalnoise = 0;
1212                 res = 1;
1213         } else {
1214                 dsp->totalnoise += len/8;
1215                 if (dsp->totalsilence) {
1216                         int silence1 = dsp->historicsilence[DSP_HISTORY - 1];
1217                         int silence2 = dsp->historicsilence[DSP_HISTORY - 2];
1218                         /* Move and save history */
1219                         memmove(dsp->historicsilence + DSP_HISTORY - dsp->busycount, dsp->historicsilence + DSP_HISTORY - dsp->busycount + 1, dsp->busycount*sizeof(dsp->historicsilence[0]));
1220                         dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
1221                         /* check if the previous sample differs only by BUSY_PERCENT from the one before it */
1222                         if (silence1 < silence2) {
1223                                 if (silence1 + silence1/BUSY_PERCENT >= silence2)
1224                                         dsp->busymaybe = 1;
1225                                 else
1226                                         dsp->busymaybe = 0;
1227                         } else {
1228                                 if (silence1 - silence1/BUSY_PERCENT <= silence2)
1229                                         dsp->busymaybe = 1;
1230                                 else
1231                                         dsp->busymaybe = 0;
1232                         }
1233
1234                 }
1235                 dsp->totalsilence = 0;
1236         }
1237         if (totalsilence)
1238                 *totalsilence = dsp->totalsilence;
1239         return res;
1240 }
1241 #ifdef BUSYDETECT_MARTIN
1242 int ast_dsp_busydetect(struct ast_dsp *dsp)
1243 {
1244         int res = 0, x;
1245 #ifndef BUSYDETECT_TONEONLY
1246         int avgsilence = 0, hitsilence = 0;
1247 #endif
1248         int avgtone = 0, hittone = 0;
1249         if (!dsp->busymaybe)
1250                 return res;
1251         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1252 #ifndef BUSYDETECT_TONEONLY
1253                 avgsilence += dsp->historicsilence[x];
1254 #endif
1255                 avgtone += dsp->historicnoise[x];
1256         }
1257 #ifndef BUSYDETECT_TONEONLY
1258         avgsilence /= dsp->busycount;
1259 #endif
1260         avgtone /= dsp->busycount;
1261         for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1262 #ifndef BUSYDETECT_TONEONLY
1263                 if (avgsilence > dsp->historicsilence[x]) {
1264                         if (avgsilence - (avgsilence / BUSY_PERCENT) <= dsp->historicsilence[x])
1265                                 hitsilence++;
1266                 } else {
1267                         if (avgsilence + (avgsilence / BUSY_PERCENT) >= dsp->historicsilence[x])
1268                                 hitsilence++;
1269                 }
1270 #endif
1271                 if (avgtone > dsp->historicnoise[x]) {
1272                         if (avgtone - (avgtone / BUSY_PERCENT) <= dsp->historicsilence[x])
1273                                 hittone++;
1274                 } else {
1275                         if (avgtone + (avgtone / BUSY_PERCENT) >= dsp->historicsilence[x])
1276                                 hittone++;
1277                 }
1278         }
1279 #ifndef BUSYDETECT_TONEONLY
1280         if ((hittone >= dsp->busycount - 1) && (hitsilence >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX) && (avgsilence >= BUSY_MIN && avgsilence <= BUSY_MAX)) {
1281 #else
1282         if ((hittone >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX)) {
1283 #endif
1284 #ifdef BUSYDETECT_COMPARE_TONE_AND_SILENCE
1285 #ifdef BUSYDETECT_TONEONLY
1286 #error You cant use BUSYDETECT_TONEONLY together with BUSYDETECT_COMPARE_TONE_AND_SILENCE
1287 #endif
1288                 if (avgtone > avgsilence) {
1289                         if (avgtone - avgtone/(BUSY_PERCENT*2) <= avgsilence)
1290                                 res = 1;
1291                 } else {
1292                         if (avgtone + avgtone/(BUSY_PERCENT*2) >= avgsilence)
1293                                 res = 1;
1294                 }
1295 #else
1296                 res = 1;
1297 #endif
1298         }
1299 #if 0
1300         if (res)
1301                 ast_log(LOG_NOTICE, "detected busy, avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
1302 #endif
1303         return res;
1304 }
1305 #endif
1306
1307 #ifdef BUSYDETECT
1308 int ast_dsp_busydetect(struct ast_dsp *dsp)
1309 {
1310         int x;
1311         int res = 0;
1312         int max, min;
1313
1314 #if 0
1315         if (dsp->busy_hits > 5);
1316         return 0;
1317 #endif
1318         if (dsp->busymaybe) {
1319 #if 0
1320                 printf("Maybe busy!\n");
1321 #endif
1322                 dsp->busymaybe = 0;
1323                 min = 9999;
1324                 max = 0;
1325                 for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
1326 #if 0
1327                         printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
1328 #endif
1329                         if (dsp->historicsilence[x] < min)
1330                                 min = dsp->historicsilence[x];
1331                         if (dsp->historicnoise[x] < min)
1332                                 min = dsp->historicnoise[x];
1333                         if (dsp->historicsilence[x] > max)
1334                                 max = dsp->historicsilence[x];
1335                         if (dsp->historicnoise[x] > max)
1336                                 max = dsp->historicnoise[x];
1337                 }
1338                 if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
1339 #if 0
1340                         printf("Busy!\n");
1341 #endif
1342                         res = 1;
1343                 }
1344 #if 0
1345                 printf("Min: %d, max: %d\n", min, max);
1346 #endif
1347         }
1348         return res;
1349 }
1350 #endif
1351
1352 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
1353 {
1354         short *s;
1355         int len;
1356
1357         if (f->frametype != AST_FRAME_VOICE) {
1358                 ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
1359                 return 0;
1360         }
1361         if (f->subclass != AST_FORMAT_SLINEAR) {
1362                 ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
1363                 return 0;
1364         }
1365         s = f->data;
1366         len = f->datalen/2;
1367         return __ast_dsp_silence(dsp, s, len, totalsilence);
1368 }
1369
1370 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af, int needlock)
1371 {
1372         int silence;
1373         int res;
1374         int digit;
1375         int x;
1376         unsigned short *shortdata;
1377         unsigned char *odata;
1378         int len;
1379         int writeback = 0;
1380
1381 #define FIX_INF(inf) do { \
1382                 if (writeback) { \
1383                         switch(inf->subclass) { \
1384                         case AST_FORMAT_SLINEAR: \
1385                                 break; \
1386                         case AST_FORMAT_ULAW: \
1387                                 for (x=0;x<len;x++) \
1388                                         odata[x] = AST_LIN2MU(shortdata[x]); \
1389                                 break; \
1390                         case AST_FORMAT_ALAW: \
1391                                 for (x=0;x<len;x++) \
1392                                         odata[x] = AST_LIN2A(shortdata[x]); \
1393                                 break; \
1394                         } \
1395                 } \
1396         } while(0)
1397
1398         if (!af)
1399                 return NULL;
1400         if (af->frametype != AST_FRAME_VOICE)
1401                 return af;
1402         odata = af->data;
1403         len = af->datalen;
1404         /* Make sure we have short data */
1405         switch(af->subclass) {
1406         case AST_FORMAT_SLINEAR:
1407                 shortdata = af->data;
1408                 len = af->datalen / 2;
1409                 break;
1410         case AST_FORMAT_ULAW:
1411                 shortdata = alloca(af->datalen * 2);
1412                 if (!shortdata) {
1413                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1414                         return af;
1415                 }
1416                 for (x=0;x<len;x++)
1417                         shortdata[x] = AST_MULAW(odata[x]);
1418                 break;
1419         case AST_FORMAT_ALAW:
1420                 shortdata = alloca(af->datalen * 2);
1421                 if (!shortdata) {
1422                         ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
1423                         return af;
1424                 }
1425                 for (x=0;x<len;x++)
1426                         shortdata[x] = AST_ALAW(odata[x]);
1427                 break;
1428         default:
1429                 ast_log(LOG_WARNING, "Unable to detect process %d frames\n", af->subclass);
1430                 return af;
1431         }
1432         silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
1433         if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
1434                 memset(&dsp->f, 0, sizeof(dsp->f));
1435                 dsp->f.frametype = AST_FRAME_NULL;
1436                 return &dsp->f;
1437         }
1438         if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
1439                 chan->_softhangup |= AST_SOFTHANGUP_DEV;
1440                 memset(&dsp->f, 0, sizeof(dsp->f));
1441                 dsp->f.frametype = AST_FRAME_CONTROL;
1442                 dsp->f.subclass = AST_CONTROL_BUSY;
1443                 ast_log(LOG_DEBUG, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name);
1444                 return &dsp->f;
1445         }
1446         if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
1447                 digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
1448 #if 0
1449                 if (digit)
1450                         printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
1451 #endif
1452                 if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
1453                         if (!dsp->thinkdigit) {
1454                                 if (digit) {
1455                                         /* Looks like we might have something.  Request a conference mute for the moment */
1456                                         memset(&dsp->f, 0, sizeof(dsp->f));
1457                                         dsp->f.frametype = AST_FRAME_DTMF;
1458                                         dsp->f.subclass = 'm';
1459                                         dsp->thinkdigit = 'x';
1460                                         FIX_INF(af);
1461                                         if (chan)
1462                                                 ast_queue_frame(chan, af, needlock);
1463                                         ast_frfree(af);
1464                                         return &dsp->f;
1465                                 }
1466                         } else {
1467                                 if (digit) {
1468                                         /* Thought we saw one last time.  Pretty sure we really have now */
1469                                         if (dsp->thinkdigit) {
1470                                                 if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
1471                                                         /* If we found a digit, and we're changing digits, go
1472                                                            ahead and send this one, but DON'T stop confmute because
1473                                                            we're detecting something else, too... */
1474                                                         memset(&dsp->f, 0, sizeof(dsp->f));
1475                                                         dsp->f.frametype = AST_FRAME_DTMF;
1476                                                         dsp->f.subclass = dsp->thinkdigit;
1477                                                         FIX_INF(af);
1478                                                         if (chan)
1479                                                                 ast_queue_frame(chan, af, needlock);
1480                                                         ast_frfree(af);
1481                                                 }
1482                                                 dsp->thinkdigit = digit;
1483                                                 return &dsp->f;
1484                                         }
1485                                         dsp->thinkdigit = digit;
1486                                 } else {
1487                                         if (dsp->thinkdigit) {
1488                                                 memset(&dsp->f, 0, sizeof(dsp->f));
1489                                                 if (dsp->thinkdigit != 'x') {
1490                                                         /* If we found a digit, send it now */
1491                                                         dsp->f.frametype = AST_FRAME_DTMF;
1492                                                         dsp->f.subclass = dsp->thinkdigit;
1493                                                         dsp->thinkdigit = 0;
1494                                                 } else {
1495                                                         dsp->f.frametype = AST_FRAME_DTMF;
1496                                                         dsp->f.subclass = 'u';
1497                                                         dsp->thinkdigit = 0;
1498                                                 }
1499                                                 FIX_INF(af);
1500                                                 if (chan)
1501                                                         ast_queue_frame(chan, af, needlock);
1502                                                 ast_frfree(af);
1503                                                 return &dsp->f;
1504                                         }
1505                                 }
1506                         }
1507                 } else if (!digit) {
1508                         /* Only check when there is *not* a hit... */
1509                         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1510                                 if (dsp->td.mf.current_digits) {
1511                                         memset(&dsp->f, 0, sizeof(dsp->f));
1512                                         dsp->f.frametype = AST_FRAME_DTMF;
1513                                         dsp->f.subclass = dsp->td.mf.digits[0];
1514                                         memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
1515                                         dsp->td.mf.current_digits--;
1516                                         FIX_INF(af);
1517                                         if (chan)
1518                                                 ast_queue_frame(chan, af, needlock);
1519                                         ast_frfree(af);
1520                                         return &dsp->f;
1521                                 }
1522                         } else {
1523                                 if (dsp->td.dtmf.current_digits) {
1524                                         memset(&dsp->f, 0, sizeof(dsp->f));
1525                                         dsp->f.frametype = AST_FRAME_DTMF;
1526                                         dsp->f.subclass = dsp->td.dtmf.digits[0];
1527                                         memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
1528                                         dsp->td.dtmf.current_digits--;
1529                                         FIX_INF(af);
1530                                         if (chan)
1531                                                 ast_queue_frame(chan, af, needlock);
1532                                         ast_frfree(af);
1533                                         return &dsp->f;
1534                                 }
1535                         }
1536                 }
1537         }
1538         if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
1539                 res = __ast_dsp_call_progress(dsp, shortdata, len);
1540                 memset(&dsp->f, 0, sizeof(dsp->f));
1541                 dsp->f.frametype = AST_FRAME_CONTROL;
1542                 if (res) {
1543                         switch(res) {
1544                         case AST_CONTROL_ANSWER:
1545                         case AST_CONTROL_BUSY:
1546                         case AST_CONTROL_RINGING:
1547                         case AST_CONTROL_CONGESTION:
1548                                 dsp->f.subclass = res;
1549                                 if (chan)
1550                                         ast_queue_frame(chan, &dsp->f, needlock);
1551                                 break;
1552                         default:
1553                                 ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
1554                         }
1555                 }
1556         }
1557         FIX_INF(af);
1558         return af;
1559 }
1560
1561 struct ast_dsp *ast_dsp_new(void)
1562 {
1563         struct ast_dsp *dsp;
1564         dsp = malloc(sizeof(struct ast_dsp));
1565         if (dsp) {
1566                 memset(dsp, 0, sizeof(struct ast_dsp));
1567                 dsp->threshold = DEFAULT_THRESHOLD;
1568                 dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
1569                 dsp->busycount = DSP_HISTORY;
1570                 /* Initialize goertzels */
1571                 goertzel_init(&dsp->freqs[HZ_350], 350.0, GSAMP_SIZE);
1572                 goertzel_init(&dsp->freqs[HZ_440], 440.0, GSAMP_SIZE);
1573                 goertzel_init(&dsp->freqs[HZ_480], 480.0, GSAMP_SIZE);
1574                 goertzel_init(&dsp->freqs[HZ_620], 620.0, GSAMP_SIZE);
1575                 goertzel_init(&dsp->freqs[HZ_950], 950.0, GSAMP_SIZE);
1576                 goertzel_init(&dsp->freqs[HZ_1400], 1400.0, GSAMP_SIZE);
1577                 goertzel_init(&dsp->freqs[HZ_1800], 1800.0, GSAMP_SIZE);
1578                 /* Initialize DTMF detector */
1579                 ast_dtmf_detect_init(&dsp->td.dtmf);
1580         }
1581         return dsp;
1582 }
1583
1584 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
1585 {
1586         dsp->features = features;
1587 }
1588
1589 void ast_dsp_free(struct ast_dsp *dsp)
1590 {
1591         free(dsp);
1592 }
1593
1594 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
1595 {
1596         dsp->threshold = threshold;
1597 }
1598
1599 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
1600 {
1601         if (cadences < 4)
1602                 cadences = 4;
1603         if (cadences > DSP_HISTORY)
1604                 cadences = DSP_HISTORY;
1605         dsp->busycount = cadences;
1606 }
1607
1608 void ast_dsp_digitreset(struct ast_dsp *dsp)
1609 {
1610         int i;
1611         dsp->thinkdigit = 0;
1612         if (dsp->digitmode & DSP_DIGITMODE_MF) {
1613                 memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
1614                 dsp->td.mf.current_digits = 0;
1615                 /* Reinitialise the detector for the next block */
1616                 for (i = 0;  i < 6;  i++) {
1617                 goertzel_reset(&dsp->td.mf.tone_out[i]);
1618 #ifdef OLD_DSP_ROUTINES
1619                     goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
1620 #endif
1621                 }
1622 #ifdef OLD_DSP_ROUTINES
1623                 dsp->td.mf.energy = 0.0;
1624             dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
1625 #else
1626             dsp->td.mf.hits[4] = dsp->td.mf.hits[3] = dsp->td.mf.hits[2] = dsp->td.mf.hits[1] = dsp->td.mf.hits[0] = dsp->td.mf.mhit = 0;
1627 #endif
1628                 dsp->td.mf.current_sample = 0;
1629         } else {
1630                 memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
1631                 dsp->td.dtmf.current_digits = 0;
1632                 /* Reinitialise the detector for the next block */
1633                 for (i = 0;  i < 4;  i++) {
1634                 goertzel_reset(&dsp->td.dtmf.row_out[i]);
1635                     goertzel_reset(&dsp->td.dtmf.col_out[i]);
1636 #ifdef OLD_DSP_ROUTINES
1637                 goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
1638                 goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
1639 #endif
1640                 }
1641             goertzel_reset (&dsp->td.dtmf.fax_tone);
1642 #ifdef OLD_DSP_ROUTINES
1643             goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
1644             dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
1645 #else
1646             dsp->td.dtmf.hits[2] = dsp->td.dtmf.hits[1] = dsp->td.dtmf.hits[0] =  dsp->td.dtmf.mhit = 0;
1647 #endif
1648                 dsp->td.dtmf.energy = 0.0;
1649                 dsp->td.dtmf.current_sample = 0;
1650         }
1651 }
1652
1653 void ast_dsp_reset(struct ast_dsp *dsp)
1654 {
1655         int x;
1656         dsp->totalsilence = 0;
1657         dsp->gsamps = 0;
1658         for (x=0;x<4;x++)
1659                 dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
1660         memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
1661         memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
1662
1663 }
1664
1665 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
1666 {
1667         int new, old;
1668         old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1669         new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
1670         if (old != new) {
1671                 /* Must initialize structures if switching from MF to DTMF or vice-versa */
1672                 if (new & DSP_DIGITMODE_MF)
1673                         ast_mf_detect_init(&dsp->td.mf);
1674                 else
1675                         ast_dtmf_detect_init(&dsp->td.dtmf);
1676         }
1677         dsp->digitmode = digitmode;
1678         return 0;
1679 }
1680