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