From 76e01b25f55fb0b82900681ba0616a34d1e4771f Mon Sep 17 00:00:00 2001 From: "Igor V. Kovalenko" Date: Wed, 8 Sep 2021 21:03:54 +0300 Subject: bluetooth: Handle fragmented faststream frames Incoming frames can span multiple packets, add support for this. Part-of: --- src/modules/bluetooth/a2dp-codec-sbc.c | 74 ++++++++++++++++++++++++++++------ 1 file changed, 61 insertions(+), 13 deletions(-) diff --git a/src/modules/bluetooth/a2dp-codec-sbc.c b/src/modules/bluetooth/a2dp-codec-sbc.c index e912ffb89..2c069cb6f 100644 --- a/src/modules/bluetooth/a2dp-codec-sbc.c +++ b/src/modules/bluetooth/a2dp-codec-sbc.c @@ -39,6 +39,8 @@ #define SBC_BITPOOL_DEC_STEP 5 #define SBC_BITPOOL_INC_STEP 1 +#define SBC_SYNCWORD 0x9C + struct sbc_info { sbc_t sbc; /* Codec data */ size_t codesize, frame_length; /* SBC Codesize, frame_length. We simply cache those values here */ @@ -54,6 +56,11 @@ struct sbc_info { uint8_t nr_blocks; uint8_t nr_subbands; + + /* Size of SBC frame fragment left over from previous decoding iteration */ + size_t frame_fragment_size; + /* Maximum SBC frame size is 512 bytes when SBC compression ratio > 1 */ + uint8_t frame_fragment[512]; }; static bool can_be_supported(bool for_encoding) { @@ -936,6 +943,9 @@ static int reset(void *codec_info) { struct sbc_info *sbc_info = (struct sbc_info *) codec_info; int ret; + /* forget last saved frame fragment */ + sbc_info->frame_fragment_size = 0; + ret = sbc_reinit(&sbc_info->sbc, 0); if (ret != 0) { pa_log_error("SBC reinitialization failed: %d", ret); @@ -1279,27 +1289,60 @@ static size_t decode_buffer_faststream(void *codec_info, const uint8_t *input_bu .rate = 16000U }; uint8_t decode_buffer[4096]; + uint8_t frame_buffer[4096]; - p = input_buffer; to_decode = input_size; + /* append input buffer to fragment left from previous decode call */ + if (sbc_info->frame_fragment_size) { + + if (sbc_info->frame_fragment_size + to_decode > sizeof(frame_buffer)) { + pa_log_debug("FastStream SBC input (saved + incoming) size %lu larger than buffer size %lu, input truncated to fit", + sbc_info->frame_fragment_size + to_decode, sizeof(frame_buffer)); + to_decode = sizeof(frame_buffer) - sbc_info->frame_fragment_size; + } + + memcpy(frame_buffer, sbc_info->frame_fragment, sbc_info->frame_fragment_size); + memcpy(frame_buffer + sbc_info->frame_fragment_size, input_buffer, to_decode); + + to_decode += sbc_info->frame_fragment_size; + p = frame_buffer; + + /* clear saved fragment */ + sbc_info->frame_fragment_size = 0; + } else + p = input_buffer; + d = output_buffer; to_write = output_size; while (PA_LIKELY(to_decode > 0 && to_write > 0)) { - size_t written; + size_t written = 0; ssize_t decoded; + /* skip to SBC sync word before attempting decode */ + if (*p != SBC_SYNCWORD) { + ++p; + --to_decode; + continue; + } + decoded = sbc_decode(&sbc_info->sbc, p, to_decode, decode_buffer, sizeof(decode_buffer), &written); if (PA_UNLIKELY(decoded <= 0)) { + /* sbc_decode returns -1 if input too short, + * break from loop to save this frame fragment for next decode iteration */ + if (decoded == -1) { + pa_log_debug("FastStream SBC decoding error (%li) input %lu is too short", (long) decoded, to_decode); + break; + } + + /* otherwise failed to decode frame, skip to next SBC sync word */ pa_log_error("FastStream SBC decoding error (%li)", (long) decoded); - decoded = PA_MIN(sbc_info->frame_length, to_decode); - written = PA_MIN(sbc_info->codesize, to_write); - pa_silence_memory(decode_buffer, written, &decoded_sample_spec); + decoded = 1; } else { /* Reset codesize and frame_length to values found by decoder */ sbc_info->codesize = sbc_get_codesize(&sbc_info->sbc); @@ -1325,14 +1368,7 @@ static size_t decode_buffer_faststream(void *codec_info, const uint8_t *input_bu memcpy(d, decode_buffer, written); } - if ((sbc_info->frame_length & 1) && decoded < to_decode) { - ++decoded; - ++sbc_info->frame_length; - } - pa_assert_fp((size_t) decoded <= to_decode); - pa_assert_fp((size_t) decoded == PA_MIN(sbc_info->frame_length, to_decode)); - pa_assert_fp((size_t) written <= to_write); p += decoded; @@ -1342,7 +1378,19 @@ static size_t decode_buffer_faststream(void *codec_info, const uint8_t *input_bu to_write -= written; } - /* XXX eat remainder, may need to fix this if input frames are split across packets */ + if (to_decode) { + if (to_decode > sizeof(sbc_info->frame_fragment)) { + pa_log_debug("FastStream remaining SBC fragment size %lu larger than buffer size %lu, remainder truncated to fit", + to_decode, sizeof(sbc_info->frame_fragment)); + p += to_decode - sizeof(sbc_info->frame_fragment); + to_decode = sizeof(sbc_info->frame_fragment); + } + + pa_log_debug("FastStream saving SBC fragment size %lu for next decoding iteration", to_decode); + memcpy(sbc_info->frame_fragment, p, to_decode); + sbc_info->frame_fragment_size = to_decode; + } + *processed = input_size; return d - output_buffer; -- cgit v1.2.1