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 btstack_cvsd_plc_process_data(&cvsd_plc_state, audio_frame_in, num_samples, audio_frame_out); 452 453 #ifdef USE_PORTAUDIO 454 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 455 #endif 456 } 457 458 void sco_demo_close(void){ 459 printf("SCO demo close\n"); 460 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 461 462 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 463 wav_writer_close(); 464 #endif 465 printf("SCO demo statistics: "); 466 if (negotiated_codec == HFP_CODEC_MSBC){ 467 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); 468 } else { 469 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); 470 } 471 472 #ifdef HAVE_PORTAUDIO 473 portaudio_terminate(); 474 #endif 475 476 #ifdef SCO_WAV_FILENAME 477 #if 0 478 printf("SCO Demo: closing wav file\n"); 479 if (negotiated_codec == HFP_CODEC_MSBC){ 480 wav_writer_state_t * writer_state = &wav_writer_state; 481 if (!writer_state->wav_file) return; 482 rewind(writer_state->wav_file); 483 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); 484 fclose(writer_state->wav_file); 485 writer_state->wav_file = NULL; 486 } 487 #endif 488 #endif 489 490 negotiated_codec = -1; 491 492 #endif 493 } 494 495 void sco_demo_set_codec(uint8_t codec){ 496 if (negotiated_codec == codec) return; 497 negotiated_codec = codec; 498 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 499 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 500 if (negotiated_codec == HFP_CODEC_MSBC){ 501 sco_demo_init_mSBC(); 502 } else { 503 sco_demo_init_CVSD(); 504 } 505 #endif 506 #endif 507 } 508 509 void sco_demo_init(void){ 510 // status 511 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 512 printf("SCO Demo: Sending and receiving audio via portaudio.\n"); 513 #endif 514 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 515 #ifdef HAVE_PORTAUDIO 516 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 517 #else 518 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 519 #endif 520 #endif 521 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 522 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 523 #endif 524 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 525 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 526 #endif 527 528 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 529 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 530 #else 531 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 532 #endif 533 534 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 535 phase = 'a'; 536 #endif 537 } 538 539 void sco_report(void); 540 void sco_report(void){ 541 printf("SCO: sent %u, received %u\n", count_sent, count_received); 542 } 543 544 void sco_demo_send(hci_con_handle_t sco_handle){ 545 546 if (!sco_handle) return; 547 548 int sco_packet_length = hci_get_sco_packet_length(); 549 int sco_payload_length = sco_packet_length - 3; 550 551 hci_reserve_packet_buffer(); 552 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 553 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 554 if (negotiated_codec == HFP_CODEC_MSBC){ 555 // overwrite 556 sco_payload_length = 24; 557 sco_packet_length = sco_payload_length + 3; 558 559 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 560 log_error("mSBC stream is empty."); 561 } 562 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 563 if (msbc_file_out){ 564 // log outgoing mSBC data for testing 565 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 566 } 567 568 sco_demo_msbc_fill_sine_audio_frame(); 569 } else { 570 const int audio_samples_per_packet = sco_payload_length / CVSD_BYTES_PER_FRAME; 571 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, (int16_t *) (sco_packet+3)); 572 } 573 #endif 574 575 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 576 577 #ifdef HAVE_PORTAUDIO 578 if (negotiated_codec == HFP_CODEC_MSBC){ 579 // MSBC 580 581 // overwrite 582 sco_payload_length = 24; 583 sco_packet_length = sco_payload_length + 3; 584 585 if (pa_input_paused){ 586 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 587 // resume sending 588 pa_input_paused = 0; 589 } 590 } 591 592 if (!pa_input_paused){ 593 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 594 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= (num_samples * MSBC_BYTES_PER_FRAME)){ 595 int16_t sample_buffer[num_samples]; 596 uint32_t bytes_read; 597 btstack_ring_buffer_read(&pa_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * MSBC_BYTES_PER_FRAME, &bytes_read); 598 hfp_msbc_encode_audio_frame(sample_buffer); 599 num_audio_frames++; 600 } 601 } 602 603 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 604 log_error("mSBC stream should not be empty."); 605 memset(sco_packet + 3, 0, sco_payload_length); 606 pa_input_paused = 1; 607 } else { 608 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 609 if (msbc_file_out){ 610 // log outgoing mSBC data for testing 611 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 612 } 613 } 614 615 } else { 616 // CVSD 617 618 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); 619 // fill with silence while paused 620 int bytes_to_copy = sco_payload_length; 621 if (pa_input_paused){ 622 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 623 // resume sending 624 pa_input_paused = 0; 625 } 626 } 627 628 // get data from ringbuffer 629 uint16_t pos = 0; 630 uint8_t * sample_data = &sco_packet[3]; 631 if (!pa_input_paused){ 632 uint32_t bytes_read = 0; 633 btstack_ring_buffer_read(&pa_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read); 634 // flip 16 on big endian systems 635 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 636 if (btstack_is_big_endian()){ 637 int i; 638 for (i=0;i<bytes_read;i+=2){ 639 uint8_t tmp = sample_data[i*2]; 640 sample_data[i*2] = sample_data[i*2+1]; 641 sample_data[i*2+1] = tmp; 642 } 643 } 644 bytes_to_copy -= bytes_read; 645 pos += bytes_read; 646 } 647 648 // fill with 0 if not enough 649 if (bytes_to_copy){ 650 memset(sample_data + pos, 0, bytes_to_copy); 651 pa_input_paused = 1; 652 } 653 } 654 #else 655 // just send '0's 656 if (negotiated_codec == HFP_CODEC_MSBC){ 657 sco_payload_length = 24; 658 sco_packet_length = sco_payload_length + 3; 659 } 660 memset(sco_packet + 3, 0, sco_payload_length); 661 #endif 662 #endif 663 664 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 665 memset(&sco_packet[3], phase++, sco_payload_length); 666 if (phase > 'z') phase = 'a'; 667 #endif 668 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 669 int j; 670 for (j=0;j<sco_payload_length;j++){ 671 sco_packet[3+j] = phase++; 672 } 673 #endif 674 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 675 int j; 676 for (j=0;j<sco_payload_length;j++){ 677 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 678 sco_packet[3+j] = 0x55; 679 } 680 #endif 681 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 682 int j; 683 for (j=0;j<sco_payload_length;j++){ 684 sco_packet[3+j] = 0x00; 685 } 686 // additional hack 687 // big_endian_store_16(sco_packet, 5, phase++); 688 (void) phase; 689 #endif 690 691 // test silence 692 // memset(sco_packet+3, 0, sco_payload_length); 693 694 // set handle + flags 695 little_endian_store_16(sco_packet, 0, sco_handle); 696 // set len 697 sco_packet[2] = sco_payload_length; 698 // finally send packet 699 hci_send_sco_packet_buffer(sco_packet_length); 700 701 // request another send event 702 hci_request_sco_can_send_now_event(); 703 704 count_sent++; 705 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 706 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 707 #endif 708 } 709 710 /** 711 * @brief Process received data 712 */ 713 #define ANSI_COLOR_RED "\x1b[31m" 714 #define ANSI_COLOR_GREEN "\x1b[32m" 715 #define ANSI_COLOR_YELLOW "\x1b[33m" 716 #define ANSI_COLOR_BLUE "\x1b[34m" 717 #define ANSI_COLOR_MAGENTA "\x1b[35m" 718 #define ANSI_COLOR_CYAN "\x1b[36m" 719 #define ANSI_COLOR_RESET "\x1b[0m" 720 721 void sco_demo_receive(uint8_t * packet, uint16_t size){ 722 723 dump_data = 1; 724 725 count_received++; 726 static uint32_t packets = 0; 727 static uint32_t crc_errors = 0; 728 static uint32_t data_received = 0; 729 static uint32_t byte_errors = 0; 730 731 data_received += size - 3; 732 packets++; 733 if (data_received > 100000){ 734 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", data_received, packets, crc_errors, byte_errors); 735 crc_errors = 0; 736 byte_errors = 0; 737 data_received = 0; 738 packets = 0; 739 } 740 741 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 742 switch (negotiated_codec){ 743 case HFP_CODEC_MSBC: 744 sco_demo_receive_mSBC(packet, size); 745 break; 746 case HFP_CODEC_CVSD: 747 sco_demo_receive_CVSD(packet, size); 748 break; 749 default: 750 break; 751 } 752 dump_data = 0; 753 #endif 754 755 if (packet[1] & 0x30){ 756 crc_errors++; 757 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 758 // printf_hexdump(&packet[3], size-3); 759 return; 760 } 761 if (dump_data){ 762 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 763 printf("data: "); 764 int i; 765 for (i=3;i<size;i++){ 766 printf("%c", packet[i]); 767 } 768 printf("\n"); 769 dump_data = 0; 770 #endif 771 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 772 // colored hexdump with expected 773 static uint8_t expected_byte = 0; 774 int i; 775 printf("data: "); 776 for (i=3;i<size;i++){ 777 if (packet[i] != expected_byte){ 778 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 779 } else { 780 printf("%02x ", packet[i]); 781 } 782 expected_byte = packet[i]+1; 783 } 784 printf("\n"); 785 #endif 786 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 787 int i; 788 int contains_error = 0; 789 for (i=3;i<size;i++){ 790 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 791 contains_error = 1; 792 byte_errors++; 793 } 794 } 795 if (contains_error){ 796 printf("data: "); 797 for (i=0;i<3;i++){ 798 printf("%02x ", packet[i]); 799 } 800 for (i=3;i<size;i++){ 801 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 802 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 803 } else { 804 printf("%02x ", packet[i]); 805 } 806 } 807 printf("\n"); 808 } 809 #endif 810 } 811 } 812