path: root/cras/src/plc/cras_plc.c

/* Copyright 2019 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include <float.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#include "cras_plc.h"

#define MSBC_SAMPLE_SIZE 2 /* 2 bytes*/
#define MSBC_PKT_LEN 57 /* Packet length without the header */
#define MSBC_FS 120 /* Frame Size */
#define MSBC_CODE_SIZE 240 /* MSBC_SAMPLE_SIZE * MSBC_FS */

#define PLC_WL 256 /* 16ms - Window Length for pattern matching */
#define PLC_TL 64 /* 4ms - Template Length for matching */
#define PLC_HL (PLC_WL + MSBC_FS - 1) /* Length of History buffer required */
#define PLC_SBCRL 36 /* SBC Reconvergence sample Length */
#define PLC_OLAL 16 /* OverLap-Add Length */

#define PLC_WINDOW_SIZE 5
#define PLC_PL_THRESHOLD 2

/* The pre-computed zero input bit stream of mSBC codec, per HFP 1.7 spec.
 * This mSBC frame will be decoded into all-zero input PCM. */
static const uint8_t msbc_zero_frame[] = {
	0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d, 0xb6, 0xdd,
	0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 0xb6,
	0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
	0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77,
	0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c
};

/* Raised Cosine table for OLA */
static const float rcos[PLC_OLAL] = { 0.99148655f, 0.96623611f, 0.92510857f,
				      0.86950446f, 0.80131732f, 0.72286918f,
				      0.63683150f, 0.54613418f, 0.45386582f,
				      0.36316850f, 0.27713082f, 0.19868268f,
				      0.13049554f, 0.07489143f, 0.03376389f,
				      0.00851345f };

/* This structure tracks the packet loss information for last PLC_WINDOW_SIZE
 * of packets:
 *    loss_hist - The packet loss history of receiving packets. 1 means lost.
 *    ptr - The index of the to be updated packet loss status.
 *    count - The count of lost packets in the window.
 */
struct packet_window {
	uint8_t loss_hist[PLC_WINDOW_SIZE];
	unsigned int ptr;
	unsigned int count;
};

/* The PLC is specifically designed for mSBC. The algorithm searches the
 * history of receiving samples to find the best match samples and constructs
 * substitutions for the lost samples. The selection is based on pattern
 * matching a template, composed of a length of samples preceding to the lost
 * samples. It then uses the following samples after the best match as the
 * replacement samples and applies Overlap-Add to reduce the audible
 * distortion.
 *
 * This structure holds related info needed to conduct the PLC algorithm.
 * Members:
 *    hist - The history buffer for receiving samples, we also use it to
 *           buffer the processed replacement samples.
 *    best_lag - The index of the best substitution samples in sample history.
 *    handled_bad_frames - Number of bad frames handled since the last good
 *                         frame.
 *    zero_frame - A buffer used for storing the samples from decoding the
 *                 mSBC zero frame packet.
 *    pl_window - A window monitoring how many packets are bad within the recent
 *                PLC_WINDOW_SIZE of packets. This is used to determine if we
 *                want to disable the PLC temporarily.
 */
struct cras_msbc_plc {
	int16_t hist[PLC_HL + MSBC_FS + PLC_SBCRL + PLC_OLAL];
	unsigned int best_lag;
	int handled_bad_frames;
	int16_t zero_frame[MSBC_FS];
	struct packet_window *pl_window;
};

struct cras_msbc_plc *cras_msbc_plc_create()
{
	struct cras_msbc_plc *plc =
		(struct cras_msbc_plc *)calloc(1, sizeof(*plc));
	plc->pl_window =
		(struct packet_window *)calloc(1, sizeof(*plc->pl_window));
	return plc;
}

void cras_msbc_plc_destroy(struct cras_msbc_plc *plc)
{
	free(plc->pl_window);
	free(plc);
}

static int16_t f_to_s16(float input)
{
	return input > INT16_MAX ?
		       INT16_MAX :
		       input < INT16_MIN ? INT16_MIN : (int16_t)input;
}

void overlap_add(int16_t *output, float scaler_d, const int16_t *desc,
		 float scaler_a, const int16_t *asc)
{
	for (int i = 0; i < PLC_OLAL; i++) {
		output[i] =
			f_to_s16(scaler_d * desc[i] * rcos[i] +
				 scaler_a * asc[i] * rcos[PLC_OLAL - 1 - i]);
	}
}

void update_plc_state(struct packet_window *w, uint8_t is_packet_loss)
{
	uint8_t *curr = &w->loss_hist[w->ptr];
	if (is_packet_loss != *curr) {
		w->count += (is_packet_loss - *curr);
		*curr = is_packet_loss;
	}
	w->ptr = (w->ptr + 1) % PLC_WINDOW_SIZE;
}

int possibly_pause_plc(struct packet_window *w)
{
	/* The packet loss count comes from a time window and we use it as an
	 * indicator of our confidence of the PLC algorithm. It is known to
	 * generate poorer and robotic feeling sounds, when the majority of
	 * samples in the PLC history buffer are from the concealment results.
	 */
	return w->count >= PLC_PL_THRESHOLD;
}

