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