Home page | Mailing list | Docs

Last updated: Sat Feb 3 05:00:47 2007

Asterisk developer's documentation :: Codename Pineapple

---

dsp.c

Go to the documentation of this file.

/*
 * Asterisk -- An open source telephony toolkit.
 *
 * Copyright (C) 1999 - 2005, Digium, Inc.
 *
 * Mark Spencer <markster@digium.com>
 *
 * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
 * DTMF detector.
 *
 * See http://www.asterisk.org for more information about
 * the Asterisk project. Please do not directly contact
 * any of the maintainers of this project for assistance;
 * the project provides a web site, mailing lists and IRC
 * channels for your use.
 *
 * This program is free software, distributed under the terms of
 * the GNU General Public License Version 2. See the LICENSE file
 * at the top of the source tree.
 */

/*! \file
 *
 * \brief Convenience Signal Processing routines
 *
 * \author Mark Spencer <markster@digium.com>
 * \author Steve Underwood <steveu@coppice.org>
 */

/* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
/*
   tone_detect.c - General telephony tone detection, and specific
                        detection of DTMF.

        Copyright (C) 2001  Steve Underwood <steveu@coppice.org>

        Despite my general liking of the GPL, I place this code in the
        public domain for the benefit of all mankind - even the slimy
        ones who might try to proprietize my work and use it to my
        detriment.
*/

#include "asterisk.h"

ASTERISK_FILE_VERSION(__FILE__, "$Revision: 51499 $")

#include <sys/types.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <errno.h>
#include <stdio.h>

#include "asterisk/frame.h"
#include "asterisk/channel.h"
#include "asterisk/logger.h"
#include "asterisk/dsp.h"
#include "asterisk/ulaw.h"
#include "asterisk/alaw.h"
#include "asterisk/utils.h"
#include "asterisk/options.h"

/*! Number of goertzels for progress detect */
enum gsamp_size {
   GSAMP_SIZE_NA = 183,       /*!< North America - 350, 440, 480, 620, 950, 1400, 1800 Hz */
   GSAMP_SIZE_CR = 188,       /*!< Costa Rica, Brazil - Only care about 425 Hz */
   GSAMP_SIZE_UK = 160        /*!< UK disconnect goertzel feed - should trigger 400hz */
};

enum prog_mode {
   PROG_MODE_NA = 0,
   PROG_MODE_CR,
   PROG_MODE_UK
};

enum freq_index { 
   /*! For US modes { */
   HZ_350 = 0,
   HZ_440,
   HZ_480,
   HZ_620,
   HZ_950,
   HZ_1400,
   HZ_1800, /*!< } */

   /*! For CR/BR modes */
   HZ_425 = 0,

   /*! For UK mode */
   HZ_400 = 0
};

static struct progalias {
   char *name;
   enum prog_mode mode;
} aliases[] = {
   { "us", PROG_MODE_NA },
   { "ca", PROG_MODE_NA },
   { "cr", PROG_MODE_CR },
   { "br", PROG_MODE_CR },
   { "uk", PROG_MODE_UK },
};

static struct progress {
   enum gsamp_size size;
   int freqs[7];
} modes[] = {
   { GSAMP_SIZE_NA, { 350, 440, 480, 620, 950, 1400, 1800 } }, /*!< North America */
   { GSAMP_SIZE_CR, { 425 } },                                 /*!< Costa Rica, Brazil */
   { GSAMP_SIZE_UK, { 400 } },                                 /*!< UK */
};

#define DEFAULT_THRESHOLD  512

enum busy_detect {
   BUSY_PERCENT = 10,      /*!< The percentage difference between the two last silence periods */
   BUSY_PAT_PERCENT = 7,   /*!< The percentage difference between measured and actual pattern */
   BUSY_THRESHOLD = 100,   /*!< Max number of ms difference between max and min times in busy */
   BUSY_MIN = 75,          /*!< Busy must be at least 80 ms in half-cadence */
   BUSY_MAX =3100          /*!< Busy can't be longer than 3100 ms in half-cadence */
};

/*! Remember last 15 units */
#define DSP_HISTORY     15

/*! Define if you want the fax detector -- NOT RECOMMENDED IN -STABLE */
#define FAX_DETECT

