Crash_Override/OVMS3-idf
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

components/bt: Add packet loss concealment (PLC) for HFP

Browse source
This commit is contained in:
baohongde 2019-04-15 15:32:19 +08:00
parent e10d5aaddc
commit 20758443df
6 changed files with 446 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
components/bt/CMakeLists.txt
View file

@ -21,6 +21,7 @@ if(CONFIG_BT_ENABLED)
bluedroid/osi/include
bluedroid/external/sbc/decoder/include
bluedroid/external/sbc/encoder/include
bluedroid/external/sbc/plc/include
bluedroid/btc/profile/esp/blufi/include
bluedroid/btc/profile/esp/include
bluedroid/btc/profile/std/a2dp/include
@ -156,6 +157,7 @@ if(CONFIG_BT_ENABLED)
"bluedroid/external/sbc/encoder/srce/sbc_enc_coeffs.c"
"bluedroid/external/sbc/encoder/srce/sbc_encoder.c"
"bluedroid/external/sbc/encoder/srce/sbc_packing.c"
"bluedroid/external/sbc/plc/sbc_plc.c"
"bluedroid/hci/hci_audio.c"
"bluedroid/hci/hci_hal_h4.c"
"bluedroid/hci/hci_layer.c"

48
components/bt/bluedroid/btc/profile/std/hf_client/bta_hf_client_co.c
View file

