/* * Copyright (C) 2016 BlueKitchen GmbH * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * 3. Neither the name of the copyright holders nor the names of * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * 4. Any redistribution, use, or modification is done solely for * personal benefit and not for any commercial purpose or for * monetary gain. * * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * Please inquire about commercial licensing options at * contact@bluekitchen-gmbh.com * */ /* * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo */ #include #include "sco_demo_util.h" #include "btstack_debug.h" #include "classic/btstack_sbc.h" #include "classic/btstack_cvsd_plc.h" #include "classic/hfp_msbc.h" #include "classic/hfp.h" #ifdef HAVE_POSIX_FILE_IO #include "wav_util.h" #endif // configure test mode #define SCO_DEMO_MODE_SINE 0 #define SCO_DEMO_MODE_ASCII 1 #define SCO_DEMO_MODE_COUNTER 2 // SCO demo configuration #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE #define SCO_REPORT_PERIOD 100 #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" #define SCO_WAV_DURATION_IN_SECONDS 15 #endif #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) #define USE_PORTAUDIO #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 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; static int num_audio_frames = 0; 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[] = { 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, 31163, 30466, 29648, 28714, 27666, 26509, 25247, 23886, 22431, 20886, 19260, 17557, 15786, 13952, 12062, 10126, 8149, 6140, 4107, 2057, 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, -31163, -30466, -29648, -28714, -27666, -26509, -25247, -23886, -22431, -20886, -19260, -17557, -15786, -13952, -12062, -10126, -8149, -6140, -4107, -2057, }; static int phase = 0; 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){ int i; for (i=0; i < num_samples; i++){ data[i] = sine_int16[phase++]; if (phase >= (sizeof(sine_int16) / sizeof(int16_t))){ phase = 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); 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 USE_PORTAUDIO static int patestCallback( const void *inputBuffer, void *outputBuffer, unsigned long framesPerBuffer, const PaStreamCallbackTimeInfo* timeInfo, PaStreamCallbackFlags statusFlags, void *userData ) { (void) timeInfo; /* Prevent unused variable warnings. */ (void) statusFlags; (void) inputBuffer; 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; } 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); } // 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){ // 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){ /* -- 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 *)data, num_samples*num_channels*2); // printf("bytes avail after write: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); #endif if (!num_samples_to_write) return; num_samples = btstack_min(num_samples, num_samples_to_write); num_samples_to_write -= num_samples; wav_writer_write_int16(num_samples, data); if (num_samples_to_write == 0){ sco_demo_close(); } } static void sco_demo_init_mSBC(void){ int sample_rate = 16000; wav_writer_open(SCO_WAV_FILENAME, 1, 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; hfp_msbc_init(); sco_demo_fill_audio_frame(); #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); #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; #endif } static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ if (num_samples_to_write){ if (msbc_file_in){ // log incoming mSBC data for testing fwrite(packet+3, size-3, 1, msbc_file_in); } } btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); } static void sco_demo_init_CVSD(void){ int sample_rate = 8000; wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); btstack_cvsd_plc_init(&cvsd_plc_state); num_samples_to_write = 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; #endif } static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ if (!num_samples_to_write) return; const int num_samples = size - 3; const int samples_to_write = btstack_min(num_samples, num_samples_to_write); int8_t audio_frame_out[24]; // memcpy(audio_frame_out, (int8_t*)(packet+3), 24); btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); wav_writer_write_int8(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); #endif } #endif #endif void sco_demo_close(void){ #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) wav_writer_close(); 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 #endif #ifdef HAVE_PORTAUDIO if (pa_stream_started){ PaError err = Pa_StopStream(stream); if (err != paNoError){ printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); return; } pa_stream_started = 0; err = Pa_CloseStream(stream); if (err != paNoError){ printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); return; } err = Pa_Terminate(); if (err != paNoError){ printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); return; } } #endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE #ifdef SCO_WAV_FILENAME #if 0 printf("SCO Demo: closing wav file\n"); if (negotiated_codec == HFP_CODEC_MSBC){ wav_writer_state_t * writer_state = &wav_writer_state; if (!writer_state->wav_file) return; rewind(writer_state->wav_file); write_wav_header(writer_state->wav_file, writer_state->total_num_samples, btstack_sbc_decoder_num_channels(&decoder_state), btstack_sbc_decoder_sample_rate(&decoder_state),2); fclose(writer_state->wav_file); writer_state->wav_file = NULL; } #endif #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 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) if (negotiated_codec == HFP_CODEC_MSBC){ sco_demo_init_mSBC(); } else { sco_demo_init_CVSD(); } #endif #endif } void sco_demo_init(void){ // status #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE #ifdef HAVE_PORTAUDIO printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); #else printf("SCO Demo: Sending sine wave, hexdump received data.\n"); #endif #endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII printf("SCO Demo: Sending ASCII blocks, print received data.\n"); #endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER printf("SCO Demo: Sending counter value, hexdump received data.\n"); #endif #if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent #endif #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII phase = 'a'; #endif } static void sco_report(void){ printf("SCO: sent %u, received %u\n", count_sent, count_received); } 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; 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){ 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); } sco_demo_fill_audio_frame(); } else { sco_demo_sine_wave_int8(audio_samples_per_packet, (int8_t *) (sco_packet+3)); } #else #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII memset(&sco_packet[3], phase++, audio_samples_per_packet); if (phase > 'z') phase = 'a'; #else int j; for (j=0;j> 4); log_info("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); printf_hexdump(&packet[3], size-3); return; } if (dump_data){ printf("data: "); #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII int i; for (i=3;i