#define TONE_THRESH     10.0  /*!< How much louder the tone should be than channel energy */
#define TONE_MIN_THRESH    1e8   /*!< How much tone there should be at least to attempt */

/*! All THRESH_XXX values are in GSAMP_SIZE chunks (us = 22ms) */
enum gsamp_thresh {
   THRESH_RING = 8,           /*!< Need at least 150ms ring to accept */
   THRESH_TALK = 2,           /*!< Talk detection does not work continuously */
   THRESH_BUSY = 4,           /*!< Need at least 80ms to accept */
   THRESH_CONGESTION = 4,     /*!< Need at least 80ms to accept */
   THRESH_HANGUP = 60,        /*!< Need at least 1300ms to accept hangup */
   THRESH_RING2ANSWER = 300   /*!< Timeout from start of ring to answer (about 6600 ms) */
};

#define  MAX_DTMF_DIGITS      128

/* Basic DTMF specs:
 *
 * Minimum tone on = 40ms
 * Minimum tone off = 50ms
 * Maximum digit rate = 10 per second
 * Normal twist <= 8dB accepted
 * Reverse twist <= 4dB accepted
 * S/N >= 15dB will detect OK
 * Attenuation <= 26dB will detect OK
 * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
 */

#define DTMF_THRESHOLD     8.0e7
#define FAX_THRESHOLD      8.0e7
#define FAX_2ND_HARMONIC   2.0     /* 4dB */
#define DTMF_NORMAL_TWIST  6.3     /* 8dB */
#ifdef   RADIO_RELAX
#define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 6.5 : 2.5)     /* 4dB normal */
#else
#define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5)     /* 4dB normal */
#endif
#define DTMF_RELATIVE_PEAK_ROW   6.3     /* 8dB */
#define DTMF_RELATIVE_PEAK_COL   6.3     /* 8dB */
#define DTMF_2ND_HARMONIC_ROW       ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 1.7 : 2.5)     /* 4dB normal */
#define DTMF_2ND_HARMONIC_COL 63.1    /* 18dB */
#define DTMF_TO_TOTAL_ENERGY  42.0

#ifdef OLD_DSP_ROUTINES
#define MF_THRESHOLD    8.0e7
#define MF_NORMAL_TWIST    5.3     /* 8dB */
#define MF_REVERSE_TWIST   4.0     /* was 2.5 */
#define MF_RELATIVE_PEAK   5.3     /* 8dB */
#define MF_2ND_HARMONIC    1.7   /* was 2.5  */
#else
#define BELL_MF_THRESHOLD  1.6e9
#define BELL_MF_TWIST      4.0     /* 6dB */
#define BELL_MF_RELATIVE_PEAK 12.6    /* 11dB */
#endif

#if !defined(BUSYDETECT_MARTIN) && !defined(BUSYDETECT) && !defined(BUSYDETECT_TONEONLY) && !defined(BUSYDETECT_COMPARE_TONE_AND_SILENCE)
#define BUSYDETECT_MARTIN
#endif

typedef struct {
   float v2;
   float v3;
   float fac;
#ifndef OLD_DSP_ROUTINES
   int samples;
#endif   
} goertzel_state_t;

typedef struct
{
   goertzel_state_t row_out[4];
   goertzel_state_t col_out[4];
#ifdef FAX_DETECT
   goertzel_state_t fax_tone;
#endif
#ifdef OLD_DSP_ROUTINES
   goertzel_state_t row_out2nd[4];
   goertzel_state_t col_out2nd[4];
#ifdef FAX_DETECT
   goertzel_state_t fax_tone2nd;    
#endif
   int hit1;
   int hit2;
   int hit3;
   int hit4;
#else
   int hits[3];
#endif   
   int mhit;
   float energy;
   int current_sample;

   char digits[MAX_DTMF_DIGITS + 1];
   
   int current_digits;
   int detected_digits;
   int lost_digits;
   int digit_hits[16];
#ifdef FAX_DETECT
   int fax_hits;
#endif
} dtmf_detect_state_t;

