freetdm: saturated add and pri tapping mixing

diff --git a/libs/freetdm/src/ftdm_io.c b/libs/freetdm/src/ftdm_io.c
index 81bef16a623558c8f18b053967321063a0c669ae..afdde55757bad1ae7d297b12e309e8d62ed28ea8 100644 (file)
--- a/libs/freetdm/src/ftdm_io.c
+++ b/libs/freetdm/src/ftdm_io.c
@@ -2975,6 +2975,11 @@ static FIO_READ_FUNCTION(ftdm_raw_read)
                        ftdm_log(FTDM_LOG_WARNING, "Raw input trace failed to write all of the %zd bytes\n", dlen);
                }
        }
+
+       if (status == FTDM_SUCCESS && ftdmchan->span->sig_read) {
+               ftdmchan->span->sig_read(ftdmchan, data, *datalen);
+       }
+
 #ifdef FTDM_DEBUG_DTMF
        if (status == FTDM_SUCCESS) {
                int dlen = (int) *datalen;
@@ -3131,17 +3136,21 @@ FT_DECLARE(ftdm_status_t) ftdm_channel_read(ftdm_channel_t *ftdmchan, void *data
 
        ftdm_assert_return(ftdmchan != NULL, FTDM_FAIL, "ftdmchan is null\n");
        ftdm_assert_return(ftdmchan->fio != NULL, FTDM_FAIL, "No I/O module attached to ftdmchan\n");
+
+       ftdm_channel_lock(ftdmchan);
        
        if (!ftdm_test_flag(ftdmchan, FTDM_CHANNEL_OPEN)) {
                snprintf(ftdmchan->last_error, sizeof(ftdmchan->last_error), "channel not open");
                ftdm_log_chan_msg(ftdmchan, FTDM_LOG_WARNING, "cannot read from channel that is not open\n");
-               return FTDM_FAIL;
+               status = FTDM_FAIL;
+               goto done;
        }
 
        if (!ftdmchan->fio->read) {
                snprintf(ftdmchan->last_error, sizeof(ftdmchan->last_error), "method not implemented");
                ftdm_log_chan_msg(ftdmchan, FTDM_LOG_ERROR, "read method not implemented\n");
-               return FTDM_FAIL;
+               status = FTDM_FAIL;
+               goto done;
        }
 
        status = ftdm_raw_read(ftdmchan, data, datalen);
@@ -3210,7 +3219,8 @@ FT_DECLARE(ftdm_status_t) ftdm_channel_read(ftdm_channel_t *ftdmchan, void *data
                                } else {
                                        snprintf(ftdmchan->last_error, sizeof(ftdmchan->last_error), "codec error!");
                                        ftdm_log_chan(ftdmchan, FTDM_LOG_ERROR, "invalid effective codec %d\n", ftdmchan->effective_codec);
-                                       return FTDM_FAIL;
+                                       status = FTDM_FAIL;
+                                       goto done;
                                }
                        }
                        sln = (int16_t *) sln_buf;
@@ -3332,6 +3342,9 @@ FT_DECLARE(ftdm_status_t) ftdm_channel_read(ftdm_channel_t *ftdmchan, void *data
                ftdm_mutex_unlock(ftdmchan->pre_buffer_mutex);
        }
 
+done:
+
+       ftdm_channel_unlock(ftdmchan);
 
        return status;
 }

diff --git a/libs/freetdm/src/ftmod/ftmod_pritap/ftmod_pritap.c b/libs/freetdm/src/ftmod/ftmod_pritap/ftmod_pritap.c
index fbd7979ace688a893b0199ced8761267b031f6da..9719a831db65bd1240e408c21be243d829c9e7ba 100644 (file)
--- a/libs/freetdm/src/ftmod/ftmod_pritap/ftmod_pritap.c
+++ b/libs/freetdm/src/ftmod/ftmod_pritap/ftmod_pritap.c
@@ -749,6 +749,59 @@ static ftdm_status_t ftdm_pritap_stop(ftdm_span_t *span)
        return FTDM_SUCCESS;
 }
 
