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_msbc_fill_sine_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_msbc_fill_sine_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_msbc_fill_sine_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 // MSBC 561 562 // overwrite 563 sco_payload_length = 24; 564 sco_packet_length = sco_payload_length + 3; 565 566 if (pa_input_paused){ 567 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 568 // resume sending 569 pa_input_paused = 0; 570 } 571 } 572 573 if (!pa_input_paused){ 574 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 575 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= (num_samples * MSBC_BYTES_PER_FRAME)){ 576 int16_t sample_buffer[num_samples]; 577 uint32_t bytes_read; 578 btstack_ring_buffer_read(&pa_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * MSBC_BYTES_PER_FRAME, &bytes_read); 579 hfp_msbc_encode_audio_frame(sample_buffer); 580 num_audio_frames++; 581 } 582 } 583 584 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 585 log_error("mSBC stream should not be empty."); 586 memset(sco_packet + 3, 0, sco_payload_length); 587 pa_input_paused = 1; 588 } else { 589 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 590 if (msbc_file_out){ 591 // log outgoing mSBC data for testing 592 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 593 } 594 } 595 596 } else { 597 // CVSD 598 599 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); 600 // fill with silence while paused 601 int bytes_to_copy = sco_payload_length; 602 if (pa_input_paused){ 603 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 604 // resume sending 605 pa_input_paused = 0; 606 } 607 } 608 609 // get data from ringbuffer 610 uint16_t pos = 0; 611 if (!pa_input_paused){ 612 uint32_t bytes_read = 0; 613 btstack_ring_buffer_read(&pa_input_ring_buffer, sco_packet + 3, bytes_to_copy, &bytes_read); 614 bytes_to_copy -= bytes_read; 615 pos += bytes_read; 616 } 617 618 // fill with 0 if not enough 619 if (bytes_to_copy){ 620 memset(sco_packet + 3 + pos, 0, bytes_to_copy); 621 pa_input_paused = 1; 622 } 623 } 624 #else 625 // just send '0's 626 if (negotiated_codec == HFP_CODEC_MSBC){ 627 sco_payload_length = 24; 628 sco_packet_length = sco_payload_length + 3; 629 } 630 memset(sco_packet + 3, 0, sco_payload_length); 631 #endif 632 #endif 633 634 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 635 memset(&sco_packet[3], phase++, sco_payload_length); 636 if (phase > 'z') phase = 'a'; 637 #endif 638 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 639 int j; 640 for (j=0;j<sco_payload_length;j++){ 641 sco_packet[3+j] = phase++; 642 } 643 #endif 644 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 645 int j; 646 for (j=0;j<sco_payload_length;j++){ 647 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 648 sco_packet[3+j] = 0x55; 649 } 650 #endif 651 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 652 int j; 653 for (j=0;j<sco_payload_length;j++){ 654 sco_packet[3+j] = 0x00; 655 } 656 // additional hack 657 // big_endian_store_16(sco_packet, 5, phase++); 658 (void) phase; 659 #endif 660 661 // test silence 662 // memset(sco_packet+3, 0, sco_payload_length); 663 664 // set handle + flags 665 little_endian_store_16(sco_packet, 0, sco_handle); 666 // set len 667 sco_packet[2] = sco_payload_length; 668 // finally send packet 669 hci_send_sco_packet_buffer(sco_packet_length); 670 671 // request another send event 672 hci_request_sco_can_send_now_event(); 673 674 count_sent++; 675 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 676 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 677 #endif 678 } 679 680 /** 681 * @brief Process received data 682 */ 683 #define ANSI_COLOR_RED "\x1b[31m" 684 #define ANSI_COLOR_GREEN "\x1b[32m" 685 #define ANSI_COLOR_YELLOW "\x1b[33m" 686 #define ANSI_COLOR_BLUE "\x1b[34m" 687 #define ANSI_COLOR_MAGENTA "\x1b[35m" 688 #define ANSI_COLOR_CYAN "\x1b[36m" 689 #define ANSI_COLOR_RESET "\x1b[0m" 690 691 void sco_demo_receive(uint8_t * packet, uint16_t size){ 692 693 dump_data = 1; 694 695 count_received++; 696 static uint32_t packets = 0; 697 static uint32_t crc_errors = 0; 698 static uint32_t data_received = 0; 699 static uint32_t byte_errors = 0; 700 701 data_received += size - 3; 702 packets++; 703 if (data_received > 100000){ 704 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", data_received, packets, crc_errors, byte_errors); 705 crc_errors = 0; 706 byte_errors = 0; 707 data_received = 0; 708 packets = 0; 709 } 710 711 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 712 switch (negotiated_codec){ 713 case HFP_CODEC_MSBC: 714 sco_demo_receive_mSBC(packet, size); 715 break; 716 case HFP_CODEC_CVSD: 717 sco_demo_receive_CVSD(packet, size); 718 break; 719 default: 720 break; 721 } 722 dump_data = 0; 723 #endif 724 725 if (packet[1] & 0x30){ 726 crc_errors++; 727 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 728 // printf_hexdump(&packet[3], size-3); 729 return; 730 } 731 if (dump_data){ 732 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 733 printf("data: "); 734 int i; 735 for (i=3;i<size;i++){ 736 printf("%c", packet[i]); 737 } 738 printf("\n"); 739 dump_data = 0; 740 #endif 741 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 742 // colored hexdump with expected 743 static uint8_t expected_byte = 0; 744 int i; 745 printf("data: "); 746 for (i=3;i<size;i++){ 747 if (packet[i] != expected_byte){ 748 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 749 } else { 750 printf("%02x ", packet[i]); 751 } 752 expected_byte = packet[i]+1; 753 } 754 printf("\n"); 755 #endif 756 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 757 int i; 758 int contains_error = 0; 759 for (i=3;i<size;i++){ 760 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 761 contains_error = 1; 762 byte_errors++; 763 } 764 } 765 if (contains_error){ 766 printf("data: "); 767 for (i=0;i<3;i++){ 768 printf("%02x ", packet[i]); 769 } 770 for (i=3;i<size;i++){ 771 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 772 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 773 } else { 774 printf("%02x ", packet[i]); 775 } 776 } 777 printf("\n"); 778 } 779 #endif 780 } 781 } 782