From: baohongde Date: Mon, 15 Apr 2019 07:32:19 +0000 (+0800) Subject: components/bt: Add packet loss concealment (PLC) for HFP X-Git-Tag: v4.0-beta1~299^2~2 X-Git-Url: https://granicus.if.org/sourcecode?a=commitdiff_plain;h=20758443df1367164d51e0da750d777da9289868;p=esp-idf components/bt: Add packet loss concealment (PLC) for HFP --- diff --git a/components/bt/CMakeLists.txt b/components/bt/CMakeLists.txt index 50ff9abf60..eb4aed7972 100644 --- a/components/bt/CMakeLists.txt +++ b/components/bt/CMakeLists.txt @@ -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" diff --git a/components/bt/bluedroid/btc/profile/std/hf_client/bta_hf_client_co.c b/components/bt/bluedroid/btc/profile/std/hf_client/bta_hf_client_co.c index 27eda746d3..a0e003872b 100644 --- a/components/bt/bluedroid/btc/profile/std/hf_client/bta_hf_client_co.c +++ b/components/bt/bluedroid/btc/profile/std/hf_client/bta_hf_client_co.c @@ -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 + #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); } diff --git a/components/bt/bluedroid/common/include/common/bt_target.h b/components/bt/bluedroid/common/include/common/bt_target.h index 98dbe7a16c..258c13d2d1 100644 --- a/components/bt/bluedroid/common/include/common/bt_target.h +++ b/components/bt/bluedroid/common/include/common/bt_target.h @@ -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 diff --git a/components/bt/bluedroid/external/sbc/plc/include/sbc_plc.h b/components/bt/bluedroid/external/sbc/plc/include/sbc_plc.h new file mode 100644 index 0000000000..6d7f4d2ea1 --- /dev/null +++ b/components/bt/bluedroid/external/sbc/plc/include/sbc_plc.h @@ -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 + +#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 diff --git a/components/bt/bluedroid/external/sbc/plc/sbc_plc.c b/components/bt/bluedroid/external/sbc/plc/sbc_plc.c new file mode 100644 index 0000000000..b44593082e --- /dev/null +++ b/components/bt/bluedroid/external/sbc/plc/sbc_plc.c @@ -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 +#include +#include +#include +#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) \ No newline at end of file diff --git a/components/bt/component.mk b/components/bt/component.mk index d04846919c..b77e8f6640 100644 --- a/components/bt/component.mk +++ b/components/bt/component.mk @@ -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 \