From 6bb8ee321ae03b719df542c3908c645f32588c9e Mon Sep 17 00:00:00 2001
From: Brian <brian@freeswitch.org>
Date: Fri, 5 Jun 2015 18:11:14 -0500
Subject: [PATCH] FS-7601 improve opus packet loss routines #resolve

---
 conf/vanilla/autoload_configs/opus.conf.xml |   2 -
 src/include/switch_rtcp_frame.h             |   1 +
 src/include/switch_stfu.h                   |   2 +-
 src/include/switch_types.h                  |   3 +-
 src/mod/codecs/mod_opus/mod_opus.c          | 100 ++++++++++++--------
 src/mod/endpoints/mod_rtc/mod_rtc.c         |   9 +-
 src/switch_core_media.c                     |   3 +
 src/switch_rtp.c                            |  64 ++++++++-----
 src/switch_stfu.c                           |  37 +++-----
 9 files changed, 128 insertions(+), 93 deletions(-)

diff --git a/conf/vanilla/autoload_configs/opus.conf.xml b/conf/vanilla/autoload_configs/opus.conf.xml
index f6c5294760..2ac3f66f86 100644
--- a/conf/vanilla/autoload_configs/opus.conf.xml
+++ b/conf/vanilla/autoload_configs/opus.conf.xml
@@ -2,8 +2,6 @@
     <settings>
         <param name="use-vbr" value="1"/>
         <param name="complexity" value="10"/>
-	<!-- Packet Loss Percent, Default 0 -->
-        <!--<param name="packet-loss-percent" value="20"/>-->
 
         <!--
            maxaveragebitrate: the maximum average codec bitrate (values: 6000 to 510000 in bps) 0 is not considered
diff --git a/src/include/switch_rtcp_frame.h b/src/include/switch_rtcp_frame.h
index b0b52134e3..919a0210da 100644
--- a/src/include/switch_rtcp_frame.h
+++ b/src/include/switch_rtcp_frame.h
@@ -50,6 +50,7 @@ struct switch_rtcp_report_block_frame {
 	uint32_t jitter; /* An estimate of the statistical variance of the RTP data packet interarrival time, measured in timestamp units and expressed as an unsigned integer. */
 	uint32_t lsr; /* The middle 32 bits out of 64 in the NTP timestamp */
 	uint32_t dlsr; /* The delay, expressed in units of 1/65536 seconds, between receiving the last SR packet from source SSRC_n and sending this reception report block */
+	uint32_t loss_avg;
 };
 
 /*! \brief An abstraction of a rtcp frame */
diff --git a/src/include/switch_stfu.h b/src/include/switch_stfu.h
index 228b78533d..116f139c09 100644
--- a/src/include/switch_stfu.h
+++ b/src/include/switch_stfu.h
@@ -185,7 +185,7 @@ stfu_status_t _stfu_n_resize(stfu_instance_t *i, uint32_t qlen, int line);
 #define stfu_n_resize(_i, _ql) _stfu_n_resize(_i, _ql, __LINE__)
 stfu_status_t stfu_n_add_data(stfu_instance_t *i, uint32_t ts, uint16_t seq, uint32_t pt, void *data, size_t datalen, uint32_t timer_ts, int last);
 stfu_frame_t *stfu_n_read_a_frame(stfu_instance_t *i);
-SWITCH_DECLARE(int32_t) stfu_n_copy_next_frame(stfu_instance_t *jb, uint32_t timestamp, uint16_t seq, uint16_t distance, stfu_frame_t *next_frame);
+SWITCH_DECLARE(int32_t) stfu_n_peek_frame(stfu_instance_t *jb, uint32_t timestamp, uint16_t seq, uint16_t distance, stfu_frame_t **rframe);
 void _stfu_n_reset(stfu_instance_t *i, const char *file, const char *func, int line);
 #define stfu_n_reset(_i) _stfu_n_reset(_i, STFU_PRE)
 stfu_status_t stfu_n_sync(stfu_instance_t *i, uint32_t packets);
