1 /*
2 * SpanDSP - a series of DSP components for telephony
3 *
4 * bell_r2_mf.c - Bell MF and MFC/R2 tone generation and detection.
5 *
6 * Written by Steve Underwood <steveu@coppice.org>
7 *
8 * Copyright (C) 2001 Steve Underwood
9 *
10 * All rights reserved.
11 *
12 * This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU Lesser General Public License version 2.1,
14 * as published by the Free Software Foundation.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public
22 * License along with this program; if not, write to the Free Software
23 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 */
25
26 /*! \file */
27
28 #if defined(HAVE_CONFIG_H)
29 #include "config.h"
30 #endif
31
32 #include <stdlib.h>
33 #include <inttypes.h>
34 #include <string.h>
35 #include <time.h>
36 #include <fcntl.h>
37 #if defined(HAVE_TGMATH_H)
38 #include <tgmath.h>
39 #endif
40 #if defined(HAVE_MATH_H)
41 #include <math.h>
42 #endif
43 #if defined(HAVE_STDBOOL_H)
44 #include <stdbool.h>
45 #else
46 #include "spandsp/stdbool.h"
47 #endif
48 #include "floating_fudge.h"
49
50 #include "spandsp/telephony.h"
51 #include "spandsp/alloc.h"
52 #include "spandsp/logging.h"
53 #include "spandsp/fast_convert.h"
54 #include "spandsp/queue.h"
55 #include "spandsp/complex.h"
56 #include "spandsp/dds.h"
57 #include "spandsp/tone_detect.h"
58 #include "spandsp/tone_generate.h"
59 #include "spandsp/super_tone_rx.h"
60 #include "spandsp/dtmf.h"
61 #include "spandsp/bell_r2_mf.h"
62
63 #include "spandsp/private/logging.h"
64 #include "spandsp/private/queue.h"
65 #include "spandsp/private/tone_generate.h"
66 #include "spandsp/private/bell_r2_mf.h"
67
68 #if !defined(M_PI)
69 /* C99 systems may not define M_PI */
70 #define M_PI 3.14159265358979323846264338327
71 #endif
72
73 /*!
74 MF tone descriptor.
75 */
76 typedef struct
77 {
78 int f1; /* First freq */
79 int f2; /* Second freq */
80 int8_t level1; /* Level of the first freq (dB) */
81 int8_t level2; /* Level of the second freq (dB) */
82 uint8_t on_time; /* Tone on time (ms) */
83 uint8_t off_time; /* Minimum post tone silence (ms) */
84 } mf_digit_tones_t;
85
86 int bell_mf_gen_inited = false;
87 tone_gen_descriptor_t bell_mf_digit_tones[15];
88
89 int r2_mf_gen_inited = false;
90 tone_gen_descriptor_t r2_mf_fwd_digit_tones[15];
91 tone_gen_descriptor_t r2_mf_back_digit_tones[15];
92
93 #if 0
94 tone_gen_descriptor_t socotel_mf_digit_tones[18];
95 #endif
96
97 /* Bell R1 tone generation specs.
98 * Power: -7dBm +- 1dB
99 * Frequency: within +-1.5%
100 * Mismatch between the start time of a pair of tones: <=6ms.
101 * Mismatch between the end time of a pair of tones: <=6ms.
102 * Tone duration: 68+-7ms, except KP which is 100+-7ms.
103 * Inter-tone gap: 68+-7ms.
104 */
105 static const mf_digit_tones_t bell_mf_tones[] =
106 {
107 { 700, 900, -7, -7, 68, 68},
108 { 700, 1100, -7, -7, 68, 68},
109 { 900, 1100, -7, -7, 68, 68},
110 { 700, 1300, -7, -7, 68, 68},
111 { 900, 1300, -7, -7, 68, 68},
112 {1100, 1300, -7, -7, 68, 68},
113 { 700, 1500, -7, -7, 68, 68},
114 { 900, 1500, -7, -7, 68, 68},
115 {1100, 1500, -7, -7, 68, 68},
116 {1300, 1500, -7, -7, 68, 68},
117 { 700, 1700, -7, -7, 68, 68}, /* ST''' - use 'C' */
118 { 900, 1700, -7, -7, 68, 68}, /* ST' - use 'A' */
119 {1100, 1700, -7, -7, 100, 68}, /* KP - use '*' */
120 {1300, 1700, -7, -7, 68, 68}, /* ST'' - use 'B' */
121 {1500, 1700, -7, -7, 68, 68}, /* ST - use '#' */
122 {0, 0, 0, 0, 0, 0}
123 };
124
125 /* The order of the digits here must match the list above */
126 static const char bell_mf_tone_codes[] = "1234567890CA*B#";
127
128 /* R2 tone generation specs.
129 * Power: -11.5dBm +- 1dB
130 * Frequency: within +-4Hz
131 * Mismatch between the start time of a pair of tones: <=1ms.
132 * Mismatch between the end time of a pair of tones: <=1ms.
133 */
134 static const mf_digit_tones_t r2_mf_fwd_tones[] =
135 {
136 {1380, 1500, -11, -11, 1, 0},
137 {1380, 1620, -11, -11, 1, 0},
138 {1500, 1620, -11, -11, 1, 0},
139 {1380, 1740, -11, -11, 1, 0},
140 {1500, 1740, -11, -11, 1, 0},
141 {1620, 1740, -11, -11, 1, 0},
142 {1380, 1860, -11, -11, 1, 0},
143 {1500, 1860, -11, -11, 1, 0},
144 {1620, 1860, -11, -11, 1, 0},
145 {1740, 1860, -11, -11, 1, 0},
146 {1380, 1980, -11, -11, 1, 0},
147 {1500, 1980, -11, -11, 1, 0},
148 {1620, 1980, -11, -11, 1, 0},
149 {1740, 1980, -11, -11, 1, 0},
150 {1860, 1980, -11, -11, 1, 0},
151 {0, 0, 0, 0, 0, 0}
152 };
153
154 static const mf_digit_tones_t r2_mf_back_tones[] =
155 {
156 {1140, 1020, -11, -11, 1, 0},
157 {1140, 900, -11, -11, 1, 0},
158 {1020, 900, -11, -11, 1, 0},
159 {1140, 780, -11, -11, 1, 0},
160 {1020, 780, -11, -11, 1, 0},
161 { 900, 780, -11, -11, 1, 0},
162 {1140, 660, -11, -11, 1, 0},
163 {1020, 660, -11, -11, 1, 0},
164 { 900, 660, -11, -11, 1, 0},
165 { 780, 660, -11, -11, 1, 0},
166 {1140, 540, -11, -11, 1, 0},
167 {1020, 540, -11, -11, 1, 0},
168 { 900, 540, -11, -11, 1, 0},
169 { 780, 540, -11, -11, 1, 0},
170 { 660, 540, -11, -11, 1, 0},
171 {0, 0, 0, 0, 0, 0}
172 };
173
174 /* The order of the digits here must match the lists above */
175 static const char r2_mf_tone_codes[] = "1234567890BCDEF";
176
177 #if 0
178 static const mf_digit_tones_t socotel_tones[] =
179 {
180 { 700, 900, -11, -11, 1, 0},
181 { 700, 1100, -11, -11, 1, 0},
182 { 900, 1100, -11, -11, 1, 0},
183 { 700, 1300, -11, -11, 1, 0},
184 { 900, 1300, -11, -11, 1, 0},
185 {1100, 1300, -11, -11, 1, 0},
186 { 700, 1500, -11, -11, 1, 0},
187 { 900, 1500, -11, -11, 1, 0},
188 {1100, 1500, -11, -11, 1, 0},
189 {1300, 1500, -11, -11, 1, 0},
190 {1500, 1700, -11, -11, 1, 0},
191 { 700, 1700, -11, -11, 1, 0},
192 { 900, 1700, -11, -11, 1, 0},
193 {1300, 1700, -11, -11, 1, 0},
194 {1100, 1700, -11, -11, 1, 0},
195 {1700, 0, -11, -11, 1, 0}, /* Use 'F' */
196 {1900, 0, -11, -11, 1, 0}, /* Use 'G' */
197 {0, 0, 0, 0, 0, 0}
198 };
199
200 /* The order of the digits here must match the list above */
201 static char socotel_mf_tone_codes[] = "1234567890ABCDEFG";
202 #endif
203
204 #if defined(SPANDSP_USE_FIXED_POINT)
205 #define BELL_MF_THRESHOLD 204089 /* -30.5dBm0 */
206 #define BELL_MF_TWIST 3.981f /* 6dB */
207 #define BELL_MF_RELATIVE_PEAK 12.589f /* 11dB */
208 #define BELL_MF_SAMPLES_PER_BLOCK 120
209
210 #define R2_MF_THRESHOLD 62974 /* -36.5dBm0 */
211 #define R2_MF_TWIST 5.012f /* 7dB */
212 #define R2_MF_RELATIVE_PEAK 12.589f /* 11dB */
213 #define R2_MF_SAMPLES_PER_BLOCK 133
214 #else
215 #define BELL_MF_THRESHOLD 3343803100.0f /* -30.5dBm0 [((120.0*32768.0/1.4142)*10^((-30.5 - DBM0_MAX_SINE_POWER)/20.0))^2 => 3343803100.0] */
216 #define BELL_MF_TWIST 3.981f /* 6dB [10^(6/10) => 3.981] */
217 #define BELL_MF_RELATIVE_PEAK 12.589f /* 11dB */
218 #define BELL_MF_SAMPLES_PER_BLOCK 120
219
220 #define R2_MF_THRESHOLD 1031766650.0f /* -36.5dBm0 [((133.0*32768.0/1.4142)*10^((-36.5 - DBM0_MAX_SINE_POWER)/20.0))^2 => 1031766650.0] */
221 #define R2_MF_TWIST 5.012f /* 7dB */
222 #define R2_MF_RELATIVE_PEAK 12.589f /* 11dB */
223 #define R2_MF_SAMPLES_PER_BLOCK 133
224 #endif
225
226 static goertzel_descriptor_t bell_mf_detect_desc[6];
227
228 static goertzel_descriptor_t mf_fwd_detect_desc[6];
229 static goertzel_descriptor_t mf_back_detect_desc[6];
230
231 static const int bell_mf_frequencies[] =
232 {
233 700, 900, 1100, 1300, 1500, 1700
234 };
235
236 /* Use the follow characters for the Bell MF special signals:
237 KP - use '*'
238 ST - use '#'
239 ST' - use 'A'
240 ST'' - use 'B'
241 ST''' - use 'C' */
242 static const char bell_mf_positions[] = "1247C-358A--69*---0B----#";
243
244 static const int r2_mf_fwd_frequencies[] =
245 {
246 1380, 1500, 1620, 1740, 1860, 1980
247 };
248
249 static const int r2_mf_back_frequencies[] =
250 {
251 1140, 1020, 900, 780, 660, 540
252 };
253
254 /* Use codes '1' to 'F' for the R2 signals 1 to 15, except for signal 'A'.
255 Use '0' for this, so the codes match the digits 0-9. */
256 static const char r2_mf_positions[] = "1247B-358C--69D---0E----F";
257
bell_mf_gen_init(void)258 static void bell_mf_gen_init(void)
259 {
260 int i;
261 const mf_digit_tones_t *tones;
262
263 if (bell_mf_gen_inited)
264 return;
265 i = 0;
266 tones = bell_mf_tones;
267 while (tones->on_time)
268 {
269 /* Note: The duration of KP is longer than the other signals. */
270 tone_gen_descriptor_init(&bell_mf_digit_tones[i++],
271 tones->f1,
272 tones->level1,
273 tones->f2,
274 tones->level2,
275 tones->on_time,
276 tones->off_time,
277 0,
278 0,
279 false);
280 tones++;
281 }
282 bell_mf_gen_inited = true;
283 }
284 /*- End of function --------------------------------------------------------*/
285
bell_mf_tx(bell_mf_tx_state_t * s,int16_t amp[],int max_samples)286 SPAN_DECLARE(int) bell_mf_tx(bell_mf_tx_state_t *s, int16_t amp[], int max_samples)
287 {
288 int len;
289 const char *cp;
290 int digit;
291
292 len = 0;
293 if (s->tones.current_section >= 0)
294 {
295 /* Deal with the fragment left over from last time */
296 len = tone_gen(&s->tones, amp, max_samples);
297 }
298 while (len < max_samples && (digit = queue_read_byte(&s->queue.queue)) >= 0)
299 {
300 /* Step to the next digit */
301 if ((cp = strchr(bell_mf_tone_codes, digit)) == NULL)
302 continue;
303 tone_gen_init(&s->tones, &bell_mf_digit_tones[cp - bell_mf_tone_codes]);
304 len += tone_gen(&s->tones, amp + len, max_samples - len);
305 }
306 return len;
307 }
308 /*- End of function --------------------------------------------------------*/
309
bell_mf_tx_put(bell_mf_tx_state_t * s,const char * digits,int len)310 SPAN_DECLARE(int) bell_mf_tx_put(bell_mf_tx_state_t *s, const char *digits, int len)
311 {
312 size_t space;
313
314 /* This returns the number of characters that would not fit in the buffer.
315 The buffer will only be loaded if the whole string of digits will fit,
316 in which case zero is returned. */
317 if (len < 0)
318 {
319 if ((len = strlen(digits)) == 0)
320 return 0;
321 }
322 if ((space = queue_free_space(&s->queue.queue)) < (size_t) len)
323 return len - (int) space;
324 if (queue_write(&s->queue.queue, (const uint8_t *) digits, len) >= 0)
325 return 0;
326 return -1;
327 }
328 /*- End of function --------------------------------------------------------*/
329
bell_mf_tx_init(bell_mf_tx_state_t * s)330 SPAN_DECLARE(bell_mf_tx_state_t *) bell_mf_tx_init(bell_mf_tx_state_t *s)
331 {
332 if (s == NULL)
333 {
334 if ((s = (bell_mf_tx_state_t *) span_alloc(sizeof(*s))) == NULL)
335 return NULL;
336 }
337 memset(s, 0, sizeof(*s));
338
339 if (!bell_mf_gen_inited)
340 bell_mf_gen_init();
341 tone_gen_init(&s->tones, &bell_mf_digit_tones[0]);
342 s->current_sample = 0;
343 queue_init(&s->queue.queue, MAX_BELL_MF_DIGITS, QUEUE_READ_ATOMIC | QUEUE_WRITE_ATOMIC);
344 s->tones.current_section = -1;
345 return s;
346 }
347 /*- End of function --------------------------------------------------------*/
348
bell_mf_tx_release(bell_mf_tx_state_t * s)349 SPAN_DECLARE(int) bell_mf_tx_release(bell_mf_tx_state_t *s)
350 {
351 queue_release(&s->queue.queue);
352 return 0;
353 }
354 /*- End of function --------------------------------------------------------*/
355
bell_mf_tx_free(bell_mf_tx_state_t * s)356 SPAN_DECLARE(int) bell_mf_tx_free(bell_mf_tx_state_t *s)
357 {
358 queue_release(&s->queue.queue);
359 span_free(s);
360 return 0;
361 }
362 /*- End of function --------------------------------------------------------*/
363
r2_mf_tx(r2_mf_tx_state_t * s,int16_t amp[],int samples)364 SPAN_DECLARE(int) r2_mf_tx(r2_mf_tx_state_t *s, int16_t amp[], int samples)
365 {
366 int len;
367
368 if (s->digit == 0)
369 {
370 len = samples;
371 memset(amp, 0, len*sizeof(int16_t));
372 }
373 else
374 {
375 len = tone_gen(&s->tone, amp, samples);
376 }
377 return len;
378 }
379 /*- End of function --------------------------------------------------------*/
380
r2_mf_tx_put(r2_mf_tx_state_t * s,char digit)381 SPAN_DECLARE(int) r2_mf_tx_put(r2_mf_tx_state_t *s, char digit)
382 {
383 char *cp;
384
385 if (digit && (cp = strchr(r2_mf_tone_codes, digit)))
386 {
387 if (s->fwd)
388 tone_gen_init(&s->tone, &r2_mf_fwd_digit_tones[cp - r2_mf_tone_codes]);
389 else
390 tone_gen_init(&s->tone, &r2_mf_back_digit_tones[cp - r2_mf_tone_codes]);
391 s->digit = digit;
392 }
393 else
394 {
395 s->digit = 0;
396 }
397 return 0;
398 }
399 /*- End of function --------------------------------------------------------*/
400
r2_mf_tx_init(r2_mf_tx_state_t * s,bool fwd)401 SPAN_DECLARE(r2_mf_tx_state_t *) r2_mf_tx_init(r2_mf_tx_state_t *s, bool fwd)
402 {
403 int i;
404 const mf_digit_tones_t *tones;
405
406 if (s == NULL)
407 {
408 if ((s = (r2_mf_tx_state_t *) span_alloc(sizeof(*s))) == NULL)
409 return NULL;
410 }
411 memset(s, 0, sizeof(*s));
412
413 if (!r2_mf_gen_inited)
414 {
415 i = 0;
416 tones = r2_mf_fwd_tones;
417 while (tones->on_time)
418 {
419 tone_gen_descriptor_init(&r2_mf_fwd_digit_tones[i++],
420 tones->f1,
421 tones->level1,
422 tones->f2,
423 tones->level2,
424 tones->on_time,
425 tones->off_time,
426 0,
427 0,
428 (tones->off_time == 0));
429 tones++;
430 }
431 i = 0;
432 tones = r2_mf_back_tones;
433 while (tones->on_time)
434 {
435 tone_gen_descriptor_init(&r2_mf_back_digit_tones[i++],
436 tones->f1,
437 tones->level1,
438 tones->f2,
439 tones->level2,
440 tones->on_time,
441 tones->off_time,
442 0,
443 0,
444 (tones->off_time == 0));
445 tones++;
446 }
447 r2_mf_gen_inited = true;
448 }
449 s->fwd = fwd;
450 return s;
451 }
452 /*- End of function --------------------------------------------------------*/
453
r2_mf_tx_release(r2_mf_tx_state_t * s)454 SPAN_DECLARE(int) r2_mf_tx_release(r2_mf_tx_state_t *s)
455 {
456 return 0;
457 }
458 /*- End of function --------------------------------------------------------*/
459
r2_mf_tx_free(r2_mf_tx_state_t * s)460 SPAN_DECLARE(int) r2_mf_tx_free(r2_mf_tx_state_t *s)
461 {
462 span_free(s);
463 return 0;
464 }
465 /*- End of function --------------------------------------------------------*/
466
bell_mf_rx(bell_mf_rx_state_t * s,const int16_t amp[],int samples)467 SPAN_DECLARE(int) bell_mf_rx(bell_mf_rx_state_t *s, const int16_t amp[], int samples)
468 {
469 #if defined(SPANDSP_USE_FIXED_POINT)
470 int32_t energy[6];
471 int16_t xamp;
472 #else
473 float energy[6];
474 float xamp;
475 #endif
476 int i;
477 int j;
478 int sample;
479 int best;
480 int second_best;
481 int limit;
482 uint8_t hit;
483
484 for (sample = 0; sample < samples; sample = limit)
485 {
486 if ((samples - sample) >= (BELL_MF_SAMPLES_PER_BLOCK - s->current_sample))
487 limit = sample + (BELL_MF_SAMPLES_PER_BLOCK - s->current_sample);
488 else
489 limit = samples;
490 for (j = sample; j < limit; j++)
491 {
492 xamp = goertzel_preadjust_amp(amp[j]);
493 goertzel_samplex(&s->out[0], xamp);
494 goertzel_samplex(&s->out[1], xamp);
495 goertzel_samplex(&s->out[2], xamp);
496 goertzel_samplex(&s->out[3], xamp);
497 goertzel_samplex(&s->out[4], xamp);
498 goertzel_samplex(&s->out[5], xamp);
499 }
500 s->current_sample += (limit - sample);
501 if (s->current_sample < BELL_MF_SAMPLES_PER_BLOCK)
502 continue;
503
504 /* We are at the end of an MF detection block */
505 /* Find the two highest energies. The spec says to look for
506 two tones and two tones only. Taking this literally -ie
507 only two tones pass the minimum threshold - doesn't work
508 well. The sinc function mess, due to rectangular windowing
509 ensure that! Find the two highest energies and ensure they
510 are considerably stronger than any of the others. */
511 energy[0] = goertzel_result(&s->out[0]);
512 energy[1] = goertzel_result(&s->out[1]);
513 if (energy[0] > energy[1])
514 {
515 best = 0;
516 second_best = 1;
517 }
518 else
519 {
520 best = 1;
521 second_best = 0;
522 }
523 for (i = 2; i < 6; i++)
524 {
525 energy[i] = goertzel_result(&s->out[i]);
526 if (energy[i] >= energy[best])
527 {
528 second_best = best;
529 best = i;
530 }
531 else if (energy[i] >= energy[second_best])
532 {
533 second_best = i;
534 }
535 }
536 /* Basic signal level and twist tests */
537 hit = 0;
538 if (energy[best] >= BELL_MF_THRESHOLD
539 &&
540 energy[second_best] >= BELL_MF_THRESHOLD
541 &&
542 energy[best] < energy[second_best]*BELL_MF_TWIST
543 &&
544 energy[best]*BELL_MF_TWIST > energy[second_best])
545 {
546 /* Relative peak test */
547 hit = 'X';
548 for (i = 0; i < 6; i++)
549 {
550 if (i != best && i != second_best)
551 {
552 if (energy[i]*BELL_MF_RELATIVE_PEAK >= energy[second_best])
553 {
554 /* The best two are not clearly the best */
555 hit = 0;
556 break;
557 }
558 }
559 }
560 }
561 if (hit)
562 {
563 /* Get the values into ascending order */
564 if (second_best < best)
565 {
566 i = best;
567 best = second_best;
568 second_best = i;
569 }
570 best = best*5 + second_best - 1;
571 hit = bell_mf_positions[best];
572 /* Look for two successive similar results */
573 /* The logic in the next test is:
574 For KP we need 4 successive identical clean detects, with
575 two blocks of something different preceeding it. For anything
576 else we need two successive identical clean detects, with
577 two blocks of something different preceeding it. */
578 if (hit == s->hits[4]
579 &&
580 hit == s->hits[3]
581 &&
582 ((hit != '*' && hit != s->hits[2] && hit != s->hits[1])
583 ||
584 (hit == '*' && hit == s->hits[2] && hit != s->hits[1] && hit != s->hits[0])))
585 {
586 if (s->current_digits < MAX_BELL_MF_DIGITS)
587 {
588 s->digits[s->current_digits++] = (char) hit;
589 s->digits[s->current_digits] = '\0';
590 if (s->digits_callback)
591 {
592 s->digits_callback(s->digits_callback_data, s->digits, s->current_digits);
593 s->current_digits = 0;
594 }
595 }
596 else
597 {
598 s->lost_digits++;
599 }
600 }
601 }
602 s->hits[0] = s->hits[1];
603 s->hits[1] = s->hits[2];
604 s->hits[2] = s->hits[3];
605 s->hits[3] = s->hits[4];
606 s->hits[4] = hit;
607 s->current_sample = 0;
608 }
609 if (s->current_digits && s->digits_callback)
610 {
611 s->digits_callback(s->digits_callback_data, s->digits, s->current_digits);
612 s->digits[0] = '\0';
613 s->current_digits = 0;
614 }
615 return 0;
616 }
617 /*- End of function --------------------------------------------------------*/
618
bell_mf_rx_get(bell_mf_rx_state_t * s,char * buf,int max)619 SPAN_DECLARE(size_t) bell_mf_rx_get(bell_mf_rx_state_t *s, char *buf, int max)
620 {
621 if (max > s->current_digits)
622 max = s->current_digits;
623 if (max > 0)
624 {
625 memcpy(buf, s->digits, max);
626 memmove(s->digits, s->digits + max, s->current_digits - max);
627 s->current_digits -= max;
628 }
629 buf[max] = '\0';
630 return max;
631 }
632 /*- End of function --------------------------------------------------------*/
633
bell_mf_rx_init(bell_mf_rx_state_t * s,digits_rx_callback_t callback,void * user_data)634 SPAN_DECLARE(bell_mf_rx_state_t *) bell_mf_rx_init(bell_mf_rx_state_t *s,
635 digits_rx_callback_t callback,
636 void *user_data)
637 {
638 int i;
639 static int initialised = false;
640
641 if (s == NULL)
642 {
643 if ((s = (bell_mf_rx_state_t *) span_alloc(sizeof(*s))) == NULL)
644 return NULL;
645 }
646 memset(s, 0, sizeof(*s));
647
648 if (!initialised)
649 {
650 for (i = 0; i < 6; i++)
651 make_goertzel_descriptor(&bell_mf_detect_desc[i], (float) bell_mf_frequencies[i], BELL_MF_SAMPLES_PER_BLOCK);
652 initialised = true;
653 }
654 s->digits_callback = callback;
655 s->digits_callback_data = user_data;
656
657 s->hits[0] =
658 s->hits[1] =
659 s->hits[2] =
660 s->hits[3] =
661 s->hits[4] = 0;
662
663 for (i = 0; i < 6; i++)
664 goertzel_init(&s->out[i], &bell_mf_detect_desc[i]);
665 s->current_sample = 0;
666 s->lost_digits = 0;
667 s->current_digits = 0;
668 s->digits[0] = '\0';
669 return s;
670 }
671 /*- End of function --------------------------------------------------------*/
672
bell_mf_rx_release(bell_mf_rx_state_t * s)673 SPAN_DECLARE(int) bell_mf_rx_release(bell_mf_rx_state_t *s)
674 {
675 return 0;
676 }
677 /*- End of function --------------------------------------------------------*/
678
bell_mf_rx_free(bell_mf_rx_state_t * s)679 SPAN_DECLARE(int) bell_mf_rx_free(bell_mf_rx_state_t *s)
680 {
681 span_free(s);
682 return 0;
683 }
684 /*- End of function --------------------------------------------------------*/
685
r2_mf_rx(r2_mf_rx_state_t * s,const int16_t amp[],int samples)686 SPAN_DECLARE(int) r2_mf_rx(r2_mf_rx_state_t *s, const int16_t amp[], int samples)
687 {
688 #if defined(SPANDSP_USE_FIXED_POINT)
689 int32_t energy[6];
690 int16_t xamp;
691 #else
692 float energy[6];
693 float xamp;
694 #endif
695 int i;
696 int j;
697 int sample;
698 int best;
699 int second_best;
700 int hit;
701 int hit_digit;
702 int limit;
703
704 for (sample = 0; sample < samples; sample = limit)
705 {
706 if ((samples - sample) >= (R2_MF_SAMPLES_PER_BLOCK - s->current_sample))
707 limit = sample + (R2_MF_SAMPLES_PER_BLOCK - s->current_sample);
708 else
709 limit = samples;
710 for (j = sample; j < limit; j++)
711 {
712 xamp = goertzel_preadjust_amp(amp[j]);
713 goertzel_samplex(&s->out[0], xamp);
714 goertzel_samplex(&s->out[1], xamp);
715 goertzel_samplex(&s->out[2], xamp);
716 goertzel_samplex(&s->out[3], xamp);
717 goertzel_samplex(&s->out[4], xamp);
718 goertzel_samplex(&s->out[5], xamp);
719 }
720 s->current_sample += (limit - sample);
721 if (s->current_sample < R2_MF_SAMPLES_PER_BLOCK)
722 continue;
723
724 /* We are at the end of an MF detection block */
725 /* Find the two highest energies */
726 energy[0] = goertzel_result(&s->out[0]);
727 energy[1] = goertzel_result(&s->out[1]);
728 if (energy[0] > energy[1])
729 {
730 best = 0;
731 second_best = 1;
732 }
733 else
734 {
735 best = 1;
736 second_best = 0;
737 }
738
739 for (i = 2; i < 6; i++)
740 {
741 energy[i] = goertzel_result(&s->out[i]);
742 if (energy[i] >= energy[best])
743 {
744 second_best = best;
745 best = i;
746 }
747 else if (energy[i] >= energy[second_best])
748 {
749 second_best = i;
750 }
751 }
752 /* Basic signal level and twist tests */
753 hit = false;
754 if (energy[best] >= R2_MF_THRESHOLD
755 &&
756 energy[second_best] >= R2_MF_THRESHOLD
757 &&
758 energy[best] < energy[second_best]*R2_MF_TWIST
759 &&
760 energy[best]*R2_MF_TWIST > energy[second_best])
761 {
762 /* Relative peak test */
763 hit = true;
764 for (i = 0; i < 6; i++)
765 {
766 if (i != best && i != second_best)
767 {
768 if (energy[i]*R2_MF_RELATIVE_PEAK >= energy[second_best])
769 {
770 /* The best two are not clearly the best */
771 hit = false;
772 break;
773 }
774 }
775 }
776 }
777 if (hit)
778 {
779 /* Get the values into ascending order */
780 if (second_best < best)
781 {
782 i = best;
783 best = second_best;
784 second_best = i;
785 }
786 best = best*5 + second_best - 1;
787 hit_digit = r2_mf_positions[best];
788 }
789 else
790 {
791 hit_digit = 0;
792 }
793 if (s->current_digit != hit_digit && s->callback)
794 {
795 i = (hit_digit) ? -10 : -99;
796 s->callback(s->callback_data, hit_digit, i, 0);
797 }
798 s->current_digit = hit_digit;
799 s->current_sample = 0;
800 }
801 return 0;
802 }
803 /*- End of function --------------------------------------------------------*/
804
r2_mf_rx_get(r2_mf_rx_state_t * s)805 SPAN_DECLARE(int) r2_mf_rx_get(r2_mf_rx_state_t *s)
806 {
807 return s->current_digit;
808 }
809 /*- End of function --------------------------------------------------------*/
810
r2_mf_rx_init(r2_mf_rx_state_t * s,bool fwd,tone_report_func_t callback,void * user_data)811 SPAN_DECLARE(r2_mf_rx_state_t *) r2_mf_rx_init(r2_mf_rx_state_t *s,
812 bool fwd,
813 tone_report_func_t callback,
814 void *user_data)
815 {
816 int i;
817 static int initialised = false;
818
819 if (s == NULL)
820 {
821 if ((s = (r2_mf_rx_state_t *) span_alloc(sizeof(*s))) == NULL)
822 return NULL;
823 }
824 memset(s, 0, sizeof(*s));
825
826 s->fwd = fwd;
827
828 if (!initialised)
829 {
830 for (i = 0; i < 6; i++)
831 {
832 make_goertzel_descriptor(&mf_fwd_detect_desc[i], (float) r2_mf_fwd_frequencies[i], R2_MF_SAMPLES_PER_BLOCK);
833 make_goertzel_descriptor(&mf_back_detect_desc[i], (float) r2_mf_back_frequencies[i], R2_MF_SAMPLES_PER_BLOCK);
834 }
835 initialised = true;
836 }
837 if (fwd)
838 {
839 for (i = 0; i < 6; i++)
840 goertzel_init(&s->out[i], &mf_fwd_detect_desc[i]);
841 }
842 else
843 {
844 for (i = 0; i < 6; i++)
845 goertzel_init(&s->out[i], &mf_back_detect_desc[i]);
846 }
847 s->callback = callback;
848 s->callback_data = user_data;
849 s->current_digit = 0;
850 s->current_sample = 0;
851 return s;
852 }
853 /*- End of function --------------------------------------------------------*/
854
r2_mf_rx_release(r2_mf_rx_state_t * s)855 SPAN_DECLARE(int) r2_mf_rx_release(r2_mf_rx_state_t *s)
856 {
857 return 0;
858 }
859 /*- End of function --------------------------------------------------------*/
860
r2_mf_rx_free(r2_mf_rx_state_t * s)861 SPAN_DECLARE(int) r2_mf_rx_free(r2_mf_rx_state_t *s)
862 {
863 span_free(s);
864 return 0;
865 }
866 /*- End of function --------------------------------------------------------*/
867 /*- End of file ------------------------------------------------------------*/
868