diff --git a/3rd-party/bluedroid/decoder/include/oi_codec_sbc.h b/3rd-party/bluedroid/decoder/include/oi_codec_sbc.h index abe49cecd..17c75eb62 100644 --- a/3rd-party/bluedroid/decoder/include/oi_codec_sbc.h +++ b/3rd-party/bluedroid/decoder/include/oi_codec_sbc.h @@ -148,6 +148,11 @@ typedef struct { be used by the bit allocator. */ OI_UINT8 cachedInfo; /**< Information about the previous frame */ + +/* BK4BTSTACK_CHANGE START */ + OI_UINT8 reserved_for_future_use[2]; + OI_UINT8 mSBCEnabled; // default 0 +/* BK4BTSTACK_CHANGE END */ } OI_CODEC_SBC_FRAME_INFO; /** Used internally. */ @@ -170,9 +175,6 @@ typedef struct { OI_BYTE formatByte; OI_UINT8 pcmStride; OI_UINT8 maxChannels; - /* BK4BTSTACK_CHANGE START */ - OI_UINT8 mSBCEnabled; // default 0 - /* BK4BTSTACK_CHANGE END */ } OI_CODEC_SBC_COMMON_CONTEXT; diff --git a/3rd-party/bluedroid/decoder/include/oi_codec_sbc_private.h b/3rd-party/bluedroid/decoder/include/oi_codec_sbc_private.h index bf6079c45..6c35beb85 100644 --- a/3rd-party/bluedroid/decoder/include/oi_codec_sbc_private.h +++ b/3rd-party/bluedroid/decoder/include/oi_codec_sbc_private.h @@ -185,6 +185,7 @@ PRIVATE OI_UINT32 OI_SBC_MaxBitpool(OI_CODEC_SBC_FRAME_INFO *frame); PRIVATE void OI_SBC_ComputeBitAllocation(OI_CODEC_SBC_COMMON_CONTEXT *frame); PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data); +PRIVATE OI_UINT8 OI_SBC_CalculateChecksum_mSBC(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data); /* Transform functions */ PRIVATE void shift_buffer(SBC_BUFFER_T *dest, SBC_BUFFER_T *src, OI_UINT wordCount); @@ -212,7 +213,7 @@ PRIVATE void analyze8_enhanced_generated(SBC_BUFFER_T analysisBuffer[RESTRICT 11 #endif /* Decoder functions */ - +INLINE void OI_SBC_ReadHeader_mSBC(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data); INLINE void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data); PRIVATE void OI_SBC_ReadScalefactors(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *b, OI_BITSTREAM *bs); PRIVATE void OI_SBC_ReadSamples(OI_CODEC_SBC_DECODER_CONTEXT *common, OI_BITSTREAM *ob); diff --git a/3rd-party/bluedroid/decoder/srce/decoder-private.c b/3rd-party/bluedroid/decoder/srce/decoder-private.c index e77042ea2..43bb4a224 100644 --- a/3rd-party/bluedroid/decoder/srce/decoder-private.c +++ b/3rd-party/bluedroid/decoder/srce/decoder-private.c @@ -85,18 +85,51 @@ INLINE OI_STATUS internal_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, * Read the SBC header up to but not including the joint stereo mask. The syncword has already been * examined, and the enhanced mode flag set, by FindSyncword. */ + +/* BK4BTSTACK_CHANGE START */ +INLINE void OI_SBC_ReadHeader_mSBC(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data){ + OI_CODEC_SBC_FRAME_INFO *frame = &common->frameInfo; + + OI_ASSERT(data[0] == OI_mSBC_SYNCWORD); + + /* Avoid filling out all these strucutures if we already remember the values + * from last time. Just in case we get a stream corresponding to data[1] == + * 0, DecoderReset is responsible for ensuring the lookup table entries have + * already been populated + */ + frame->reserved_for_future_use[0] = data[1]; + frame->reserved_for_future_use[1] = data[2]; + + frame->freqIndex = 0; + frame->frequency = 16000; + + frame->blocks = 4; // ? + frame->nrof_blocks = 15; + + frame->mode = 0; + frame->nrof_channels = 1; + + frame->alloc = SBC_LOUDNESS; + + frame->subbands = 1; + frame->nrof_subbands = 8; + + frame->bitpool = 26; + + frame->crc = data[3]; + + frame->cachedInfo = 0; +} + +/* BK4BTSTACK_CHANGE END */ + + INLINE void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE *data) { OI_CODEC_SBC_FRAME_INFO *frame = &common->frameInfo; OI_UINT8 d1; - /* BK4BTSTACK_CHANGE START */ - if (common->mSBCEnabled){ - OI_ASSERT(data[0] == OI_mSBC_SYNCWORD); - } else { - OI_ASSERT(data[0] == OI_SBC_SYNCWORD || data[0] == OI_SBC_ENHANCED_SYNCWORD); - } - /* BK4BTSTACK_CHANGE END */ + OI_ASSERT(data[0] == OI_SBC_SYNCWORD || data[0] == OI_SBC_ENHANCED_SYNCWORD); /* Avoid filling out all these strucutures if we already remember the values * from last time. Just in case we get a stream corresponding to data[1] == @@ -110,15 +143,8 @@ INLINE void OI_SBC_ReadHeader(OI_CODEC_SBC_COMMON_CONTEXT *common, const OI_BYTE frame->frequency = freq_values[frame->freqIndex]; frame->blocks = (d1 & (BIT5 | BIT4)) >> 4; + frame->nrof_blocks = block_values[frame->blocks]; - /* BK4BTSTACK_CHANGE START */ - if (common->mSBCEnabled){ - frame->nrof_blocks = 15; - } else { - frame->nrof_blocks = block_values[frame->blocks]; - } - /* BK4BTSTACK_CHANGE END */ - frame->mode = (d1 & (BIT3 | BIT2)) >> 2; frame->nrof_channels = channel_values[frame->mode]; diff --git a/3rd-party/bluedroid/decoder/srce/decoder-sbc.c b/3rd-party/bluedroid/decoder/srce/decoder-sbc.c index cd30abccd..1f768829a 100644 --- a/3rd-party/bluedroid/decoder/srce/decoder-sbc.c +++ b/3rd-party/bluedroid/decoder/srce/decoder-sbc.c @@ -81,7 +81,7 @@ PRIVATE OI_STATUS FindSyncword(OI_CODEC_SBC_DECODER_CONTEXT *context, #else // SBC_ENHANCED /* BK4BTSTACK_CHANGE START */ OI_UINT8 syncword = OI_SBC_SYNCWORD; - if (context->common.mSBCEnabled){ + if (context->common.frameInfo.mSBCEnabled){ syncword = OI_mSBC_SYNCWORD; } /* BK4BTSTACK_CHANGE END */ @@ -249,7 +249,7 @@ OI_STATUS OI_CODEC_mSBC_DecoderReset(OI_CODEC_SBC_DECODER_CONTEXT *context, OI_UINT32 decoderDataBytes) { OI_STATUS status = OI_CODEC_SBC_DecoderReset(context, decoderData, decoderDataBytes, 1, 1, FALSE); - context->common.mSBCEnabled = TRUE; + context->common.frameInfo.mSBCEnabled = TRUE; return status; } /* BK4BTSTACK_CHANGE END */ @@ -279,7 +279,11 @@ OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, } TRACE(("Reading Header")); - OI_SBC_ReadHeader(&context->common, *frameData); + if (context->common.frameInfo.mSBCEnabled){ + OI_SBC_ReadHeader_mSBC(&context->common, *frameData); + } else { + OI_SBC_ReadHeader(&context->common, *frameData); + } /* * Some implementations load the decoder into RAM and use overlays for 4 vs 8 subbands. We need @@ -317,8 +321,12 @@ OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context, TRACE(("frame len %d\n", framelen)); TRACE(("Calculating checksum")); + if (context->common.frameInfo.mSBCEnabled){ + crc = OI_SBC_CalculateChecksum_mSBC(&context->common.frameInfo, *frameData); + } else { + crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData); + } - crc = OI_SBC_CalculateChecksum(&context->common.frameInfo, *frameData); if (crc != context->common.frameInfo.crc) { TRACE(("CRC Mismatch: calc=%02x read=%02x\n", crc, context->common.frameInfo.crc)); TRACE(("-OI_CODEC_SBC_DecodeFrame: OI_CODEC_SBC_CHECKSUM_MISMATCH")); diff --git a/3rd-party/bluedroid/decoder/srce/framing.c b/3rd-party/bluedroid/decoder/srce/framing.c index 1448bd0f6..d7d4641ce 100644 --- a/3rd-party/bluedroid/decoder/srce/framing.c +++ b/3rd-party/bluedroid/decoder/srce/framing.c @@ -169,6 +169,28 @@ INLINE OI_UINT8 crc_iterate(OI_UINT8 crc, OI_UINT8 next) #endif // USE_WIDE_CRC +PRIVATE OI_UINT8 OI_SBC_CalculateChecksum_mSBC(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data) +{ + OI_UINT i; + OI_UINT8 crc = 0x0f; + /* Count is the number of whole bytes subject to CRC. Actually, it's one + * more than this number, because data[3] is the CRC field itself, which is + * explicitly skipped. Since crc_iterate (should be) inlined, it's cheaper + * spacewise to include the check in the loop. This shouldn't be much of a + * bottleneck routine in the first place. */ + + // 0 - syncword (skip) + // 1 - reserved + crc = crc_iterate(crc,frame->reserved_for_future_use[0]); + // 2 - reserved + crc = crc_iterate(crc,frame->reserved_for_future_use[1]); + // 3 - crc (skip) + // 4..7 - scale factors (8 x 4 bit = 4 byte) + for (i = 0; i < 4; i++) { + crc = crc_iterate(crc,data[4+i]); + } + return crc; +} PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYTE const *data) { @@ -179,6 +201,7 @@ PRIVATE OI_UINT8 OI_SBC_CalculateChecksum(OI_CODEC_SBC_FRAME_INFO *frame, OI_BYT * explicitly skipped. Since crc_iterate (should be) inlined, it's cheaper * spacewise to include the check in the loop. This shouldn't be much of a * bottleneck routine in the first place. */ + OI_UINT count = (frame->nrof_subbands * frame->nrof_channels / 2u) + 4; if (frame->mode == SBC_JOINT_STEREO && frame->nrof_subbands == 8) { diff --git a/example/Makefile.inc b/example/Makefile.inc index 53dc424a5..9ae6e77f3 100644 --- a/example/Makefile.inc +++ b/example/Makefile.inc @@ -1,8 +1,13 @@ +SBC_DECODER_ROOT = ${BTSTACK_ROOT}/3rd-party/bluedroid/decoder +SBC_ENCODER_ROOT = ${BTSTACK_ROOT}/3rd-party/bluedroid/encoder + VPATH += ${BTSTACK_ROOT}/src VPATH += ${BTSTACK_ROOT}/src/ble VPATH += ${BTSTACK_ROOT}/src/classic VPATH += ${BTSTACK_ROOT}/example VPATH += ${BTSTACK_ROOT}/3rd-party/mbedtls/library +VPATH += ${SBC_DECODER_ROOT}/srce +VPATH += ${SBC_ENCODER_ROOT}/srce CFLAGS += -I. CFLAGS += -I${BTSTACK_ROOT}/src/ble @@ -10,6 +15,8 @@ CFLAGS += -I${BTSTACK_ROOT}/src/classic CFLAGS += -I${BTSTACK_ROOT}/src CFLAGS += -I${BTSTACK_ROOT}/3rd-party/mbedtls/include CFLAGS += -I${BTSTACK_ROOT}/test/security_manager +CFLAGS += -I${SBC_DECODER_ROOT}/include -D OI_DEBUG # -D PRINT_SAMPLES -D PRINT_SCALEFACTORS +CFLAGS += -I${SBC_ENCODER_ROOT}/include -D SBC_NO_PCM_CPY_OPTION CORE += \ btstack_memory.c \ @@ -61,6 +68,13 @@ MBEDTLS = \ memory_buffer_alloc.c \ platform.c \ +# +include ${SBC_DECODER_ROOT}/Makefile.inc +include ${SBC_ENCODER_ROOT}/Makefile.inc + +SBC_DECODER += \ + ${BTSTACK_ROOT}/src/classic/sbc_decoder_bludroid.c \ + EXAMPLES = \ ancs_client_demo \ gap_dedicated_bonding \ @@ -103,6 +117,8 @@ ATT_OBJ = $(ATT:.c=.o) GATT_CLIENT_OBJ = $(GATT_CLIENT:.c=.o) GATT_SERVER_OBJ = $(GATT_SERVER:.c=.o) PAN_OBJ = $(PAN:.c=.o) +SBC_DECODER_OBJ = $(SBC_DECODER:.c=.o) +SBC_ENCODER_OBJ = $(SBC_ENCODER:.c=.o) default_target: all @@ -187,7 +203,7 @@ hsp_ag_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} sco_demo_util.o hsp_ag.o hs hfp_ag_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} sco_demo_util.o hfp.o hfp_gsm_model.o hfp_ag.o hfp_ag_demo.c ${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@ -hfp_hf_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} sco_demo_util.o hfp.o hfp_hf.o hfp_hf_demo.c +hfp_hf_demo: ${CORE_OBJ} ${COMMON_OBJ} ${SDP_CLIENT} ${SBC_DECODER_OBJ} sco_demo_util.o hfp.o hfp_hf.o hfp_hf_demo.c ${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@ clean: diff --git a/example/hfp_hf_demo.c b/example/hfp_hf_demo.c index e3b76a0f5..02b740965 100644 --- a/example/hfp_hf_demo.c +++ b/example/hfp_hf_demo.c @@ -59,6 +59,8 @@ #include #include "btstack.h" +#include "sbc_decoder.h" + #include "sco_demo_util.h" #ifdef HAVE_POSIX_STDIN #include "stdin_support.h" @@ -86,7 +88,10 @@ char cmd; // Testig User Interface static void show_usage(void){ - printf("\n--- Bluetooth HFP Hands-Free (HF) unit Test Console ---\n"); + bd_addr_t iut_address; + gap_local_bd_addr(iut_address); + + printf("\n--- Bluetooth HFP Hands-Free (HF) unit Test Console %s ---\n", bd_addr_to_str(iut_address)); printf("---\n"); printf("a - establish SLC connection to device %s\n", bd_addr_to_str(device_addr)); @@ -453,7 +458,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t * even switch (packet_type){ case HCI_SCO_DATA_PACKET: - sco_demo_receive_with_codec(event, event_size, negotiated_codec); + sco_demo_receive(event, event_size); break; case HCI_EVENT_PACKET: @@ -467,6 +472,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t * even case HFP_SUBEVENT_CODECS_CONNECTION_COMPLETE: negotiated_codec = hfp_subevent_codecs_connection_complete_get_negotiated_codec(event); printf("Codec connection established with codec 0x%02x.\n", negotiated_codec); + sco_demo_set_codec(negotiated_codec); break; case HFP_SUBEVENT_SERVICE_LEVEL_CONNECTION_ESTABLISHED: @@ -491,6 +497,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t * even case HFP_SUBEVENT_AUDIO_CONNECTION_RELEASED: sco_handle = 0; printf("Audio connection released\n"); + sco_demo_close(); break; case HFP_SUBEVENT_COMPLETE: switch (cmd){ @@ -616,9 +623,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t * even /* LISTING_START(MainConfiguration): Setup HFP Hands-Free unit */ int btstack_main(int argc, const char * argv[]); int btstack_main(int argc, const char * argv[]){ - sco_demo_init(); - gap_discoverable_control(1); // HFP AG address is hardcoded, please change it @@ -628,7 +633,7 @@ int btstack_main(int argc, const char * argv[]){ sdp_init(); hfp_hf_init(rfcomm_channel_nr); - hfp_hf_init_supported_features(438 | (1< // configure test mode @@ -49,7 +52,7 @@ // SCO demo configuration -#define SCO_DEMO_MODE SCO_DEMO_MODE_ASCII +#define SCO_DEMO_MODE SCO_DEMO_MODE_SINE #define SCO_REPORT_PERIOD 100 #ifdef HAVE_POSIX_FILE_IO @@ -73,6 +76,19 @@ static PaStream * stream; #endif +typedef struct wav_writer_state { + FILE * wav_file; + int total_num_samples; + int frame_count; +} wav_writer_state_t; + +static int dump_data = 1; + +static int phase; +static int count_sent = 0; +static int count_received = 0; +static uint8_t negotiated_codec = 0; // CVSD + #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE // input signal: pre-computed sine wave, 160 Hz static const uint8_t sine[] = { @@ -82,19 +98,15 @@ static const uint8_t sine[] = { 182, 170, 159, 149, 142, 136, 132, 130, 130, 132, 136, 142, 149, 159, 170, 182, 195, 210, 225, 241, }; -#endif -static int phase; -static int count_sent = 0; -static int count_received = 0; -#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE #ifdef SCO_WAV_FILENAME -static FILE * wav_file; static int num_samples_to_write; +static wav_writer_state_t wav_writer_state; - +static sbc_decoder_state_t decoder_state; + static void little_endian_fstore_16(FILE * file, uint16_t value){ uint8_t buf[2]; little_endian_store_32(buf, 0, value); @@ -108,14 +120,12 @@ static void little_endian_fstore_32(FILE * file, uint32_t value){ } static FILE * wav_init(const char * filename){ - printf("SCO Demo: creating wav file %s\n", filename); - return fopen(filename, "wb"); + FILE * f = fopen(filename, "wb"); + printf("SCO Demo: creating wav file %s, %p\n", filename, f); + return f; } static void write_wav_header(FILE * file, int sample_rate, int num_channels, int num_samples, int bytes_per_sample){ - printf("SCO Demo: writing wav header: sample rate %u, num channels %u, duration %u s, bytes per sample %u\n", - sample_rate, num_channels, num_samples / sample_rate / num_channels, bytes_per_sample); - /* write RIFF header */ fwrite("RIFF", 1, 4, file); // num_samples = blocks * subbands @@ -148,9 +158,119 @@ static void write_wav_data_uint8(FILE * file, unsigned long num_samples, uint8_t fwrite(data, num_samples, 1, file); } +static void write_wav_data_int16(FILE * file, int num_samples, int16_t * data){ + fwrite(data, num_samples, 2, file); +} + +static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ + log_info("handle_pcm_data num samples %u / %u", num_samples, num_samples_to_write); + if (!num_samples_to_write) return; + + wav_writer_state_t * writer_state = (wav_writer_state_t*) context; + num_samples = btstack_min(num_samples, num_samples_to_write); + num_samples_to_write -= num_samples; + + write_wav_data_int16(writer_state->wav_file, num_samples, data); + writer_state->total_num_samples+=num_samples; + writer_state->frame_count++; + + if (num_samples_to_write == 0){ + sco_demo_close(); + } +} + + +static void sco_demo_init_mSBC(void){ + wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); + wav_writer_state.frame_count = 0; + wav_writer_state.total_num_samples = 0; + + sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, (void*)&wav_writer_state); + + const int sample_rate = 16000; + const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; + const int bytes_per_sample = 2; + const int num_channels = 1; + num_samples_to_write = num_samples; + + write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); +} + +static void sco_demo_init_CVSD(void){ + wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); + wav_writer_state.frame_count = 0; + wav_writer_state.total_num_samples = 0; + + const int sample_rate = 8000; + const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; + const int num_channels = 1; + const int bytes_per_sample = 1; + num_samples_to_write = num_samples; + write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); +} + + +static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ + printf("sco_demo_receive_mSBC size %u, need %u\n", size, num_samples_to_write); + if (num_samples_to_write){ + sbc_decoder_process_data(&decoder_state, packet+3, size-3); + dump_data = 0; + } +} + +static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ + if (num_samples_to_write){ + const int num_samples = size - 3; + const int samples_to_write = btstack_min(num_samples, num_samples_to_write); + // convert 8 bit signed to 8 bit unsigned + int i; + for (i=0;iwav_file) return; + rewind(writer_state->wav_file); + write_wav_header(writer_state->wav_file, writer_state->total_num_samples, sbc_decoder_num_channels(&decoder_state), sbc_decoder_sample_rate(&decoder_state),2); + fclose(writer_state->wav_file); + writer_state->wav_file = NULL; + } + +#endif +#endif +} + +void sco_demo_set_codec(uint8_t codec){ + if (negotiated_codec == codec) return; + negotiated_codec = codec; +#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE +#ifdef SCO_WAV_FILENAME + if (negotiated_codec == 0x02){ + sco_demo_init_mSBC(); + } else { + sco_demo_init_CVSD(); + } +#endif +#endif +} + void sco_demo_init(void){ // status @@ -168,18 +288,6 @@ void sco_demo_init(void){ printf("SCO Demo: Sending counter value, hexdump received data.\n"); #endif -#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE -#ifdef SCO_WAV_FILENAME - wav_file = wav_init(SCO_WAV_FILENAME); - const int sample_rate = 8000; - const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; - const int num_channels = 1; - const int bytes_per_sample = 1; - write_wav_header(wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); - num_samples_to_write = num_samples; -#endif -#endif - #ifdef USE_PORTAUDIO int err; PaStreamParameters outputParameters; @@ -209,9 +317,9 @@ void sco_demo_init(void){ if( err != paNoError ) return; #endif -#if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE +//#if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent -#endif +//#endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII phase = 'a'; @@ -264,13 +372,14 @@ void sco_demo_send(hci_con_handle_t sco_handle){ if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); } + /** * @brief Process received data */ void sco_demo_receive(uint8_t * packet, uint16_t size){ - int dump_data = 1; + dump_data = 1; count_received++; // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); @@ -278,21 +387,10 @@ void sco_demo_receive(uint8_t * packet, uint16_t size){ #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE #ifdef SCO_WAV_FILENAME - if (num_samples_to_write){ - const int num_samples = size - 3; - const int samples_to_write = btstack_min(num_samples, num_samples_to_write); - // convert 8 bit signed to 8 bit unsigned - int i; - for (i=0;iremote_supported_features, HFP_AGSF_CODEC_NEGOTIATION); + printf("local %d, remote %d\n", hf, ag); return hf && ag; } @@ -520,7 +521,7 @@ static int codecs_exchange_state_machine(hfp_connection_t * hfp_connection){ hfp_connection->codec_confirmed = hfp_connection->suggested_codec; hfp_connection->ok_pending = 1; hfp_connection->codecs_state = HFP_CODECS_HF_CONFIRMED_CODEC; - hfp_hf_cmd_confirm_codec(hfp_connection->rfcomm_cid, hfp_connection->suggested_codec); + hfp_hf_cmd_confirm_codec(hfp_connection->rfcomm_cid, hfp_connection->codec_confirmed); } else { hfp_connection->codec_confirmed = 0; hfp_connection->suggested_codec = 0; diff --git a/src/classic/sbc_decoder.h b/src/classic/sbc_decoder.h index 19fdd8c69..37509a384 100644 --- a/src/classic/sbc_decoder.h +++ b/src/classic/sbc_decoder.h @@ -49,14 +49,20 @@ extern "C" { #endif +typedef enum{ + SBC_MODE_STANDARD, + SBC_MODE_mSBC +} sbc_mode_t; + typedef struct { void * context; void (*handle_pcm_data)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context); // private void * decoder_state; + sbc_mode_t mode; } sbc_decoder_state_t; -void sbc_decoder_init(sbc_decoder_state_t * state, void (*callback)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context), void * context); +void sbc_decoder_init(sbc_decoder_state_t * state, sbc_mode_t mode, void (*callback)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context), void * context); void sbc_decoder_process_data(sbc_decoder_state_t * state, uint8_t * buffer, int size); int sbc_decoder_num_samples_per_frame(sbc_decoder_state_t * state); diff --git a/src/classic/sbc_decoder_bludroid.c b/src/classic/sbc_decoder_bludroid.c index c66fb7ee7..f1b0d6311 100644 --- a/src/classic/sbc_decoder_bludroid.c +++ b/src/classic/sbc_decoder_bludroid.c @@ -90,13 +90,20 @@ void OI_AssertFail(char* file, int line, char* reason){ printf("AssertFail file %s, line %d, reason %s\n", file, line, reason); } -void sbc_decoder_init(sbc_decoder_state_t * state, void (*callback)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context), void * context){ +void sbc_decoder_init(sbc_decoder_state_t * state, sbc_mode_t mode, void (*callback)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context), void * context){ if (sbc_state_singelton && sbc_state_singelton != state ){ log_error("SBC decoder: different sbc decoder state is allready registered"); } + OI_STATUS status; + switch (mode){ + case SBC_MODE_STANDARD: + status = OI_CODEC_SBC_DecoderReset(&(bd_state.decoder_context), bd_state.decoder_data, sizeof(bd_state.decoder_data), 2, 1, FALSE); + break; + case SBC_MODE_mSBC: + status = OI_CODEC_mSBC_DecoderReset(&(bd_state.decoder_context), bd_state.decoder_data, sizeof(bd_state.decoder_data)); + break; + } - OI_STATUS status = OI_CODEC_mSBC_DecoderReset(&(bd_state.decoder_context), - bd_state.decoder_data, sizeof(bd_state.decoder_data)); if (status != 0){ log_error("SBC decoder: error during reset %d\n", status); } @@ -106,9 +113,9 @@ void sbc_decoder_init(sbc_decoder_state_t * state, void (*callback)(int16_t * da bd_state.pcm_bytes = sizeof(bd_state.pcm_data); state->handle_pcm_data = callback; + state->mode = mode; state->context = context; state->decoder_state = &bd_state; - //OI_STATUS status = OI_CODEC_SBC_DecoderReset(&context, decoder_data, sizeof(decoder_data), 1, 1, FALSE); } static void append_received_sbc_data(bludroid_decoder_state_t * state, uint8_t * buffer, int size){ @@ -124,13 +131,13 @@ static void append_received_sbc_data(bludroid_decoder_state_t * state, uint8_t * void sbc_decoder_process_data(sbc_decoder_state_t * state, uint8_t * buffer, int size){ int bytes_read = size; + bludroid_decoder_state_t * bd_decoder_state = (bludroid_decoder_state_t*)state->decoder_state; + while (bytes_read > 0){ - bludroid_decoder_state_t * bd_decoder_state = (bludroid_decoder_state_t*)state->decoder_state; int space_in_frame_buffer = sizeof(bd_decoder_state->frame_buffer) - bd_decoder_state->bytes_in_frame; int bytes_to_append = space_in_frame_buffer > bytes_read ? bytes_read : space_in_frame_buffer; append_received_sbc_data(bd_decoder_state, buffer, bytes_to_append); - while (1){ uint16_t bytes_in_buffer_before = bd_decoder_state->bytes_in_frame; const OI_BYTE *frame_data = bd_decoder_state->frame_buffer; @@ -139,22 +146,34 @@ void sbc_decoder_process_data(sbc_decoder_state_t * state, uint8_t * buffer, int &(bd_decoder_state->bytes_in_frame), bd_decoder_state->pcm_data, &(bd_decoder_state->pcm_bytes)); + + if (status == OI_CODEC_SBC_CHECKSUM_MISMATCH){ + // advance at least one byte + OI_CODEC_SBC_DumpConfig(&(bd_decoder_state->decoder_context.common.frameInfo)); + bd_decoder_state->bytes_in_frame--; + } + uint16_t bytes_processed = bytes_in_buffer_before - bd_decoder_state->bytes_in_frame; + // log_info("sbc_decoder_process_data: decode status %u, processed %u, left %u", status, bytes_processed, bd_decoder_state->bytes_in_frame); memmove(bd_decoder_state->frame_buffer, bd_decoder_state->frame_buffer + bytes_processed, bd_decoder_state->bytes_in_frame); - if (status != 0){ - if (status != OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA && status != OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA){ - OI_CODEC_SBC_DumpConfig(&(bd_decoder_state->decoder_context.common.frameInfo)); - printf("Frame decode error %d\n", status); - } - break; - } - - state->handle_pcm_data(bd_decoder_state->pcm_data, + switch(status){ + case 0: + state->handle_pcm_data(bd_decoder_state->pcm_data, sbc_decoder_num_samples_per_frame(state), sbc_decoder_num_channels(state), sbc_decoder_sample_rate(state), state->context); + break; + case OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA: + case OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA: + case OI_CODEC_SBC_NO_SYNCWORD: + break; + default: + printf("Frame decode error %d\n", status); + break; + } } + buffer += bytes_to_append; bytes_read -= bytes_to_append; diff --git a/test/sbc/Makefile b/test/sbc/Makefile index 0bcf799c6..b00692967 100644 --- a/test/sbc/Makefile +++ b/test/sbc/Makefile @@ -10,7 +10,10 @@ include ${SBC_ENCODER_ROOT}/Makefile.inc SBC_DECODER_OBJ = $(SBC_DECODER:.c=.o) SBC_ENCODER_OBJ = $(SBC_ENCODER:.c=.o) -CFLAGS = -g -Wall -I. -I../ -I${BTSTACK_ROOT}/src -I${BTSTACK_ROOT}/src/classic -I${SBC_DECODER_ROOT}/include -I${SBC_ENCODER_ROOT}/include -D PRINT_SAMPLES -D PRINT_SCALEFACTORS -D OI_DEBUG -D SBC_NO_PCM_CPY_OPTION +CFLAGS = -g -Wall -I. -I../ -I${BTSTACK_ROOT}/src -I${BTSTACK_ROOT}/src/classic +CFLAGS += -I${SBC_DECODER_ROOT}/include +CFLAGS += -I${SBC_ENCODER_ROOT}/include +CFLAGS += -D PRINT_SAMPLES -D PRINT_SCALEFACTORS -D OI_DEBUG -D SBC_NO_PCM_CPY_OPTION # -D TRACE_EXECUTION -D CODEC_DEBUG LDFLAGS += -lCppUTest -lCppUTestExt VPATH += ${SBC_DECODER_ROOT}/srce diff --git a/test/sbc/sbc_decoder_test.c b/test/sbc/sbc_decoder_test.c index 1f06ad9c2..4a87bff0d 100644 --- a/test/sbc/sbc_decoder_test.c +++ b/test/sbc/sbc_decoder_test.c @@ -54,7 +54,7 @@ #include "btstack.h" -static uint8_t read_buffer[6000]; +static uint8_t read_buffer[24]; static uint8_t buf[4]; typedef struct wav_writer_state { @@ -64,7 +64,7 @@ typedef struct wav_writer_state { } wav_writer_state_t; static void show_usage(void){ - printf("Usage: ./sbc_decoder_test input.sbc"); + printf("\n\nUsage: ./sbc_decoder_test input.sbc output.wav msbc|sbc\n\n"); } static ssize_t __read(int fd, void *buf, size_t count){ @@ -123,13 +123,7 @@ static void write_wav_header(FILE * wav_file, int total_num_samples, int num_ch } static void write_wav_data(FILE * wav_file, int num_samples, int num_channels, int16_t * data){ - int i; - for (i=0; i < num_samples; i++){ - little_endian_fstore_16(wav_file, (uint16_t)data[i]); - if (num_channels == 2){ - little_endian_fstore_16(wav_file, (uint16_t)data); - } - } + fwrite(data, num_samples, 2, wav_file); } static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ @@ -140,14 +134,22 @@ static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, i } int main (int argc, const char * argv[]){ - if (argc < 2){ + if (argc < 4){ show_usage(); return -1; } + sbc_mode_t mode = SBC_MODE_STANDARD; + const char * sbc_filename = argv[1]; const char * wav_filename = argv[2]; + if (strncmp(argv[3], "msbc", 4) == 0 ){ + mode = SBC_MODE_mSBC; + printf("Using SBC_MODE_mSBC mode\n"); + } else { + printf("Using SBC_MODE_STANDARD mode\n"); + } int fd = open(sbc_filename, O_RDONLY); if (fd < 0) { @@ -162,7 +164,7 @@ int main (int argc, const char * argv[]){ wav_writer_state.total_num_samples = 0; sbc_decoder_state_t state; - sbc_decoder_init(&state, &handle_pcm_data, (void*)&wav_writer_state); + sbc_decoder_init(&state, mode, &handle_pcm_data, (void*)&wav_writer_state); write_wav_header(wav_writer_state.wav_file, 0, 0, 0); while (1){