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 disk 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 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 350 351 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 352 UNUSED(context); 353 UNUSED(sample_rate); 354 UNUSED(data); 355 UNUSED(num_samples); 356 UNUSED(num_channels); 357 358 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 359 360 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 361 #ifdef HAVE_PORTAUDIO 362 // samples in callback in host endianess, ready for PortAudio playback 363 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 364 #endif 365 366 #ifdef SCO_WAV_FILENAME 367 if (!num_samples_to_write) return; 368 num_samples = btstack_min(num_samples, num_samples_to_write); 369 num_samples_to_write -= num_samples; 370 wav_writer_write_int16(num_samples, data); 371 if (num_samples_to_write == 0){ 372 wav_writer_close(); 373 } 374 #endif 375 376 #endif 377 378 } 379 380 static void sco_demo_init_mSBC(void){ 381 printf("SCO Demo: Init mSBC\n"); 382 383 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 384 hfp_msbc_init(); 385 386 #ifdef SCO_WAV_FILENAME 387 num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 388 wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); 389 #endif 390 391 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 392 sco_demo_msbc_fill_sine_audio_frame(); 393 #endif 394 395 #ifdef SCO_MSBC_IN_FILENAME 396 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 397 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 398 #endif 399 400 #ifdef SCO_MSBC_OUT_FILENAME 401 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 402 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 403 #endif 404 405 #ifdef USE_PORTAUDIO 406 portaudio_initialize(MSBC_SAMPLE_RATE); 407 #endif 408 } 409 410 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 411 if (num_samples_to_write){ 412 if (msbc_file_in){ 413 // log incoming mSBC data for testing 414 fwrite(packet+3, size-3, 1, msbc_file_in); 415 } 416 } 417 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 418 } 419 420 static void sco_demo_init_CVSD(void){ 421 printf("SCO Demo: Init CVSD\n"); 422 423 btstack_cvsd_plc_init(&cvsd_plc_state); 424 425 #ifdef SCO_WAV_FILENAME 426 num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 427 wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); 428 #endif 429 430 #ifdef USE_PORTAUDIO 431 portaudio_initialize(CVSD_SAMPLE_RATE); 432 #endif 433 } 434 435 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 436 if (!num_samples_to_write) return; 437 438 int16_t audio_frame_out[128]; // 439 440 if (size > sizeof(audio_frame_out)){ 441 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 442 return; 443 } 444 445 const int audio_bytes_read = size - 3; 446 const int num_samples = audio_bytes_read / CVSD_BYTES_PER_FRAME; 447 448 // convert into host endian 449 int16_t audio_frame_in[128]; 450 int i; 451 for (i=0;i<num_samples;i++){ 452 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2); 453 } 454 455 btstack_cvsd_plc_process_data(&cvsd_plc_state, audio_frame_in, num_samples, audio_frame_out); 456 457 #ifdef SCO_WAV_FILENAME 458 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 459 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 460 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 461 num_samples_to_write -= samples_to_write; 462 if (num_samples_to_write == 0){ 463 wav_writer_close(); 464 } 465 #endif 466 467 #ifdef USE_PORTAUDIO 468 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 469 #endif 470 } 471 472 #endif 473 474 475 void sco_demo_close(void){ 476 printf("SCO demo close\n"); 477 478 printf("SCO demo statistics: "); 479 if (negotiated_codec == HFP_CODEC_MSBC){ 480 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); 481 } else { 482 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); 483 } 484 485 negotiated_codec = -1; 486 487 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 488 489 #if defined(SCO_WAV_FILENAME) 490 wav_writer_close(); 491 #endif 492 493 #ifdef HAVE_PORTAUDIO 494 portaudio_terminate(); 495 #endif 496 497 #endif 498 } 499 500 void sco_demo_set_codec(uint8_t codec){ 501 if (negotiated_codec == codec) return; 502 negotiated_codec = codec; 503 504 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 505 if (negotiated_codec == HFP_CODEC_MSBC){ 506 sco_demo_init_mSBC(); 507 } else { 508 sco_demo_init_CVSD(); 509 } 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