typedef struct
{
   goertzel_state_t tone_out[6];
   int mhit;
#ifdef OLD_DSP_ROUTINES
   int hit1;
   int hit2;
   int hit3;
   int hit4;
   goertzel_state_t tone_out2nd[6];
   float energy;
#else
   int hits[5];
#endif
   int current_sample;
   
   char digits[MAX_DTMF_DIGITS + 1];

   int current_digits;
   int detected_digits;
   int lost_digits;
#ifdef FAX_DETECT
   int fax_hits;
#endif
} mf_detect_state_t;

static float dtmf_row[] =
{
   697.0,  770.0,  852.0,  941.0
};
static float dtmf_col[] =
{
   1209.0, 1336.0, 1477.0, 1633.0
};

static float mf_tones[] =
{
   700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
};

#ifdef FAX_DETECT
static float fax_freq = 1100.0;
#endif

static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";

#ifdef OLD_DSP_ROUTINES
static char mf_hit[6][6] = {
   /*  700 + */ {   0, '1', '2', '4', '7', 'C' },
   /*  900 + */ { '1',   0, '3', '5', '8', 'A' },
   /* 1100 + */ { '2', '3',   0, '6', '9', '*' },
   /* 1300 + */ { '4', '5', '6',   0, '0', 'B' },
   /* 1500 + */ { '7', '8', '9', '0',  0, '#' },
   /* 1700 + */ { 'C', 'A', '*', 'B', '#',  0  },
};
#else
static char bell_mf_positions[] = "1247C-358A--69*---0B----#";
#endif

static inline void goertzel_sample(goertzel_state_t *s, short sample)
{
   float v1;
   float fsamp  = sample;
   
   v1 = s->v2;
   s->v2 = s->v3;
   s->v3 = s->fac * s->v2 - v1 + fsamp;
}

static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
{
   int i;
   
   for (i=0;i<count;i++) 
      goertzel_sample(s, samps[i]);
}


static inline float goertzel_result(goertzel_state_t *s)
{
   return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
}

static inline void goertzel_init(goertzel_state_t *s, float freq, int samples)
{
   s->v2 = s->v3 = 0.0;
   s->fac = 2.0 * cos(2.0 * M_PI * (freq / 8000.0));
#ifndef OLD_DSP_ROUTINES
   s->samples = samples;
#endif
}

static inline void goertzel_reset(goertzel_state_t *s)
{
   s->v2 = s->v3 = 0.0;
}

struct ast_dsp {
   struct ast_frame f;
   int threshold;
   int totalsilence;
   int totalnoise;
   int features;
   int ringtimeout;
   int busymaybe;
   int busycount;
   int busy_tonelength;
   int busy_quietlength;
   int historicnoise[DSP_HISTORY];
   int historicsilence[DSP_HISTORY];
   goertzel_state_t freqs[7];
   int freqcount;
   int gsamps;
   enum gsamp_size gsamp_size;
   enum prog_mode progmode;
   int tstate;
   int tcount;
   int digitmode;
   int thinkdigit;
   float genergy;
   union {
      dtmf_detect_state_t dtmf;
      mf_detect_state_t mf;
   } td;
};

static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
{
   int i;

#ifdef OLD_DSP_ROUTINES
   s->hit1 = 
   s->mhit = 
   s->hit3 =
   s->hit4 = 
   s->hit2 = 0;
#else
   s->hits[0] = s->hits[1] = s->hits[2] = 0;
#endif
   for (i = 0;  i < 4;  i++) {
      goertzel_init (&s->row_out[i], dtmf_row[i], 102);
      goertzel_init (&s->col_out[i], dtmf_col[i], 102);
#ifdef OLD_DSP_ROUTINES
      goertzel_init (&s->row_out2nd[i], dtmf_row[i] * 2.0, 102);
      goertzel_init (&s->col_out2nd[i], dtmf_col[i] * 2.0, 102);
#endif   
      s->energy = 0.0;
   }
#ifdef FAX_DETECT
   /* Same for the fax dector */
   goertzel_init (&s->fax_tone, fax_freq, 102);

#ifdef OLD_DSP_ROUTINES
   /* Same for the fax dector 2nd harmonic */
   goertzel_init (&s->fax_tone2nd, fax_freq * 2.0, 102);
#endif   
#endif /* FAX_DETECT */
   s->current_sample = 0;
   s->detected_digits = 0;
   s->current_digits = 0;
   memset(&s->digits, 0, sizeof(s->digits));
   s->lost_digits = 0;
   s->digits[0] = '\0';
}