diff --git a/src/include/switch_types.h b/src/include/switch_types.h
index d4512d2df5..aeea4f3040 100644
--- a/src/include/switch_types.h
+++ b/src/include/switch_types.h
@@ -2231,7 +2231,8 @@ typedef switch_status_t (*switch_core_codec_video_decode_func_t) (switch_codec_t
 typedef enum {
 	SCC_VIDEO_REFRESH = 0,
 	SCC_VIDEO_BANDWIDTH,
-	SCC_VIDEO_RESET
+	SCC_VIDEO_RESET,
+	SCC_AUDIO_PACKET_LOSS
 } switch_codec_control_command_t;
 
 typedef enum {
diff --git a/src/mod/codecs/mod_opus/mod_opus.c b/src/mod/codecs/mod_opus/mod_opus.c
index f36725e2ed..2d7ee4c77a 100644
--- a/src/mod/codecs/mod_opus/mod_opus.c
+++ b/src/mod/codecs/mod_opus/mod_opus.c
@@ -74,7 +74,8 @@ struct opus_context {
 	OpusDecoder *decoder_object;
 	uint32_t enc_frame_size;
 	uint32_t dec_frame_size;
-	uint32_t counter_plc_fec;
+	uint32_t old_plpct;
+	opus_codec_settings_t codec_settings;
 };
 
 struct {
@@ -246,6 +247,7 @@ static switch_status_t switch_opus_init(switch_codec_t *codec, switch_codec_flag
 	memset(&codec_fmtp, '\0', sizeof(struct switch_codec_fmtp));
 	codec_fmtp.private_info = &opus_codec_settings;
 	switch_opus_fmtp_parse(codec->fmtp_in, &codec_fmtp);
+	context->codec_settings = opus_codec_settings;
 
 	/* Verify if the local or remote configuration are lowering maxaveragebitrate and/or maxplaybackrate */
 	if ( opus_prefs.maxaveragebitrate && (opus_prefs.maxaveragebitrate < opus_codec_settings.maxaveragebitrate || !opus_codec_settings.maxaveragebitrate) ) {
@@ -346,7 +348,6 @@ static switch_status_t switch_opus_init(switch_codec_t *codec, switch_codec_flag
             
 			return SWITCH_STATUS_GENERR;
 		}
-		context->counter_plc_fec = 0;
 	}
     
 	codec->private_info = context;
@@ -360,7 +361,6 @@ static switch_status_t switch_opus_destroy(switch_codec_t *codec)
     
 	if (context) {
 		if (context->decoder_object) {
-			switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_DEBUG, "tried PLC or FEC %d times \n", context->counter_plc_fec);
 			opus_decoder_destroy(context->decoder_object);
 			context->decoder_object = NULL;
 		}
@@ -409,13 +409,10 @@ static switch_status_t switch_opus_decode(switch_codec_t *codec,
 										  unsigned int *flag)
 {
 	struct opus_context *context = codec->private_info;
-	switch_core_session_t *session = codec->session;
-	stfu_instance_t *jb = NULL;
-	stfu_frame_t next_frame;
 	int samples = 0;
-    uint32_t frame_size;
+	int fec = 0, plc = 0;
+	int32_t frame_size;
 	uint32_t frame_samples;
-	uint32_t found_frame;
 
 	if (!context) {
 		return SWITCH_STATUS_FALSE;
@@ -423,41 +420,24 @@ static switch_status_t switch_opus_decode(switch_codec_t *codec,
 
 	frame_samples = *decoded_data_len / 2 / codec->implementation->number_of_channels;
 	frame_size = frame_samples - (frame_samples % (codec->implementation->actual_samples_per_second / 400));
-	
-	if (*flag & SFF_PLC) {
-		if (session) {
-			jb = switch_core_session_get_jb(session, SWITCH_MEDIA_TYPE_AUDIO);
-		}
-
-		if (jb && codec->cur_frame) {
-
-			found_frame = stfu_n_copy_next_frame(jb, (uint32_t)codec->cur_frame->timestamp, codec->cur_frame->seq, 1, &next_frame);
-
-			if (found_frame) {
-				samples = opus_decode(context->decoder_object, next_frame.data, next_frame.dlen, decoded_data, frame_size, 1); 
-
-				if (samples < 0 ) {
-					switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_ERROR, "Decoder Error (FEC): %s!\n", opus_strerror(samples));
-				} else {
-					context->counter_plc_fec++;
-					*flag &= ~SFF_PLC;
-					*decoded_data_len = samples * 2 * codec->implementation->number_of_channels;
-					return SWITCH_STATUS_SUCCESS;
-				}
-			}
-		}
-	}
-
-	/* opus_decode() does PLC if there's no FEC in the packet*/
-	samples = opus_decode(context->decoder_object, (*flag & SFF_PLC) ? NULL : encoded_data, encoded_data_len, decoded_data, frame_size, 0);
 
 	if (*flag & SFF_PLC) {
-		context->counter_plc_fec++;
+		plc = 1;
+		encoded_data = NULL;
+		opus_decoder_ctl(context->decoder_object, OPUS_GET_LAST_PACKET_DURATION(&frame_size));
+
+		if (context->codec_settings.useinbandfec) {
+			fec = 1;
+		}
+
 		*flag &= ~SFF_PLC;
 	}
+
+	samples = opus_decode(context->decoder_object, encoded_data, encoded_data_len, decoded_data, frame_size, fec);
  
 	if (samples < 0) {
-		switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_ERROR, "Decoder Error: %s fs:%u plc:%d!\n", opus_strerror(samples), frame_size, !!(*flag & SFF_PLC));
+		switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_ERROR, "Decoder Error: %s fs:%u plc:%s!\n",
+						  opus_strerror(samples), frame_size, plc ? "true" : "false");
 		return SWITCH_STATUS_GENERR;
 	}
 	
@@ -510,6 +490,46 @@ static switch_status_t opus_load_config(switch_bool_t reload)
     return status;
 }
 
+static switch_status_t switch_opus_control(switch_codec_t *codec,
+										   switch_codec_control_command_t cmd,
+										   switch_codec_control_type_t ctype,
+										   void *cmd_data,
+										   switch_codec_control_type_t *rtype,
+										   void **ret_data)
+{
+	struct opus_context *context = codec->private_info;
+
+	switch(cmd) {
+	case SCC_AUDIO_PACKET_LOSS:
+		{
+			uint32_t plpct = *((uint32_t *) cmd_data);
+			uint32_t calc;
+
+			if (plpct < 0) {
+				plpct = 0;
+			}
+
+			if (plpct > 100) {
+				plpct = 100;
+			}
+
+			calc = plpct % 10;
+			plpct = plpct - calc + ( calc ? 10 : 0);
+
+			if (plpct != context->old_plpct) {
+				opus_encoder_ctl(context->encoder_object, OPUS_SET_PACKET_LOSS_PERC(plpct));
+			}
+			context->old_plpct = plpct;
+		}
+		break;
+	default:
+		break;
+	}
+
+	return SWITCH_STATUS_SUCCESS;
+}
+
+
 SWITCH_MODULE_LOAD_FUNCTION(mod_opus_load)
 {
 	switch_codec_interface_t *codec_interface;
@@ -569,6 +589,8 @@ SWITCH_MODULE_LOAD_FUNCTION(mod_opus_load)
 											 switch_opus_encode,	/* function to encode raw data into encoded data */
 											 switch_opus_decode,	/* function to decode encoded data into raw data */
 											 switch_opus_destroy);	/* deinitalize a codec handle using this implementation */
+
+		codec_interface->implementations->codec_control = switch_opus_control;
 		
 		settings.stereo = 1;
 		if (x < 2) {
@@ -590,6 +612,7 @@ SWITCH_MODULE_LOAD_FUNCTION(mod_opus_load)
 											 switch_opus_encode,	/* function to encode raw data into encoded data */
 											 switch_opus_decode,	/* function to decode encoded data into raw data */
 											 switch_opus_destroy);	/* deinitalize a codec handle using this implementation */
+		codec_interface->implementations->codec_control = switch_opus_control;
 		}
 		bytes *= 2;
 		samples *= 2;
@@ -659,6 +682,9 @@ SWITCH_MODULE_LOAD_FUNCTION(mod_opus_load)
 	return SWITCH_STATUS_SUCCESS;
 }
 
+
+
+
 /* For Emacs:
  * Local Variables:
  * mode:c
diff --git a/src/mod/endpoints/mod_rtc/mod_rtc.c b/src/mod/endpoints/mod_rtc/mod_rtc.c
index 06b2815052..48d12706a5 100644
--- a/src/mod/endpoints/mod_rtc/mod_rtc.c
+++ b/src/mod/endpoints/mod_rtc/mod_rtc.c
@@ -269,6 +269,13 @@ static switch_status_t rtc_receive_event(switch_core_session_t *session, switch_
 	return SWITCH_STATUS_SUCCESS;
 }
 
+static stfu_instance_t *rtc_get_jb(switch_core_session_t *session, switch_media_type_t type)
+{
+	private_object_t *tech_pvt = (private_object_t *) switch_core_session_get_private(session);
+
+	return switch_core_media_get_jb(tech_pvt->session, type);
+}
+
 switch_io_routines_t rtc_io_routines = {
 	/*.outgoing_channel */ rtc_outgoing_channel,
 	/*.read_frame */ rtc_read_frame,
@@ -281,7 +288,7 @@ switch_io_routines_t rtc_io_routines = {
 	/*.read_video_frame */ rtc_read_video_frame,
 	/*.write_video_frame */ rtc_write_video_frame,
 	/*.state_run*/ NULL,
-	/*.get_jb*/ NULL
+	/*.get_jb*/ rtc_get_jb
 };
 
 switch_state_handler_table_t rtc_event_handlers = {
diff --git a/src/switch_core_media.c b/src/switch_core_media.c
index ed095b075c..90e69fe6f7 100644
--- a/src/switch_core_media.c
+++ b/src/switch_core_media.c
@@ -2237,6 +2237,9 @@ SWITCH_DECLARE(switch_status_t) switch_core_media_read_frame(switch_core_session
 					snprintf(header, sizeof(header), "Source%u-Lost", i);
 					snprintf(value, sizeof(value), "%u", rtcp_frame.reports[i].lost);
 					switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, header, value);
+					snprintf(header, sizeof(header), "Source%u-Loss-Avg", i);
+					snprintf(value, sizeof(value), "%u", rtcp_frame.reports[i].loss_avg);
+					switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, header, value);
 					snprintf(header, sizeof(header), "Source%u-Highest-Sequence-Number-Received", i);
 					snprintf(value, sizeof(value), "%u", rtcp_frame.reports[i].highest_sequence_number_received);
 					switch_event_add_header_string(event, SWITCH_STACK_BOTTOM, header, value);
diff --git a/src/switch_rtp.c b/src/switch_rtp.c
index 46f3dca55d..d9fddd9549 100644
--- a/src/switch_rtp.c
+++ b/src/switch_rtp.c
@@ -304,6 +304,7 @@ struct switch_rtp {
 	switch_sockaddr_t *local_addr, *rtcp_local_addr;
 	rtp_msg_t send_msg;
 	rtcp_msg_t rtcp_send_msg;
+	switch_rtcp_frame_t rtcp_frame;
 
 	uint8_t fir_seq;
 	uint16_t fir_count;
@@ -5581,7 +5582,7 @@ static void handle_nack(switch_rtp_t *rtp_session, uint32_t nack)
 static switch_status_t process_rtcp_report(switch_rtp_t *rtp_session, rtcp_msg_t *msg, switch_size_t bytes)
 {
 	switch_status_t status = SWITCH_STATUS_FALSE;
-
+	int i;
 
 	switch_log_printf(SWITCH_CHANNEL_SESSION_LOG(rtp_session->session), SWITCH_LOG_DEBUG3,
 					  "RTCP packet bytes %" SWITCH_SIZE_T_FMT " type %d pad %d\n", 
@@ -5658,10 +5659,46 @@ static switch_status_t process_rtcp_report(switch_rtp_t *rtp_session, rtcp_msg_t
 								  ntohl(sr->sender_info.ts),
 								  ntohl(sr->sender_info.pc),
 								  ntohl(sr->sender_info.oc));
+
+
+				rtp_session->rtcp_frame.ssrc = ntohl(sr->ssrc);
+				rtp_session->rtcp_frame.packet_type = (uint16_t)rtp_session->rtcp_recv_msg_p->header.type;
+				rtp_session->rtcp_frame.ntp_msw = ntohl(sr->sender_info.ntp_msw);
+				rtp_session->rtcp_frame.ntp_lsw = ntohl(sr->sender_info.ntp_lsw);
+				rtp_session->rtcp_frame.timestamp = ntohl(sr->sender_info.ts);
+				rtp_session->rtcp_frame.packet_count =  ntohl(sr->sender_info.pc);
+				rtp_session->rtcp_frame.octect_count = ntohl(sr->sender_info.oc);
+
+				for (i = 0; i < (int)msg->header.count && i < MAX_REPORT_BLOCKS ; i++) {
+					struct switch_rtcp_report_block *report = (struct switch_rtcp_report_block *) (msg->body + (sizeof(struct switch_rtcp_sr_head) + (i * sizeof(struct switch_rtcp_report_block))));
+					uint32_t old_avg = rtp_session->rtcp_frame.reports[i].loss_avg;
+
+					if (!rtp_session->rtcp_frame.reports[i].loss_avg) {
+						rtp_session->rtcp_frame.reports[i].loss_avg = (uint8_t)report->fraction;
+					} else {
+						rtp_session->rtcp_frame.reports[i].loss_avg = (uint32_t)(((float)rtp_session->rtcp_frame.reports[i].loss_avg * .7) +
+																				 ((float)(uint8_t)report->fraction * .3));
+					}
+
+					if (!rtp_session->flags[SWITCH_RTP_FLAG_VIDEO] && rtp_session->rtcp_frame.reports[i].loss_avg != old_avg) {
+						switch_core_media_codec_control(rtp_session->session, SWITCH_MEDIA_TYPE_AUDIO, SWITCH_IO_WRITE, SCC_AUDIO_PACKET_LOSS, SCCT_INT, (void *)&rtp_session->rtcp_frame.reports[i].loss_avg, NULL, NULL);
+					}
+
+					rtp_session->rtcp_frame.reports[i].ssrc = ntohl(report->ssrc);
+					rtp_session->rtcp_frame.reports[i].fraction = (uint8_t)report->fraction;
+					rtp_session->rtcp_frame.reports[i].lost = ntohl(report->lost);
+					rtp_session->rtcp_frame.reports[i].highest_sequence_number_received = ntohl(report->highest_sequence_number_received);
+					rtp_session->rtcp_frame.reports[i].jitter = ntohl(report->jitter);
+					rtp_session->rtcp_frame.reports[i].lsr = ntohl(report->lsr);
+					rtp_session->rtcp_frame.reports[i].dlsr = ntohl(report->dlsr);
+				}
+				rtp_session->rtcp_frame.report_count = (uint16_t)i;
+
 			} else { /* Receiver report */
 				struct switch_rtcp_receiver_report* rr = (struct switch_rtcp_receiver_report*)msg->body;
 				report_block = &rr->report_block;
 				packet_ssrc = rr->ssrc;
+				memset(&rtp_session->rtcp_frame, 0, sizeof(rtp_session->rtcp_frame));
 			}
 
 			/* Currently in passthru mode RTT will not be accurate, some work as to be done (something like mapping the NTP timestamp with a local one) to have RTT from both legs */
@@ -6722,39 +6759,16 @@ SWITCH_DECLARE(switch_status_t) switch_rtp_read(switch_rtp_t *rtp_session, void
 SWITCH_DECLARE(switch_status_t) switch_rtcp_zerocopy_read_frame(switch_rtp_t *rtp_session, switch_rtcp_frame_t *frame)
 {
 
-
-
 	if (!rtp_session->flags[SWITCH_RTP_FLAG_ENABLE_RTCP]) {
 		return SWITCH_STATUS_FALSE;
 	}
 
 	/* A fresh frame has been found! */
 	if (rtp_session->rtcp_fresh_frame) {
-		struct switch_rtcp_sender_report* sr = (struct switch_rtcp_sender_report*)rtp_session->rtcp_recv_msg_p->body;
-		int i = 0;
-
 		/* turn the flag off! */
 		rtp_session->rtcp_fresh_frame = 0;
 
-                frame->ssrc = ntohl(sr->ssrc);
-                frame->packet_type = (uint16_t)rtp_session->rtcp_recv_msg_p->header.type;
-                frame->ntp_msw = ntohl(sr->sender_info.ntp_msw);
-                frame->ntp_lsw = ntohl(sr->sender_info.ntp_lsw);
-                frame->timestamp = ntohl(sr->sender_info.ts);
-                frame->packet_count =  ntohl(sr->sender_info.pc);
-                frame->octect_count = ntohl(sr->sender_info.oc);
-
-		for (i = 0; i < (int)rtp_session->rtcp_recv_msg_p->header.count && i < MAX_REPORT_BLOCKS ; i++) {
-			struct switch_rtcp_report_block* report = (struct switch_rtcp_report_block*) (rtp_session->rtcp_recv_msg_p->body + (sizeof(struct switch_rtcp_sr_head) + (i * sizeof(struct switch_rtcp_report_block))));
-			frame->reports[i].ssrc = ntohl(report->ssrc);
-			frame->reports[i].fraction = (uint8_t)report->fraction;
-			frame->reports[i].lost = ntohl(report->lost);
-			frame->reports[i].highest_sequence_number_received = ntohl(report->highest_sequence_number_received);
-			frame->reports[i].jitter = ntohl(report->jitter);
-			frame->reports[i].lsr = ntohl(report->lsr);
-			frame->reports[i].dlsr = ntohl(report->dlsr);
-		}
-		frame->report_count = (uint16_t)i;
+		*frame = rtp_session->rtcp_frame;
 
 		return SWITCH_STATUS_SUCCESS;
 	}
diff --git a/src/switch_stfu.c b/src/switch_stfu.c
index dcd0a8d0b5..bc9c56abef 100644
--- a/src/switch_stfu.c
+++ b/src/switch_stfu.c
@@ -976,38 +976,23 @@ stfu_frame_t *stfu_n_read_a_frame(stfu_instance_t *i)
     return rframe;
 }
 
-SWITCH_DECLARE(int32_t) stfu_n_copy_next_frame(stfu_instance_t *jb, uint32_t timestamp, uint16_t seq, uint16_t distance, stfu_frame_t *next_frame)
+SWITCH_DECLARE(int32_t) stfu_n_peek_frame(stfu_instance_t *jb, uint32_t timestamp, uint16_t seq, uint16_t distance, stfu_frame_t **rframe)
 {
-	uint32_t i = 0, j = 0;
-#ifdef WIN32
-#pragma warning (disable:4204)
-#endif
-	stfu_queue_t *queues[] = { jb->out_queue, jb->in_queue, jb->old_queue};
-#ifdef WIN32
-#pragma warning (default:4204)
-#endif
-	stfu_queue_t *queue = NULL;
+	uint32_t i = 0, qi = 0;
 	stfu_frame_t *frame = NULL;
+	uint16_t want_seq = seq + distance;
+	stfu_queue_t *queues[2] = {jb->out_queue, jb->in_queue};
 
-	uint32_t target_ts = 0;
+	switch_assert(rframe);
 
-#ifdef WIN32
-	UNREFERENCED_PARAMETER(seq);
-#endif
-	if (!next_frame) return 0;
+	for (qi = 0; qi < 2; qi++) {
+		stfu_queue_t *queue = queues[qi];
 
-	target_ts = timestamp + (distance - 1) * jb->samples_per_packet;
+		for(i = 0; i < queue->array_len; i++) {
+			frame = &queue->array[i];
 
-	for (i = 0; i < sizeof(queues)/sizeof(queues[0]); i++) {
-		queue = queues[i];
-
-		if (!queue) continue;
-
-		for(j = 0; j < queue->array_size; j++) {
-			frame = &queue->array[j];
-			/* FIXME: ts rollover happened? bad luck */
-			if (frame->ts > target_ts) {
-				memcpy(next_frame, frame, sizeof(stfu_frame_t));
+			if (frame->seq == want_seq) {
+				*rframe = frame;
 				return 1;
 			}
 		}