summaryrefslogtreecommitdiffstats
path: root/src/ss5/dsp.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/ss5/dsp.c')
-rw-r--r--src/ss5/dsp.c552
1 files changed, 552 insertions, 0 deletions
diff --git a/src/ss5/dsp.c b/src/ss5/dsp.c
new file mode 100644
index 0000000..4a15937
--- /dev/null
+++ b/src/ss5/dsp.c
@@ -0,0 +1,552 @@
+/* DSP functions
+ *
+ * (C) 2020 by Andreas Eversberg <jolly@eversberg.eu>
+ * All Rights Reserved
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define CHAN ((ss5_t *)(dsp->priv))->name
+
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <math.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <arpa/inet.h>
+#include "../libdebug/debug.h"
+#include "ss5.h"
+
+//#define DEBUG_DEMODULATOR
+
+#define NUM_TONES 8
+#define db2level(db) pow(10, (double)(db) / 20.0)
+#define level2db(level) (20 * log10(level))
+
+static double tone_dbm[NUM_TONES] = { -7, -7, -7, -7, -7, -7, -9, -9 };
+static double tone_freq[NUM_TONES] = { 700, 900, 1100, 1300, 1500, 1700, 2400, 2600 };
+static double tone_width[NUM_TONES] = { 25, 25, 25, 25, 25, 25, 25, 25 };
+
+static double tone_min_dbm[NUM_TONES] = { -14, -14, -14, -14, -14, -14, -16, -16 };
+static double tone_min_ampl_sq[NUM_TONES];
+static double tone_diff_db[NUM_TONES] = { 4, 4, 4, 4, 4, 4, 5, 5 };
+static double tone_diff_ampl_sq[NUM_TONES];
+
+#define INTERRUPT_DURATION 0.015
+#define SPLIT_DURATION 0.030
+#define MF_RECOGNITION 0.025
+
+int dsp_init_inst(dsp_t *dsp, void *priv, double samplerate, double sense_db)
+{
+ double tone_amplitude[NUM_TONES];
+ int t;
+
+ PDEBUG(DDSP, DEBUG_DEBUG, "Init DSP for SS5 instance.\n");
+
+ memset(dsp, 0, sizeof(*dsp));
+ dsp->priv = priv;
+ dsp->samplerate = samplerate;
+ dsp->interrupt_duration = (int)(1000.0 * INTERRUPT_DURATION);
+ dsp->split_duration = (int)(1000.0 * SPLIT_DURATION);
+ dsp->mf_detect_duration = (int)(1000.0 * MF_RECOGNITION);
+ dsp->ms_per_sample = 1000.0 / samplerate;
+ dsp->detect_tone = ' ';
+
+ /* convert dbm of tones to speech level */
+ for (t = 0; t < NUM_TONES; t++) {
+ tone_amplitude[t] = db2level(tone_dbm[t]) / SPEECH_LEVEL;
+ tone_min_ampl_sq[t] = pow(db2level(tone_min_dbm[t] - sense_db) / SPEECH_LEVEL, 2);
+ tone_diff_ampl_sq[t] = pow(db2level(tone_diff_db[t]), 2);
+ }
+
+ /* init MF modulator */
+ dsp->mf_mod = mf_mod_init(samplerate, NUM_TONES, tone_freq, tone_amplitude);
+ if (!dsp->mf_mod)
+ return -EINVAL;
+
+ /* init MF demodulator */
+ dsp->mf_demod = mf_demod_init(samplerate, NUM_TONES, tone_freq, tone_width);
+ if (!dsp->mf_mod)
+ return -EINVAL;
+
+ return 0;
+}
+
+void dsp_cleanup_inst(dsp_t *dsp)
+{
+ PDEBUG(DDSP, DEBUG_DEBUG, "Cleanup DSP of SS5 instance.\n");
+
+ if (dsp->mf_mod) {
+ mf_mod_exit(dsp->mf_mod);
+ dsp->mf_mod = NULL;
+ }
+
+ if (dsp->mf_demod) {
+ mf_demod_exit(dsp->mf_demod);
+ dsp->mf_demod = NULL;
+ }
+}
+
+/*
+ * tone encoder
+ */
+
+static struct dsp_digits {
+ char tone;
+ uint32_t mask;
+} dsp_digits[] = {
+ { '1', 0x01 + 0x02 },
+ { '2', 0x01 + 0x04 },
+ { '3', 0x02 + 0x04 },
+ { '4', 0x01 + 0x08 },
+ { '5', 0x02 + 0x08 },
+ { '6', 0x04 + 0x08 },
+ { '7', 0x01 + 0x10 },
+ { '8', 0x02 + 0x10 },
+ { '9', 0x04 + 0x10 },
+ { '0', 0x08 + 0x10 },
+ { '*', 0x01 + 0x20 }, /* code 11 */
+ { '#', 0x02 + 0x20 }, /* code 12 */
+ { 'a', 0x04 + 0x20 }, /* KP1 */
+ { 'b', 0x08 + 0x20 }, /* KP2 */
+ { 'c', 0x10 + 0x20 }, /* ST */
+ { 'A', 0x40 }, /* 2400 answer, acknowledge */
+ { 'B', 0x80 }, /* 2600 busy */
+ { 'C', 0x40 + 0x80 }, /* 2600+2400 clear forward */
+ { ' ', 0 }, /* silence */
+ { 0 , 0 },
+};
+
+#define KP_DIGIT_DURATION 0.100
+#define OTHER_DIGIT_DURATION 0.055
+#define DIGIT_PAUSE 0.055
+
+/* set signaling tone duration threshold */
+void set_sig_detect_duration(dsp_t *dsp, double duration_AB, double duration_C)
+{
+ dsp->detect_count = 0;
+ dsp->sig_detect_duration_AB = (int)(1000.0 * duration_AB);
+ dsp->sig_detect_duration_C = (int)(1000.0 * duration_C);
+}
+
+/* set given tone with duration (ms) or continuously (0) */
+void set_tone(dsp_t *dsp, char tone, double duration)
+{
+ int i;
+
+ dsp->tone = 0;
+
+ if (!tone) {
+ PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Remove tone\n");
+ return;
+ }
+
+ for (i = 0; dsp_digits[i].tone; i++) {
+ if (dsp_digits[i].tone == tone) {
+ dsp->tone_mask = dsp_digits[i].mask;
+ dsp->tone = tone;
+ dsp->tone_duration = (int)(dsp->samplerate * duration);
+ PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Set tone=\'%c\' duration=%.0fms (mask = 0x%02x)\n", dsp->tone, 1000.0 * duration, dsp->tone_mask);
+ return;
+ }
+ }
+}
+
+/* get next tone from dial string, if any */
+static void get_tone_from_dial_string(dsp_t *dsp)
+{
+ char tone;
+ double duration;
+
+ dsp->tone = 0;
+
+ if (dsp->dial_index == dsp->dial_length) {
+ dsp->dial_length = 0;
+ dialing_complete(dsp->priv);
+ return;
+ }
+
+ /* get alternating tone/pause from dial string */
+ if (!dsp->digit_pause) {
+ /* digit on */
+ tone = dsp->dial_string[dsp->dial_index++];
+ dsp->digit_pause = 1;
+ if (tone == 'a' || tone == 'b')
+ duration = KP_DIGIT_DURATION;
+ else
+ duration = OTHER_DIGIT_DURATION;
+ PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Send digit \'%c\' from dial string\n", tone);
+ } else {
+ /* digit pause */
+ tone = ' ';
+ dsp->digit_pause = 0;
+ duration = DIGIT_PAUSE;
+ PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Send pause after digit from dial string\n");
+ }
+
+ set_tone(dsp, tone, duration);
+}
+
+/* set given dial string */
+void set_dial_string(dsp_t *dsp, const char *dial)
+{
+ dsp->digit_pause = 0;
+ strncpy(dsp->dial_string, dial, sizeof(dsp->dial_string) - 1);
+ dsp->dial_index = 0;
+ dsp->dial_length = strlen(dsp->dial_string);
+}
+
+/* determine which tones to be modulated, get next tone, if elapsed */
+static int assemble_tones(dsp_t *dsp, uint32_t *mask, int length)
+{
+ int i;
+
+ for (i = 0; i < length; i++) {
+ /* if tone was done, try to get next digit */
+ if (!dsp->tone) {
+ if (!dsp->dial_length)
+ return i;
+ get_tone_from_dial_string(dsp);
+ if (!dsp->tone)
+ return i;
+ }
+ *mask++ = dsp->tone_mask;
+ if (dsp->tone_duration) {
+ /* count down duration, if tones is not continuous */
+ if (!(--dsp->tone_duration))
+ dsp->tone = 0;
+ }
+ }
+
+ return i;
+}
+
+/*
+ * tone deencoder
+ */
+
+/* detection array for one frequency */
+static char decode_one[8] =
+ { ' ', ' ', ' ', ' ', ' ', ' ', 'A', 'B' }; /* A = 2400, B = 2600 */
+
+/* detection matrix for two frequencies */
+static char decode_two[8][8] =
+{
+ { ' ', '1', '2', '4', '7', '*', ' ', ' ' }, /* * = code 11 */
+ { '1', ' ', '3', '5', '8', '#', ' ', ' ' }, /* # = code 12 */
+ { '2', '3', ' ', '6', '9', 'a', ' ', ' ' }, /* a = KP1 */
+ { '4', '5', '6', ' ', '0', 'b', ' ', ' ' }, /* b = KP2 */
+ { '7', '8', '9', '0', ' ', 'c', ' ', ' ' }, /* c = ST */
+ { '*', '#', 'a', 'b', 'c', ' ', ' ', ' ' },
+ { ' ', ' ', ' ', ' ', ' ', ' ', ' ', 'C' }, /* C = 2600+2400 */
+ { ' ', ' ', ' ', ' ', ' ', ' ', 'C', ' ' }
+};
+
+#define NONE_MIN_LEVEL_SQUARED
+
+/* determine which tone is played */
+static void detect_tones(dsp_t *dsp, sample_t *samples, sample_t **levels_squared, int length, int incoming)
+{
+ int f1, f2;
+ double f1_level_squared, f2_level_squared;
+ char tone;
+ int s, t;
+
+ for (s = 0; s < length; s++) {
+ /* mute if split duration reached */
+ if (dsp->split_duration && dsp->split_count == dsp->split_duration)
+ samples[s] = 0.0;
+ /* only perform tone detection every millisecond */
+ dsp->detect_interval += dsp->ms_per_sample;
+ if (dsp->detect_interval < 1.0)
+ continue;
+ dsp->detect_interval -= 1.0;
+
+ if (incoming) {
+#ifdef DEBUG_DEMODULATOR
+ for (t = 0; t < dsp->mf_demod->tones; t++) {
+ char level[20];
+ int db;
+ memset(level, 32, sizeof(level));
+ db = roundf(level2db(sqrt(levels_squared[t][s]) * SPEECH_LEVEL) + 25);
+ if (db >= 0 && db < (int)sizeof(level))
+ level[db] = '*';
+ level[sizeof(level)-1]=0;
+ printf("%s|", level);
+ }
+ printf("\n");
+#endif
+ }
+
+ /* find the tone which is the loudest */
+ f1 = -1;
+ f1_level_squared = -1.0;
+ for (t = 0; t < dsp->mf_demod->tones; t++) {
+ if (levels_squared[t][s] > f1_level_squared) {
+ f1_level_squared = levels_squared[t][s];
+ f1 = t;
+ }
+ }
+ /* find the tone which is the second loudest */
+ f2 = -1;
+ f2_level_squared = -1.0;
+ for (t = 0; t < dsp->mf_demod->tones; t++) {
+ if (t == f1)
+ continue;
+ if (levels_squared[t][s] > f2_level_squared) {
+ f2_level_squared = levels_squared[t][s];
+ f2 = t;
+ }
+ }
+ /* now check if the minimum level is reached */
+ if (f1 >= 0 && f1_level_squared < tone_min_ampl_sq[f1])
+ f1 = -1;
+ if (f2 >= 0 && f2_level_squared < tone_min_ampl_sq[f2])
+ f2 = -1;
+
+// printf("%s f1=%.0f (%.1f dBm) f2=%.0f (%.1f dBm)\n", CHAN, (f1 >= 0) ? tone_freq[f1] : 0, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL), (f2 >= 0) ? tone_freq[f2] : 0, level2db(sqrt(f2_level_squared) * SPEECH_LEVEL));
+ /* check if no, one or two tones are detected */
+ if (f1 < 0)
+ tone = ' ';
+ else if (f2 < 0)
+ tone = decode_one[f1];
+ else {
+ if (f2_level_squared * tone_diff_ampl_sq[f2] < f1_level_squared)
+ tone = ' ';
+ else
+ tone = decode_two[f1][f2];
+ }
+ //printf("tone=%c\n", tone);
+
+ /* process interrupt counting, keep tone until interrupt counter expires */
+ if (dsp->detect_tone != ' ' && tone != dsp->detect_tone) {
+ if (dsp->interrupt_count < dsp->interrupt_duration) {
+ dsp->interrupt_count++;
+ tone = dsp->detect_tone;
+ }
+ } else
+ dsp->interrupt_count = 0;
+
+ /* split audio, after minimum duration of detecting a tone */
+ if (tone >= 'A' && tone <= 'C') {
+ if (dsp->split_count < dsp->split_duration)
+ dsp->split_count++;
+ } else
+ dsp->split_count = 0;
+
+
+ /* some change in tone */
+ if (dsp->detect_tone != tone) {
+ if (dsp->detect_count == 0)
+ PDEBUG_CHAN(DDSP, DEBUG_DEBUG, "Detected new tone '%c' (%.1f dBm)\n", tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
+ switch (tone) {
+ case 'A':
+ case 'B':
+ /* tone appears, wait some time */
+ if (dsp->detect_count < dsp->sig_detect_duration_AB)
+ dsp->detect_count++;
+ else {
+ /* sign tone detected */
+ dsp->detect_count = 0;
+ dsp->detect_tone = tone;
+ receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
+ }
+ break;
+ case 'C':
+ /* tone appears, wait some time */
+ if (dsp->detect_count < dsp->sig_detect_duration_C)
+ dsp->detect_count++;
+ else {
+ /* sign tone detected */
+ dsp->detect_count = 0;
+ dsp->detect_tone = tone;
+ receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
+ }
+ break;
+ case ' ':
+ /* tone appears or ceases */
+ dsp->detect_count = 0;
+ dsp->detect_tone = tone;
+ receive_digit(dsp->priv, tone, 0.0);
+ break;
+ default:
+ /* tone appears, wait some time */
+ if (dsp->detect_count < dsp->mf_detect_duration)
+ dsp->detect_count++;
+ else {
+ /* sign tone detected */
+ dsp->detect_count = 0;
+ dsp->detect_tone = tone;
+ receive_digit(dsp->priv, tone, level2db(sqrt(f1_level_squared) * SPEECH_LEVEL));
+ }
+ }
+ } else
+ dsp->detect_count = 0;
+ }
+}
+
+/* process audio from one link (source) to another (destination) */
+static void process_audio(ss5_t *ss5_a, ss5_t *ss5_b, int length)
+{
+ sample_t samples[2][length], s;
+ sample_t b1[length], b2[length], b3[length], b4[length], b5[length], b6[length], b7[length], b8[length];
+ sample_t *levels_squared[NUM_TONES] = { b1, b2, b3, b4, b5, b6, b7, b8 };
+ uint32_t mask[length];
+ int16_t data[160];
+ int count1, count2;
+ int i;
+
+ /* trigger reception of RTP stuff */
+ if (ss5_a->cc_session)
+ osmo_cc_session_handle(ss5_a->cc_session);
+ if (ss5_b->cc_session)
+ osmo_cc_session_handle(ss5_b->cc_session);
+
+ /* get audio from jitter buffer */
+ jitter_load(&ss5_a->dejitter, samples[0], length);
+ jitter_load(&ss5_b->dejitter, samples[1], length);
+
+ /* optionally add comfort noise */
+ if (!ss5_a->cc_callref && ss5_a->ss5_ep->comfort_noise) {
+ for (i = 0; i < length; i++)
+ samples[0][i] += (double)((int8_t)random()) / 2000.0;
+ }
+ if (!ss5_b->cc_callref && ss5_b->ss5_ep->comfort_noise) {
+ for (i = 0; i < length; i++)
+ samples[1][i] += (double)((int8_t)random()) / 2000.0;
+ }
+
+ /* modulate tone/digit. if no tone has to be played (or it stopped), count is less than length */
+ count1 = assemble_tones(&ss5_a->dsp, mask, length);
+ mf_mod(ss5_a->dsp.mf_mod, mask, samples[0], count1);
+ count2 = assemble_tones(&ss5_b->dsp, mask, length);
+ mf_mod(ss5_b->dsp.mf_mod, mask, samples[1], count2);
+
+ /* optionally add some crosstalk */
+ if (ss5_a->ss5_ep->crosstalk) {
+ /* use count, since it carries number of samples with signalling */
+ for (i = 0; i < count1; i++)
+ samples[1][i] += samples[0][i] / 70.0;
+ }
+ if (ss5_b->ss5_ep->crosstalk) {
+ /* use count, since it carries number of samples with signalling */
+ for (i = 0; i < count2; i++)
+ samples[0][i] += samples[1][i] / 70.0;
+ }
+
+ /* ! here is the bridge from a to b and from b to a ! */
+
+ /* optionally add one way delay */
+ if (ss5_b->delay_buffer) {
+ for (i = 0; i < length; i++) {
+ s = ss5_b->delay_buffer[ss5_b->delay_index];
+ ss5_b->delay_buffer[ss5_b->delay_index] = samples[0][i];
+ if (++(ss5_b->delay_index) == ss5_b->delay_length)
+ ss5_b->delay_index = 0;
+ samples[0][i] = s;
+ }
+ }
+ if (ss5_a->delay_buffer) {
+ for (i = 0; i < length; i++) {
+ s = ss5_a->delay_buffer[ss5_a->delay_index];
+ ss5_a->delay_buffer[ss5_a->delay_index] = samples[1][i];
+ if (++(ss5_a->delay_index) == ss5_a->delay_length)
+ ss5_a->delay_index = 0;
+ samples[1][i] = s;
+ }
+ }
+
+ /* demodulate and call tone detector */
+ mf_demod(ss5_b->dsp.mf_demod, samples[0], length, levels_squared);
+ detect_tones(&ss5_b->dsp, samples[0], levels_squared, length, 1);
+ mf_demod(ss5_a->dsp.mf_demod, samples[1], length, levels_squared);
+ detect_tones(&ss5_a->dsp, samples[1], levels_squared, length, 0);
+
+ /* forward audio to CC if call exists */
+ if (ss5_b->cc_callref && ss5_b->codec) {
+ samples_to_int16(data, samples[0], length);
+ osmo_cc_rtp_send(ss5_b->codec, (uint8_t *)data, length * 2, 1, length);
+ }
+ if (ss5_a->cc_callref && ss5_a->codec) {
+ samples_to_int16(data, samples[1], length);
+ osmo_cc_rtp_send(ss5_a->codec, (uint8_t *)data, length * 2, 1, length);
+ }
+}
+
+/* clock is called every given number of samples (20ms) */
+void audio_clock(ss5_endpoint_t *ss5_ep_sunset, ss5_endpoint_t *ss5_ep_sunrise, int len)
+{
+ ss5_t *ss5_a, *ss5_b;
+
+ if (!ss5_ep_sunset)
+ return;
+
+ if (!ss5_ep_sunrise) {
+ /* each pair of links on the same endpoint are bridged */
+ for (ss5_b = ss5_ep_sunset->link_list; ss5_b; ss5_b = ss5_b->next) {
+ ss5_a = ss5_b;
+ ss5_b = ss5_b->next;
+ if (!ss5_b)
+ break;
+ process_audio(ss5_a, ss5_b, len);
+ }
+ } else {
+ /* each link on two endpoints are bridged */
+ for (ss5_a = ss5_ep_sunset->link_list, ss5_b = ss5_ep_sunrise->link_list; ss5_a && ss5_b; ss5_a = ss5_a->next, ss5_b = ss5_b->next) {
+ process_audio(ss5_a, ss5_b, len);
+ }
+ }
+}
+
+/* take audio from CC and store in jitter buffer */
+void down_audio(struct osmo_cc_session_codec *codec, uint16_t __attribute__((unused)) sequence_number, uint32_t __attribute__((unused)) timestamp, uint8_t *data, int len)
+{
+ ss5_t *ss5 = codec->media->session->priv;
+ sample_t samples[len / 2];
+
+ int16_to_samples(samples, (int16_t *)data, len / 2);
+ jitter_save(&ss5->dejitter, samples, len / 2);
+}
+
+void encode_l16(uint8_t *src_data, int src_len, uint8_t **dst_data, int *dst_len)
+{
+ uint16_t *src = (uint16_t *)src_data, *dst;
+ int len = src_len / 2, i;
+
+ dst = malloc(len * 2);
+ if (!dst)
+ return;
+ for (i = 0; i < len; i++)
+ dst[i] = htons(src[i]);
+ *dst_data = (uint8_t *)dst;
+ *dst_len = len * 2;
+}
+
+void decode_l16(uint8_t *src_data, int src_len, uint8_t **dst_data, int *dst_len)
+{
+ uint16_t *src = (uint16_t *)src_data, *dst;
+ int len = src_len / 2, i;
+
+ dst = malloc(len * 2);
+ if (!dst)
+ return;
+ for (i = 0; i < len; i++)
+ dst[i] = ntohs(src[i]);
+ *dst_data = (uint8_t *)dst;
+ *dst_len = len * 2;
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-18 23:49:13 +0000