static void ast_mf_detect_init (mf_detect_state_t *s)
{
   int i;
#ifdef OLD_DSP_ROUTINES
   s->hit1 = 
   s->hit2 = 0;
#else 
   s->hits[0] = s->hits[1] = s->hits[2] = s->hits[3] = s->hits[4] = 0;
#endif
   for (i = 0;  i < 6;  i++) {
      goertzel_init (&s->tone_out[i], mf_tones[i], 160);
#ifdef OLD_DSP_ROUTINES
      goertzel_init (&s->tone_out2nd[i], mf_tones[i] * 2.0, 160);
      s->energy = 0.0;
#endif
   }
   s->current_digits = 0;
   memset(&s->digits, 0, sizeof(s->digits));
   s->current_sample = 0;
   s->detected_digits = 0;
   s->lost_digits = 0;
   s->digits[0] = '\0';
   s->mhit = 0;
}

static int dtmf_detect (dtmf_detect_state_t *s, int16_t amp[], int samples, 
       int digitmode, int *writeback, int faxdetect)
{
   float row_energy[4];
   float col_energy[4];
#ifdef FAX_DETECT
   float fax_energy;
#ifdef OLD_DSP_ROUTINES
   float fax_energy_2nd;
#endif   
#endif /* FAX_DETECT */
   float famp;
   float v1;
   int i;
   int j;
   int sample;
   int best_row;
   int best_col;
   int hit;
   int limit;

   hit = 0;
   for (sample = 0;  sample < samples;  sample = limit) {
      /* 102 is optimised to meet the DTMF specs. */
      if ((samples - sample) >= (102 - s->current_sample))
         limit = sample + (102 - s->current_sample);
      else
         limit = samples;
#if defined(USE_3DNOW)
      _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
      _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
#ifdef OLD_DSP_ROUTINES
      _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
      _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
#endif      
      /* XXX Need to fax detect for 3dnow too XXX */
      #warning "Fax Support Broken"
#else
      /* The following unrolled loop takes only 35% (rough estimate) of the 
         time of a rolled loop on the machine on which it was developed */
      for (j=sample;j<limit;j++) {
         famp = amp[j];
         s->energy += famp*famp;
         /* With GCC 2.95, the following unrolled code seems to take about 35%
            (rough estimate) as long as a neat little 0-3 loop */
         v1 = s->row_out[0].v2;
         s->row_out[0].v2 = s->row_out[0].v3;
         s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
         v1 = s->col_out[0].v2;
         s->col_out[0].v2 = s->col_out[0].v3;
         s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
         v1 = s->row_out[1].v2;
         s->row_out[1].v2 = s->row_out[1].v3;
         s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
         v1 = s->col_out[1].v2;
         s->col_out[1].v2 = s->col_out[1].v3;
         s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
         v1 = s->row_out[2].v2;
         s->row_out[2].v2 = s->row_out[2].v3;
         s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
         v1 = s->col_out[2].v2;
         s->col_out[2].v2 = s->col_out[2].v3;
         s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
         v1 = s->row_out[3].v2;
         s->row_out[3].v2 = s->row_out[3].v3;
         s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;
         v1 = s->col_out[3].v2;
         s->col_out[3].v2 = s->col_out[3].v3;
         s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;
#ifdef FAX_DETECT
         /* Update fax tone */
         v1 = s->fax_tone.v2;
         s->fax_tone.v2 = s->fax_tone.v3;
         s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;
#endif /* FAX_DETECT */
#ifdef OLD_DSP_ROUTINES
         v1 = s->col_out2nd[0].v2;
         s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
         s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
         v1 = s->row_out2nd[0].v2;
         s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
         s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
         v1 = s->col_out2nd[1].v2;
         s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
         s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
         v1 = s->row_out2nd[1].v2;
         s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
         s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
         v1 = s->col_out2nd[2].v2;
         s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
         s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
         v1 = s->row_out2nd[2].v2;
         s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
         s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
         v1 = s->col_out2nd[3].v2;
         s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
         s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
         v1 = s->row_out2nd[3].v2;
         s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
         s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;
#ifdef FAX_DETECT
         /* Update fax tone */            
         v1 = s->fax_tone.v2;
         s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
         s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
#endif /* FAX_DETECT */
#endif
      }
#endif
      s->current_sample += (limit - sample);
      if (s->current_sample < 102) {
         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
            /* If we had a hit last time, go ahead and clear this out since likely it
               will be another hit */
            for (i=sample;i<limit;i++) 
               amp[i] = 0;
            *writeback = 1;
         }
         continue;
      }
#ifdef FAX_DETECT
      /* Detect the fax energy, too */
      fax_energy = goertzel_result(&s->fax_tone);
#endif
      /* We are at the end of a DTMF detection block */
      /* Find the peak row and the peak column */
      row_energy[0] = goertzel_result (&s->row_out[0]);
      col_energy[0] = goertzel_result (&s->col_out[0]);

      for (best_row = best_col = 0, i = 1;  i < 4;  i++) {
         row_energy[i] = goertzel_result (&s->row_out[i]);
         if (row_energy[i] > row_energy[best_row])
            best_row = i;
         col_energy[i] = goertzel_result (&s->col_out[i]);
         if (col_energy[i] > col_energy[best_col])
            best_col = i;
      }
      hit = 0;
      /* Basic signal level test and the twist test */
      if (row_energy[best_row] >= DTMF_THRESHOLD && 
          col_energy[best_col] >= DTMF_THRESHOLD &&
          col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST &&
          col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row]) {
         /* Relative peak test */
         for (i = 0;  i < 4;  i++) {
            if ((i != best_col &&
                col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col]) ||
                (i != best_row 
                 && row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row])) {
               break;
            }
         }
