gst/dtmf/tone_detect.c

   1 /*
   2  *   DTMF Receiver module, part of:
   3  *      BSD Telephony Of Mexico "Zapata" Telecom Library, version 1.10  12/9/01
   4  *
   5  *   Part of the "Zapata" Computer Telephony Technology.
   6  *
   7  *   See http://www.bsdtelephony.com.mx
   8  *
   9  *
  10  *  The technologies, software, hardware, designs, drawings, scheumatics, board
  11  *  layouts and/or artwork, concepts, methodologies (including the use of all
  12  *  of these, and that which is derived from the use of all of these), all other
  13  *  intellectual properties contained herein, and all intellectual property
  14  *  rights have been and shall continue to be expressly for the benefit of all
  15  *  mankind, and are perpetually placed in the public domain, and may be used,
  16  *  copied, and/or modified by anyone, in any manner, for any legal purpose,
  17  *  without restriction.
  18  *
  19  *   This module written by Stephen Underwood.
  20  */
  21
  22 /*
  23         tone_detect.c - General telephony tone detection, and specific
  24                         detection of DTMF.
  25
  26         Copyright (C) 2001  Steve Underwood <steveu@coppice.org>
  27
  28         Despite my general liking of the GPL, I place this code in the
  29         public domain for the benefit of all mankind - even the slimy
  30         ones who might try to proprietize my work and use it to my
  31         detriment.
  32 */
  33
  34 #include <math.h>
  35 #include <string.h>
  36 #include <stdio.h>
  37 #include <time.h>
  38 #include <fcntl.h>
  39 #include "tone_detect.h"
  40
  41 #ifndef FALSE
  42 #define FALSE   0
  43 #endif
  44 #ifndef TRUE
  45 #define TRUE    (!FALSE)
  46 #endif
  47
  48 //#define USE_3DNOW
  49
  50 /* Basic DTMF specs:
  51  *
  52  * Minimum tone on = 40ms
  53  * Minimum tone off = 50ms
  54  * Maximum digit rate = 10 per second
  55  * Normal twist <= 8dB accepted
  56  * Reverse twist <= 4dB accepted
  57  * S/N >= 15dB will detect OK
  58  * Attenuation <= 26dB will detect OK
  59  * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
  60  */
  61
  62 #define SAMPLE_RATE                 8000.0
  63
  64 #define DTMF_THRESHOLD              8.0e7
  65 #define FAX_THRESHOLD              8.0e7
  66 #define FAX_2ND_HARMONIC                2.0     /* 4dB */
  67 #define DTMF_NORMAL_TWIST           6.3 /* 8dB */
  68 #define DTMF_REVERSE_TWIST          ((isradio) ? 4.0 : 2.5)     /* 4dB normal */
  69 #define DTMF_RELATIVE_PEAK_ROW      6.3 /* 8dB */
  70 #define DTMF_RELATIVE_PEAK_COL      6.3 /* 8dB */
  71 #define DTMF_2ND_HARMONIC_ROW       ((isradio) ? 1.7 : 2.5)     /* 4dB normal */
  72 #define DTMF_2ND_HARMONIC_COL       63.1        /* 18dB */
  73
  74 static tone_detection_descriptor_t dtmf_detect_row[4];
  75 static tone_detection_descriptor_t dtmf_detect_col[4];
  76 static tone_detection_descriptor_t dtmf_detect_row_2nd[4];
  77 static tone_detection_descriptor_t dtmf_detect_col_2nd[4];
  78 static tone_detection_descriptor_t fax_detect;
  79 static tone_detection_descriptor_t fax_detect_2nd;
  80
  81 static float dtmf_row[] = {
  82   697.0, 770.0, 852.0, 941.0
  83 };
  84
  85 static float dtmf_col[] = {
  86   1209.0, 1336.0, 1477.0, 1633.0
  87 };
  88
  89 static float fax_freq = 1100.0;
  90
  91 static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";
  92
  93 static void
  94 goertzel_init (goertzel_state_t * s, tone_detection_descriptor_t * t)
  95 {
  96   s->v2 = s->v3 = 0.0;
  97   s->fac = t->fac;
  98 }
  99
 100 /*- End of function --------------------------------------------------------*/
 101
 102 #if defined(USE_3DNOW)
 103 static inline void
 104 _dtmf_goertzel_update (goertzel_state_t * s, float x[], int samples)
 105 {
 106   int n;
 107   float v;
 108   int i;
 109   float vv[16];
 110
 111   vv[4] = s[0].v2;
 112   vv[5] = s[1].v2;
 113   vv[6] = s[2].v2;
 114   vv[7] = s[3].v2;
 115   vv[8] = s[0].v3;
 116   vv[9] = s[1].v3;
 117   vv[10] = s[2].v3;
 118   vv[11] = s[3].v3;
 119   vv[12] = s[0].fac;
 120   vv[13] = s[1].fac;
 121   vv[14] = s[2].fac;
 122   vv[15] = s[3].fac;
 123
 124   //v1 = s->v2;
 125   //s->v2 = s->v3;
 126   //s->v3 = s->fac*s->v2 - v1 + x[0];
 127
 128   __asm__ __volatile__ (" femms;\n"
 129       " movq        16(%%edx),%%mm2;\n"
 130       " movq        24(%%edx),%%mm3;\n"
 131       " movq        32(%%edx),%%mm4;\n"
 132       " movq        40(%%edx),%%mm5;\n"
 133       " movq        48(%%edx),%%mm6;\n"
 134       " movq        56(%%edx),%%mm7;\n"
 135       " jmp         1f;\n"
 136       " .align 32;\n"
 137       " 1: ;\n"
 138       " prefetch    (%%eax);\n"
 139       " movq        %%mm3,%%mm1;\n"
 140       " movq        %%mm2,%%mm0;\n"
 141       " movq        %%mm5,%%mm3;\n"
 142       " movq        %%mm4,%%mm2;\n"
 143       " pfmul       %%mm7,%%mm5;\n"
 144       " pfmul       %%mm6,%%mm4;\n"
 145       " pfsub       %%mm1,%%mm5;\n"
 146       " pfsub       %%mm0,%%mm4;\n"
 147       " movq        (%%eax),%%mm0;\n"
 148       " movq        %%mm0,%%mm1;\n"
 149       " punpckldq   %%mm0,%%mm1;\n"
 150       " add         $4,%%eax;\n"
 151       " pfadd       %%mm1,%%mm5;\n"
 152       " pfadd       %%mm1,%%mm4;\n"
 153       " dec         %%ecx;\n"
 154       " jnz         1b;\n"
 155       " movq        %%mm2,16(%%edx);\n"
 156       " movq        %%mm3,24(%%edx);\n"
 157       " movq        %%mm4,32(%%edx);\n"
 158       " movq        %%mm5,40(%%edx);\n"
 159       " femms;\n"::"c" (samples), "a" (x), "d" (vv)
 160       :"memory", "eax", "ecx");
 161
 162   s[0].v2 = vv[4];
 163   s[1].v2 = vv[5];
 164   s[2].v2 = vv[6];
 165   s[3].v2 = vv[7];
 166   s[0].v3 = vv[8];
 167   s[1].v3 = vv[9];
 168   s[2].v3 = vv[10];
 169   s[3].v3 = vv[11];
 170 }
 171 #endif
 172 /*- End of function --------------------------------------------------------*/
 173
 174 void
 175 zap_goertzel_update (goertzel_state_t * s, int16_t x[], int samples)
 176 {
 177   int i;
 178   float v1;
 179
 180   for (i = 0; i < samples; i++) {
 181     v1 = s->v2;
 182     s->v2 = s->v3;
 183     s->v3 = s->fac * s->v2 - v1 + x[i];
 184   }
 185 }
 186
 187 /*- End of function --------------------------------------------------------*/
 188
 189 float
 190 zap_goertzel_result (goertzel_state_t * s)
 191 {
 192   return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
 193 }
 194
 195 /*- End of function --------------------------------------------------------*/
 196
 197 void
 198 zap_dtmf_detect_init (dtmf_detect_state_t * s)
 199 {
 200   int i;
 201   float theta;
 202
 203   s->hit1 = s->hit2 = 0;
 204
 205   for (i = 0; i < 4; i++) {
 206     theta = 2.0 * G_PI * (dtmf_row[i] / SAMPLE_RATE);
 207     dtmf_detect_row[i].fac = 2.0 * cos (theta);
 208
 209     theta = 2.0 * G_PI * (dtmf_col[i] / SAMPLE_RATE);
 210     dtmf_detect_col[i].fac = 2.0 * cos (theta);
 211
 212     theta = 2.0 * G_PI * (dtmf_row[i] * 2.0 / SAMPLE_RATE);
 213     dtmf_detect_row_2nd[i].fac = 2.0 * cos (theta);
 214
 215     theta = 2.0 * G_PI * (dtmf_col[i] * 2.0 / SAMPLE_RATE);
 216     dtmf_detect_col_2nd[i].fac = 2.0 * cos (theta);
 217
 218     goertzel_init (&s->row_out[i], &dtmf_detect_row[i]);
 219     goertzel_init (&s->col_out[i], &dtmf_detect_col[i]);
 220     goertzel_init (&s->row_out2nd[i], &dtmf_detect_row_2nd[i]);
 221     goertzel_init (&s->col_out2nd[i], &dtmf_detect_col_2nd[i]);
 222
 223     s->energy = 0.0;
 224   }
 225
 226   /* Same for the fax dector */
 227   theta = 2.0 * G_PI * (fax_freq / SAMPLE_RATE);
 228   fax_detect.fac = 2.0 * cos (theta);
 229   goertzel_init (&s->fax_tone, &fax_detect);
 230
 231   /* Same for the fax dector 2nd harmonic */
 232   theta = 2.0 * G_PI * (fax_freq * 2.0 / SAMPLE_RATE);
 233   fax_detect_2nd.fac = 2.0 * cos (theta);
 234   goertzel_init (&s->fax_tone2nd, &fax_detect_2nd);
 235
 236   s->current_sample = 0;
 237   s->detected_digits = 0;
 238   s->lost_digits = 0;
 239   s->digits[0] = '\0';
 240   s->mhit = 0;
 241 }
 242
 243 /*- End of function --------------------------------------------------------*/
 244
 245 int
 246 zap_dtmf_detect (dtmf_detect_state_t * s,
 247     int16_t amp[], int samples, int isradio)
 248 {
 249
 250   float row_energy[4];
 251   float col_energy[4];
 252   float fax_energy;
 253   float fax_energy_2nd;
 254   float famp;
 255   float v1;
 256   int i;
 257   int j;
 258   int sample;
 259   int best_row;
 260   int best_col;
 261   int hit;
 262   int limit;
 263
 264   hit = 0;
 265   for (sample = 0; sample < samples; sample = limit) {
 266     /* 102 is optimised to meet the DTMF specs. */
 267     if ((samples - sample) >= (102 - s->current_sample))
 268       limit = sample + (102 - s->current_sample);
 269     else
 270       limit = samples;
 271 #if defined(USE_3DNOW)
 272     _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
 273     _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
 274     _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
 275     _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
 276     /* XXX Need to fax detect for 3dnow too XXX */
 277 #warning "Fax Support Broken"
 278 #else
 279     /* The following unrolled loop takes only 35% (rough estimate) of the
 280        time of a rolled loop on the machine on which it was developed */
 281     for (j = sample; j < limit; j++) {
 282       famp = amp[j];
 283
 284       s->energy += famp * famp;
 285
 286       /* With GCC 2.95, the following unrolled code seems to take about 35%
 287          (rough estimate) as long as a neat little 0-3 loop */
 288       v1 = s->row_out[0].v2;
 289       s->row_out[0].v2 = s->row_out[0].v3;
 290       s->row_out[0].v3 = s->row_out[0].fac * s->row_out[0].v2 - v1 + famp;
 291
 292       v1 = s->col_out[0].v2;
 293       s->col_out[0].v2 = s->col_out[0].v3;
 294       s->col_out[0].v3 = s->col_out[0].fac * s->col_out[0].v2 - v1 + famp;
 295
 296       v1 = s->row_out[1].v2;
 297       s->row_out[1].v2 = s->row_out[1].v3;
 298       s->row_out[1].v3 = s->row_out[1].fac * s->row_out[1].v2 - v1 + famp;
 299
 300       v1 = s->col_out[1].v2;
 301       s->col_out[1].v2 = s->col_out[1].v3;
 302       s->col_out[1].v3 = s->col_out[1].fac * s->col_out[1].v2 - v1 + famp;
 303
 304       v1 = s->row_out[2].v2;
 305       s->row_out[2].v2 = s->row_out[2].v3;
 306       s->row_out[2].v3 = s->row_out[2].fac * s->row_out[2].v2 - v1 + famp;
 307
 308       v1 = s->col_out[2].v2;
 309       s->col_out[2].v2 = s->col_out[2].v3;
 310       s->col_out[2].v3 = s->col_out[2].fac * s->col_out[2].v2 - v1 + famp;
 311
 312       v1 = s->row_out[3].v2;
 313       s->row_out[3].v2 = s->row_out[3].v3;
 314       s->row_out[3].v3 = s->row_out[3].fac * s->row_out[3].v2 - v1 + famp;
 315
 316       v1 = s->col_out[3].v2;
 317       s->col_out[3].v2 = s->col_out[3].v3;
 318       s->col_out[3].v3 = s->col_out[3].fac * s->col_out[3].v2 - v1 + famp;
 319
 320       v1 = s->col_out2nd[0].v2;
 321       s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
 322       s->col_out2nd[0].v3 =
 323           s->col_out2nd[0].fac * s->col_out2nd[0].v2 - v1 + famp;
 324
 325       v1 = s->row_out2nd[0].v2;
 326       s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
 327       s->row_out2nd[0].v3 =
 328           s->row_out2nd[0].fac * s->row_out2nd[0].v2 - v1 + famp;
 329
 330       v1 = s->col_out2nd[1].v2;
 331       s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
 332       s->col_out2nd[1].v3 =
 333           s->col_out2nd[1].fac * s->col_out2nd[1].v2 - v1 + famp;
 334
 335       v1 = s->row_out2nd[1].v2;
 336       s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
 337       s->row_out2nd[1].v3 =
 338           s->row_out2nd[1].fac * s->row_out2nd[1].v2 - v1 + famp;
 339
 340       v1 = s->col_out2nd[2].v2;
 341       s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
 342       s->col_out2nd[2].v3 =
 343           s->col_out2nd[2].fac * s->col_out2nd[2].v2 - v1 + famp;
 344
 345       v1 = s->row_out2nd[2].v2;
 346       s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
 347       s->row_out2nd[2].v3 =
 348           s->row_out2nd[2].fac * s->row_out2nd[2].v2 - v1 + famp;
 349
 350       v1 = s->col_out2nd[3].v2;
 351       s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
 352       s->col_out2nd[3].v3 =
 353           s->col_out2nd[3].fac * s->col_out2nd[3].v2 - v1 + famp;
 354
 355       v1 = s->row_out2nd[3].v2;
 356       s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
 357       s->row_out2nd[3].v3 =
 358           s->row_out2nd[3].fac * s->row_out2nd[3].v2 - v1 + famp;
 359
 360       /* Update fax tone */
 361       v1 = s->fax_tone.v2;
 362       s->fax_tone.v2 = s->fax_tone.v3;
 363       s->fax_tone.v3 = s->fax_tone.fac * s->fax_tone.v2 - v1 + famp;
 364
 365       v1 = s->fax_tone.v2;
 366       s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
 367       s->fax_tone2nd.v3 = s->fax_tone2nd.fac * s->fax_tone2nd.v2 - v1 + famp;
 368     }
 369 #endif
 370     s->current_sample += (limit - sample);
 371     if (s->current_sample < 102)
 372       continue;
 373
 374     /* Detect the fax energy, too */
 375     fax_energy = zap_goertzel_result (&s->fax_tone);
 376
 377     /* We are at the end of a DTMF detection block */
 378     /* Find the peak row and the peak column */
 379     row_energy[0] = zap_goertzel_result (&s->row_out[0]);
 380     col_energy[0] = zap_goertzel_result (&s->col_out[0]);
 381
 382     for (best_row = best_col = 0, i = 1; i < 4; i++) {
 383       row_energy[i] = zap_goertzel_result (&s->row_out[i]);
 384       if (row_energy[i] > row_energy[best_row])
 385         best_row = i;
 386       col_energy[i] = zap_goertzel_result (&s->col_out[i]);
 387       if (col_energy[i] > col_energy[best_col])
 388         best_col = i;
 389     }
 390     hit = 0;
 391     /* Basic signal level test and the twist test */
 392     if (row_energy[best_row] >= DTMF_THRESHOLD
 393         &&
 394         col_energy[best_col] >= DTMF_THRESHOLD
 395         &&
 396         col_energy[best_col] < row_energy[best_row] * DTMF_REVERSE_TWIST
 397         && col_energy[best_col] * DTMF_NORMAL_TWIST > row_energy[best_row]) {
 398       /* Relative peak test */
 399       for (i = 0; i < 4; i++) {
 400         if ((i != best_col
 401                 && col_energy[i] * DTMF_RELATIVE_PEAK_COL >
 402                 col_energy[best_col])
 403             || (i != best_row
 404                 && row_energy[i] * DTMF_RELATIVE_PEAK_ROW >
 405                 row_energy[best_row])) {
 406           break;
 407         }
 408       }
 409       /* ... and second harmonic test */
 410       if (i >= 4
 411           &&
 412           (row_energy[best_row] + col_energy[best_col]) > 42.0 * s->energy
 413           &&
 414           zap_goertzel_result (&s->col_out2nd[best_col]) *
 415           DTMF_2ND_HARMONIC_COL < col_energy[best_col]
 416           && zap_goertzel_result (&s->row_out2nd[best_row]) *
 417           DTMF_2ND_HARMONIC_ROW < row_energy[best_row]) {
 418         hit = dtmf_positions[(best_row << 2) + best_col];
 419         /* Look for two successive similar results */
 420         /* The logic in the next test is:
 421            We need two successive identical clean detects, with
 422            something different preceeding it. This can work with
 423            back to back differing digits. More importantly, it
 424            can work with nasty phones that give a very wobbly start
 425            to a digit. */
 426         if (hit == s->hit3 && s->hit3 != s->hit2) {
 427           s->mhit = hit;
 428           s->digit_hits[(best_row << 2) + best_col]++;
 429           s->detected_digits++;
 430           if (s->current_digits < MAX_DTMF_DIGITS) {
 431             s->digits[s->current_digits++] = hit;
 432             s->digits[s->current_digits] = '\0';
 433           } else {
 434             s->lost_digits++;
 435           }
 436         }
 437       }
 438     }
 439     if (!hit && (fax_energy >= FAX_THRESHOLD)
 440         && (fax_energy > s->energy * 21.0)) {
 441       fax_energy_2nd = zap_goertzel_result (&s->fax_tone2nd);
 442       if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
 443 #if 0
 444         printf ("Fax energy/Second Harmonic: %f/%f\n", fax_energy,
 445             fax_energy_2nd);
 446 #endif
 447         /* XXX Probably need better checking than just this the energy XXX */
 448         hit = 'f';
 449         s->fax_hits++;
 450       }                         /* Don't reset fax hits counter */
 451     } else {
 452       if (s->fax_hits > 5) {
 453         s->mhit = 'f';
 454         s->detected_digits++;
 455         if (s->current_digits < MAX_DTMF_DIGITS) {
 456           s->digits[s->current_digits++] = hit;
 457           s->digits[s->current_digits] = '\0';
 458         } else {
 459           s->lost_digits++;
 460         }
 461       }
 462       s->fax_hits = 0;
 463     }
 464     s->hit1 = s->hit2;
 465     s->hit2 = s->hit3;
 466     s->hit3 = hit;
 467     /* Reinitialise the detector for the next block */
 468     for (i = 0; i < 4; i++) {
 469       goertzel_init (&s->row_out[i], &dtmf_detect_row[i]);
 470       goertzel_init (&s->col_out[i], &dtmf_detect_col[i]);
 471       goertzel_init (&s->row_out2nd[i], &dtmf_detect_row_2nd[i]);
 472       goertzel_init (&s->col_out2nd[i], &dtmf_detect_col_2nd[i]);
 473     }
 474     goertzel_init (&s->fax_tone, &fax_detect);
 475     goertzel_init (&s->fax_tone2nd, &fax_detect_2nd);
 476     s->energy = 0.0;
 477     s->current_sample = 0;
 478   }
 479   if ((!s->mhit) || (s->mhit != hit)) {
 480     s->mhit = 0;
 481     return (0);
 482   }
 483   return (hit);
 484 }
 485
 486 /*- End of function --------------------------------------------------------*/
 487
 488 int
 489 zap_dtmf_get (dtmf_detect_state_t * s, char *buf, int max)
 490 {
 491   if (max > s->current_digits)
 492     max = s->current_digits;
 493   if (max > 0) {
 494     memcpy (buf, s->digits, max);
 495     memmove (s->digits, s->digits + max, s->current_digits - max);
 496     s->current_digits -= max;
 497   }
 498   buf[max] = '\0';
 499   return max;
 500 }
 501
 502 /*- End of function --------------------------------------------------------*/
 503 /*- End of file ------------------------------------------------------------*/