1 /* 2 * Copyright (C) 2016 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 #define __BTSTACK_FILE__ "sco_demo_util.c" 39 40 /* 41 * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo 42 */ 43 44 #include <stdio.h> 45 46 #include "sco_demo_util.h" 47 #include "btstack_debug.h" 48 #include "classic/btstack_sbc.h" 49 #include "classic/btstack_cvsd_plc.h" 50 #include "classic/hfp_msbc.h" 51 #include "classic/hfp.h" 52 53 #ifdef HAVE_POSIX_FILE_IO 54 #include "wav_util.h" 55 #endif 56 57 #ifdef HAVE_PORTAUDIO 58 #include <portaudio.h> 59 #include "btstack_ring_buffer.h" 60 #endif 61 62 63 // test modes 64 #define SCO_DEMO_MODE_SINE 0 65 #define SCO_DEMO_MODE_ASCII 1 66 #define SCO_DEMO_MODE_COUNTER 2 67 #define SCO_DEMO_MODE_55 3 68 #define SCO_DEMO_MODE_00 4 69 #define SCO_DEMO_MODE_MICROPHONE 5 70 71 // SCO demo configuration 72 #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE 73 74 // number of sco packets until 'report' on console 75 #define SCO_REPORT_PERIOD 100 76 77 #ifdef HAVE_POSIX_FILE_IO 78 // length and name of wav file on disk 79 #define SCO_WAV_DURATION_IN_SECONDS 15 80 #define SCO_WAV_FILENAME "sco_input.wav" 81 #endif 82 83 // name of sbc test files 84 #define SCO_MSBC_OUT_FILENAME "sco_output.msbc" 85 #define SCO_MSBC_IN_FILENAME "sco_input.msbc" 86 87 // pre-buffer for CVSD and mSBC - also defines latency 88 #define SCO_CVSD_PA_PREBUFFER_MS 50 89 #define SCO_MSBC_PA_PREBUFFER_MS 50 90 91 // constants 92 #define NUM_CHANNELS 1 93 #define CVSD_BYTES_PER_FRAME (2*NUM_CHANNELS) 94 #define CVSD_SAMPLE_RATE 8000 95 #define MSBC_SAMPLE_RATE 16000 96 #define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) 97 98 #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE || SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 99 #define USE_PORTAUDIO 100 #define CVSD_PA_PREBUFFER_BYTES (SCO_CVSD_PA_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * CVSD_BYTES_PER_FRAME) 101 #define MSBC_PA_PREBUFFER_BYTES (SCO_MSBC_PA_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * MSBC_BYTES_PER_FRAME) 102 #endif 103 104 #ifdef USE_PORTAUDIO 105 106 // bidirectional audio stream 107 static PaStream * pa_stream; 108 109 // output 110 static int pa_output_started = 0; 111 static int pa_output_paused = 0; 112 static uint8_t pa_output_ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES]; 113 static btstack_ring_buffer_t pa_output_ring_buffer; 114 115 // input 116 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 117 #define USE_PORTAUDIO_INPUT 118 static int pa_input_started = 0; 119 static int pa_input_paused = 0; 120 static uint8_t pa_input_ring_buffer_storage[2*8000]; // full second input buffer 121 static btstack_ring_buffer_t pa_input_ring_buffer; 122 static int pa_input_counter; 123 #endif 124 125 #endif 126 127 static int dump_data = 1; 128 static int count_sent = 0; 129 static int count_received = 0; 130 static int negotiated_codec = -1; 131 132 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 133 btstack_sbc_decoder_state_t decoder_state; 134 #endif 135 136 btstack_cvsd_plc_state_t cvsd_plc_state; 137 138 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 139 FILE * msbc_file_in; 140 FILE * msbc_file_out; 141 #endif 142 143 int num_samples_to_write; 144 int num_audio_frames; 145 unsigned int phase; 146 147 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 148 149 // input signal: pre-computed sine wave, 160 Hz at 16000 kHz 150 static const int16_t sine_int16_at_16000hz[] = { 151 0, 2057, 4107, 6140, 8149, 10126, 12062, 13952, 15786, 17557, 152 19260, 20886, 22431, 23886, 25247, 26509, 27666, 28714, 29648, 30466, 153 31163, 31738, 32187, 32509, 32702, 32767, 32702, 32509, 32187, 31738, 154 31163, 30466, 29648, 28714, 27666, 26509, 25247, 23886, 22431, 20886, 155 19260, 17557, 15786, 13952, 12062, 10126, 8149, 6140, 4107, 2057, 156 0, -2057, -4107, -6140, -8149, -10126, -12062, -13952, -15786, -17557, 157 -19260, -20886, -22431, -23886, -25247, -26509, -27666, -28714, -29648, -30466, 158 -31163, -31738, -32187, -32509, -32702, -32767, -32702, -32509, -32187, -31738, 159 -31163, -30466, -29648, -28714, -27666, -26509, -25247, -23886, -22431, -20886, 160 -19260, -17557, -15786, -13952, -12062, -10126, -8149, -6140, -4107, -2057, 161 }; 162 163 // 8 kHz samples for CVSD/SCO packets in little endian 164 static void sco_demo_sine_wave_int16_at_8000_hz_little_endian(unsigned int num_samples, uint8_t * data){ 165 unsigned int i; 166 for (i=0; i < num_samples; i++){ 167 int16_t sample = sine_int16_at_16000hz[phase]; 168 little_endian_store_16(data, i * 2, sample); 169 // ony use every second sample from 16khz table to get 8khz 170 phase += 2; 171 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 172 phase = 0; 173 } 174 } 175 } 176 177 // 16 kHz samples for mSBC encoder in host endianess 178 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 179 static void sco_demo_sine_wave_int16_at_16000_hz_host_endian(unsigned int num_samples, int16_t * data){ 180 unsigned int i; 181 for (i=0; i < num_samples; i++){ 182 data[i] = sine_int16_at_16000hz[phase++]; 183 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 184 phase = 0; 185 } 186 } 187 } 188 189 #define MAX_NUM_SAMPLES (16*8) 190 static void sco_demo_msbc_fill_sine_audio_frame(void){ 191 if (!hfp_msbc_can_encode_audio_frame_now()) return; 192 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 193 if (num_samples > MAX_NUM_SAMPLES) return; 194 int16_t sample_buffer[MAX_NUM_SAMPLES]; 195 sco_demo_sine_wave_int16_at_16000_hz_host_endian(num_samples, sample_buffer); 196 hfp_msbc_encode_audio_frame(sample_buffer); 197 num_audio_frames++; 198 } 199 #endif 200 #endif 201 202 #ifdef USE_PORTAUDIO 203 static int portaudio_callback( const void *inputBuffer, void *outputBuffer, 204 unsigned long framesPerBuffer, 205 const PaStreamCallbackTimeInfo* timeInfo, 206 PaStreamCallbackFlags statusFlags, 207 void *userData ) { 208 (void) timeInfo; /* Prevent unused variable warnings. */ 209 (void) statusFlags; 210 (void) inputBuffer; 211 (void) userData; 212 213 // output part 214 215 // config based on codec 216 int bytes_to_copy; 217 uint32_t prebuffer_bytes; 218 switch (negotiated_codec){ 219 case HFP_CODEC_MSBC: 220 bytes_to_copy = framesPerBuffer * MSBC_BYTES_PER_FRAME; 221 prebuffer_bytes = MSBC_PA_PREBUFFER_BYTES; 222 break; 223 case HFP_CODEC_CVSD: 224 bytes_to_copy = framesPerBuffer * CVSD_BYTES_PER_FRAME; 225 prebuffer_bytes = CVSD_PA_PREBUFFER_BYTES; 226 break; 227 default: 228 bytes_to_copy = framesPerBuffer * 2; // assume 1 channel / 16 bit audio samples 229 prebuffer_bytes = 0xfffffff; 230 break; 231 } 232 233 // fill with silence while paused 234 if (pa_output_paused){ 235 if (btstack_ring_buffer_bytes_available(&pa_output_ring_buffer) < prebuffer_bytes){ 236 memset(outputBuffer, 0, bytes_to_copy); 237 return 0; 238 } else { 239 // resume playback 240 pa_output_paused = 0; 241 } 242 } 243 244 // get data from ringbuffer 245 uint32_t bytes_read = 0; 246 btstack_ring_buffer_read(&pa_output_ring_buffer, outputBuffer, bytes_to_copy, &bytes_read); 247 bytes_to_copy -= bytes_read; 248 249 // fill with 0 if not enough 250 if (bytes_to_copy){ 251 memset(outputBuffer + bytes_read, 0, bytes_to_copy); 252 pa_output_paused = 1; 253 } 254 // end of output part 255 256 // input part -- just store in ring buffer 257 #ifdef USE_PORTAUDIO_INPUT 258 btstack_ring_buffer_write(&pa_input_ring_buffer, (uint8_t *)inputBuffer, framesPerBuffer * 2); 259 pa_input_counter += framesPerBuffer * 2; 260 #endif 261 return 0; 262 } 263 264 // return 1 if ok 265 static int portaudio_initialize(int sample_rate){ 266 PaError err; 267 268 /* -- initialize PortAudio -- */ 269 printf("PortAudio: Initialize\n"); 270 err = Pa_Initialize(); 271 if( err != paNoError ) return 0; 272 273 /* -- setup input and output -- */ 274 const PaDeviceInfo *deviceInfo; 275 PaStreamParameters * inputParameters = NULL; 276 PaStreamParameters outputParameters; 277 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 278 outputParameters.channelCount = NUM_CHANNELS; 279 outputParameters.sampleFormat = paInt16; 280 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 281 outputParameters.hostApiSpecificStreamInfo = NULL; 282 deviceInfo = Pa_GetDeviceInfo( outputParameters.device ); 283 log_info("PortAudio: Output device: %s", deviceInfo->name); 284 #ifdef USE_PORTAUDIO_INPUT 285 PaStreamParameters theInputParameters; 286 theInputParameters.device = Pa_GetDefaultInputDevice(); /* default input device */ 287 theInputParameters.channelCount = NUM_CHANNELS; 288 theInputParameters.sampleFormat = paInt16; 289 theInputParameters.suggestedLatency = Pa_GetDeviceInfo( theInputParameters.device )->defaultHighOutputLatency; 290 theInputParameters.hostApiSpecificStreamInfo = NULL; 291 inputParameters = &theInputParameters; 292 deviceInfo = Pa_GetDeviceInfo( inputParameters->device ); 293 log_info("PortAudio: Input device: %s", deviceInfo->name); 294 #endif 295 296 /* -- setup output stream -- */ 297 printf("PortAudio: Open stream\n"); 298 err = Pa_OpenStream( 299 &pa_stream, 300 inputParameters, 301 &outputParameters, 302 sample_rate, 303 0, 304 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 305 portaudio_callback, 306 NULL ); 307 if (err != paNoError){ 308 printf("Error opening portaudio stream: \"%s\"\n", Pa_GetErrorText(err)); 309 return 0; 310 } 311 memset(pa_output_ring_buffer_storage, 0, sizeof(pa_output_ring_buffer_storage)); 312 btstack_ring_buffer_init(&pa_output_ring_buffer, pa_output_ring_buffer_storage, sizeof(pa_output_ring_buffer_storage)); 313 #ifdef USE_PORTAUDIO_INPUT 314 memset(pa_input_ring_buffer_storage, 0, sizeof(pa_input_ring_buffer_storage)); 315 btstack_ring_buffer_init(&pa_input_ring_buffer, pa_input_ring_buffer_storage, sizeof(pa_input_ring_buffer_storage)); 316 printf("PortAudio: Input buffer size %u\n", btstack_ring_buffer_bytes_free(&pa_input_ring_buffer)); 317 #endif 318 319 /* -- start stream -- */ 320 err = Pa_StartStream(pa_stream); 321 if (err != paNoError){ 322 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 323 return 0; 324 } 325 pa_output_started = 1; 326 pa_output_paused = 1; 327 #ifdef USE_PORTAUDIO_INPUT 328 pa_input_started = 1; 329 pa_input_paused = 1; 330 #endif 331 332 return 1; 333 } 334 335 static void portaudio_terminate(void){ 336 if (!pa_stream) return; 337 338 PaError err; 339 printf("PortAudio: Stop Stream\n"); 340 err = Pa_StopStream(pa_stream); 341 if (err != paNoError){ 342 printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); 343 return; 344 } 345 printf("PortAudio: Close Stream\n"); 346 err = Pa_CloseStream(pa_stream); 347 if (err != paNoError){ 348 printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); 349 return; 350 } 351 pa_stream = NULL; 352 printf("PortAudio: Terminate\n"); 353 err = Pa_Terminate(); 354 if (err != paNoError){ 355 printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); 356 return; 357 } 358 } 359 #endif 360 361 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 362 363 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 364 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 365 UNUSED(context); 366 UNUSED(sample_rate); 367 UNUSED(data); 368 UNUSED(num_samples); 369 UNUSED(num_channels); 370 371 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 372 373 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 374 #ifdef HAVE_PORTAUDIO 375 // samples in callback in host endianess, ready for PortAudio playback 376 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 377 #endif /* HAVE_PORTAUDIO */ 378 379 #ifdef SCO_WAV_FILENAME 380 if (!num_samples_to_write) return; 381 num_samples = btstack_min(num_samples, num_samples_to_write); 382 num_samples_to_write -= num_samples; 383 wav_writer_write_int16(num_samples, data); 384 if (num_samples_to_write == 0){ 385 wav_writer_close(); 386 } 387 #endif /* SCO_WAV_FILENAME */ 388 389 #endif /* Demo mode sine or microphone */ 390 } 391 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */ 392 393 394 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 395 396 static void sco_demo_init_mSBC(void){ 397 printf("SCO Demo: Init mSBC\n"); 398 399 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 400 hfp_msbc_init(); 401 402 #ifdef SCO_WAV_FILENAME 403 num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 404 wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); 405 #endif 406 407 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 408 sco_demo_msbc_fill_sine_audio_frame(); 409 #endif 410 411 #ifdef SCO_MSBC_IN_FILENAME 412 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 413 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 414 #endif 415 416 #ifdef SCO_MSBC_OUT_FILENAME 417 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 418 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 419 #endif 420 421 #ifdef USE_PORTAUDIO 422 portaudio_initialize(MSBC_SAMPLE_RATE); 423 #endif 424 } 425 426 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 427 if (num_samples_to_write){ 428 if (msbc_file_in){ 429 // log incoming mSBC data for testing 430 fwrite(packet+3, size-3, 1, msbc_file_in); 431 } 432 } 433 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 434 } 435 #endif 436 437 static void sco_demo_init_CVSD(void){ 438 printf("SCO Demo: Init CVSD\n"); 439 440 #if defined(SCO_WAV_FILENAME) || defined(USE_PORTAUDIO) 441 btstack_cvsd_plc_init(&cvsd_plc_state); 442 #endif 443 444 #ifdef SCO_WAV_FILENAME 445 num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 446 wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); 447 #endif 448 449 #ifdef USE_PORTAUDIO 450 portaudio_initialize(CVSD_SAMPLE_RATE); 451 #endif 452 } 453 454 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 455 if (!num_samples_to_write) return; 456 457 int16_t audio_frame_out[128]; // 458 459 if (size > sizeof(audio_frame_out)){ 460 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 461 return; 462 } 463 464 #if defined(SCO_WAV_FILENAME) || defined(USE_PORTAUDIO) 465 const int audio_bytes_read = size - 3; 466 const int num_samples = audio_bytes_read / CVSD_BYTES_PER_FRAME; 467 468 // convert into host endian 469 int16_t audio_frame_in[128]; 470 int i; 471 for (i=0;i<num_samples;i++){ 472 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2); 473 } 474 475 btstack_cvsd_plc_process_data(&cvsd_plc_state, audio_frame_in, num_samples, audio_frame_out); 476 #endif 477 478 #ifdef SCO_WAV_FILENAME 479 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 480 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 481 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 482 num_samples_to_write -= samples_to_write; 483 if (num_samples_to_write == 0){ 484 wav_writer_close(); 485 } 486 #endif 487 488 #ifdef USE_PORTAUDIO 489 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 490 #endif 491 } 492 493 #endif 494 495 496 void sco_demo_close(void){ 497 printf("SCO demo close\n"); 498 499 printf("SCO demo statistics: "); 500 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 501 if (negotiated_codec == HFP_CODEC_MSBC){ 502 printf("Used mSBC with PLC, number of processed frames: \n - %d good frames, \n - %d zero frames, \n - %d bad frames.\n", decoder_state.good_frames_nr, decoder_state.zero_frames_nr, decoder_state.bad_frames_nr); 503 } else 504 #endif 505 { 506 printf("Used CVSD with PLC, number of proccesed frames: \n - %d good frames, \n - %d bad frames.\n", cvsd_plc_state.good_frames_nr, cvsd_plc_state.bad_frames_nr); 507 } 508 509 negotiated_codec = -1; 510 511 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 512 513 #if defined(SCO_WAV_FILENAME) 514 wav_writer_close(); 515 #endif 516 517 #ifdef HAVE_PORTAUDIO 518 portaudio_terminate(); 519 #endif 520 521 #endif 522 } 523 524 void sco_demo_set_codec(uint8_t codec){ 525 if (negotiated_codec == codec) return; 526 negotiated_codec = codec; 527 528 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 529 if (negotiated_codec == HFP_CODEC_MSBC){ 530 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 531 sco_demo_init_mSBC(); 532 #endif 533 } else { 534 sco_demo_init_CVSD(); 535 } 536 #endif 537 } 538 539 void sco_demo_init(void){ 540 // status 541 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 542 printf("SCO Demo: Sending and receiving audio via portaudio.\n"); 543 #endif 544 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 545 #ifdef HAVE_PORTAUDIO 546 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 547 #else 548 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 549 #endif 550 #endif 551 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 552 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 553 #endif 554 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 555 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 556 #endif 557 558 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 559 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 560 #else 561 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 562 #endif 563 564 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 565 phase = 'a'; 566 #endif 567 } 568 569 void sco_report(void); 570 void sco_report(void){ 571 printf("SCO: sent %u, received %u\n", count_sent, count_received); 572 } 573 574 void sco_demo_send(hci_con_handle_t sco_handle){ 575 576 if (!sco_handle) return; 577 578 int sco_packet_length = hci_get_sco_packet_length(); 579 int sco_payload_length = sco_packet_length - 3; 580 581 hci_reserve_packet_buffer(); 582 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 583 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 584 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 585 if (negotiated_codec == HFP_CODEC_MSBC){ 586 // overwrite 587 sco_payload_length = 24; 588 sco_packet_length = sco_payload_length + 3; 589 590 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 591 log_error("mSBC stream is empty."); 592 } 593 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 594 if (msbc_file_out){ 595 // log outgoing mSBC data for testing 596 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 597 } 598 599 sco_demo_msbc_fill_sine_audio_frame(); 600 } else 601 #endif 602 { 603 const int audio_samples_per_packet = sco_payload_length / CVSD_BYTES_PER_FRAME; 604 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]); 605 } 606 #endif 607 608 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 609 610 #ifdef HAVE_PORTAUDIO 611 if (negotiated_codec == HFP_CODEC_MSBC){ 612 // MSBC 613 614 // overwrite 615 sco_payload_length = 24; 616 sco_packet_length = sco_payload_length + 3; 617 618 if (pa_input_paused){ 619 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 620 // resume sending 621 pa_input_paused = 0; 622 } 623 } 624 625 if (!pa_input_paused){ 626 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 627 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= (num_samples * MSBC_BYTES_PER_FRAME)){ 628 int16_t sample_buffer[num_samples]; 629 uint32_t bytes_read; 630 btstack_ring_buffer_read(&pa_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * MSBC_BYTES_PER_FRAME, &bytes_read); 631 hfp_msbc_encode_audio_frame(sample_buffer); 632 num_audio_frames++; 633 } 634 } 635 636 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 637 log_error("mSBC stream should not be empty."); 638 memset(sco_packet + 3, 0, sco_payload_length); 639 pa_input_paused = 1; 640 } else { 641 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 642 if (msbc_file_out){ 643 // log outgoing mSBC data for testing 644 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 645 } 646 } 647 648 } else { 649 // CVSD 650 651 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); 652 // fill with silence while paused 653 int bytes_to_copy = sco_payload_length; 654 if (pa_input_paused){ 655 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 656 // resume sending 657 pa_input_paused = 0; 658 } 659 } 660 661 // get data from ringbuffer 662 uint16_t pos = 0; 663 uint8_t * sample_data = &sco_packet[3]; 664 if (!pa_input_paused){ 665 uint32_t bytes_read = 0; 666 btstack_ring_buffer_read(&pa_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read); 667 // flip 16 on big endian systems 668 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 669 if (btstack_is_big_endian()){ 670 int i; 671 for (i=0;i<bytes_read;i+=2){ 672 uint8_t tmp = sample_data[i*2]; 673 sample_data[i*2] = sample_data[i*2+1]; 674 sample_data[i*2+1] = tmp; 675 } 676 } 677 bytes_to_copy -= bytes_read; 678 pos += bytes_read; 679 } 680 681 // fill with 0 if not enough 682 if (bytes_to_copy){ 683 memset(sample_data + pos, 0, bytes_to_copy); 684 pa_input_paused = 1; 685 } 686 } 687 #else 688 // just send '0's 689 if (negotiated_codec == HFP_CODEC_MSBC){ 690 sco_payload_length = 24; 691 sco_packet_length = sco_payload_length + 3; 692 } 693 memset(sco_packet + 3, 0, sco_payload_length); 694 #endif 695 #endif 696 697 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 698 memset(&sco_packet[3], phase++, sco_payload_length); 699 if (phase > 'z') phase = 'a'; 700 #endif 701 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 702 int j; 703 for (j=0;j<sco_payload_length;j++){ 704 sco_packet[3+j] = phase++; 705 } 706 #endif 707 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 708 int j; 709 for (j=0;j<sco_payload_length;j++){ 710 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 711 sco_packet[3+j] = 0x55; 712 } 713 #endif 714 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 715 int j; 716 for (j=0;j<sco_payload_length;j++){ 717 sco_packet[3+j] = 0x00; 718 } 719 // additional hack 720 // big_endian_store_16(sco_packet, 5, phase++); 721 (void) phase; 722 #endif 723 724 // test silence 725 // memset(sco_packet+3, 0, sco_payload_length); 726 727 // set handle + flags 728 little_endian_store_16(sco_packet, 0, sco_handle); 729 // set len 730 sco_packet[2] = sco_payload_length; 731 // finally send packet 732 hci_send_sco_packet_buffer(sco_packet_length); 733 734 // request another send event 735 hci_request_sco_can_send_now_event(); 736 737 count_sent++; 738 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 739 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 740 #endif 741 } 742 743 /** 744 * @brief Process received data 745 */ 746 #define ANSI_COLOR_RED "\x1b[31m" 747 #define ANSI_COLOR_GREEN "\x1b[32m" 748 #define ANSI_COLOR_YELLOW "\x1b[33m" 749 #define ANSI_COLOR_BLUE "\x1b[34m" 750 #define ANSI_COLOR_MAGENTA "\x1b[35m" 751 #define ANSI_COLOR_CYAN "\x1b[36m" 752 #define ANSI_COLOR_RESET "\x1b[0m" 753 754 void sco_demo_receive(uint8_t * packet, uint16_t size){ 755 756 dump_data = 1; 757 758 count_received++; 759 static uint32_t packets = 0; 760 static uint32_t crc_errors = 0; 761 static uint32_t data_received = 0; 762 static uint32_t byte_errors = 0; 763 764 data_received += size - 3; 765 packets++; 766 if (data_received > 100000){ 767 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", (unsigned int) data_received, (unsigned int) packets, (unsigned int) crc_errors, (unsigned int) byte_errors); 768 crc_errors = 0; 769 byte_errors = 0; 770 data_received = 0; 771 packets = 0; 772 } 773 774 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 775 switch (negotiated_codec){ 776 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 777 case HFP_CODEC_MSBC: 778 sco_demo_receive_mSBC(packet, size); 779 break; 780 #endif 781 case HFP_CODEC_CVSD: 782 sco_demo_receive_CVSD(packet, size); 783 break; 784 default: 785 break; 786 } 787 dump_data = 0; 788 #endif 789 790 if (packet[1] & 0x30){ 791 crc_errors++; 792 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 793 // printf_hexdump(&packet[3], size-3); 794 return; 795 } 796 if (dump_data){ 797 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 798 printf("data: "); 799 int i; 800 for (i=3;i<size;i++){ 801 printf("%c", packet[i]); 802 } 803 printf("\n"); 804 dump_data = 0; 805 #endif 806 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 807 // colored hexdump with expected 808 static uint8_t expected_byte = 0; 809 int i; 810 printf("data: "); 811 for (i=3;i<size;i++){ 812 if (packet[i] != expected_byte){ 813 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 814 } else { 815 printf("%02x ", packet[i]); 816 } 817 expected_byte = packet[i]+1; 818 } 819 printf("\n"); 820 #endif 821 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 822 int i; 823 int contains_error = 0; 824 for (i=3;i<size;i++){ 825 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 826 contains_error = 1; 827 byte_errors++; 828 } 829 } 830 if (contains_error){ 831 printf("data: "); 832 for (i=0;i<3;i++){ 833 printf("%02x ", packet[i]); 834 } 835 for (i=3;i<size;i++){ 836 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 837 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 838 } else { 839 printf("%02x ", packet[i]); 840 } 841 } 842 printf("\n"); 843 } 844 #endif 845 } 846 } 847