#ifdef OLD_DSP_ROUTINES
         /* ... and second harmonic test */
         if (i >= 4 && 
             (row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy &&
                      goertzel_result(&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
             && goertzel_result(&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row]) {
#else
         /* ... and fraction of total energy test */
         if (i >= 4 &&
             (row_energy[best_row] + col_energy[best_col]) > DTMF_TO_TOTAL_ENERGY*s->energy) {
#endif
            /* Got a hit */
            hit = dtmf_positions[(best_row << 2) + best_col];
            if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
               /* Zero out frame data if this is part DTMF */
               for (i=sample;i<limit;i++) 
                  amp[i] = 0;
               *writeback = 1;
            }
            /* Look for two successive similar results */
            /* The logic in the next test is:
               We need two successive identical clean detects, with
               something different preceeding it. This can work with
               back to back differing digits. More importantly, it
               can work with nasty phones that give a very wobbly start
               to a digit */
#ifdef OLD_DSP_ROUTINES
            if (hit == s->hit3  &&  s->hit3 != s->hit2) {
               s->mhit = hit;
               s->digit_hits[(best_row << 2) + best_col]++;
               s->detected_digits++;
               if (s->current_digits < MAX_DTMF_DIGITS) {
                  s->digits[s->current_digits++] = hit;
                  s->digits[s->current_digits] = '\0';
               } else {
                  s->lost_digits++;
               }
            }
#else          
            if (hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0]) {
               s->mhit = hit;
               s->digit_hits[(best_row << 2) + best_col]++;
               s->detected_digits++;
               if (s->current_digits < MAX_DTMF_DIGITS) {
                  s->digits[s->current_digits++] = hit;
                  s->digits[s->current_digits] = '\0';
               } else {
                  s->lost_digits++;
               }
            }
#endif
         }
      } 
#ifdef FAX_DETECT
      if (!hit && (fax_energy >= FAX_THRESHOLD) && 
         (fax_energy >= DTMF_TO_TOTAL_ENERGY*s->energy) &&
         (faxdetect)) {
#if 0
         printf("Fax energy/Second Harmonic: %f\n", fax_energy);
#endif               
         /* XXX Probably need better checking than just this the energy XXX */
         hit = 'f';
         s->fax_hits++;
      } else {
         if (s->fax_hits > 5) {
            hit = 'f';
            s->mhit = 'f';
            s->detected_digits++;
            if (s->current_digits < MAX_DTMF_DIGITS) {
               s->digits[s->current_digits++] = hit;
               s->digits[s->current_digits] = '\0';
            } else {
               s->lost_digits++;
            }
         }
         s->fax_hits = 0;
      }