@ -15,9 +15,30 @@
#include "bta/bta_hf_client_co.h"
#include "hci/hci_audio.h"
#include "btc_hf_client.h"
#include "osi/allocator.h"
#include <string.h>
#if (BTA_HF_INCLUDED == TRUE)
#if (BTM_SCO_HCI_INCLUDED == TRUE)
#if (PLC_INCLUDED == TRUE)
#include "sbc_plc.h"
typedef struct {
sbc_plc_state_t plc_state;
int16_t sbc_plc_out[SBC_FS];
} bta_hf_ct_plc_t;
#if HFP_DYNAMIC_MEMORY == FALSE
static bta_hf_ct_plc_t bta_hf_ct_plc;
#else
static bta_hf_ct_plc_t *bta_hf_ct_plc_ptr;
#define bta_hf_ct_plc (*bta_hf_ct_plc_ptr)
#endif
#endif ///(PLC_INCLUDED == TRUE)
/*******************************************************************************
**
** Function bta_hf_client_co_audio_state
@ -65,6 +86,7 @@ tBTA_HFP_SCO_ROUTE_TYPE bta_hf_client_sco_co_init(UINT32 rx_bw, UINT32 tx_bw,
{
APPL_TRACE_EVENT("%s rx_bw %d, tx_bw %d, codec %d", __FUNCTION__, rx_bw, tx_bw,
p_codec_info->codec_type);
return BTA_HFP_SCO_ROUTE_HCI;
}
@ -82,6 +104,19 @@ void bta_hf_client_sco_co_open(UINT16 handle, UINT8 pkt_size, UINT16 event)
{
APPL_TRACE_EVENT("%s hdl %x, pkt_sz %u, event %u", __FUNCTION__, handle,
pkt_size, event);
#if (PLC_INCLUDED == TRUE)
#if (HFP_DYNAMIC_MEMORY == TRUE)
if ((bta_hf_ct_plc_ptr = (bta_hf_ct_plc_t *)osi_malloc(sizeof(bta_hf_ct_plc_t))) == NULL) {
APPL_TRACE_ERROR("%s malloc fail.", __FUNCTION__);
return;
}
memset((void *)bta_hf_ct_plc_ptr, 0, sizeof(bta_hf_ct_plc_t));
#endif /// (HFP_DYNAMIC_MEMORY == TRUE)
sbc_plc_init(&(bta_hf_ct_plc.plc_state));
#endif ///(PLC_INCLUDED == TRUE)
}
/*******************************************************************************
@ -97,6 +132,15 @@ void bta_hf_client_sco_co_open(UINT16 handle, UINT8 pkt_size, UINT16 event)
void bta_hf_client_sco_co_close(void)
{
APPL_TRACE_EVENT("%s", __FUNCTION__);
#if (PLC_INCLUDED == TRUE)
sbc_plc_deinit(&(bta_hf_ct_plc.plc_state));
#if (HFP_DYNAMIC_MEMORY == TRUE)
osi_free(bta_hf_ct_plc_ptr);
#endif /// (HFP_DYNAMIC_MEMORY == TRUE)
#endif ///(PLC_INCLUDED == TRUE)
}
/*******************************************************************************
@ -129,6 +173,10 @@ void bta_hf_client_sco_co_in_data(BT_HDR *p_buf, tBTM_SCO_DATA_FLAG status)
STREAM_SKIP_UINT16(p);
STREAM_TO_UINT8 (pkt_size, p);
if(status != BTM_SCO_DATA_CORRECT){
APPL_TRACE_DEBUG("%s: not a correct frame(%d).", __func__, status);
}
btc_hf_client_incoming_data_cb_to_app(p, pkt_size);
}

1
components/bt/bluedroid/common/include/common/bt_target.h
View file

@ -77,6 +77,7 @@
#if CONFIG_BT_HFP_CLIENT_ENABLE
#define BTC_HF_CLIENT_INCLUDED TRUE
#define BTA_HF_INCLUDED TRUE
#define PLC_INCLUDED TRUE
#ifndef RFCOMM_INCLUDED
#define RFCOMM_INCLUDED TRUE
#endif

91
components/bt/bluedroid/external/sbc/plc/include/sbc_plc.h vendored Normal file
View file

@ -0,0 +1,91 @@
// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef _SBC_PLC_H
#define _SBC_PLC_H
#include <stdint.h>
#if defined __cplusplus
extern "C" {
#endif
/* Paramter for PLC (16 kHZ)*/
#define SBC_FS 120 /* SBC Frame Size */
#define SBC_N 256 /* 16ms - Window Length for pattern matching */
#define SBC_M 64 /* 4ms - Template for matching */
#define SBC_LHIST (SBC_N + SBC_FS - 1) /* Length of history buffer required */
#define SBC_RT 36 /* SBC Reconvergence Time (samples) */
#define SBC_OLAL 16 /* OverLap-Add Length (samples) */
/* PLC State Information */
typedef struct sbc_plc_state {
int16_t hist[SBC_LHIST + SBC_FS + SBC_RT + SBC_OLAL];
int16_t bestlag;
int nbf;
} sbc_plc_state_t;
/* Prototypes */
/**
* Perform PLC initialization of memory vectors.
*
* @param plc_state pointer to PLC state memory
*/
void sbc_plc_init(sbc_plc_state_t *plc_state);
/**
* Perform PLC deinitialization of memory vectors.
*
* @param plc_state pointer to PLC state memory
*/
void sbc_plc_deinit(sbc_plc_state_t *plc_state);
/**
* Perform bad frame processing.
*
* @param plc_state pointer to PLC state memory
* @param ZIRbuf pointer to the ZIR response of the SBC decoder
* @param out pointer to the output samples
*/
void sbc_plc_bad_frame(sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out);
/**
* Perform good frame processing. Most of the time, this function
* just updates history buffers and passes the input to the output,
* but in the first good frame after frame loss, it must conceal the
* received signal as it reconverges with the true output.
*
* @param plc_state pointer to PLC state memory
* @param in pointer to the input vector
* @param out pointer to the output samples
*/
void sbc_plc_good_frame(sbc_plc_state_t *plc_state, int16_t *in, int16_t *out);
/**
* Get a zero signal eSCO frame
* @return pointer to data buffer
*/
uint8_t * sbc_plc_zero_signal_frame(void);
/**
* Get a zero signal eSCO pcm frame
* @return pointer to data buffer
*/
int16_t * sbc_plc_zero_signal_frame_pcm(void);
#if defined __cplusplus
}
#endif
#endif /// _SBC_PLC_H

302
components/bt/bluedroid/external/sbc/plc/sbc_plc.c vendored Normal file
View file

