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