From 463c9c89cdd7599d89d8bc1ecdc22cc8db4e19c1 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Tue, 24 Jan 2017 17:26:55 +0100 Subject: [PATCH 1/6] sco_util: prefix port audio vars with pa_output --- example/sco_demo_util.c | 59 +++++++++++++++++++++-------------------- 1 file changed, 30 insertions(+), 29 deletions(-) diff --git a/example/sco_demo_util.c b/example/sco_demo_util.c index 656a7c07f..095e528fe 100644 --- a/example/sco_demo_util.c +++ b/example/sco_demo_util.c @@ -59,11 +59,12 @@ // test modes -#define SCO_DEMO_MODE_SINE 0 -#define SCO_DEMO_MODE_ASCII 1 -#define SCO_DEMO_MODE_COUNTER 2 -#define SCO_DEMO_MODE_55 3 -#define SCO_DEMO_MODE_00 4 +#define SCO_DEMO_MODE_SINE 0 +#define SCO_DEMO_MODE_ASCII 1 +#define SCO_DEMO_MODE_COUNTER 2 +#define SCO_DEMO_MODE_55 3 +#define SCO_DEMO_MODE_00 4 +#define SCO_DEMO_MODE_MICROPHONE 5 // SCO demo configuration #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE @@ -90,18 +91,18 @@ #define MSBC_SAMPLE_RATE 16000 #define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) -#if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) +#if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE || SCO_DEMO_MODE_MICROPHONE) #define USE_PORTAUDIO #define CVSD_PA_PREBUFFER_BYTES (SCO_CVSD_PA_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * CVSD_BYTES_PER_FRAME) #define MSBC_PA_PREBUFFER_BYTES (SCO_MSBC_PA_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * MSBC_BYTES_PER_FRAME) #endif #ifdef USE_PORTAUDIO -static PaStream * stream; -static int pa_stream_started = 0; -static int pa_stream_paused = 0; -static uint8_t ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES]; -static btstack_ring_buffer_t ring_buffer; +static PaStream * pa_output_stream; +static int pa_output_started = 0; +static int pa_output_paused = 0; +static uint8_t pa_output_ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES]; +static btstack_ring_buffer_t pa_output_ring_buffer; #endif static int dump_data = 1; @@ -186,39 +187,39 @@ static int portaudio_callback( const void *inputBuffer, void *outputBuffer, } // fill with silence while paused - if (pa_stream_paused){ - if (btstack_ring_buffer_bytes_available(&ring_buffer) < prebuffer_bytes){ + if (pa_output_paused){ + if (btstack_ring_buffer_bytes_available(&pa_output_ring_buffer) < prebuffer_bytes){ memset(outputBuffer, 0, bytes_to_copy); return 0; } else { // resume playback - pa_stream_paused = 0; + pa_output_paused = 0; } } // get data from ringbuffer uint32_t bytes_read = 0; - btstack_ring_buffer_read(&ring_buffer, outputBuffer, bytes_to_copy, &bytes_read); + btstack_ring_buffer_read(&pa_output_ring_buffer, outputBuffer, bytes_to_copy, &bytes_read); bytes_to_copy -= bytes_read; // fill with 0 if not enough if (bytes_to_copy){ memset(outputBuffer + bytes_read, 0, bytes_to_copy); - pa_stream_paused = 1; + pa_output_paused = 1; } return 0; } static void portaudio_start(void){ - if (!pa_stream_started){ + if (!pa_output_started){ /* -- start stream -- */ - PaError err = Pa_StartStream(stream); + PaError err = Pa_StartStream(pa_output_stream); if (err != paNoError){ printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); return; } - pa_stream_started = 1; - pa_stream_paused = 1; + pa_output_started = 1; + pa_output_paused = 1; } } @@ -252,9 +253,9 @@ static int portaudio_initialize(int sample_rate){ printf("Error opening portaudio stream: \"%s\"\n", Pa_GetErrorText(err)); return 0; } - memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); - btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); - pa_stream_started = 0; + memset(pa_output_ring_buffer_storage, 0, sizeof(pa_output_ring_buffer_storage)); + btstack_ring_buffer_init(&pa_output_ring_buffer, pa_output_ring_buffer_storage, sizeof(pa_output_ring_buffer_storage)); + pa_output_started = 0; return 1; } #endif @@ -267,7 +268,7 @@ static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, i // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); #ifdef HAVE_PORTAUDIO portaudio_start(); - btstack_ring_buffer_write(&ring_buffer, (uint8_t *)data, num_samples*num_channels*2); + btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); #else UNUSED(num_channels); #endif @@ -357,7 +358,7 @@ static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ } #ifdef USE_PORTAUDIO portaudio_start(); - btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); + btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); #endif } @@ -378,16 +379,16 @@ void sco_demo_close(void){ #endif #ifdef HAVE_PORTAUDIO - if (pa_stream_started){ + if (pa_output_started){ printf("PortAudio: Stop Stream\n"); - PaError err = Pa_StopStream(stream); + PaError err = Pa_StopStream(pa_output_stream); if (err != paNoError){ printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); return; } - pa_stream_started = 0; + pa_output_started = 0; printf("PortAudio: Close Stream\n"); - err = Pa_CloseStream(stream); + err = Pa_CloseStream(pa_output_stream); if (err != paNoError){ printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); return; From c79b4cab43c41297de70183f469da23c49cd2aff Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Tue, 24 Jan 2017 19:14:53 +0100 Subject: [PATCH 2/6] hfp_ag: remove debug output --- src/classic/hfp_ag.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/classic/hfp_ag.c b/src/classic/hfp_ag.c index 901823dca..7902c0799 100644 --- a/src/classic/hfp_ag.c +++ b/src/classic/hfp_ag.c @@ -650,7 +650,7 @@ static void hfp_ag_slc_established(hfp_connection_t * hfp_connection){ } static int hfp_ag_run_for_context_service_level_connection(hfp_connection_t * hfp_connection){ - log_info("hfp_ag_run_for_context_service_level_connection state %u, command %u", hfp_connection->state, hfp_connection->command); + // log_info("hfp_ag_run_for_context_service_level_connection state %u, command %u", hfp_connection->state, hfp_connection->command); if (hfp_connection->state >= HFP_SERVICE_LEVEL_CONNECTION_ESTABLISHED) return 0; int done = 0; switch(hfp_connection->command){ @@ -1641,7 +1641,7 @@ static void hfp_ag_send_call_status(hfp_connection_t * hfp_connection, int call_ static void hfp_run_for_context(hfp_connection_t *hfp_connection){ - log_info("hfp_run_for_context %p", hfp_connection); + // log_info("hfp_run_for_context %p", hfp_connection); if (!hfp_connection) return; From 2b89dbfc6bb27df2ec8d6a87caf9eeb8d3765ccd Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Wed, 25 Jan 2017 16:58:03 +0100 Subject: [PATCH 3/6] sco_util: send PortAudio input as CVSD --- example/sco_demo_util.c | 245 +++++++++++++++++++++++++++++----------- 1 file changed, 181 insertions(+), 64 deletions(-) diff --git a/example/sco_demo_util.c b/example/sco_demo_util.c index 095e528fe..e0fa32c06 100644 --- a/example/sco_demo_util.c +++ b/example/sco_demo_util.c @@ -91,34 +91,50 @@ #define MSBC_SAMPLE_RATE 16000 #define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) -#if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE || SCO_DEMO_MODE_MICROPHONE) +#if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE || SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) #define USE_PORTAUDIO #define CVSD_PA_PREBUFFER_BYTES (SCO_CVSD_PA_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * CVSD_BYTES_PER_FRAME) #define MSBC_PA_PREBUFFER_BYTES (SCO_MSBC_PA_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * MSBC_BYTES_PER_FRAME) #endif #ifdef USE_PORTAUDIO -static PaStream * pa_output_stream; + +// bidirectional audio stream +static PaStream * pa_stream; + +// output static int pa_output_started = 0; static int pa_output_paused = 0; static uint8_t pa_output_ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES]; static btstack_ring_buffer_t pa_output_ring_buffer; + +// input +#if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE +#define USE_PORTAUDIO_INPUT +static int pa_input_started = 0; +static int pa_input_paused = 0; +static uint8_t pa_input_ring_buffer_storage[2*8000]; // full second input buffer +static btstack_ring_buffer_t pa_input_ring_buffer; +static int pa_input_counter; +#endif + #endif static int dump_data = 1; static int count_sent = 0; static int count_received = 0; static int negotiated_codec = -1; -static int phase = 0; -static int num_audio_frames = 0; -static btstack_sbc_decoder_state_t decoder_state; -static btstack_cvsd_plc_state_t cvsd_plc_state; -static int num_samples_to_write; +btstack_sbc_decoder_state_t decoder_state; +btstack_cvsd_plc_state_t cvsd_plc_state; FILE * msbc_file_in; FILE * msbc_file_out; +int num_samples_to_write; +int num_audio_frames; +int phase; + #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE // input signal: pre-computed sine wave, 160 Hz at 16000 kHz @@ -155,8 +171,8 @@ static void sco_demo_fill_audio_frame(void){ hfp_msbc_encode_audio_frame(sample_buffer); num_audio_frames++; } +#endif -#ifdef SCO_WAV_FILENAME #ifdef USE_PORTAUDIO static int portaudio_callback( const void *inputBuffer, void *outputBuffer, unsigned long framesPerBuffer, @@ -168,6 +184,8 @@ static int portaudio_callback( const void *inputBuffer, void *outputBuffer, (void) inputBuffer; (void) userData; +// output part + // config based on codec int bytes_to_copy; int prebuffer_bytes; @@ -207,42 +225,54 @@ static int portaudio_callback( const void *inputBuffer, void *outputBuffer, memset(outputBuffer + bytes_read, 0, bytes_to_copy); pa_output_paused = 1; } - return 0; -} +// end of output part -static void portaudio_start(void){ - if (!pa_output_started){ - /* -- start stream -- */ - PaError err = Pa_StartStream(pa_output_stream); - if (err != paNoError){ - printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - pa_output_started = 1; - pa_output_paused = 1; - } +// input part -- just store in ring buffer +#ifdef USE_PORTAUDIO_INPUT + btstack_ring_buffer_write(&pa_input_ring_buffer, (uint8_t *)inputBuffer, framesPerBuffer * 2); + pa_input_counter += framesPerBuffer * 2; +#endif + + return 0; } // return 1 if ok static int portaudio_initialize(int sample_rate){ PaError err; - PaStreamParameters outputParameters; /* -- initialize PortAudio -- */ printf("PortAudio: Initialize\n"); err = Pa_Initialize(); if( err != paNoError ) return 0; + /* -- setup input and output -- */ + const PaDeviceInfo *deviceInfo; + PaStreamParameters * inputParameters = NULL; + PaStreamParameters outputParameters; outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ outputParameters.channelCount = NUM_CHANNELS; outputParameters.sampleFormat = paInt16; outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; outputParameters.hostApiSpecificStreamInfo = NULL; - /* -- setup stream -- */ + deviceInfo = Pa_GetDeviceInfo( outputParameters.device ); + log_info("PortAudio: Output device: %s", deviceInfo->name); +#ifdef USE_PORTAUDIO_INPUT + PaStreamParameters theInputParameters; + theInputParameters.device = Pa_GetDefaultInputDevice(); /* default input device */ + theInputParameters.channelCount = NUM_CHANNELS; + theInputParameters.sampleFormat = paInt16; + theInputParameters.suggestedLatency = Pa_GetDeviceInfo( theInputParameters.device )->defaultHighOutputLatency; + theInputParameters.hostApiSpecificStreamInfo = NULL; + inputParameters = &theInputParameters; + deviceInfo = Pa_GetDeviceInfo( inputParameters->device ); + log_info("PortAudio: Input device: %s", deviceInfo->name); +#endif + + /* -- setup output stream -- */ printf("PortAudio: Open stream\n"); err = Pa_OpenStream( - &stream, - NULL, // &inputParameters, + &pa_stream, + inputParameters, &outputParameters, sample_rate, 0, @@ -255,9 +285,52 @@ static int portaudio_initialize(int sample_rate){ } memset(pa_output_ring_buffer_storage, 0, sizeof(pa_output_ring_buffer_storage)); btstack_ring_buffer_init(&pa_output_ring_buffer, pa_output_ring_buffer_storage, sizeof(pa_output_ring_buffer_storage)); - pa_output_started = 0; +#ifdef USE_PORTAUDIO_INPUT + memset(pa_input_ring_buffer_storage, 0, sizeof(pa_input_ring_buffer_storage)); + btstack_ring_buffer_init(&pa_input_ring_buffer, pa_input_ring_buffer_storage, sizeof(pa_input_ring_buffer_storage)); + printf("PortAudio: Input buffer size %u\n", btstack_ring_buffer_bytes_free(&pa_input_ring_buffer)); +#endif + + /* -- start stream -- */ + err = Pa_StartStream(pa_stream); + if (err != paNoError){ + printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); + return 0; + } + pa_output_started = 1; + pa_output_paused = 1; +#ifdef USE_PORTAUDIO_INPUT + pa_input_started = 1; + pa_input_paused = 1; +#endif + return 1; } + +static void portaudio_terminate(void){ + if (!pa_stream) return; + + PaError err; + printf("PortAudio: Stop Stream\n"); + err = Pa_StopStream(pa_stream); + if (err != paNoError){ + printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); + return; + } + printf("PortAudio: Close Stream\n"); + err = Pa_CloseStream(pa_stream); + if (err != paNoError){ + printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); + return; + } + pa_stream = NULL; + printf("PortAudio: Terminate\n"); + err = Pa_Terminate(); + if (err != paNoError){ + printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); + return; + } +} #endif @@ -267,7 +340,6 @@ static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, i // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); #ifdef HAVE_PORTAUDIO - portaudio_start(); btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); #else UNUSED(num_channels); @@ -294,12 +366,15 @@ static void sco_demo_init_mSBC(void){ num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; hfp_msbc_init(); +#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE sco_demo_fill_audio_frame(); +#endif #ifdef SCO_MSBC_IN_FILENAME msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); #endif + #ifdef SCO_MSBC_OUT_FILENAME msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); @@ -357,50 +432,26 @@ static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ sco_demo_close(); } #ifdef USE_PORTAUDIO - portaudio_start(); btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); #endif } -#endif -#endif - void sco_demo_close(void){ printf("SCO demo close\n"); -#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE +#if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) + #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) wav_writer_close(); +#endif printf("SCO demo statistics: "); if (negotiated_codec == HFP_CODEC_MSBC){ printf("Used mSBC with PLC, number of processed frames: \n - %d good frames, \n - %d zero frames, \n - %d bad frames.", decoder_state.good_frames_nr, decoder_state.zero_frames_nr, decoder_state.bad_frames_nr); } else { printf("Used CVSD with PLC, number of proccesed frames: \n - %d good frames, \n - %d bad frames.", cvsd_plc_state.good_frames_nr, cvsd_plc_state.bad_frames_nr); } -#endif #ifdef HAVE_PORTAUDIO - if (pa_output_started){ - printf("PortAudio: Stop Stream\n"); - PaError err = Pa_StopStream(pa_output_stream); - if (err != paNoError){ - printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - pa_output_started = 0; - printf("PortAudio: Close Stream\n"); - err = Pa_CloseStream(pa_output_stream); - if (err != paNoError){ - printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - - printf("PortAudio: Terminate\n"); - err = Pa_Terminate(); - if (err != paNoError){ - printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - } + portaudio_terminate(); #endif #ifdef SCO_WAV_FILENAME @@ -425,7 +476,7 @@ void sco_demo_close(void){ void sco_demo_set_codec(uint8_t codec){ if (negotiated_codec == codec) return; negotiated_codec = codec; -#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE +#if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) if (negotiated_codec == HFP_CODEC_MSBC){ sco_demo_init_mSBC(); @@ -437,8 +488,10 @@ void sco_demo_set_codec(uint8_t codec){ } void sco_demo_init(void){ - // status +#if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE + printf("SCO Demo: Sending and receiving audio via portaudio.\n"); +#endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE #ifdef HAVE_PORTAUDIO printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); @@ -453,7 +506,7 @@ void sco_demo_init(void){ printf("SCO Demo: Sending counter value, hexdump received data.\n"); #endif -#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE +#if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD #else hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent @@ -480,7 +533,6 @@ void sco_demo_send(hci_con_handle_t sco_handle){ uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE if (negotiated_codec == HFP_CODEC_MSBC){ - // overwrite sco_payload_length = 24; sco_packet_length = sco_payload_length + 3; @@ -500,6 +552,70 @@ void sco_demo_send(hci_con_handle_t sco_handle){ sco_demo_sine_wave_int16_at_8000_hz(audio_samples_per_packet, (int16_t *) (sco_packet+3)); } #endif + +#if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE + +#ifdef HAVE_PORTAUDIO + if (negotiated_codec == HFP_CODEC_MSBC){ +#if 1 + log_error("Microphone not supported with mSBC yet"); +#else + // MSBC + + // overwrite + sco_payload_length = 24; + sco_packet_length = sco_payload_length + 3; + + if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ + log_error("mSBC stream is empty."); + } + hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); + if (msbc_file_out){ + // log outgoing mSBC data for testing + fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); + } + + // TODO: + sco_demo_fill_audio_frame(); +#endif + } else { + // CVSD + + log_info("send: bytes avail %u, free %u, counter %u", btstack_ring_buffer_bytes_available(&pa_input_ring_buffer), btstack_ring_buffer_bytes_free(&pa_input_ring_buffer), pa_input_counter); + // fill with silence while paused + int bytes_to_copy = sco_payload_length; + if (pa_input_paused){ + if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ + // resume sending + pa_input_paused = 0; + } + } + + // get data from ringbuffer + uint16_t pos = 0; + if (!pa_input_paused){ + uint32_t bytes_read = 0; + btstack_ring_buffer_read(&pa_input_ring_buffer, sco_packet + 3, bytes_to_copy, &bytes_read); + bytes_to_copy -= bytes_read; + pos += bytes_read; + } + + // fill with 0 if not enough + if (bytes_to_copy){ + memset(sco_packet + 3 + pos, 0, bytes_to_copy); + pa_input_paused = 1; + } + } +#else + // just send '0's + if (negotiated_codec == HFP_CODEC_MSBC){ + sco_payload_length = 24; + sco_packet_length = sco_payload_length + 3; + } + memset(sco_packet + 3, 0, sco_payload_length); +#endif +#endif + #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII memset(&sco_packet[3], phase++, sco_payload_length); if (phase > 'z') phase = 'a'; @@ -527,6 +643,9 @@ void sco_demo_send(hci_con_handle_t sco_handle){ (void) phase; #endif + // test silence + // memset(sco_packet+3, 0, sco_payload_length); + // set handle + flags little_endian_store_16(sco_packet, 0, sco_handle); // set len @@ -574,20 +693,18 @@ void sco_demo_receive(uint8_t * packet, uint16_t size){ packets = 0; } -#if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE -#ifdef SCO_WAV_FILENAME +#if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) switch (negotiated_codec){ case HFP_CODEC_MSBC: - sco_demo_receive_mSBC(packet, size); + sco_demo_receive_mSBC(packet, size); break; case HFP_CODEC_CVSD: - sco_demo_receive_CVSD(packet, size); + sco_demo_receive_CVSD(packet, size); break; default: break; } dump_data = 0; -#endif #endif if (packet[1] & 0x30){ From b025eb5fa2115abdeda723210cb8c448c7e00010 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Wed, 25 Jan 2017 17:23:55 +0100 Subject: [PATCH 4/6] sco_util: support mSBC with PortAudio input --- example/sco_demo_util.c | 47 +++++++++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/example/sco_demo_util.c b/example/sco_demo_util.c index e0fa32c06..59f5db21e 100644 --- a/example/sco_demo_util.c +++ b/example/sco_demo_util.c @@ -163,7 +163,7 @@ static void sco_demo_sine_wave_int16_at_8000_hz(int num_samples, int16_t * data) } } -static void sco_demo_fill_audio_frame(void){ +static void sco_demo_msbc_fill_sine_audio_frame(void){ if (!hfp_msbc_can_encode_audio_frame_now()) return; int num_samples = hfp_msbc_num_audio_samples_per_frame(); int16_t sample_buffer[num_samples]; @@ -367,7 +367,7 @@ static void sco_demo_init_mSBC(void){ hfp_msbc_init(); #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE - sco_demo_fill_audio_frame(); + sco_demo_msbc_fill_sine_audio_frame(); #endif #ifdef SCO_MSBC_IN_FILENAME @@ -546,7 +546,7 @@ void sco_demo_send(hci_con_handle_t sco_handle){ fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); } - sco_demo_fill_audio_frame(); + sco_demo_msbc_fill_sine_audio_frame(); } else { const int audio_samples_per_packet = sco_payload_length / CVSD_BYTES_PER_FRAME; sco_demo_sine_wave_int16_at_8000_hz(audio_samples_per_packet, (int16_t *) (sco_packet+3)); @@ -557,27 +557,42 @@ void sco_demo_send(hci_con_handle_t sco_handle){ #ifdef HAVE_PORTAUDIO if (negotiated_codec == HFP_CODEC_MSBC){ -#if 1 - log_error("Microphone not supported with mSBC yet"); -#else // MSBC // overwrite sco_payload_length = 24; sco_packet_length = sco_payload_length + 3; - if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ - log_error("mSBC stream is empty."); - } - hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); - if (msbc_file_out){ - // log outgoing mSBC data for testing - fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); + if (pa_input_paused){ + if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ + // resume sending + pa_input_paused = 0; + } + } + + if (!pa_input_paused){ + int num_samples = hfp_msbc_num_audio_samples_per_frame(); + if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= (num_samples * MSBC_BYTES_PER_FRAME)){ + int16_t sample_buffer[num_samples]; + uint32_t bytes_read; + btstack_ring_buffer_read(&pa_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * MSBC_BYTES_PER_FRAME, &bytes_read); + hfp_msbc_encode_audio_frame(sample_buffer); + num_audio_frames++; + } + } + + if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ + log_error("mSBC stream should not be empty."); + memset(sco_packet + 3, 0, sco_payload_length); + pa_input_paused = 1; + } else { + hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); + if (msbc_file_out){ + // log outgoing mSBC data for testing + fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); + } } - // TODO: - sco_demo_fill_audio_frame(); -#endif } else { // CVSD From 2c36c7916996e318bb90a89f2d4cf0bae1286925 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Wed, 25 Jan 2017 21:13:50 +0100 Subject: [PATCH 5/6] Ring Buffer: speed up by using memcpy --- src/btstack_ring_buffer.c | 68 ++++++++++++++++++++++++++------------- 1 file changed, 46 insertions(+), 22 deletions(-) diff --git a/src/btstack_ring_buffer.c b/src/btstack_ring_buffer.c index 8663c784a..1c78f6cde 100644 --- a/src/btstack_ring_buffer.c +++ b/src/btstack_ring_buffer.c @@ -42,8 +42,10 @@ #include #include +#include #include "btstack_ring_buffer.h" +#include "btstack_util.h" #define ERROR_CODE_MEMORY_CAPACITY_EXCEEDED 0x07 @@ -79,15 +81,27 @@ int btstack_ring_buffer_write(btstack_ring_buffer_t * ring_buffer, uint8_t * dat if (btstack_ring_buffer_bytes_free(ring_buffer) < data_length){ return ERROR_CODE_MEMORY_CAPACITY_EXCEEDED; } - int count = 0; - while (count < data_length){ - if (ring_buffer->last_written_index < ring_buffer->size - 1){ - ring_buffer->last_written_index++; - } else { - ring_buffer->last_written_index = 0; - } - ring_buffer->storage[ring_buffer->last_written_index] = data[count++]; + + // copy first chunk + unsigned int bytes_until_end = ring_buffer->size - ring_buffer->last_written_index; + unsigned int bytes_to_copy = btstack_min(bytes_until_end, data_length); + memcpy(&ring_buffer->storage[ring_buffer->last_written_index], data, bytes_to_copy); + data_length -= bytes_to_copy; + data += bytes_to_copy; + + // update last written index + ring_buffer->last_written_index += bytes_to_copy; + if (ring_buffer->last_written_index == ring_buffer->size){ + ring_buffer->last_written_index = 0; } + + // copy second chunk + if (data_length) { + memcpy(&ring_buffer->storage[0], data, data_length); + ring_buffer->last_written_index += data_length; + } + + // mark buffer as full if (ring_buffer->last_written_index == ring_buffer->last_read_index){ ring_buffer->full = 1; } @@ -96,20 +110,30 @@ int btstack_ring_buffer_write(btstack_ring_buffer_t * ring_buffer, uint8_t * dat // fetch data_length bytes from ring buffer void btstack_ring_buffer_read(btstack_ring_buffer_t * ring_buffer, uint8_t * data, uint32_t data_length, uint32_t * number_of_bytes_read){ - uint32_t count = 0; - while (count < data_length && btstack_ring_buffer_bytes_available(ring_buffer)){ - if (ring_buffer->last_read_index < ring_buffer->last_written_index ) { - ring_buffer->last_read_index++; - } else { - if (ring_buffer->last_read_index < ring_buffer->size - 1){ - ring_buffer->last_read_index++; - } else { - ring_buffer->last_read_index = 0; - } - } - ring_buffer->full = 0; - data[count++] = ring_buffer->storage[ring_buffer->last_read_index]; + // limit data to get and report + data_length = btstack_min(data_length, btstack_ring_buffer_bytes_available(ring_buffer)); + *number_of_bytes_read = data_length; + + // copy first chunk + unsigned int bytes_until_end = ring_buffer->size - ring_buffer->last_read_index; + unsigned int bytes_to_copy = btstack_min(bytes_until_end, data_length); + memcpy(data, &ring_buffer->storage[ring_buffer->last_read_index], bytes_to_copy); + data_length -= bytes_to_copy; + data += bytes_to_copy; + + // update last read index + ring_buffer->last_read_index += bytes_to_copy; + if (ring_buffer->last_read_index == ring_buffer->size){ + ring_buffer->last_read_index = 0; } - *number_of_bytes_read = count; + + // copy second chunk + if (data_length) { + memcpy(data, &ring_buffer->storage[0], data_length); + ring_buffer->last_read_index += data_length; + } + + // clear full flag + ring_buffer->full = 0; } From d43fe610a00629e57536be68513fb5cf1c8d3a85 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Thu, 26 Jan 2017 17:44:52 +0100 Subject: [PATCH 6/6] test/ringbuffer: update test --- test/ring_buffer/btstack_ring_buffer_test.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/test/ring_buffer/btstack_ring_buffer_test.c b/test/ring_buffer/btstack_ring_buffer_test.c index eee95e6ca..1074eaeb7 100644 --- a/test/ring_buffer/btstack_ring_buffer_test.c +++ b/test/ring_buffer/btstack_ring_buffer_test.c @@ -1,9 +1,14 @@ #include "CppUTest/TestHarness.h" #include "CppUTest/CommandLineTestRunner.h" #include "btstack_ring_buffer.h" +#include "btstack_util.h" static uint8_t storage[10]; +uint32_t btstack_min(uint32_t a, uint32_t b){ + return a < b ? a : b; +} + TEST_GROUP(RingBuffer){ btstack_ring_buffer_t ring_buffer; int storage_size;