@ -0,0 +1,302 @@
// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "common/bt_target.h"
#include "sbc_plc.h"
#if (PLC_INCLUDED == TRUE)
/* msbc */
static const uint8_t indices0[] = { 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};
/* 8 kHZ */
static const int16_t indices0_pcm[] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
/* Raised COSine table for OLA */
/* 16 kHZ */
static float rcos[SBC_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};
// /* 8 kHZ */
// static float rcos[SBC_OLAL] = {
// 0.96984631f,0.88302222f, 0.75f,0.58682409f,
// 0.41317591f, 0.25f,0.11697778f,0.09015369f};
static float SqrtByCarmack(const float x){
int i;
float x2, y;
const float threehalfs = 1.5f;
x2 = x * 0.5f;
y = x;
i = *(int *)&y;
i = 0x5f375a86 - (i >> 1);
y = *(float *)&i;
y = y * (threehalfs - (x2 * y *y));
// y = y * (threehalfs - (x2 * y *y));
// y = y * (threehalfs - (x2 * y *y));
return (x * y);
}
static float absolute(float x){
if (x < 0) {
x = -x;
}
return x;
}
/**
* Compute the cross correlation according to Eq. (4) of Goodman
* paper, except that the true correlation is used. His formula
* seems to be incorrect.
*
* @param x pointer to x input vector
* @param y pointer to y input vector
*
* @return value containing the cross-correlation of x and y
*/
static float CrossCorrelation(int16_t *x, int16_t *y){
int m;
float num = 0;
float den = 0;
float x2 = 0;
float y2 = 0;
for (m = 0; m < SBC_M; m++) {
num += ((float)x[m]) * y[m];
x2 += ((float)x[m]) * x[m];
y2 += ((float)y[m]) * y[m];
}
den = (float)SqrtByCarmack(x2 * y2);
return num / den;
}
/**
* Perform pattern matching to find the match of template with the
* history buffer according to Section B of Goodman paper.
*
* @param y pointer to history buffer
*
* @return the lag corresponding to the best match. The lag is
* with respect to the beginning of the history buffer.
*
*/
static int PatternMatch(int16_t *y){
int n;
float maxCn = -999999.0; // large negative number
float Cn;
int bestmatch = 0;
for (n = 0; n < SBC_N; n++){
Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M], &y[n]);
if (Cn > maxCn){
bestmatch = n;
maxCn = Cn;
}
}
return bestmatch;
}
/**
* Perform amplitude matching using mean-absolute-value according
* to Goodman paper.
*
* @param y pointer to history buffer
* @param bestmatch value of the lag to the best match
*
* @return scale factor
*/
static float AmplitudeMatch(int16_t *y, int16_t bestmatch) {
int i;
float sumx = 0;
float sumy = 0.000001f;
float sf;
for (i = 0; i < SBC_FS; i++){
sumx += absolute(y[SBC_LHIST - SBC_FS + i]);
sumy += absolute(y[bestmatch + i]);
}
sf = sumx / sumy;
// This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts
if (sf < 0.75f) {
sf = 0.75f;
}
if (sf > 1.2f) {
sf = 1.2f;
}
return sf;
}
static int16_t crop_sample(float val){
float croped_val = val;
if (croped_val > 32767.0) croped_val= 32767.0;
if (croped_val < -32768.0) croped_val=-32768.0;
return (int16_t) croped_val;
}
/**
* Get a zero signal eSCO frame
* @return pointer to data buffer
*/
uint8_t * sbc_plc_zero_signal_frame(void){
return (uint8_t *)&indices0;
}
/**
* Get a zero signal eSCO pcm frame
* @return pointer to data buffer
*/
int16_t * sbc_plc_zero_signal_frame_pcm(void){
return (int16_t *)&indices0_pcm;
}
/**
* Perform PLC initialization of memory vectors.
*
* @param plc_state pointer to PLC state memory
*/
void sbc_plc_init(sbc_plc_state_t *plc_state){
plc_state->nbf=0;
plc_state->bestlag=0;
memset(plc_state->hist, 0, sizeof(plc_state->hist));
}
/**
* Perform PLC deinitialization of memory vectors.
*
* @param plc_state pointer to PLC state memory
*/
void sbc_plc_deinit(sbc_plc_state_t *plc_state){
plc_state->nbf=0;
plc_state->bestlag=0;
memset(plc_state->hist, 0, sizeof(plc_state->hist));
}
/**
* Perform bad frame processing.
*
* @param plc_state pointer to PLC state memory
* @param ZIRbuf pointer to the ZIR response of the SBC decoder
* @param out pointer to the output samples
*/
void sbc_plc_bad_frame(sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out){
int i = 0;
float val;
float sf = 1;
plc_state->nbf++;
if (plc_state->nbf == 1){
// Perform pattern matching to find where to replicate
plc_state->bestlag = PatternMatch(plc_state->hist);
// the replication begins after the template match
plc_state->bestlag += SBC_M;
// Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet
sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);
for (i = 0; i < SBC_OLAL; i++){
val = ZIRbuf[i] * rcos[i]
+ sf * plc_state->hist[plc_state->bestlag + i] * rcos[SBC_OLAL - i - 1];
plc_state->hist[SBC_LHIST + i] = crop_sample(val);
}
for (; i < SBC_FS; i++){
val = sf*plc_state->hist[plc_state->bestlag + i];
plc_state->hist[SBC_LHIST + i] = crop_sample(val);
}
for (; i < SBC_FS + SBC_OLAL; i++){
val = sf * plc_state->hist[plc_state->bestlag + i] * rcos[i-SBC_FS]
+ plc_state->hist[plc_state->bestlag + i] * rcos[SBC_OLAL - 1 - i + SBC_FS];
plc_state->hist[SBC_LHIST + i] = crop_sample(val);
}
for (; i < SBC_FS + SBC_RT + SBC_OLAL; i++){
plc_state->hist[SBC_LHIST + i] = plc_state->hist[plc_state->bestlag + i];
}
} else {
for ( ;i < SBC_FS + SBC_RT + SBC_OLAL; i++){
plc_state->hist[SBC_LHIST + i] = plc_state->hist[plc_state->bestlag + i];
}
}
for (i = 0; i < SBC_FS; i++){
out[i] = plc_state->hist[SBC_LHIST + i];
}
// shift the history buffer
for (i = 0; i < SBC_LHIST + SBC_RT + SBC_OLAL; i++){
plc_state->hist[i] = plc_state->hist[i + SBC_FS];
}
}
/**
* Perform good frame processing. Most of the time, this function
* just updates history buffers and passes the input to the output,
* but in the first good frame after frame loss, it must conceal the
* received signal as it reconverges with the true output.
*
* @param plc_state pointer to PLC state memory
* @param in pointer to the input vector
* @param out pointer to the output samples
*/
void sbc_plc_good_frame(sbc_plc_state_t *plc_state, int16_t *in, int16_t *out){
int i = 0;
if (plc_state->nbf > 0){
for (i = 0; i < SBC_RT; i++){
out[i] = plc_state->hist[SBC_LHIST + i];
}
for (i = SBC_RT; i < SBC_RT + SBC_OLAL; i++){
out[i] = (int16_t)(plc_state->hist[SBC_LHIST + i] * rcos[i - SBC_RT] + in[i] * rcos[SBC_OLAL - 1 - i + SBC_RT]);
}
}
for (; i < SBC_FS; i++){
out[i] = in[i];
}
// Copy the output to the history buffer
for (i = 0; i < SBC_FS; i++){
plc_state->hist[SBC_LHIST + i] = out[i];
}
// shift the history buffer
for (i = 0; i < SBC_LHIST; i++){
plc_state->hist[i] = plc_state->hist[i + SBC_FS];
}
plc_state->nbf = 0;
}
#endif ///(PLC_INCLUDED == TRUE)

2
components/bt/component.mk
View file

@ -45,6 +45,7 @@ COMPONENT_PRIV_INCLUDEDIRS += bluedroid/bta/include \
bluedroid/utils/include \
bluedroid/external/sbc/decoder/include \
bluedroid/external/sbc/encoder/include \
bluedroid/external/sbc/plc/include \
bluedroid/btc/core/include \
bluedroid/btc/profile/esp/blufi/include \
bluedroid/btc/profile/esp/include \
@ -92,6 +93,7 @@ COMPONENT_SRCDIRS += bluedroid/bta/dm \
bluedroid/osi \
bluedroid/external/sbc/decoder/srce \
bluedroid/external/sbc/encoder/srce \
bluedroid/external/sbc/plc \
bluedroid/btc/core \
bluedroid/btc/profile/esp/blufi \
bluedroid/btc/profile/std/gap \