Main Page | Alphabetical List | Data Structures | File List | Data Fields | Globals

dsp.c

Go to the documentation of this file.
00001 /*
00002  * Asterisk -- A telephony toolkit for Linux.
00003  *
00004  * Convenience Signal Processing routines
00005  * 
00006  * Copyright (C) 2002, Digium
00007  *
00008  * Mark Spencer <markster@linux-support.net>
00009  *
00010  * This program is free software, distributed under the terms of
00011  * the GNU General Public License.
00012  *
00013  * Goertzel routines are borrowed from Steve Underwood's tremendous work on the
00014  * DTMF detector.
00015  *
00016  */
00017 
00018 /* Some routines from tone_detect.c by Steven Underwood as published under the zapata library */
00019 /*
00020    tone_detect.c - General telephony tone detection, and specific
00021                         detection of DTMF.
00022 
00023         Copyright (C) 2001  Steve Underwood <steveu@coppice.org>
00024 
00025         Despite my general liking of the GPL, I place this code in the
00026         public domain for the benefit of all mankind - even the slimy
00027         ones who might try to proprietize my work and use it to my
00028         detriment.
00029 */
00030 
00031 #include <sys/types.h>
00032 #include <asterisk/frame.h>
00033 #include <asterisk/channel.h>
00034 #include <asterisk/channel_pvt.h>
00035 #include <asterisk/logger.h>
00036 #include <asterisk/dsp.h>
00037 #include <asterisk/ulaw.h>
00038 #include <asterisk/alaw.h>
00039 #include <stdlib.h>
00040 #include <unistd.h>
00041 #include <string.h>
00042 #include <math.h>
00043 #include <errno.h>
00044 #include <stdio.h>
00045 
00046 #define DEFAULT_THRESHOLD 1024
00047 
00048 #define BUSY_PERCENT    10 /* The percentage diffrence between the two last silence periods */
00049 #define BUSY_THRESHOLD     100   /* Max number of ms difference between max and min times in busy */
00050 #define BUSY_MIN     75 /* Busy must be at least 80 ms in half-cadence */
00051 #define BUSY_MAX     1100  /* Busy can't be longer than 1100 ms in half-cadence */
00052 
00053 /* Remember last 15 units */
00054 #define DSP_HISTORY 15
00055 
00056 /* Number of goertzels for progress detect */
00057 #define GSAMP_SIZE 183
00058 
00059 /* Define if you want the fax detector -- NOT RECOMMENDED IN -STABLE */
00060 /* #define FAX_DETECT */
00061 
00062 #define HZ_350  0
00063 #define HZ_440  1
00064 #define HZ_480  2
00065 #define HZ_620  3
00066 #define HZ_950  4
00067 #define HZ_1400 5
00068 #define HZ_1800 6
00069 
00070 #define TONE_THRESH 10.0   /* How much louder the tone should be than channel energy */
00071 #define TONE_MIN_THRESH 1e8   /* How much tone there should be at least to attempt */
00072 #define COUNT_THRESH  3    /* Need at least 50ms of stuff to count it */
00073 
00074 #define TONE_STATE_SILENCE  0
00075 #define TONE_STATE_RINGING  1 
00076 #define TONE_STATE_DIALTONE 2
00077 #define TONE_STATE_TALKING  3
00078 #define TONE_STATE_BUSY     4
00079 #define TONE_STATE_SPECIAL1   5
00080 #define TONE_STATE_SPECIAL2 6
00081 #define TONE_STATE_SPECIAL3 7
00082 
00083 #define  MAX_DTMF_DIGITS 128
00084 
00085 /* Basic DTMF specs:
00086  *
00087  * Minimum tone on = 40ms
00088  * Minimum tone off = 50ms
00089  * Maximum digit rate = 10 per second
00090  * Normal twist <= 8dB accepted
00091  * Reverse twist <= 4dB accepted
00092  * S/N >= 15dB will detect OK
00093  * Attenuation <= 26dB will detect OK
00094  * Frequency tolerance +- 1.5% will detect, +-3.5% will reject
00095  */
00096 
00097 #define DTMF_THRESHOLD              8.0e7
00098 #define FAX_THRESHOLD              8.0e7
00099 #define FAX_2ND_HARMONIC            2.0     /* 4dB */
00100 #define DTMF_NORMAL_TWIST           6.3     /* 8dB */
00101 #define DTMF_REVERSE_TWIST          ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 4.0 : 2.5)     /* 4dB normal */
00102 #define DTMF_RELATIVE_PEAK_ROW      6.3     /* 8dB */
00103 #define DTMF_RELATIVE_PEAK_COL      6.3     /* 8dB */
00104 #define DTMF_2ND_HARMONIC_ROW       ((digitmode & DSP_DIGITMODE_RELAXDTMF) ? 1.7 : 2.5)     /* 4dB normal */
00105 #define DTMF_2ND_HARMONIC_COL       63.1    /* 18dB */
00106 #ifndef OLD_DSP_ROUTINES
00107 #define DTMF_TO_TOTAL_ENERGY      42.0
00108 #endif
00109 
00110 #ifdef OLD_DSP_ROUTINES
00111 #define MF_THRESHOLD              8.0e7
00112 #define MF_NORMAL_TWIST           5.3     /* 8dB */
00113 #define MF_REVERSE_TWIST          4.0     /* was 2.5 */
00114 #define MF_RELATIVE_PEAK      5.3     /* 8dB */
00115 #define MF_2ND_HARMONIC       1.7 /* was 2.5  */
00116 #else
00117 #define BELL_MF_THRESHOLD           1.6e9
00118 #define BELL_MF_TWIST               4.0     /* 6dB */
00119 #define BELL_MF_RELATIVE_PEAK       12.6    /* 11dB */
00120 #endif
00121 
00122 typedef struct {
00123    float v2;
00124    float v3;
00125    float fac;
00126 #ifndef OLD_DSP_ROUTINES
00127    int samples;
00128 #endif   
00129 } goertzel_state_t;
00130 
00131 typedef struct
00132 {
00133 
00134     goertzel_state_t row_out[4];
00135     goertzel_state_t col_out[4];
00136 #ifdef FAX_DETECT
00137    goertzel_state_t fax_tone;
00138 #endif
00139 #ifdef OLD_DSP_ROUTINES
00140     goertzel_state_t row_out2nd[4];
00141     goertzel_state_t col_out2nd[4];
00142 #ifdef FAX_DETECT
00143    goertzel_state_t fax_tone2nd;    
00144 #endif
00145     int hit1;
00146     int hit2;
00147     int hit3;
00148     int hit4;
00149 #else
00150     int hits[3];
00151 #endif   
00152     int mhit;
00153     float energy;
00154     int current_sample;
00155 
00156     char digits[MAX_DTMF_DIGITS + 1];
00157     int current_digits;
00158     int detected_digits;
00159     int lost_digits;
00160     int digit_hits[16];
00161 
00162 
00163 #ifdef FAX_DETECT
00164    int fax_hits;
00165 #endif
00166 } dtmf_detect_state_t;
00167 
00168 typedef struct
00169 {
00170     goertzel_state_t tone_out[6];
00171     int mhit;
00172 #ifdef OLD_DSP_ROUTINES
00173     int hit1;
00174     int hit2;
00175     int hit3;
00176     int hit4;
00177     goertzel_state_t tone_out2nd[6];
00178     float energy;
00179 #else
00180     int hits[5];
00181 #endif
00182 
00183     int current_sample;
00184     char digits[MAX_DTMF_DIGITS + 1];
00185     int current_digits;
00186     int detected_digits;
00187     int lost_digits;
00188 #ifdef FAX_DETECT
00189    int fax_hits;
00190 #endif
00191 } mf_detect_state_t;
00192 
00193 static float dtmf_row[] =
00194 {
00195      697.0,  770.0,  852.0,  941.0
00196 };
00197 static float dtmf_col[] =
00198 {
00199     1209.0, 1336.0, 1477.0, 1633.0
00200 };
00201 
00202 static float mf_tones[] =
00203 {
00204    700.0, 900.0, 1100.0, 1300.0, 1500.0, 1700.0
00205 };
00206 
00207 #ifdef FAX_DETECT
00208 static float fax_freq = 1100.0;
00209 #endif
00210 
00211 static char dtmf_positions[] = "123A" "456B" "789C" "*0#D";
00212 
00213 #ifdef OLD_DSP_ROUTINES
00214 static char mf_hit[6][6] = {
00215    /*  700 + */ {   0, '1', '2', '4', '7', 'C' },
00216    /*  900 + */ { '1',   0, '3', '5', '8', 'A' },
00217    /* 1100 + */ { '2', '3',   0, '6', '9', '*' },
00218    /* 1300 + */ { '4', '5', '6',   0, '0', 'B' },
00219    /* 1500 + */ { '7', '8', '9', '0',  0, '#' },
00220    /* 1700 + */ { 'C', 'A', '*', 'B', '#',  0  },
00221 };
00222 #else
00223 static char bell_mf_positions[] = "1247C-358A--69*---0B----#";
00224 #endif
00225 
00226 static inline void goertzel_sample(goertzel_state_t *s, short sample)
00227 {
00228    float v1;
00229    float fsamp  = sample;
00230    v1 = s->v2;
00231    s->v2 = s->v3;
00232    s->v3 = s->fac * s->v2 - v1 + fsamp;
00233 }
00234 
00235 static inline void goertzel_update(goertzel_state_t *s, short *samps, int count)
00236 {
00237    int i;
00238    for (i=0;i<count;i++) 
00239       goertzel_sample(s, samps[i]);
00240 }
00241 
00242 
00243 static inline float goertzel_result(goertzel_state_t *s)
00244 {
00245    return s->v3 * s->v3 + s->v2 * s->v2 - s->v2 * s->v3 * s->fac;
00246 }
00247 
00248 static inline void goertzel_init(goertzel_state_t *s, float freq, int samples)
00249 {
00250    s->v2 = s->v3 = 0.0;
00251    s->fac = 2.0 * cos(2.0 * M_PI * (freq / 8000.0));
00252 #ifndef OLD_DSP_ROUTINES
00253    s->samples = samples;
00254 #endif
00255 }
00256 
00257 static inline void goertzel_reset(goertzel_state_t *s)
00258 {
00259    s->v2 = s->v3 = 0.0;
00260 }
00261 
00262 struct ast_dsp {
00263    struct ast_frame f;
00264    int threshold;
00265    int totalsilence;
00266    int totalnoise;
00267    int features;
00268    int busymaybe;
00269    int busycount;
00270    int historicnoise[DSP_HISTORY];
00271    int historicsilence[DSP_HISTORY];
00272    goertzel_state_t freqs[7];
00273    int gsamps;
00274    int tstate;
00275    int tcount;
00276    int digitmode;
00277    int thinkdigit;
00278    float genergy;
00279    union {
00280       dtmf_detect_state_t dtmf;
00281       mf_detect_state_t mf;
00282    } td;
00283 };
00284 
00285 static void ast_dtmf_detect_init (dtmf_detect_state_t *s)
00286 {
00287     int i;
00288 
00289 #ifdef OLD_DSP_ROUTINES
00290     s->hit1 = 
00291     s->mhit = 
00292    s->hit3 =
00293    s->hit4 = 
00294     s->hit2 = 0;
00295 #else
00296    s->hits[0] = s->hits[1] = s->hits[2] = 0;
00297 #endif
00298     for (i = 0;  i < 4;  i++)
00299     {
00300     
00301          goertzel_init (&s->row_out[i], dtmf_row[i], 102);
00302       goertzel_init (&s->col_out[i], dtmf_col[i], 102);
00303 #ifdef OLD_DSP_ROUTINES
00304       goertzel_init (&s->row_out2nd[i], dtmf_row[i] * 2.0, 102);
00305       goertzel_init (&s->col_out2nd[i], dtmf_col[i] * 2.0, 102);
00306 #endif   
00307       s->energy = 0.0;
00308     }
00309 
00310 #ifdef FAX_DETECT
00311    /* Same for the fax dector */
00312     goertzel_init (&s->fax_tone, fax_freq, 102);
00313 
00314 #ifdef OLD_DSP_ROUTINES
00315    /* Same for the fax dector 2nd harmonic */
00316     goertzel_init (&s->fax_tone2nd, fax_freq * 2.0, 102);
00317 #endif   
00318 #endif /* FAX_DETECT */
00319    
00320     s->current_sample = 0;
00321     s->detected_digits = 0;
00322    s->current_digits = 0;
00323    memset(&s->digits, 0, sizeof(s->digits));
00324     s->lost_digits = 0;
00325     s->digits[0] = '\0';
00326 }
00327 
00328 static void ast_mf_detect_init (mf_detect_state_t *s)
00329 {
00330     int i;
00331 
00332 #ifdef OLD_DSP_ROUTINES
00333     s->hit1 = 
00334     s->hit2 = 0;
00335 #else 
00336    s->hits[0] = s->hits[1] = s->hits[2] = s->hits[3] = s->hits[4] = 0;
00337 #endif
00338     for (i = 0;  i < 6;  i++)
00339     {
00340     
00341          goertzel_init (&s->tone_out[i], mf_tones[i], 160);
00342 #ifdef OLD_DSP_ROUTINES
00343       goertzel_init (&s->tone_out2nd[i], mf_tones[i] * 2.0, 160);
00344       s->energy = 0.0;
00345 #endif
00346    
00347     }
00348 
00349    s->current_digits = 0;
00350    memset(&s->digits, 0, sizeof(s->digits));
00351     s->current_sample = 0;
00352     s->detected_digits = 0;
00353     s->lost_digits = 0;
00354     s->digits[0] = '\0';
00355     s->mhit = 0;
00356 }
00357 
00358 static int dtmf_detect (dtmf_detect_state_t *s,
00359                  int16_t amp[],
00360                  int samples, 
00361        int digitmode, int *writeback)
00362 {
00363 
00364     float row_energy[4];
00365     float col_energy[4];
00366 #ifdef FAX_DETECT
00367     float fax_energy;
00368 #ifdef OLD_DSP_ROUTINES
00369     float fax_energy_2nd;
00370 #endif   
00371 #endif /* FAX_DETECT */
00372     float famp;
00373     float v1;
00374     int i;
00375     int j;
00376     int sample;
00377     int best_row;
00378     int best_col;
00379     int hit;
00380     int limit;
00381 
00382     hit = 0;
00383     for (sample = 0;  sample < samples;  sample = limit)
00384     {
00385         /* 102 is optimised to meet the DTMF specs. */
00386         if ((samples - sample) >= (102 - s->current_sample))
00387             limit = sample + (102 - s->current_sample);
00388         else
00389             limit = samples;
00390 #if defined(USE_3DNOW)
00391         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
00392         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
00393 #ifdef OLD_DSP_ROUTINES
00394         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
00395         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
00396 #endif      
00397       /* XXX Need to fax detect for 3dnow too XXX */
00398       #warning "Fax Support Broken"
00399 #else
00400         /* The following unrolled loop takes only 35% (rough estimate) of the 
00401            time of a rolled loop on the machine on which it was developed */
00402         for (j = sample;  j < limit;  j++)
00403         {
00404             famp = amp[j];
00405        
00406        s->energy += famp*famp;
00407        
00408             /* With GCC 2.95, the following unrolled code seems to take about 35%
00409                (rough estimate) as long as a neat little 0-3 loop */
00410             v1 = s->row_out[0].v2;
00411             s->row_out[0].v2 = s->row_out[0].v3;
00412             s->row_out[0].v3 = s->row_out[0].fac*s->row_out[0].v2 - v1 + famp;
00413     
00414             v1 = s->col_out[0].v2;
00415             s->col_out[0].v2 = s->col_out[0].v3;
00416             s->col_out[0].v3 = s->col_out[0].fac*s->col_out[0].v2 - v1 + famp;
00417     
00418             v1 = s->row_out[1].v2;
00419             s->row_out[1].v2 = s->row_out[1].v3;
00420             s->row_out[1].v3 = s->row_out[1].fac*s->row_out[1].v2 - v1 + famp;
00421     
00422             v1 = s->col_out[1].v2;
00423             s->col_out[1].v2 = s->col_out[1].v3;
00424             s->col_out[1].v3 = s->col_out[1].fac*s->col_out[1].v2 - v1 + famp;
00425     
00426             v1 = s->row_out[2].v2;
00427             s->row_out[2].v2 = s->row_out[2].v3;
00428             s->row_out[2].v3 = s->row_out[2].fac*s->row_out[2].v2 - v1 + famp;
00429     
00430             v1 = s->col_out[2].v2;
00431             s->col_out[2].v2 = s->col_out[2].v3;
00432             s->col_out[2].v3 = s->col_out[2].fac*s->col_out[2].v2 - v1 + famp;
00433     
00434             v1 = s->row_out[3].v2;
00435             s->row_out[3].v2 = s->row_out[3].v3;
00436             s->row_out[3].v3 = s->row_out[3].fac*s->row_out[3].v2 - v1 + famp;
00437 
00438             v1 = s->col_out[3].v2;
00439             s->col_out[3].v2 = s->col_out[3].v3;
00440             s->col_out[3].v3 = s->col_out[3].fac*s->col_out[3].v2 - v1 + famp;
00441 
00442 #ifdef FAX_DETECT
00443          /* Update fax tone */
00444             v1 = s->fax_tone.v2;
00445             s->fax_tone.v2 = s->fax_tone.v3;
00446             s->fax_tone.v3 = s->fax_tone.fac*s->fax_tone.v2 - v1 + famp;
00447 #endif /* FAX_DETECT */
00448 #ifdef OLD_DSP_ROUTINES
00449             v1 = s->col_out2nd[0].v2;
00450             s->col_out2nd[0].v2 = s->col_out2nd[0].v3;
00451             s->col_out2nd[0].v3 = s->col_out2nd[0].fac*s->col_out2nd[0].v2 - v1 + famp;
00452         
00453             v1 = s->row_out2nd[0].v2;
00454             s->row_out2nd[0].v2 = s->row_out2nd[0].v3;
00455             s->row_out2nd[0].v3 = s->row_out2nd[0].fac*s->row_out2nd[0].v2 - v1 + famp;
00456         
00457             v1 = s->col_out2nd[1].v2;
00458             s->col_out2nd[1].v2 = s->col_out2nd[1].v3;
00459             s->col_out2nd[1].v3 = s->col_out2nd[1].fac*s->col_out2nd[1].v2 - v1 + famp;
00460     
00461             v1 = s->row_out2nd[1].v2;
00462             s->row_out2nd[1].v2 = s->row_out2nd[1].v3;
00463             s->row_out2nd[1].v3 = s->row_out2nd[1].fac*s->row_out2nd[1].v2 - v1 + famp;
00464         
00465             v1 = s->col_out2nd[2].v2;
00466             s->col_out2nd[2].v2 = s->col_out2nd[2].v3;
00467             s->col_out2nd[2].v3 = s->col_out2nd[2].fac*s->col_out2nd[2].v2 - v1 + famp;
00468         
00469             v1 = s->row_out2nd[2].v2;
00470             s->row_out2nd[2].v2 = s->row_out2nd[2].v3;
00471             s->row_out2nd[2].v3 = s->row_out2nd[2].fac*s->row_out2nd[2].v2 - v1 + famp;
00472         
00473             v1 = s->col_out2nd[3].v2;
00474             s->col_out2nd[3].v2 = s->col_out2nd[3].v3;
00475             s->col_out2nd[3].v3 = s->col_out2nd[3].fac*s->col_out2nd[3].v2 - v1 + famp;
00476         
00477             v1 = s->row_out2nd[3].v2;
00478             s->row_out2nd[3].v2 = s->row_out2nd[3].v3;
00479             s->row_out2nd[3].v3 = s->row_out2nd[3].fac*s->row_out2nd[3].v2 - v1 + famp;
00480 
00481 
00482 #ifdef FAX_DETECT
00483       /* Update fax tone */            
00484        v1 = s->fax_tone.v2;
00485             s->fax_tone2nd.v2 = s->fax_tone2nd.v3;
00486             s->fax_tone2nd.v3 = s->fax_tone2nd.fac*s->fax_tone2nd.v2 - v1 + famp;
00487 #endif /* FAX_DETECT */
00488 #endif
00489         }
00490 #endif
00491         s->current_sample += (limit - sample);
00492         if (s->current_sample < 102) {
00493          if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
00494             /* If we had a hit last time, go ahead and clear this out since likely it
00495                will be another hit */
00496             for (i=sample;i<limit;i++) 
00497                amp[i] = 0;
00498             *writeback = 1;
00499          }
00500             continue;
00501       }
00502 
00503 #ifdef FAX_DETECT
00504       /* Detect the fax energy, too */
00505       fax_energy = goertzel_result(&s->fax_tone);
00506 #endif
00507       
00508         /* We are at the end of a DTMF detection block */
00509         /* Find the peak row and the peak column */
00510         row_energy[0] = goertzel_result (&s->row_out[0]);
00511         col_energy[0] = goertzel_result (&s->col_out[0]);
00512 
00513    for (best_row = best_col = 0, i = 1;  i < 4;  i++)
00514    {
00515           row_energy[i] = goertzel_result (&s->row_out[i]);
00516             if (row_energy[i] > row_energy[best_row])
00517                 best_row = i;
00518           col_energy[i] = goertzel_result (&s->col_out[i]);
00519             if (col_energy[i] > col_energy[best_col])
00520                 best_col = i;
00521       }
00522         hit = 0;
00523         /* Basic signal level test and the twist test */
00524         if (row_energy[best_row] >= DTMF_THRESHOLD
00525        &&
00526        col_energy[best_col] >= DTMF_THRESHOLD
00527             &&
00528             col_energy[best_col] < row_energy[best_row]*DTMF_REVERSE_TWIST
00529             &&
00530             col_energy[best_col]*DTMF_NORMAL_TWIST > row_energy[best_row])
00531         {
00532             /* Relative peak test */
00533             for (i = 0;  i < 4;  i++)
00534             {
00535                 if ((i != best_col  &&  col_energy[i]*DTMF_RELATIVE_PEAK_COL > col_energy[best_col])
00536                     ||
00537                     (i != best_row  &&  row_energy[i]*DTMF_RELATIVE_PEAK_ROW > row_energy[best_row]))
00538                 {
00539                     break;
00540                 }
00541             }
00542 #ifdef OLD_DSP_ROUTINES
00543             /* ... and second harmonic test */
00544             if (i >= 4
00545            &&
00546       (row_energy[best_row] + col_energy[best_col]) > 42.0*s->energy
00547                 &&
00548                 goertzel_result (&s->col_out2nd[best_col])*DTMF_2ND_HARMONIC_COL < col_energy[best_col]
00549                 &&
00550                 goertzel_result (&s->row_out2nd[best_row])*DTMF_2ND_HARMONIC_ROW < row_energy[best_row])
00551 #else
00552             /* ... and fraction of total energy test */
00553             if (i >= 4
00554                 &&
00555                 (row_energy[best_row] + col_energy[best_col]) > DTMF_TO_TOTAL_ENERGY*s->energy)
00556 #endif
00557             {
00558             /* Got a hit */
00559                 hit = dtmf_positions[(best_row << 2) + best_col];
00560             if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
00561                /* Zero out frame data if this is part DTMF */
00562                for (i=sample;i<limit;i++) 
00563                   amp[i] = 0;
00564                *writeback = 1;
00565             }
00566                 /* Look for two successive similar results */
00567                 /* The logic in the next test is:
00568                    We need two successive identical clean detects, with
00569          something different preceeding it. This can work with
00570          back to back differing digits. More importantly, it
00571          can work with nasty phones that give a very wobbly start
00572          to a digit. */
00573          
00574 #ifdef OLD_DSP_ROUTINES
00575                 if (hit == s->hit3  &&  s->hit3 != s->hit2)
00576                 {
00577                 s->mhit = hit;
00578                     s->digit_hits[(best_row << 2) + best_col]++;
00579                     s->detected_digits++;
00580                     if (s->current_digits < MAX_DTMF_DIGITS)
00581                     {
00582                         s->digits[s->current_digits++] = hit;
00583                         s->digits[s->current_digits] = '\0';
00584                     }
00585                     else
00586                     {
00587                         s->lost_digits++;
00588                     }
00589                 }
00590 #else          
00591                 if (hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0])
00592                 {
00593                 s->mhit = hit;
00594                     s->digit_hits[(best_row << 2) + best_col]++;
00595                     s->detected_digits++;
00596                     if (s->current_digits < MAX_DTMF_DIGITS)
00597                     {
00598                         s->digits[s->current_digits++] = hit;
00599                         s->digits[s->current_digits] = '\0';
00600                     }
00601                     else
00602                     {
00603                         s->lost_digits++;
00604                     }
00605                 }
00606 #endif
00607             }
00608         } 
00609 #ifdef FAX_DETECT
00610 #ifdef OLD_DSP_ROUTINES
00611       if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy > s->energy * 21.0)) {
00612             fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
00613             fax_energy_2nd = goertzel_result(&s->fax_tone2nd);
00614             if (fax_energy_2nd * FAX_2ND_HARMONIC < fax_energy) {
00615 #if 0
00616                printf("Fax energy/Second Harmonic: %f/%f\n", fax_energy, fax_energy_2nd);
00617 #endif               
00618                /* XXX Probably need better checking than just this the energy XXX */
00619                hit = 'f';
00620                s->fax_hits++;
00621             } /* Don't reset fax hits counter */
00622       }
00623 #else /* OLD_DSP_ROUTINES */
00624       if (!hit && (fax_energy >= FAX_THRESHOLD) && (fax_energy >= DTMF_TO_TOTAL_ENERGY*s->energy)) {
00625 #if 0
00626             printf("Fax energy/Second Harmonic: %f\n", fax_energy);
00627 #endif               
00628                /* XXX Probably need better checking than just this the energy XXX */
00629             hit = 'f';
00630             s->fax_hits++;
00631       }
00632 #endif /* OLD_DSP_ROUTINES */
00633       else {
00634          if (s->fax_hits > 5) {
00635              hit = 'f';
00636              s->mhit = 'f';
00637                 s->detected_digits++;
00638                 if (s->current_digits < MAX_DTMF_DIGITS)
00639                 {
00640                      s->digits[s->current_digits++] = hit;
00641                      s->digits[s->current_digits] = '\0';
00642                 }
00643                 else
00644                 {
00645                       s->lost_digits++;
00646                 }
00647          }
00648          s->fax_hits = 0;
00649       }
00650 #endif /* FAX_DETECT */
00651 #ifdef OLD_DSP_ROUTINES
00652         s->hit1 = s->hit2;
00653         s->hit2 = s->hit3;
00654         s->hit3 = hit;
00655 #else
00656         s->hits[0] = s->hits[1];
00657         s->hits[1] = s->hits[2];
00658         s->hits[2] = hit;
00659 #endif      
00660         /* Reinitialise the detector for the next block */
00661         for (i = 0;  i < 4;  i++)
00662         {
00663              goertzel_reset(&s->row_out[i]);
00664             goertzel_reset(&s->col_out[i]);
00665 #ifdef OLD_DSP_ROUTINES
00666           goertzel_reset(&s->row_out2nd[i]);
00667           goertzel_reset(&s->col_out2nd[i]);
00668 #endif         
00669         }
00670 #ifdef FAX_DETECT
00671       goertzel_reset (&s->fax_tone);
00672 #ifdef OLD_DSP_ROUTINES
00673       goertzel_reset (&s->fax_tone2nd);
00674 #endif         
00675 #endif
00676       s->energy = 0.0;
00677         s->current_sample = 0;
00678     }
00679     if ((!s->mhit) || (s->mhit != hit))
00680     {
00681    s->mhit = 0;
00682    return(0);
00683     }
00684     return (hit);
00685 }
00686 
00687 /* MF goertzel size */
00688 #ifdef OLD_DSP_ROUTINES
00689 #define  MF_GSIZE 160
00690 #else
00691 #define MF_GSIZE 120
00692 #endif
00693 
00694 static int mf_detect (mf_detect_state_t *s,
00695                  int16_t amp[],
00696                  int samples, 
00697        int digitmode, int *writeback)
00698 {
00699 
00700 #ifdef OLD_DSP_ROUTINES
00701     float tone_energy[6];
00702     int best1;
00703     int best2;
00704    float max;
00705    int sofarsogood;
00706 #else
00707     float energy[6];
00708     int best;
00709     int second_best;
00710 #endif
00711     float famp;
00712     float v1;
00713     int i;
00714     int j;
00715     int sample;
00716     int hit;
00717     int limit;
00718 
00719     hit = 0;
00720     for (sample = 0;  sample < samples;  sample = limit)
00721     {
00722         /* 80 is optimised to meet the MF specs. */
00723         if ((samples - sample) >= (MF_GSIZE - s->current_sample))
00724             limit = sample + (MF_GSIZE - s->current_sample);
00725         else
00726             limit = samples;
00727 #if defined(USE_3DNOW)
00728         _dtmf_goertzel_update (s->row_out, amp + sample, limit - sample);
00729         _dtmf_goertzel_update (s->col_out, amp + sample, limit - sample);
00730 #ifdef OLD_DSP_ROUTINES
00731         _dtmf_goertzel_update (s->row_out2nd, amp + sample, limit2 - sample);
00732         _dtmf_goertzel_update (s->col_out2nd, amp + sample, limit2 - sample);
00733 #endif
00734       /* XXX Need to fax detect for 3dnow too XXX */
00735       #warning "Fax Support Broken"
00736 #else
00737         /* The following unrolled loop takes only 35% (rough estimate) of the 
00738            time of a rolled loop on the machine on which it was developed */
00739         for (j = sample;  j < limit;  j++)
00740         {
00741             famp = amp[j];
00742        
00743 #ifdef OLD_DSP_ROUTINES
00744        s->energy += famp*famp;
00745 #endif
00746        
00747             /* With GCC 2.95, the following unrolled code seems to take about 35%
00748                (rough estimate) as long as a neat little 0-3 loop */
00749             v1 = s->tone_out[0].v2;
00750             s->tone_out[0].v2 = s->tone_out[0].v3;
00751             s->tone_out[0].v3 = s->tone_out[0].fac*s->tone_out[0].v2 - v1 + famp;
00752 
00753             v1 = s->tone_out[1].v2;
00754             s->tone_out[1].v2 = s->tone_out[1].v3;
00755             s->tone_out[1].v3 = s->tone_out[1].fac*s->tone_out[1].v2 - v1 + famp;
00756     
00757             v1 = s->tone_out[2].v2;
00758             s->tone_out[2].v2 = s->tone_out[2].v3;
00759             s->tone_out[2].v3 = s->tone_out[2].fac*s->tone_out[2].v2 - v1 + famp;
00760     
00761             v1 = s->tone_out[3].v2;
00762             s->tone_out[3].v2 = s->tone_out[3].v3;
00763             s->tone_out[3].v3 = s->tone_out[3].fac*s->tone_out[3].v2 - v1 + famp;
00764 
00765             v1 = s->tone_out[4].v2;
00766             s->tone_out[4].v2 = s->tone_out[4].v3;
00767             s->tone_out[4].v3 = s->tone_out[4].fac*s->tone_out[4].v2 - v1 + famp;
00768 
00769             v1 = s->tone_out[5].v2;
00770             s->tone_out[5].v2 = s->tone_out[5].v3;
00771             s->tone_out[5].v3 = s->tone_out[5].fac*s->tone_out[5].v2 - v1 + famp;
00772 
00773 #ifdef OLD_DSP_ROUTINES
00774             v1 = s->tone_out2nd[0].v2;
00775             s->tone_out2nd[0].v2 = s->tone_out2nd[0].v3;
00776             s->tone_out2nd[0].v3 = s->tone_out2nd[0].fac*s->tone_out2nd[0].v2 - v1 + famp;
00777         
00778             v1 = s->tone_out2nd[1].v2;
00779             s->tone_out2nd[1].v2 = s->tone_out2nd[1].v3;
00780             s->tone_out2nd[1].v3 = s->tone_out2nd[1].fac*s->tone_out2nd[1].v2 - v1 + famp;
00781         
00782             v1 = s->tone_out2nd[2].v2;
00783             s->tone_out2nd[2].v2 = s->tone_out2nd[2].v3;
00784             s->tone_out2nd[2].v3 = s->tone_out2nd[2].fac*s->tone_out2nd[2].v2 - v1 + famp;
00785         
00786             v1 = s->tone_out2nd[3].v2;
00787             s->tone_out2nd[3].v2 = s->tone_out2nd[3].v3;
00788             s->tone_out2nd[3].v3 = s->tone_out2nd[3].fac*s->tone_out2nd[3].v2 - v1 + famp;
00789 
00790             v1 = s->tone_out2nd[4].v2;
00791             s->tone_out2nd[4].v2 = s->tone_out2nd[4].v3;
00792             s->tone_out2nd[4].v3 = s->tone_out2nd[4].fac*s->tone_out2nd[2].v2 - v1 + famp;
00793         
00794             v1 = s->tone_out2nd[3].v2;
00795             s->tone_out2nd[5].v2 = s->tone_out2nd[6].v3;
00796             s->tone_out2nd[5].v3 = s->tone_out2nd[6].fac*s->tone_out2nd[3].v2 - v1 + famp;
00797 #endif
00798         }
00799 #endif
00800         s->current_sample += (limit - sample);
00801         if (s->current_sample < MF_GSIZE) {
00802          if (hit && !((digitmode & DSP_DIGITMODE_NOQUELCH))) {
00803             /* If we had a hit last time, go ahead and clear this out since likely it
00804                will be another hit */
00805             for (i=sample;i<limit;i++) 
00806                amp[i] = 0;
00807             *writeback = 1;
00808          }
00809             continue;
00810       }
00811 
00812 
00813 #ifdef OLD_DSP_ROUTINES    
00814       /* We're at the end of an MF detection block.  Go ahead and calculate
00815          all the energies. */
00816       for (i=0;i<6;i++) {
00817          tone_energy[i] = goertzel_result(&s->tone_out[i]);
00818       }
00819       /* Find highest */
00820       best1 = 0;
00821       max = tone_energy[0];
00822       for (i=1;i<6;i++) {
00823          if (tone_energy[i] > max) {
00824             max = tone_energy[i];
00825             best1 = i;
00826          }
00827       }
00828 
00829       /* Find 2nd highest */
00830       if (best1) {
00831          max = tone_energy[0];
00832          best2 = 0;
00833       } else {
00834          max = tone_energy[1];
00835          best2 = 1;
00836       }
00837 
00838       for (i=0;i<6;i++) {
00839          if (i == best1) continue;
00840          if (tone_energy[i] > max) {
00841             max = tone_energy[i];
00842             best2 = i;
00843          }
00844       }
00845             
00846         hit = 0;
00847       if (best1 != best2) sofarsogood=1;
00848       else sofarsogood=0;
00849       /* Check for relative energies */
00850       for (i=0;i<6;i++) {
00851          if (i == best1) continue;
00852          if (i == best2) continue;
00853          if (tone_energy[best1] < tone_energy[i] * MF_RELATIVE_PEAK) {
00854             sofarsogood = 0;
00855             break;
00856          }
00857          if (tone_energy[best2] < tone_energy[i] * MF_RELATIVE_PEAK) {
00858             sofarsogood = 0;
00859             break;
00860          }
00861       }
00862       
00863       if (sofarsogood) {
00864          /* Check for 2nd harmonic */
00865          if (goertzel_result(&s->tone_out2nd[best1]) * MF_2ND_HARMONIC > tone_energy[best1]) 
00866             sofarsogood = 0;
00867          else if (goertzel_result(&s->tone_out2nd[best2]) * MF_2ND_HARMONIC > tone_energy[best2])
00868             sofarsogood = 0;
00869       }
00870       if (sofarsogood) {
00871          hit = mf_hit[best1][best2];
00872          if (!(digitmode & DSP_DIGITMODE_NOQUELCH)) {
00873             /* Zero out frame data if this is part DTMF */
00874             for (i=sample;i<limit;i++) 
00875                amp[i] = 0;
00876             *writeback = 1;
00877          }
00878          /* Look for two consecutive clean hits */
00879          if ((hit == s->hit3) && (s->hit3 != s->hit2)) {
00880             s->mhit = hit;
00881             s->detected_digits++;
00882             if (s->current_digits < MAX_DTMF_DIGITS - 2) {
00883                s->digits[s->current_digits++] = hit;
00884                s->digits[s->current_digits] = '\0';
00885             } else {
00886                s->lost_digits++;
00887             }
00888          }
00889       }
00890       
00891         s->hit1 = s->hit2;
00892         s->hit2 = s->hit3;
00893         s->hit3 = hit;
00894         /* Reinitialise the detector for the next block */
00895         for (i = 0;  i < 6;  i++)
00896         {
00897              goertzel_reset(&s->tone_out[i]);
00898             goertzel_reset(&s->tone_out2nd[i]);
00899         }
00900       s->energy = 0.0;
00901         s->current_sample = 0;
00902     }
00903 #else
00904       /* We're at the end of an MF detection block.  */
00905         /* Find the two highest energies. The spec says to look for
00906            two tones and two tones only. Taking this literally -ie
00907            only two tones pass the minimum threshold - doesn't work
00908            well. The sinc function mess, due to rectangular windowing
00909            ensure that! Find the two highest energies and ensure they
00910            are considerably stronger than any of the others. */
00911         energy[0] = goertzel_result(&s->tone_out[0]);
00912         energy[1] = goertzel_result(&s->tone_out[1]);
00913         if (energy[0] > energy[1])
00914         {
00915             best = 0;
00916             second_best = 1;
00917         }
00918         else
00919         {
00920             best = 1;
00921             second_best = 0;
00922         }
00923         /*endif*/
00924         for (i = 2;  i < 6;  i++)
00925         {
00926             energy[i] = goertzel_result(&s->tone_out[i]);
00927             if (energy[i] >= energy[best])
00928             {
00929                 second_best = best;
00930                 best = i;
00931             }
00932             else if (energy[i] >= energy[second_best])
00933             {
00934                 second_best = i;
00935             }
00936         }
00937         /* Basic signal level and twist tests */
00938         hit = 0;
00939         if (energy[best] >= BELL_MF_THRESHOLD
00940             &&
00941             energy[second_best] >= BELL_MF_THRESHOLD
00942             &&
00943             energy[best] < energy[second_best]*BELL_MF_TWIST
00944             &&
00945             energy[best]*BELL_MF_TWIST > energy[second_best])
00946         {
00947             /* Relative peak test */
00948             hit = -1;
00949             for (i = 0;  i < 6;  i++)
00950             {
00951                 if (i != best  &&  i != second_best)
00952                 {
00953                     if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best])
00954                     {
00955                         /* The best two are not clearly the best */
00956                         hit = 0;
00957                         break;
00958                     }
00959                 }
00960             }
00961         }
00962         if (hit)
00963         {
00964             /* Get the values into ascending order */
00965             if (second_best < best)
00966             {
00967                 i = best;
00968                 best = second_best;
00969                 second_best = i;
00970             }
00971             best = best*5 + second_best - 1;
00972             hit = bell_mf_positions[best];
00973             /* Look for two successive similar results */
00974             /* The logic in the next test is:
00975                For KP we need 4 successive identical clean detects, with
00976                two blocks of something different preceeding it. For anything
00977                else we need two successive identical clean detects, with
00978                two blocks of something different preceeding it. */
00979             if (hit == s->hits[4]
00980                 &&
00981                 hit == s->hits[3]
00982                 &&
00983                    ((hit != '*'  &&  hit != s->hits[2]  &&  hit != s->hits[1])
00984                     ||
00985                     (hit == '*'  &&  hit == s->hits[2]  &&  hit != s->hits[1]  &&  hit != s->hits[0])))
00986             {
00987                 s->detected_digits++;
00988                 if (s->current_digits < MAX_DTMF_DIGITS)
00989                 {
00990                     s->digits[s->current_digits++] = hit;
00991                     s->digits[s->current_digits] = '\0';
00992                 }
00993                 else
00994                 {
00995                     s->lost_digits++;
00996                 }
00997             }
00998         }
00999         else
01000         {
01001             hit = 0;
01002         }
01003         s->hits[0] = s->hits[1];
01004         s->hits[1] = s->hits[2];
01005         s->hits[2] = s->hits[3];
01006         s->hits[3] = s->hits[4];
01007         s->hits[4] = hit;
01008         /* Reinitialise the detector for the next block */
01009         for (i = 0;  i < 6;  i++)
01010              goertzel_reset(&s->tone_out[i]);
01011         s->current_sample = 0;
01012     }
01013 #endif   
01014     if ((!s->mhit) || (s->mhit != hit))
01015     {
01016       s->mhit = 0;
01017       return(0);
01018     }
01019     return (hit);
01020 }
01021 
01022 static int __ast_dsp_digitdetect(struct ast_dsp *dsp, short *s, int len, int *writeback)
01023 {
01024    int res;
01025    if (dsp->digitmode & DSP_DIGITMODE_MF)
01026       res = mf_detect(&dsp->td.mf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
01027    else
01028       res = dtmf_detect(&dsp->td.dtmf, s, len, dsp->digitmode & DSP_DIGITMODE_RELAXDTMF, writeback);
01029    return res;
01030 }
01031 
01032 int ast_dsp_digitdetect(struct ast_dsp *dsp, struct ast_frame *inf)
01033 {
01034    short *s;
01035    int len;
01036    int ign=0;
01037    if (inf->frametype != AST_FRAME_VOICE) {
01038       ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
01039       return 0;
01040    }
01041    if (inf->subclass != AST_FORMAT_SLINEAR) {
01042       ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
01043       return 0;
01044    }
01045    s = inf->data;
01046    len = inf->datalen / 2;
01047    return __ast_dsp_digitdetect(dsp, s, len, &ign);
01048 }
01049 
01050 static inline int pair_there(float p1, float p2, float i1, float i2, float e)
01051 {
01052    /* See if p1 and p2 are there, relative to i1 and i2 and total energy */
01053    /* Make sure absolute levels are high enough */
01054    if ((p1 < TONE_MIN_THRESH) || (p2 < TONE_MIN_THRESH))
01055       return 0;
01056    /* Amplify ignored stuff */
01057    i2 *= TONE_THRESH;
01058    i1 *= TONE_THRESH;
01059    e *= TONE_THRESH;
01060    /* Check first tone */
01061    if ((p1 < i1) || (p1 < i2) || (p1 < e))
01062       return 0;
01063    /* And second */
01064    if ((p2 < i1) || (p2 < i2) || (p2 < e))
01065       return 0;
01066    /* Guess it's there... */
01067    return 1;
01068 }
01069 
01070 int ast_dsp_getdigits (struct ast_dsp *dsp,
01071               char *buf,
01072               int max)
01073 {
01074    if (dsp->digitmode & DSP_DIGITMODE_MF) {
01075        if (max > dsp->td.mf.current_digits)
01076            max = dsp->td.mf.current_digits;
01077        if (max > 0)
01078        {
01079            memcpy (buf, dsp->td.mf.digits, max);
01080            memmove (dsp->td.mf.digits, dsp->td.mf.digits + max, dsp->td.mf.current_digits - max);
01081            dsp->td.mf.current_digits -= max;
01082        }
01083        buf[max] = '\0';
01084        return  max;
01085    } else {
01086        if (max > dsp->td.dtmf.current_digits)
01087            max = dsp->td.dtmf.current_digits;
01088        if (max > 0)
01089        {
01090            memcpy (buf, dsp->td.dtmf.digits, max);
01091            memmove (dsp->td.dtmf.digits, dsp->td.dtmf.digits + max, dsp->td.dtmf.current_digits - max);
01092            dsp->td.dtmf.current_digits -= max;
01093        }
01094        buf[max] = '\0';
01095        return  max;
01096    }
01097 }
01098 
01099 static int __ast_dsp_call_progress(struct ast_dsp *dsp, short *s, int len)
01100 {
01101    int x;
01102    int pass;
01103    int newstate = TONE_STATE_SILENCE;
01104    int res = 0;
01105    while(len) {
01106       /* Take the lesser of the number of samples we need and what we have */
01107       pass = len;
01108       if (pass > GSAMP_SIZE - dsp->gsamps) 
01109          pass = GSAMP_SIZE - dsp->gsamps;
01110       for (x=0;x<pass;x++) {
01111          goertzel_sample(&dsp->freqs[HZ_350], s[x]);
01112          goertzel_sample(&dsp->freqs[HZ_440], s[x]);
01113          goertzel_sample(&dsp->freqs[HZ_480], s[x]);
01114          goertzel_sample(&dsp->freqs[HZ_620], s[x]);
01115          goertzel_sample(&dsp->freqs[HZ_950], s[x]);
01116          goertzel_sample(&dsp->freqs[HZ_1400], s[x]);
01117          goertzel_sample(&dsp->freqs[HZ_1800], s[x]);
01118          dsp->genergy += s[x] * s[x];
01119       }
01120       s += pass;
01121       dsp->gsamps += pass;
01122       len -= pass;
01123       if (dsp->gsamps == GSAMP_SIZE) {
01124          float hz_350;
01125          float hz_440;
01126          float hz_480;
01127          float hz_620;
01128          float hz_950;
01129          float hz_1400;
01130          float hz_1800;
01131          hz_350 = goertzel_result(&dsp->freqs[HZ_350]);
01132          hz_440 = goertzel_result(&dsp->freqs[HZ_440]);
01133          hz_480 = goertzel_result(&dsp->freqs[HZ_480]);
01134          hz_620 = goertzel_result(&dsp->freqs[HZ_620]);
01135          hz_950 = goertzel_result(&dsp->freqs[HZ_950]);
01136          hz_1400 = goertzel_result(&dsp->freqs[HZ_1400]);
01137          hz_1800 = goertzel_result(&dsp->freqs[HZ_1800]);
01138 #if 0
01139          printf("Got whole dsp state: 350: %e, 440: %e, 480: %e, 620: %e, 950: %e, 1400: %e, 1800: %e, Energy: %e\n", 
01140             hz_350, hz_440, hz_480, hz_620, hz_950, hz_1400, hz_1800, dsp->genergy);
01141 #endif
01142          if (pair_there(hz_480, hz_620, hz_350, hz_440, dsp->genergy)) {
01143             newstate = TONE_STATE_BUSY;
01144          } else if (pair_there(hz_440, hz_480, hz_350, hz_620, dsp->genergy)) {
01145             newstate = TONE_STATE_RINGING;
01146          } else if (pair_there(hz_350, hz_440, hz_480, hz_620, dsp->genergy)) {
01147             newstate = TONE_STATE_DIALTONE;
01148          } else if (hz_950 > TONE_MIN_THRESH * TONE_THRESH) {
01149             newstate = TONE_STATE_SPECIAL1;
01150          } else if (hz_1400 > TONE_MIN_THRESH * TONE_THRESH) {
01151             if (dsp->tstate == TONE_STATE_SPECIAL1)
01152                newstate = TONE_STATE_SPECIAL2;
01153          } else if (hz_1800 > TONE_MIN_THRESH * TONE_THRESH) {
01154             if (dsp->tstate == TONE_STATE_SPECIAL2)
01155                newstate = TONE_STATE_SPECIAL3;
01156          } else if (dsp->genergy > TONE_MIN_THRESH * TONE_THRESH) {
01157             newstate = TONE_STATE_TALKING;
01158          } else
01159             newstate = TONE_STATE_SILENCE;
01160          
01161          if (newstate == dsp->tstate) {
01162             dsp->tcount++;
01163             if (dsp->tcount == COUNT_THRESH) {
01164                if (dsp->tstate == TONE_STATE_BUSY) {
01165                   res = AST_CONTROL_BUSY;
01166                   dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
01167                } else if (dsp->tstate == TONE_STATE_TALKING) {
01168                   res = AST_CONTROL_ANSWER;
01169                   dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
01170                } else if (dsp->tstate == TONE_STATE_RINGING)
01171                   res = AST_CONTROL_RINGING;
01172                else if (dsp->tstate == TONE_STATE_SPECIAL3) {
01173                   res = AST_CONTROL_CONGESTION;
01174                   dsp->features &= ~DSP_FEATURE_CALL_PROGRESS;
01175                }
01176                
01177             }
01178          } else {
01179 #if 0
01180             printf("Newstate: %d\n", newstate);
01181 #endif
01182             dsp->tstate = newstate;
01183             dsp->tcount = 1;
01184          }
01185          
01186          /* Reset goertzel */                
01187          for (x=0;x<7;x++)
01188             dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
01189          dsp->gsamps = 0;
01190          dsp->genergy = 0.0;
01191       }
01192    }
01193 #if 0
01194    if (res)
01195       printf("Returning %d\n", res);
01196 #endif      
01197    return res;
01198 }
01199 
01200 int ast_dsp_call_progress(struct ast_dsp *dsp, struct ast_frame *inf)
01201 {
01202    if (inf->frametype != AST_FRAME_VOICE) {
01203       ast_log(LOG_WARNING, "Can't check call progress of non-voice frames\n");
01204       return 0;
01205    }
01206    if (inf->subclass != AST_FORMAT_SLINEAR) {
01207       ast_log(LOG_WARNING, "Can only check call progress in signed-linear frames\n");
01208       return 0;
01209    }
01210    return __ast_dsp_call_progress(dsp, inf->data, inf->datalen / 2);
01211 }
01212 
01213 static int __ast_dsp_silence(struct ast_dsp *dsp, short *s, int len, int *totalsilence)
01214 {
01215    int accum;
01216    int x;
01217    int res = 0;
01218    
01219    accum = 0;
01220    for (x=0;x<len; x++) 
01221       accum += abs(s[x]);
01222    accum /= len;
01223    if (accum < dsp->threshold) {
01224       dsp->totalsilence += len/8;
01225       if (dsp->totalnoise) {
01226          /* Move and save history */
01227          memmove(dsp->historicnoise + DSP_HISTORY - dsp->busycount, dsp->historicnoise + DSP_HISTORY - dsp->busycount +1, dsp->busycount*sizeof(dsp->historicnoise[0]));
01228          dsp->historicnoise[DSP_HISTORY - 1] = dsp->totalnoise;
01229 /* we don't want to check for busydetect that frequently */
01230 #if 0
01231          dsp->busymaybe = 1;
01232 #endif
01233       }
01234       dsp->totalnoise = 0;
01235       res = 1;
01236    } else {
01237       dsp->totalnoise += len/8;
01238       if (dsp->totalsilence) {
01239          int silence1 = dsp->historicsilence[DSP_HISTORY - 1];
01240          int silence2 = dsp->historicsilence[DSP_HISTORY - 2];
01241          /* Move and save history */
01242          memmove(dsp->historicsilence + DSP_HISTORY - dsp->busycount, dsp->historicsilence + DSP_HISTORY - dsp->busycount + 1, dsp->busycount*sizeof(dsp->historicsilence[0]));
01243          dsp->historicsilence[DSP_HISTORY - 1] = dsp->totalsilence;
01244          /* check if the previous sample differs only by BUSY_PERCENT from the one before it */
01245          if (silence1 < silence2) {
01246             if (silence1 + silence1/BUSY_PERCENT >= silence2)
01247                dsp->busymaybe = 1;
01248             else 
01249                dsp->busymaybe = 0;
01250          } else {
01251             if (silence1 - silence1/BUSY_PERCENT <= silence2)
01252                dsp->busymaybe = 1;
01253             else 
01254                dsp->busymaybe = 0;
01255          }
01256                
01257       }
01258       dsp->totalsilence = 0;
01259    }
01260    if (totalsilence)
01261       *totalsilence = dsp->totalsilence;
01262    return res;
01263 }
01264 #ifdef BUSYDETECT_MARTIN
01265 int ast_dsp_busydetect(struct ast_dsp *dsp)
01266 {
01267    int res = 0, x;
01268 #ifndef BUSYDETECT_TONEONLY
01269    int avgsilence = 0, hitsilence = 0;
01270 #endif
01271    int avgtone = 0, hittone = 0;
01272    if (!dsp->busymaybe)
01273       return res;
01274    for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
01275 #ifndef BUSYDETECT_TONEONLY
01276       avgsilence += dsp->historicsilence[x];
01277 #endif
01278       avgtone += dsp->historicnoise[x];
01279    }
01280 #ifndef BUSYDETECT_TONEONLY
01281    avgsilence /= dsp->busycount;
01282 #endif
01283    avgtone /= dsp->busycount;
01284    for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
01285 #ifndef BUSYDETECT_TONEONLY
01286       if (avgsilence > dsp->historicsilence[x]) {
01287          if (avgsilence - (avgsilence / BUSY_PERCENT) <= dsp->historicsilence[x])
01288             hitsilence++;
01289       } else {
01290          if (avgsilence + (avgsilence / BUSY_PERCENT) >= dsp->historicsilence[x])
01291             hitsilence++;
01292       }
01293 #endif
01294       if (avgtone > dsp->historicnoise[x]) {
01295          if (avgtone - (avgtone / BUSY_PERCENT) <= dsp->historicsilence[x])
01296             hittone++;
01297       } else {
01298          if (avgtone + (avgtone / BUSY_PERCENT) >= dsp->historicsilence[x])
01299             hittone++;
01300       }
01301    }
01302 #ifndef BUSYDETECT_TONEONLY
01303    if ((hittone >= dsp->busycount - 1) && (hitsilence >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX) && (avgsilence >= BUSY_MIN && avgsilence <= BUSY_MAX)) {
01304 #else
01305    if ((hittone >= dsp->busycount - 1) && (avgtone >= BUSY_MIN && avgtone <= BUSY_MAX)) {
01306 #endif
01307 #ifdef BUSYDETECT_COMPARE_TONE_AND_SILENCE
01308 #ifdef BUSYDETECT_TONEONLY
01309 #error You cant use BUSYDETECT_TONEONLY together with BUSYDETECT_COMPARE_TONE_AND_SILENCE
01310 #endif
01311       if (avgtone > avgsilence) {
01312          if (avgtone - avgtone/(BUSY_PERCENT*2) <= avgsilence)
01313             res = 1;
01314       } else {
01315          if (avgtone + avgtone/(BUSY_PERCENT*2) >= avgsilence)
01316             res = 1;
01317       }
01318 #else
01319       res = 1;
01320 #endif
01321    }
01322 #if 0
01323    if (res)
01324       ast_log(LOG_NOTICE, "detected busy, avgtone: %d, avgsilence %d\n", avgtone, avgsilence);
01325 #endif
01326    return res;
01327 }
01328 #endif
01329 
01330 #ifdef BUSYDETECT
01331 int ast_dsp_busydetect(struct ast_dsp *dsp)
01332 {
01333    int x;
01334    int res = 0;
01335    int max, min;
01336 
01337 #if 0
01338    if (dsp->busy_hits > 5);
01339    return 0;
01340 #endif
01341    if (dsp->busymaybe) {
01342 #if 0
01343       printf("Maybe busy!\n");
01344 #endif      
01345       dsp->busymaybe = 0;
01346       min = 9999;
01347       max = 0;
01348       for (x=DSP_HISTORY - dsp->busycount;x<DSP_HISTORY;x++) {
01349 #if 0
01350          printf("Silence: %d, Noise: %d\n", dsp->historicsilence[x], dsp->historicnoise[x]);
01351 #endif         
01352          if (dsp->historicsilence[x] < min)
01353             min = dsp->historicsilence[x];
01354          if (dsp->historicnoise[x] < min)
01355             min = dsp->historicnoise[x];
01356          if (dsp->historicsilence[x] > max)
01357             max = dsp->historicsilence[x];
01358          if (dsp->historicnoise[x] > max)
01359             max = dsp->historicnoise[x];
01360       }
01361       if ((max - min < BUSY_THRESHOLD) && (max < BUSY_MAX) && (min > BUSY_MIN)) {
01362 #if 0
01363          printf("Busy!\n");
01364 #endif         
01365          res = 1;
01366       }
01367 #if 0
01368       printf("Min: %d, max: %d\n", min, max);
01369 #endif      
01370    }
01371    return res;
01372 }
01373 #endif
01374 
01375 int ast_dsp_silence(struct ast_dsp *dsp, struct ast_frame *f, int *totalsilence)
01376 {
01377    short *s;
01378    int len;
01379    
01380    if (f->frametype != AST_FRAME_VOICE) {
01381       ast_log(LOG_WARNING, "Can't calculate silence on a non-voice frame\n");
01382       return 0;
01383    }
01384    if (f->subclass != AST_FORMAT_SLINEAR) {
01385       ast_log(LOG_WARNING, "Can only calculate silence on signed-linear frames :(\n");
01386       return 0;
01387    }
01388    s = f->data;
01389    len = f->datalen/2;
01390    return __ast_dsp_silence(dsp, s, len, totalsilence);
01391 }
01392 
01393 struct ast_frame *ast_dsp_process(struct ast_channel *chan, struct ast_dsp *dsp, struct ast_frame *af, int needlock)
01394 {
01395    int silence;
01396    int res;
01397    int digit;
01398    int x;
01399    unsigned short *shortdata;
01400    unsigned char *odata;
01401    int len;
01402    int writeback = 0;
01403 
01404 #define FIX_INF(inf) do { \
01405       if (writeback) { \
01406          switch(inf->subclass) { \
01407          case AST_FORMAT_SLINEAR: \
01408             break; \
01409          case AST_FORMAT_ULAW: \
01410             for (x=0;x<len;x++) \
01411                odata[x] = AST_LIN2MU(shortdata[x]); \
01412             break; \
01413          case AST_FORMAT_ALAW: \
01414             for (x=0;x<len;x++) \
01415                odata[x] = AST_LIN2A(shortdata[x]); \
01416             break; \
01417          } \
01418       } \
01419    } while(0) 
01420 
01421    if (!af)
01422       return NULL;
01423    if (af->frametype != AST_FRAME_VOICE)
01424       return af;
01425    odata = af->data;
01426    len = af->datalen;
01427    /* Make sure we have short data */
01428    switch(af->subclass) {
01429    case AST_FORMAT_SLINEAR:
01430       shortdata = af->data;
01431       len = af->datalen / 2;
01432       break;
01433    case AST_FORMAT_ULAW:
01434       shortdata = alloca(af->datalen * 2);
01435       if (!shortdata) {
01436          ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
01437          return af;
01438       }
01439       for (x=0;x<len;x++) 
01440          shortdata[x] = AST_MULAW(odata[x]);
01441       break;
01442    case AST_FORMAT_ALAW:
01443       shortdata = alloca(af->datalen * 2);
01444       if (!shortdata) {
01445          ast_log(LOG_WARNING, "Unable to allocate stack space for data: %s\n", strerror(errno));
01446          return af;
01447       }
01448       for (x=0;x<len;x++) 
01449          shortdata[x] = AST_ALAW(odata[x]);
01450       break;
01451    default:
01452       ast_log(LOG_WARNING, "Unable to process inband DTMF on %d frames\n", af->subclass);
01453       return af;
01454    }
01455    silence = __ast_dsp_silence(dsp, shortdata, len, NULL);
01456    if ((dsp->features & DSP_FEATURE_SILENCE_SUPPRESS) && silence) {
01457       memset(&dsp->f, 0, sizeof(dsp->f));
01458       dsp->f.frametype = AST_FRAME_NULL;
01459       return &dsp->f;
01460    }
01461    if ((dsp->features & DSP_FEATURE_BUSY_DETECT) && ast_dsp_busydetect(dsp)) {
01462       chan->_softhangup |= AST_SOFTHANGUP_DEV;
01463       memset(&dsp->f, 0, sizeof(dsp->f));
01464       dsp->f.frametype = AST_FRAME_CONTROL;
01465       dsp->f.subclass = AST_CONTROL_BUSY;
01466       ast_log(LOG_DEBUG, "Requesting Hangup because the busy tone was detected on channel %s\n", chan->name);
01467       return &dsp->f;
01468    }
01469    if ((dsp->features & DSP_FEATURE_DTMF_DETECT)) {
01470       digit = __ast_dsp_digitdetect(dsp, shortdata, len, &writeback);
01471 #if 0
01472       if (digit)
01473          printf("Performing digit detection returned %d, digitmode is %d\n", digit, dsp->digitmode);
01474 #endif         
01475       if (dsp->digitmode & (DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX)) {
01476          if (!dsp->thinkdigit) {
01477             if (digit) {
01478                /* Looks like we might have something.  Request a conference mute for the moment */
01479                memset(&dsp->f, 0, sizeof(dsp->f));
01480                dsp->f.frametype = AST_FRAME_DTMF;
01481                dsp->f.subclass = 'm';
01482                dsp->thinkdigit = 'x';
01483                FIX_INF(af);
01484                if (chan)
01485                   ast_queue_frame(chan, af, needlock);
01486                ast_frfree(af);
01487                return &dsp->f;
01488             }
01489          } else {
01490             if (digit) {
01491                /* Thought we saw one last time.  Pretty sure we really have now */
01492                if (dsp->thinkdigit) {
01493                   if ((dsp->thinkdigit != 'x') && (dsp->thinkdigit != digit)) {
01494                      /* If we found a digit, and we're changing digits, go
01495                         ahead and send this one, but DON'T stop confmute because
01496                         we're detecting something else, too... */
01497                      memset(&dsp->f, 0, sizeof(dsp->f));
01498                      dsp->f.frametype = AST_FRAME_DTMF;
01499                      dsp->f.subclass = dsp->thinkdigit;
01500                      FIX_INF(af);
01501                      if (chan)
01502                         ast_queue_frame(chan, af, needlock);
01503                      ast_frfree(af);
01504                   }
01505                   dsp->thinkdigit = digit;
01506                   return &dsp->f;
01507                }
01508                dsp->thinkdigit = digit;
01509             } else {
01510                if (dsp->thinkdigit) {
01511                   memset(&dsp->f, 0, sizeof(dsp->f));
01512                   if (dsp->thinkdigit != 'x') {
01513                      /* If we found a digit, send it now */
01514                      dsp->f.frametype = AST_FRAME_DTMF;
01515                      dsp->f.subclass = dsp->thinkdigit;
01516                      dsp->thinkdigit = 0;
01517                   } else {
01518                      dsp->f.frametype = AST_FRAME_DTMF;
01519                      dsp->f.subclass = 'u';
01520                      dsp->thinkdigit = 0;
01521                   }
01522                   FIX_INF(af);
01523                   if (chan)
01524                      ast_queue_frame(chan, af, needlock);
01525                   ast_frfree(af);
01526                   return &dsp->f;
01527                }
01528             }
01529          }
01530       } else if (!digit) {
01531          /* Only check when there is *not* a hit... */
01532          if (dsp->digitmode & DSP_DIGITMODE_MF) {
01533             if (dsp->td.mf.current_digits) {
01534                memset(&dsp->f, 0, sizeof(dsp->f));
01535                dsp->f.frametype = AST_FRAME_DTMF;
01536                dsp->f.subclass = dsp->td.mf.digits[0];
01537                memmove(dsp->td.mf.digits, dsp->td.mf.digits + 1, dsp->td.mf.current_digits);
01538                dsp->td.mf.current_digits--;
01539                FIX_INF(af);
01540                if (chan)
01541                   ast_queue_frame(chan, af, needlock);
01542                ast_frfree(af);
01543                return &dsp->f;
01544             }
01545          } else {
01546             if (dsp->td.dtmf.current_digits) {
01547                memset(&dsp->f, 0, sizeof(dsp->f));
01548                dsp->f.frametype = AST_FRAME_DTMF;
01549                dsp->f.subclass = dsp->td.dtmf.digits[0];
01550                memmove(dsp->td.dtmf.digits, dsp->td.dtmf.digits + 1, dsp->td.dtmf.current_digits);
01551                dsp->td.dtmf.current_digits--;
01552                FIX_INF(af);
01553                if (chan)
01554                   ast_queue_frame(chan, af, needlock);
01555                ast_frfree(af);
01556                return &dsp->f;
01557             }
01558          }
01559       }
01560    }
01561    if ((dsp->features & DSP_FEATURE_CALL_PROGRESS)) {
01562       res = __ast_dsp_call_progress(dsp, shortdata, len);
01563       memset(&dsp->f, 0, sizeof(dsp->f));
01564       dsp->f.frametype = AST_FRAME_CONTROL;
01565       if (res) {
01566          switch(res) {
01567          case AST_CONTROL_ANSWER:
01568          case AST_CONTROL_BUSY:
01569          case AST_CONTROL_RINGING:
01570          case AST_CONTROL_CONGESTION:
01571             dsp->f.subclass = res;
01572             if (chan) 
01573                ast_queue_frame(chan, &dsp->f, needlock);
01574             break;
01575          default:
01576             ast_log(LOG_WARNING, "Don't know how to represent call progress message %d\n", res);
01577          }
01578       }
01579    }
01580    FIX_INF(af);
01581    return af;
01582 }
01583 
01584 struct ast_dsp *ast_dsp_new(void)
01585 {
01586    struct ast_dsp *dsp;
01587    dsp = malloc(sizeof(struct ast_dsp));
01588    if (dsp) {
01589       memset(dsp, 0, sizeof(struct ast_dsp));
01590       dsp->threshold = DEFAULT_THRESHOLD;
01591       dsp->features = DSP_FEATURE_SILENCE_SUPPRESS;
01592       dsp->busycount = DSP_HISTORY;
01593       /* Initialize goertzels */
01594       goertzel_init(&dsp->freqs[HZ_350], 350.0, GSAMP_SIZE);
01595       goertzel_init(&dsp->freqs[HZ_440], 440.0, GSAMP_SIZE);
01596       goertzel_init(&dsp->freqs[HZ_480], 480.0, GSAMP_SIZE);
01597       goertzel_init(&dsp->freqs[HZ_620], 620.0, GSAMP_SIZE);
01598       goertzel_init(&dsp->freqs[HZ_950], 950.0, GSAMP_SIZE);
01599       goertzel_init(&dsp->freqs[HZ_1400], 1400.0, GSAMP_SIZE);
01600       goertzel_init(&dsp->freqs[HZ_1800], 1800.0, GSAMP_SIZE);
01601       /* Initialize DTMF detector */
01602       ast_dtmf_detect_init(&dsp->td.dtmf);
01603    }
01604    return dsp;
01605 }
01606 
01607 void ast_dsp_set_features(struct ast_dsp *dsp, int features)
01608 {
01609    dsp->features = features;
01610 }
01611 
01612 void ast_dsp_free(struct ast_dsp *dsp)
01613 {
01614    free(dsp);
01615 }
01616 
01617 void ast_dsp_set_threshold(struct ast_dsp *dsp, int threshold)
01618 {
01619    dsp->threshold = threshold;
01620 }
01621 
01622 void ast_dsp_set_busy_count(struct ast_dsp *dsp, int cadences)
01623 {
01624    if (cadences < 4)
01625       cadences = 4;
01626    if (cadences > DSP_HISTORY)
01627       cadences = DSP_HISTORY;
01628    dsp->busycount = cadences;
01629 }
01630 
01631 void ast_dsp_digitreset(struct ast_dsp *dsp)
01632 {
01633    int i;
01634    dsp->thinkdigit = 0;
01635    if (dsp->digitmode & DSP_DIGITMODE_MF) {
01636       memset(dsp->td.mf.digits, 0, sizeof(dsp->td.mf.digits));
01637       dsp->td.mf.current_digits = 0;
01638       /* Reinitialise the detector for the next block */
01639       for (i = 0;  i < 6;  i++) {
01640             goertzel_reset(&dsp->td.mf.tone_out[i]);
01641 #ifdef OLD_DSP_ROUTINES
01642           goertzel_reset(&dsp->td.mf.tone_out2nd[i]);
01643 #endif         
01644       }
01645 #ifdef OLD_DSP_ROUTINES
01646       dsp->td.mf.energy = 0.0;
01647        dsp->td.mf.hit1 = dsp->td.mf.hit2 = dsp->td.mf.hit3 = dsp->td.mf.hit4 = dsp->td.mf.mhit = 0;
01648 #else
01649        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;
01650 #endif      
01651       dsp->td.mf.current_sample = 0;
01652    } else {
01653       memset(dsp->td.dtmf.digits, 0, sizeof(dsp->td.dtmf.digits));
01654       dsp->td.dtmf.current_digits = 0;
01655       /* Reinitialise the detector for the next block */
01656       for (i = 0;  i < 4;  i++) {
01657             goertzel_reset(&dsp->td.dtmf.row_out[i]);
01658           goertzel_reset(&dsp->td.dtmf.col_out[i]);
01659 #ifdef OLD_DSP_ROUTINES
01660          goertzel_reset(&dsp->td.dtmf.row_out2nd[i]);
01661          goertzel_reset(&dsp->td.dtmf.col_out2nd[i]);
01662 #endif         
01663       }
01664 #ifdef FAX_DETECT
01665        goertzel_reset (&dsp->td.dtmf.fax_tone);
01666 #endif
01667 #ifdef OLD_DSP_ROUTINES
01668 #ifdef FAX_DETECT
01669        goertzel_reset (&dsp->td.dtmf.fax_tone2nd);
01670 #endif
01671        dsp->td.dtmf.hit1 = dsp->td.dtmf.hit2 = dsp->td.dtmf.hit3 = dsp->td.dtmf.hit4 = dsp->td.dtmf.mhit = 0;
01672 #else
01673        dsp->td.dtmf.hits[2] = dsp->td.dtmf.hits[1] = dsp->td.dtmf.hits[0] =  dsp->td.dtmf.mhit = 0;
01674 #endif      
01675       dsp->td.dtmf.energy = 0.0;
01676       dsp->td.dtmf.current_sample = 0;
01677    }
01678 }
01679 
01680 void ast_dsp_reset(struct ast_dsp *dsp)
01681 {
01682    int x;
01683    dsp->totalsilence = 0;
01684    dsp->gsamps = 0;
01685    for (x=0;x<4;x++)
01686       dsp->freqs[x].v2 = dsp->freqs[x].v3 = 0.0;
01687    memset(dsp->historicsilence, 0, sizeof(dsp->historicsilence));
01688    memset(dsp->historicnoise, 0, sizeof(dsp->historicnoise));
01689    
01690 }
01691 
01692 int ast_dsp_digitmode(struct ast_dsp *dsp, int digitmode)
01693 {
01694    int new, old;
01695    old = dsp->digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
01696    new = digitmode & (DSP_DIGITMODE_DTMF | DSP_DIGITMODE_MF | DSP_DIGITMODE_MUTECONF | DSP_DIGITMODE_MUTEMAX);
01697    if (old != new) {
01698       /* Must initialize structures if switching from MF to DTMF or vice-versa */
01699       if (new & DSP_DIGITMODE_MF)
01700          ast_mf_detect_init(&dsp->td.mf);
01701       else
01702          ast_dtmf_detect_init(&dsp->td.dtmf);
01703    }
01704    dsp->digitmode = digitmode;
01705    return 0;
01706 }
01707 

Generated on Sun Apr 18 23:33:51 2004 for Asterisk by doxygen 1.3.6-20040222