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