int cras_msbc_plc_handle_good_frames(struct cras_msbc_plc *state,
				     const uint8_t *input, uint8_t *output)
{
	int16_t *frame_head, *input_samples, *output_samples;
	if (state->handled_bad_frames == 0) {
		/* If there was no packet concealment before this good frame,
		 * we just simply copy the input to output without reconverge.
		 */
		memmove(output, input, MSBC_FS * MSBC_SAMPLE_SIZE);
	} else {
		frame_head = &state->hist[PLC_HL];
		input_samples = (int16_t *)input;
		output_samples = (int16_t *)output;

		/* For the first good frame after packet loss, we need to
		 * conceal the received samples to have it reconverge with the
		 * true output.
		 */
		memcpy(output_samples, frame_head,
		       PLC_SBCRL * MSBC_SAMPLE_SIZE);
		overlap_add(&output_samples[PLC_SBCRL], 1.0,
			    &frame_head[PLC_SBCRL], 1.0,
			    &input_samples[PLC_SBCRL]);
		memmove(&output_samples[PLC_SBCRL + PLC_OLAL],
			&input_samples[PLC_SBCRL + PLC_OLAL],
			(MSBC_FS - PLC_SBCRL - PLC_OLAL) * MSBC_SAMPLE_SIZE);
		state->handled_bad_frames = 0;
	}

	/* Shift the history and update the good frame to the end of it. */
	memmove(state->hist, &state->hist[MSBC_FS],
		(PLC_HL - MSBC_FS) * MSBC_SAMPLE_SIZE);
	memcpy(&state->hist[PLC_HL - MSBC_FS], output,
	       MSBC_FS * MSBC_SAMPLE_SIZE);
	update_plc_state(state->pl_window, 0);
	return MSBC_CODE_SIZE;
}

float cross_correlation(int16_t *x, int16_t *y)
{
	float sum = 0, x2 = 0, y2 = 0;

	for (int i = 0; i < PLC_TL; i++) {
		sum += ((float)x[i]) * y[i];
		x2 += ((float)x[i]) * x[i];
		y2 += ((float)y[i]) * y[i];
	}
	return sum / sqrtf(x2 * y2);
}

int pattern_match(int16_t *hist)
{
	int best = 0;
	float cn, max_cn = FLT_MIN;

	for (int i = 0; i < PLC_WL; i++) {
		cn = cross_correlation(&hist[PLC_HL - PLC_TL], &hist[i]);
		if (cn > max_cn) {
			best = i;
			max_cn = cn;
		}
	}
	return best;
}

float amplitude_match(int16_t *x, int16_t *y)
{
	uint32_t sum_x = 0, sum_y = 0;
	float scaler;
	for (int i = 0; i < MSBC_FS; i++) {
		sum_x += abs(x[i]);
		sum_y += abs(y[i]);
	}

	if (sum_y == 0)
		return 1.2f;

	scaler = (float)sum_x / sum_y;
	return scaler > 1.2f ? 1.2f : scaler < 0.75f ? 0.75f : scaler;
}

int cras_msbc_plc_handle_bad_frames(struct cras_msbc_plc *state,
				    struct cras_audio_codec *codec,
				    uint8_t *output)
{
	float scaler;
	int16_t *best_match_hist;
	int16_t *frame_head = &state->hist[PLC_HL];
	size_t pcm_decoded = 0;

	/* mSBC codec is stateful, the history of signal would contribute to the
	 * decode result state->zero_frame.
	 */
	codec->decode(codec, msbc_zero_frame, MSBC_PKT_LEN, state->zero_frame,
		      MSBC_FS, &pcm_decoded);

	/* The PLC algorithm is more likely to generate bad results that sound
	 * robotic after severe packet losses happened. Only applying it when
	 * we are confident.
	 */
	if (!possibly_pause_plc(state->pl_window)) {
		if (state->handled_bad_frames == 0) {
			/* Finds the best matching samples and amplitude */
			state->best_lag = pattern_match(state->hist) + PLC_TL;
			best_match_hist = &state->hist[state->best_lag];
			scaler = amplitude_match(&state->hist[PLC_HL - MSBC_FS],
						 best_match_hist);

			/* Constructs the substitution samples */
			overlap_add(frame_head, 1.0, state->zero_frame, scaler,
				    best_match_hist);
			for (int i = PLC_OLAL; i < MSBC_FS; i++)
				state->hist[PLC_HL + i] =
					f_to_s16(scaler * best_match_hist[i]);
			overlap_add(&frame_head[MSBC_FS], scaler,
				    &best_match_hist[MSBC_FS], 1.0,
				    &best_match_hist[MSBC_FS]);

			memmove(&frame_head[MSBC_FS + PLC_OLAL],
				&best_match_hist[MSBC_FS + PLC_OLAL],
				PLC_SBCRL * MSBC_SAMPLE_SIZE);
		} else {
			memmove(frame_head, &state->hist[state->best_lag],
				(MSBC_FS + PLC_SBCRL + PLC_OLAL) *
					MSBC_SAMPLE_SIZE);
		}
		state->handled_bad_frames++;
	} else {
		/* This is a case similar to receiving a good frame with all
		 * zeros, we set handled_bad_frames to zero to prevent the
		 * following good frame from being concealed to reconverge with
		 * the zero frames we fill in. The concealment result sounds
		 * more artificial and weird than simply writing zeros and
		 * following samples.
		 */
		memmove(frame_head, state->zero_frame, MSBC_CODE_SIZE);
		memset(frame_head + MSBC_CODE_SIZE, 0,
		       (PLC_SBCRL + PLC_OLAL) * MSBC_SAMPLE_SIZE);
		state->handled_bad_frames = 0;
	}

	memcpy(output, frame_head, MSBC_CODE_SIZE);
	memmove(state->hist, &state->hist[MSBC_FS],
		(PLC_HL + PLC_SBCRL + PLC_OLAL) * MSBC_SAMPLE_SIZE);
	update_plc_state(state->pl_window, 1);
	return MSBC_CODE_SIZE;
}