+static ftdm_status_t ftdm_pritap_sig_read(ftdm_channel_t *ftdmchan, void *data, ftdm_size_t size)
+{
+       ftdm_status_t status;
+       fio_codec_t codec_func;
+       ftdm_channel_t *peerchan = ftdmchan->call_data;
+       int16_t peerbuf[size];
+       int16_t chanbuf[size];
+       int16_t mixedbuf[size];
+       int i = 0;
+       ftdm_size_t sizeread = size;
+
+       if (!FTDM_IS_VOICE_CHANNEL(ftdmchan) || !ftdmchan->call_data) {
+               return FTDM_SUCCESS;
+       }
+
+       if (ftdmchan->native_codec != peerchan->native_codec) {
+               ftdm_log_chan(ftdmchan, FTDM_LOG_CRIT, "Invalid peer channel with format %d, ours = %d\n", 
+                               peerchan->native_codec, ftdmchan->native_codec);
+               return FTDM_FAIL;
+       }
+
+       memcpy(chanbuf, data, size);
+       status = peerchan->fio->read(ftdmchan->call_data, peerbuf, &sizeread);
+       if (status != FTDM_SUCCESS) {
+               ftdm_log_chan_msg(ftdmchan, FTDM_LOG_ERROR, "Failed to read from peer channel!\n");
+               return FTDM_FAIL;
+       }
+       if (sizeread != size) {
+               ftdm_log_chan(ftdmchan, FTDM_LOG_ERROR, "read from peer channel only %d bytes!\n", sizeread);
+               return FTDM_FAIL;
+       }
+
+       codec_func = peerchan->native_codec == FTDM_CODEC_ULAW ? fio_ulaw2slin : peerchan->native_codec == FTDM_CODEC_ALAW ? fio_alaw2slin : NULL;
+       if (codec_func) {
+               codec_func(peerbuf, sizeof(peerbuf), &sizeread);
+               sizeread = size;
+               codec_func(chanbuf, sizeof(chanbuf), &sizeread);
+       }
+
+       for (i = 0; i < size; i++) {
+               mixedbuf[i] = ftdm_saturated_add(chanbuf[i], peerbuf[i]);
+       }
+
+       codec_func = peerchan->native_codec == FTDM_CODEC_ULAW ? fio_slin2ulaw : peerchan->native_codec == FTDM_CODEC_ALAW ? fio_slin2alaw : NULL;
+       if (codec_func) {
+               codec_func(data, size, &size);
+       } else {
+               memcpy(data, mixedbuf, sizeof(mixedbuf));
+       }
+
+       return FTDM_SUCCESS;
+}
+
 static ftdm_status_t ftdm_pritap_start(ftdm_span_t *span)
 {
        ftdm_status_t ret;
@@ -832,6 +885,7 @@ static FIO_CONFIGURE_SPAN_SIGNALING_FUNCTION(ftdm_pritap_configure_span)
 
        span->start = ftdm_pritap_start;
        span->stop = ftdm_pritap_stop;
+       span->sig_read = ftdm_pritap_sig_read;
        span->signal_cb = sig_cb;
        
        span->signal_data = pritap;

diff --git a/libs/freetdm/src/include/private/ftdm_core.h b/libs/freetdm/src/include/private/ftdm_core.h
index eff62bb58e75120f6fced7dea637a65a55c47a21..99aad5ecc2e615704a2e99648ba0823ee5193135 100644 (file)
--- a/libs/freetdm/src/include/private/ftdm_core.h
+++ b/libs/freetdm/src/include/private/ftdm_core.h
@@ -457,6 +457,7 @@ struct ftdm_span {
        fio_channel_request_t channel_request;
        ftdm_span_start_t start;
        ftdm_span_stop_t stop;
+       ftdm_channel_sig_read_t sig_read;
        void *mod_data;
        char *type;
        char *dtmf_hangup;
@@ -669,6 +670,18 @@ static __inline__ void ftdm_clear_flag_all(ftdm_span_t *span, uint32_t flag)
        ftdm_mutex_unlock(span->mutex);
 }
 
+static __inline__ int16_t ftdm_saturated_add(int16_t sample1, int16_t sample2)
+{
+       int addres;
+
+       addres = sample1 + sample2;
+       if (addres > 32767)
+               addres = 32767;
+       else if (addres < -32767)
+               addres = -32767;
+       return addres;
+}
+
 #ifdef __cplusplus
 }
 #endif

diff --git a/libs/freetdm/src/include/private/ftdm_types.h b/libs/freetdm/src/include/private/ftdm_types.h
index ae9f7db24a5bcd76a34b4d2309ce9ed5144d7943..572e63299b6521e050e4960ea559e6291e779206 100644 (file)
--- a/libs/freetdm/src/include/private/ftdm_types.h
+++ b/libs/freetdm/src/include/private/ftdm_types.h
@@ -360,6 +360,7 @@ typedef struct ftdm_bitstream ftdm_bitstream_t;
 typedef struct ftdm_fsk_modulator ftdm_fsk_modulator_t;
 typedef ftdm_status_t (*ftdm_span_start_t)(ftdm_span_t *span);
 typedef ftdm_status_t (*ftdm_span_stop_t)(ftdm_span_t *span);
+typedef ftdm_status_t (*ftdm_channel_sig_read_t)(ftdm_channel_t *ftdmchan, void *data, ftdm_size_t size);
 
 #ifdef __cplusplus
 }