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