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