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