mgcp: Allow switching the audio streams, patch the header

Patch the sequence number, the SSRC and the timestamp to
allow to mix various voice streams, e.g. toggling the loop
during the call.

2 files changed, 30 insertions, 3 deletions

diff --git a/openbsc/include/openbsc/mgcp_internal.h b/openbsc/include/openbsc/mgcp_internal.h
index adb01f467..f6f8d6ebb 100644
--- a/openbsc/include/openbsc/mgcp_internal.h
+++ b/openbsc/include/openbsc/mgcp_internal.h
@@ -39,8 +39,13 @@ enum mgcp_connection_mode {
 struct mgcp_rtp_state {
 	int initialized;
 
+	uint32_t orig_ssrc;
+	uint32_t ssrc;
 	uint16_t seq_no;
 	int lost_no;
+	int seq_offset;
+	uint32_t last_timestamp;
+	int32_t  timestamp_offset;
 };
 
 struct mgcp_endpoint {
diff --git a/openbsc/src/mgcp/mgcp_network.c b/openbsc/src/mgcp/mgcp_network.c
index d2dbeebe4..b5e2cddf8 100644
--- a/openbsc/src/mgcp/mgcp_network.c
+++ b/openbsc/src/mgcp/mgcp_network.c
@@ -97,6 +97,7 @@ int mgcp_send_dummy(struct mgcp_endpoint *endp)
 static void patch_and_count(struct mgcp_rtp_state *state, int payload, char *data, int len)
 {
 	uint16_t seq;
+	uint32_t timestamp;
 	struct rtp_hdr *rtp_hdr;
 
 	if (len < sizeof(*rtp_hdr))
@@ -104,15 +105,36 @@ static void patch_and_count(struct mgcp_rtp_state *state, int payload, char *dat
 
 	rtp_hdr = (struct rtp_hdr *) data;
 	seq = ntohs(rtp_hdr->sequence);
+	timestamp = ntohl(rtp_hdr->timestamp);
 
 	if (!state->initialized) {
-		state->seq_no = seq;
+		state->seq_no = seq - 1;
+		state->ssrc = state->orig_ssrc = rtp_hdr->ssrc;
 		state->initialized = 1;
-	} else if (state->seq_no + 1u != seq)
-		state->lost_no = abs(seq - (state->seq_no + 1));
+		state->last_timestamp = timestamp;
+	} else if (state->ssrc != rtp_hdr->ssrc) {
+		state->ssrc = rtp_hdr->ssrc;
+		state->seq_offset = (state->seq_no + 1) - seq;
+		state->timestamp_offset = state->last_timestamp - timestamp;
+		LOGP(DMGCP, LOGL_NOTICE, "The SSRC changed... SSRC: %u offset: %d\n",
+			state->ssrc, state->seq_offset);
+	}
+
+	/* apply the offset and store it back to the packet */
+	seq += state->seq_offset;
+	rtp_hdr->sequence = htons(seq);
+	rtp_hdr->ssrc = state->orig_ssrc;
 
+	timestamp += state->timestamp_offset;
+	rtp_hdr->timestamp = htonl(timestamp);
+
+	/* seq changed, now compare if we have lost something */
+	if (state->seq_no + 1u != seq)
+		state->lost_no = abs(seq - (state->seq_no + 1));
 	state->seq_no = seq;
 
+	state->last_timestamp = timestamp;
+
 	if (payload < 0)
 		return;