#endif /* FAX_DETECT */
#ifdef OLD_DSP_ROUTINES
      s->hit1 = s->hit2;
      s->hit2 = s->hit3;
      s->hit3 = hit;
#else
      s->hits[0] = s->hits[1];
      s->hits[1] = s->hits[2];
      s->hits[2] = hit;
#endif      
      /* Reinitialise the detector for the next block */
      for (i = 0;  i < 4;  i++) {
         goertzel_reset(&s->row_out[i]);
         goertzel_reset(&s->col_out[i]);
#ifdef OLD_DSP_ROUTINES
         goertzel_reset(&s->row_out2nd[i]);
         goertzel_reset(&s->col_out2nd[i]);
#endif         
      }
#ifdef FAX_DETECT
      goertzel_reset (&s->fax_tone);
#ifdef OLD_DSP_ROUTINES
      goertzel_reset (&s->fax_tone2nd);
#endif         
#endif
      s->energy = 0.0;
      s->current_sample = 0;
   }
   if ((!s->mhit) || (s->mhit != hit)) {
      s->mhit = 0;
      return(0);
   }
   return (hit);
}

/* MF goertzel size */
#ifdef OLD_DSP_ROUTINES
#define  MF_GSIZE 160
#else
#define MF_GSIZE 120
#endif

static int mf_detect (mf_detect_state_t *s, int16_t amp[],
                 int samples, int digitmode, int *writeback)
{
#ifdef OLD_DSP_ROUTINES
   float tone_energy[6];
   int best1;
   int best2;
   float max;
   int sofarsogood;
#else
   float energy[6];
   int best;
   int second_best;
#endif
   float famp;
   float v1;
   int i;
   int j;
   int sample;
   int hit;
   int limit;

   hit = 0;
   for (sample = 0;  sample < samples;  sample = limit) {
      /* 80 is optimised to meet the MF specs. */
      if ((samples - sample) >= (MF_GSIZE - s->current_sample))
         limit = sample + (MF_GSIZE - s->current_sample);
      else
         limit = samples;
#if defined(USE_3DNOW)
      _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
      _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
#ifdef OLD_DSP_ROUTINES
      _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
      _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
#endif
      /* XXX Need to fax detect for 3dnow too XXX */
      #warning "Fax Support Broken"
#else
      /* The following unrolled loop takes only 35% (rough estimate) of the 
         time of a rolled loop on the machine on which it was developed */
      for (j = sample;  j < limit;  j++) {
         famp = amp[j];
#ifdef OLD_DSP_ROUTINES
         s->energy += famp*famp;
#endif
         /* With GCC 2.95, the following unrolled code seems to take about 35%
            (rough estimate) as long as a neat little 0-3 loop */
         v1 = s->tone_out[0].v2;
         s->tone_out[0].v2 = s->tone_out[0].v3;
         s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
         v1 = s->tone_out[1].v2;
         s->tone_out[1].v2 = s->tone_out[1].v3;
         s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
         v1 = s->tone_out[2].v2;
         s->tone_out[2].v2 = s->tone_out[2].v3;
         s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
         v1 = s->tone_out[3].v2;
         s->tone_out[3].v2 = s->tone_out[3].v3;
         s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
         v1 = s->tone_out[4].v2;
         s->tone_out[4].v2 = s->tone_out[4].v3;
         s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
         v1 = s->tone_out[5].v2;
         s->tone_out[5].v2 = s->tone_out[5].v3;
         s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
#ifdef OLD_DSP_ROUTINES
         v1 = s->tone_out2nd[0].v2;
         s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
         s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
         v1 = s->tone_out2nd[1].v2;
         s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
         s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
         v1 = s->tone_out2nd[2].v2;
         s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
         s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
         v1 = s->tone_out2nd[3].v2;
         s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
         s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
         v1 = s->tone_out2nd[4].v2;
         s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
         s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
         v1 = s->tone_out2nd[3].v2;
         s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
         s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
#endif
      }
#endif
      s->current_sample += (limit - sample);
      if (s->current_sample < MF_GSIZE) {
         if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
            /* If we had a hit last time, go ahead and clear this out since likely it
               will be another hit */
            for (i=sample;i<limit;i++) 
               amp[i] = 0;
            *writeback = 1;
         }
         continue;
      }
#ifdef OLD_DSP_ROUTINES    
      /* We're at the end of an MF detection block.  Go ahead and calculate
         all the energies. */
      for (i=0;i<6;i++) {
         tone_energy[i] = goertzel_result(&s->tone_out[i]);
      }
      /* Find highest */
      best1 = 0;
      max = tone_energy[0];
      for (i=1;i<6;i++) {
         if (tone_energy[i] > max) {
            max = tone_energy[i];
            best1 = i;
         }
      }

      /* Find 2nd highest */
      if (best1) {
         max = tone_energy[0];
         best2 = 0;
      } else {
         max = tone_energy[1];
         best2 = 1;
      }

      for (i=0;i<6;i++) {
         if (i == best1) continue;
         if (tone_energy[i] > max) {
            max = tone_energy[i];
            best2 = i;
         }
      }
      hit = 0;
      if (best1 != best2) 
         sofarsogood=1;
      else 
         sofarsogood=0;
      /* Check for relative energies */
      for (i=0;i<6;i++) {
         if (i == best1) 
            continue;
         if (i == best2) 
            continue;
         if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
            sofarsogood = 0;
            break;
         }
         if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
            sofarsogood = 0;
            break;
         }
      }
      
      if (sofarsogood) {
         /* Check for 2nd harmonic */
         if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1]) 
            sofarsogood = 0;
         else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
            sofarsogood = 0;
      }
      if (sofarsogood) {
         hit = mf_hit[best1][best2];
         if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
            /* Zero out frame data if this is part DTMF */
            for (i=sample;i<limit;i++) 
               amp[i] = 0;
            *writeback = 1;
         }
         /* Look for two consecutive clean hits */
         if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
            s->mhit = hit;
            s->detected_digits++;
            if (s->current_digits < MAX_DTMF_DIGITS - 2) {
               s->digits[s->current_digits++] = hit;
               s->digits[s->current_digits] = '\0';
            } else {
               s->lost_digits++;
            }
         }
      }
      
      s->hit1 = s->hit2;
      s->hit2 = s->hit3;
      s->hit3 = hit;
      /* Reinitialise the detector for the next block */
      for (i = 0;  i < 6;  i++) {
         goertzel_reset(&s->tone_out[i]);
         goertzel_reset(&s->tone_out2nd[i]);
      }
      s->energy = 0.0;
      s->current_sample = 0;
   }
