diff --git a/example/sco_demo_util.c b/example/sco_demo_util.c index f1d2a38cd..656a7c07f 100644 --- a/example/sco_demo_util.c +++ b/example/sco_demo_util.c @@ -39,7 +39,6 @@ * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo */ - #include #include "sco_demo_util.h" @@ -53,86 +52,76 @@ #include "wav_util.h" #endif -// configure test mode +#ifdef HAVE_PORTAUDIO +#include +#include "btstack_ring_buffer.h" +#endif + + +// 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 - // SCO demo configuration -#define SCO_DEMO_MODE SCO_DEMO_MODE_SINE -#define SCO_REPORT_PERIOD 100 +#define SCO_DEMO_MODE SCO_DEMO_MODE_SINE -#ifdef HAVE_POSIX_FILE_IO -#define SCO_WAV_FILENAME "sco_input.wav" -#define SCO_MSBC_OUT_FILENAME "sco_output.msbc" -#define SCO_MSBC_IN_FILENAME "sco_input.msbc" +// number of sco packets until 'report' on console +#define SCO_REPORT_PERIOD 100 +// length and name of wav file on disc #define SCO_WAV_DURATION_IN_SECONDS 15 -#endif +#define SCO_WAV_FILENAME "sco_input.wav" +// name of sbc test files +#define SCO_MSBC_OUT_FILENAME "sco_output.msbc" +#define SCO_MSBC_IN_FILENAME "sco_input.msbc" + +// pre-buffer for CVSD and mSBC - also defines latency +#define SCO_CVSD_PA_PREBUFFER_MS 50 +#define SCO_MSBC_PA_PREBUFFER_MS 50 + +// constants +#define NUM_CHANNELS 1 +#define CVSD_BYTES_PER_FRAME (2*NUM_CHANNELS) +#define CVSD_SAMPLE_RATE 8000 +#define MSBC_SAMPLE_RATE 16000 +#define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) #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 -#include -#include "btstack_ring_buffer.h" - -// portaudio config -#define NUM_CHANNELS 1 - -#define CVSD_SAMPLE_RATE 8000 -#define CVSD_FRAMES_PER_BUFFER 24 -#define CVSD_PA_SAMPLE_TYPE paInt8 -#define CVSD_BYTES_PER_FRAME (1*NUM_CHANNELS) -#define CVSD_PREBUFFER_MS 5 -#define CVSD_PREBUFFER_BYTES (CVSD_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * CVSD_BYTES_PER_FRAME) - -#define MSBC_SAMPLE_RATE 16000 -#define MSBC_FRAMES_PER_BUFFER 120 -#define MSBC_PA_SAMPLE_TYPE paInt16 -#define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) -#define MSBC_PREBUFFER_MS 50 -#define MSBC_PREBUFFER_BYTES (MSBC_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * MSBC_BYTES_PER_FRAME) - -// portaudio globals -static PaStream * stream; -static uint8_t pa_stream_started = 0; - -static uint8_t ring_buffer_storage[2*MSBC_PREBUFFER_BYTES]; +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; #endif static int dump_data = 1; static int count_sent = 0; static int count_received = 0; -static uint8_t negotiated_codec = 0; -#if SCO_DEMO_MODE != SCO_DEMO_MODE_55 +static int negotiated_codec = -1; static int phase = 0; -#endif +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; FILE * msbc_file_in; FILE * msbc_file_out; #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE -// input signal: pre-computed sine wave, at 8000 kz -static const uint8_t sine_uint8[] = { - 0, 15, 31, 46, 61, 74, 86, 97, 107, 114, - 120, 124, 126, 126, 124, 120, 114, 107, 97, 86, - 74, 61, 46, 31, 15, 0, 241, 225, 210, 195, - 182, 170, 159, 149, 142, 136, 132, 130, 130, 132, - 136, 142, 149, 159, 170, 182, 195, 210, 225, 241, -}; - - // input signal: pre-computed sine wave, 160 Hz at 16000 kHz -static const int16_t sine_int16[] = { +static const int16_t sine_int16_at_16000hz[] = { 0, 2057, 4107, 6140, 8149, 10126, 12062, 13952, 15786, 17557, 19260, 20886, 22431, 23886, 25247, 26509, 27666, 28714, 29648, 30466, 31163, 31738, 32187, 32509, 32702, 32767, 32702, 32509, 32187, 31738, @@ -145,41 +134,30 @@ static const int16_t sine_int16[] = { -19260, -17557, -15786, -13952, -12062, -10126, -8149, -6140, -4107, -2057, }; -static void sco_demo_sine_wave_int8(int num_samples, int8_t * data){ - int i; - for (i=0; i= sizeof(sine_uint8)) phase = 0; - } -} - -static void sco_demo_sine_wave_int16(int num_samples, int16_t * data){ +// ony use every second value from 16khz table +static void sco_demo_sine_wave_int16_at_8000_hz(int num_samples, int16_t * data){ int i; for (i=0; i < num_samples; i++){ - data[i] = sine_int16[phase++]; - if (phase >= (sizeof(sine_int16) / sizeof(int16_t))){ + data[i] = sine_int16_at_16000hz[phase++]; + phase++; + if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ phase = 0; } } } -static int num_audio_frames = 0; static void sco_demo_fill_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]; - sco_demo_sine_wave_int16(num_samples, sample_buffer); + sco_demo_sine_wave_int16_at_8000_hz(num_samples, sample_buffer); hfp_msbc_encode_audio_frame(sample_buffer); num_audio_frames++; } -#ifdef SCO_WAV_FILENAME -static btstack_sbc_decoder_state_t decoder_state; -static btstack_cvsd_plc_state_t cvsd_plc_state; -static int num_samples_to_write; +#ifdef SCO_WAV_FILENAME #ifdef USE_PORTAUDIO -static int patestCallback( const void *inputBuffer, void *outputBuffer, +static int portaudio_callback( const void *inputBuffer, void *outputBuffer, unsigned long framesPerBuffer, const PaStreamCallbackTimeInfo* timeInfo, PaStreamCallbackFlags statusFlags, @@ -189,32 +167,50 @@ static int patestCallback( const void *inputBuffer, void *outputBuffer, (void) inputBuffer; (void) userData; - uint32_t bytes_read = 0; - int bytes_per_buffer = framesPerBuffer; - if (negotiated_codec == HFP_CODEC_MSBC){ - bytes_per_buffer *= MSBC_BYTES_PER_FRAME; - } else { - bytes_per_buffer *= CVSD_BYTES_PER_FRAME; + // config based on codec + int bytes_to_copy; + int prebuffer_bytes; + switch (negotiated_codec){ + case HFP_CODEC_MSBC: + bytes_to_copy = framesPerBuffer * MSBC_BYTES_PER_FRAME; + prebuffer_bytes = MSBC_PA_PREBUFFER_BYTES; + break; + case HFP_CODEC_CVSD: + bytes_to_copy = framesPerBuffer * CVSD_BYTES_PER_FRAME; + prebuffer_bytes = CVSD_PA_PREBUFFER_BYTES; + break; + default: + bytes_to_copy = framesPerBuffer * 2; // assume 1 channel / 16 bit audio samples + prebuffer_bytes = 0xfffffff; + break; } - if (btstack_ring_buffer_bytes_available(&ring_buffer) >= bytes_per_buffer){ - btstack_ring_buffer_read(&ring_buffer, outputBuffer, bytes_per_buffer, &bytes_read); - } else { - printf("NOT ENOUGH DATA!\n"); - memset(outputBuffer, 0, bytes_per_buffer); + // fill with silence while paused + if (pa_stream_paused){ + if (btstack_ring_buffer_bytes_available(&ring_buffer) < prebuffer_bytes){ + memset(outputBuffer, 0, bytes_to_copy); + return 0; + } else { + // resume playback + pa_stream_paused = 0; + } + } + + // get data from ringbuffer + uint32_t bytes_read = 0; + btstack_ring_buffer_read(&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; } - // printf("bytes avail after read: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); return 0; } -#endif -static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ - UNUSED(context); - UNUSED(sample_rate); - - // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); -#ifdef USE_PORTAUDIO - if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= MSBC_PREBUFFER_BYTES){ +static void portaudio_start(void){ + if (!pa_stream_started){ /* -- start stream -- */ PaError err = Pa_StartStream(stream); if (err != paNoError){ @@ -222,12 +218,59 @@ static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, i return; } pa_stream_started = 1; + pa_stream_paused = 1; } +} + +// 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 -- */ + 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 -- */ + printf("PortAudio: Open stream\n"); + err = Pa_OpenStream( + &stream, + NULL, // &inputParameters, + &outputParameters, + sample_rate, + 0, + paClipOff, /* we won't output out of range samples so don't bother clipping them */ + portaudio_callback, + NULL ); + if (err != paNoError){ + 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; + return 1; +} +#endif + + +static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ + UNUSED(context); + UNUSED(sample_rate); + + // 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); - // printf("bytes avail after write: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); #else UNUSED(num_channels); -#endif +#endif if (!num_samples_to_write) return; @@ -242,11 +285,12 @@ static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, i } static void sco_demo_init_mSBC(void){ - int sample_rate = 16000; - wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); + printf("SCO Demo: Init mSBC\n"); + + wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); - num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; + num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; hfp_msbc_init(); sco_demo_fill_audio_frame(); @@ -261,35 +305,7 @@ static void sco_demo_init_mSBC(void){ #endif #ifdef USE_PORTAUDIO - PaError err; - PaStreamParameters outputParameters; - - /* -- initialize PortAudio -- */ - err = Pa_Initialize(); - if( err != paNoError ) return; - /* -- setup input and output -- */ - outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ - outputParameters.channelCount = NUM_CHANNELS; - outputParameters.sampleFormat = MSBC_PA_SAMPLE_TYPE; - outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; - outputParameters.hostApiSpecificStreamInfo = NULL; - /* -- setup stream -- */ - err = Pa_OpenStream( - &stream, - NULL, // &inputParameters, - &outputParameters, - MSBC_SAMPLE_RATE, - MSBC_FRAMES_PER_BUFFER, - paClipOff, /* we won't output out of range samples so don't bother clipping them */ - patestCallback, /* no callback, use blocking API */ - NULL ); /* no callback, so no callback userData */ - if (err != paNoError){ - printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - 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; + portaudio_initialize(MSBC_SAMPLE_RATE); #endif } @@ -304,74 +320,44 @@ static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ } static void sco_demo_init_CVSD(void){ - int sample_rate = 8000; - wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); + printf("SCO Demo: Init CVSD\n"); + + wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); btstack_cvsd_plc_init(&cvsd_plc_state); - num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; + + num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; #ifdef USE_PORTAUDIO - PaError err; - PaStreamParameters outputParameters; - - /* -- initialize PortAudio -- */ - err = Pa_Initialize(); - if( err != paNoError ) return; - /* -- setup input and output -- */ - outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ - outputParameters.channelCount = NUM_CHANNELS; - outputParameters.sampleFormat = CVSD_PA_SAMPLE_TYPE; - outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; - outputParameters.hostApiSpecificStreamInfo = NULL; - /* -- setup stream -- */ - err = Pa_OpenStream( - &stream, - NULL, // &inputParameters, - &outputParameters, - CVSD_SAMPLE_RATE, - CVSD_FRAMES_PER_BUFFER, - paClipOff, /* we won't output out of range samples so don't bother clipping them */ - patestCallback, /* no callback, use blocking API */ - NULL ); /* no callback, so no callback userData */ - if (err != paNoError){ - printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - 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; + portaudio_initialize(CVSD_SAMPLE_RATE); #endif } static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ if (!num_samples_to_write) return; - int8_t audio_frame_out[255]; // + int16_t audio_frame_out[255]; // if (size > sizeof(audio_frame_out)){ printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); return; } - - const int num_samples = size - 3; + const int audio_bytes_read = size - 3; + const int num_samples = audio_bytes_read / CVSD_BYTES_PER_FRAME; const int samples_to_write = btstack_min(num_samples, num_samples_to_write); +#if 0 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); +#else + memcpy(audio_frame_out, packet+3, audio_bytes_read); +#endif - wav_writer_write_int8(samples_to_write, audio_frame_out); + wav_writer_write_int16(samples_to_write, audio_frame_out); num_samples_to_write -= samples_to_write; if (num_samples_to_write == 0){ sco_demo_close(); } #ifdef USE_PORTAUDIO - if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= CVSD_PREBUFFER_BYTES){ - /* -- start stream -- */ - PaError err = Pa_StartStream(stream); - if (err != paNoError){ - printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); - return; - } - pa_stream_started = 1; - } - btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, samples_to_write); + portaudio_start(); + btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); #endif } @@ -379,6 +365,7 @@ static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ #endif void sco_demo_close(void){ + printf("SCO demo close\n"); #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) wav_writer_close(); @@ -392,18 +379,21 @@ void sco_demo_close(void){ #ifdef HAVE_PORTAUDIO if (pa_stream_started){ + printf("PortAudio: Stop Stream\n"); PaError err = Pa_StopStream(stream); if (err != paNoError){ printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); return; } pa_stream_started = 0; + printf("PortAudio: Close Stream\n"); err = Pa_CloseStream(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)); @@ -411,8 +401,8 @@ void sco_demo_close(void){ } } #endif + #ifdef SCO_WAV_FILENAME - #if 0 printf("SCO Demo: closing wav file\n"); if (negotiated_codec == HFP_CODEC_MSBC){ @@ -425,6 +415,9 @@ void sco_demo_close(void){ } #endif #endif + + negotiated_codec = -1; + #endif } @@ -459,7 +452,9 @@ 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 + hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD +#else hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent #endif @@ -477,20 +472,18 @@ void sco_demo_send(hci_con_handle_t sco_handle){ if (!sco_handle) return; - const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); - const int sco_payload_length = sco_packet_length - 3; + int sco_packet_length = hci_get_sco_packet_length(); + int sco_payload_length = sco_packet_length - 3; hci_reserve_packet_buffer(); uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); - // set handle + flags - little_endian_store_16(sco_packet, 0, sco_handle); - // set len - sco_packet[2] = sco_payload_length; - const int audio_samples_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 - #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; + if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ log_error("mSBC stream is empty."); } @@ -502,29 +495,30 @@ void sco_demo_send(hci_con_handle_t sco_handle){ sco_demo_fill_audio_frame(); } else { - sco_demo_sine_wave_int8(audio_samples_per_packet, (int8_t *) (sco_packet+3)); + 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)); } #endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII - memset(&sco_packet[3], phase++, audio_samples_per_packet); + memset(&sco_packet[3], phase++, sco_payload_length); if (phase > 'z') phase = 'a'; #endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER int j; - for (j=0;j