freetdm: saturated add and pri tapping mixing

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;
 }

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;

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

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
 }