#else
      /* We're at the end of an MF detection block.  */
      /* Find the two highest energies. The spec says to look for
         two tones and two tones only. Taking this literally -ie
         only two tones pass the minimum threshold - doesn't work
         well. The sinc function mess, due to rectangular windowing
         ensure that! Find the two highest energies and ensure they
         are considerably stronger than any of the others. */
      energy[0] = goertzel_result(&s->tone_out[0]);
      energy[1] = goertzel_result(&s->tone_out[1]);
      if (energy[0] > energy[1]) {
         best = 0;
         second_best = 1;
      } else {
         best = 1;
         second_best = 0;
      }
      /*endif*/
      for (i=2;i<6;i++) {
         energy[i] = goertzel_result(&s->tone_out[i]);
         if (energy[i] >= energy[best]) {
            second_best = best;
            best = i;
         } else if (energy[i] >= energy[second_best]) {
            second_best = i;
         }
      }
      /* Basic signal level and twist tests */
      hit = 0;
      if (energy[best] >= BELL_MF_THRESHOLD && energy[second_best] >= BELL_MF_THRESHOLD
               && energy[best] < energy[second_best]*BELL_MF_TWIST
               && energy[best]*BELL_MF_TWIST > energy[second_best]) {
         /* Relative peak test */
         hit = -1;
         for (i=0;i<6;i++) {
            if (i != best && i != second_best) {
               if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best]) {
                  /* The best two are not clearly the best */
                  hit = 0;
                  break;
               }
            }
         }
      }
      if (hit) {
         /* Get the values into ascending order */
         if (second_best < best) {
            i = best;
            best = second_best;
            second_best = i;
         }
         best = best*5 + second_best - 1;
         hit = bell_mf_positions[best];
         /* Look for two successive similar results */
         /* The logic in the next test is:
            For KP we need 4 successive identical clean detects, with
            two blocks of something different preceeding it. For anything
            else we need two successive identical clean detects, with