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 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 357 #else 358 UNUSED(num_channels); 359 #endif 360 361 if (!num_samples_to_write) return; 362 363 num_samples = btstack_min(num_samples, num_samples_to_write); 364 num_samples_to_write -= num_samples; 365 366 wav_writer_write_int16(num_samples, data); 367 368 if (num_samples_to_write == 0){ 369 sco_demo_close(); 370 } 371 } 372 373 static void sco_demo_init_mSBC(void){ 374 printf("SCO Demo: Init mSBC\n"); 375 376 wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); 377 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 378 379 num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 380 381 hfp_msbc_init(); 382 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 383 sco_demo_msbc_fill_sine_audio_frame(); 384 #endif 385 386 #ifdef SCO_MSBC_IN_FILENAME 387 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 388 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 389 #endif 390 391 #ifdef SCO_MSBC_OUT_FILENAME 392 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 393 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 394 #endif 395 396 #ifdef USE_PORTAUDIO 397 portaudio_initialize(MSBC_SAMPLE_RATE); 398 #endif 399 } 400 401 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 402 if (num_samples_to_write){ 403 if (msbc_file_in){ 404 // log incoming mSBC data for testing 405 fwrite(packet+3, size-3, 1, msbc_file_in); 406 } 407 } 408 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 409 } 410 411 static void sco_demo_init_CVSD(void){ 412 printf("SCO Demo: Init CVSD\n"); 413 414 wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); 415 btstack_cvsd_plc_init(&cvsd_plc_state); 416 417 num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 418 419 #ifdef USE_PORTAUDIO 420 portaudio_initialize(CVSD_SAMPLE_RATE); 421 #endif 422 } 423 424 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 425 if (!num_samples_to_write) return; 426 int16_t audio_frame_out[255]; // 427 428 if (size > sizeof(audio_frame_out)){ 429 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 430 return; 431 } 432 const int audio_bytes_read = size - 3; 433 const int num_samples = audio_bytes_read / CVSD_BYTES_PER_FRAME; 434 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 435 436 #if 0 437 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 438 #else 439 memcpy(audio_frame_out, packet+3, audio_bytes_read); 440 #endif 441 442 wav_writer_write_int16(samples_to_write, audio_frame_out); 443 num_samples_to_write -= samples_to_write; 444 if (num_samples_to_write == 0){ 445 sco_demo_close(); 446 } 447 #ifdef USE_PORTAUDIO 448 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 449 #endif 450 } 451 452 void sco_demo_close(void){ 453 printf("SCO demo close\n"); 454 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 455 456 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 457 wav_writer_close(); 458 #endif 459 printf("SCO demo statistics: "); 460 if (negotiated_codec == HFP_CODEC_MSBC){ 461 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); 462 } else { 463 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); 464 } 465 466 #ifdef HAVE_PORTAUDIO 467 portaudio_terminate(); 468 #endif 469 470 #ifdef SCO_WAV_FILENAME 471 #if 0 472 printf("SCO Demo: closing wav file\n"); 473 if (negotiated_codec == HFP_CODEC_MSBC){ 474 wav_writer_state_t * writer_state = &wav_writer_state; 475 if (!writer_state->wav_file) return; 476 rewind(writer_state->wav_file); 477 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); 478 fclose(writer_state->wav_file); 479 writer_state->wav_file = NULL; 480 } 481 #endif 482 #endif 483 484 negotiated_codec = -1; 485 486 #endif 487 } 488 489 void sco_demo_set_codec(uint8_t codec){ 490 if (negotiated_codec == codec) return; 491 negotiated_codec = codec; 492 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 493 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 494 if (negotiated_codec == HFP_CODEC_MSBC){ 495 sco_demo_init_mSBC(); 496 } else { 497 sco_demo_init_CVSD(); 498 } 499 #endif 500 #endif 501 } 502 503 void sco_demo_init(void){ 504 // status 505 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 506 printf("SCO Demo: Sending and receiving audio via portaudio.\n"); 507 #endif 508 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 509 #ifdef HAVE_PORTAUDIO 510 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 511 #else 512 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 513 #endif 514 #endif 515 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 516 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 517 #endif 518 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 519 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 520 #endif 521 522 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 523 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 524 #else 525 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 526 #endif 527 528 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 529 phase = 'a'; 530 #endif 531 } 532 533 void sco_report(void); 534 void sco_report(void){ 535 printf("SCO: sent %u, received %u\n", count_sent, count_received); 536 } 537 538 void sco_demo_send(hci_con_handle_t sco_handle){ 539 540 if (!sco_handle) return; 541 542 int sco_packet_length = hci_get_sco_packet_length(); 543 int sco_payload_length = sco_packet_length - 3; 544 545 hci_reserve_packet_buffer(); 546 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 547 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 548 if (negotiated_codec == HFP_CODEC_MSBC){ 549 // overwrite 550 sco_payload_length = 24; 551 sco_packet_length = sco_payload_length + 3; 552 553 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 554 log_error("mSBC stream is empty."); 555 } 556 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 557 if (msbc_file_out){ 558 // log outgoing mSBC data for testing 559 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 560 } 561 562 sco_demo_msbc_fill_sine_audio_frame(); 563 } else { 564 const int audio_samples_per_packet = sco_payload_length / CVSD_BYTES_PER_FRAME; 565 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, (int16_t *) (sco_packet+3)); 566 } 567 #endif 568 569 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 570 571 #ifdef HAVE_PORTAUDIO 572 if (negotiated_codec == HFP_CODEC_MSBC){ 573 // MSBC 574 575 // overwrite 576 sco_payload_length = 24; 577 sco_packet_length = sco_payload_length + 3; 578 579 if (pa_input_paused){ 580 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 581 // resume sending 582 pa_input_paused = 0; 583 } 584 } 585 586 if (!pa_input_paused){ 587 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 588 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= (num_samples * MSBC_BYTES_PER_FRAME)){ 589 int16_t sample_buffer[num_samples]; 590 uint32_t bytes_read; 591 btstack_ring_buffer_read(&pa_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * MSBC_BYTES_PER_FRAME, &bytes_read); 592 hfp_msbc_encode_audio_frame(sample_buffer); 593 num_audio_frames++; 594 } 595 } 596 597 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 598 log_error("mSBC stream should not be empty."); 599 memset(sco_packet + 3, 0, sco_payload_length); 600 pa_input_paused = 1; 601 } else { 602 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 603 if (msbc_file_out){ 604 // log outgoing mSBC data for testing 605 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 606 } 607 } 608 609 } else { 610 // CVSD 611 612 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); 613 // fill with silence while paused 614 int bytes_to_copy = sco_payload_length; 615 if (pa_input_paused){ 616 if (btstack_ring_buffer_bytes_available(&pa_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 617 // resume sending 618 pa_input_paused = 0; 619 } 620 } 621 622 // get data from ringbuffer 623 uint16_t pos = 0; 624 if (!pa_input_paused){ 625 uint32_t bytes_read = 0; 626 btstack_ring_buffer_read(&pa_input_ring_buffer, sco_packet + 3, bytes_to_copy, &bytes_read); 627 bytes_to_copy -= bytes_read; 628 pos += bytes_read; 629 } 630 631 // fill with 0 if not enough 632 if (bytes_to_copy){ 633 memset(sco_packet + 3 + pos, 0, bytes_to_copy); 634 pa_input_paused = 1; 635 } 636 } 637 #else 638 // just send '0's 639 if (negotiated_codec == HFP_CODEC_MSBC){ 640 sco_payload_length = 24; 641 sco_packet_length = sco_payload_length + 3; 642 } 643 memset(sco_packet + 3, 0, sco_payload_length); 644 #endif 645 #endif 646 647 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 648 memset(&sco_packet[3], phase++, sco_payload_length); 649 if (phase > 'z') phase = 'a'; 650 #endif 651 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 652 int j; 653 for (j=0;j<sco_payload_length;j++){ 654 sco_packet[3+j] = phase++; 655 } 656 #endif 657 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 658 int j; 659 for (j=0;j<sco_payload_length;j++){ 660 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 661 sco_packet[3+j] = 0x55; 662 } 663 #endif 664 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 665 int j; 666 for (j=0;j<sco_payload_length;j++){ 667 sco_packet[3+j] = 0x00; 668 } 669 // additional hack 670 // big_endian_store_16(sco_packet, 5, phase++); 671 (void) phase; 672 #endif 673 674 // test silence 675 // memset(sco_packet+3, 0, sco_payload_length); 676 677 // set handle + flags 678 little_endian_store_16(sco_packet, 0, sco_handle); 679 // set len 680 sco_packet[2] = sco_payload_length; 681 // finally send packet 682 hci_send_sco_packet_buffer(sco_packet_length); 683 684 // request another send event 685 hci_request_sco_can_send_now_event(); 686 687 count_sent++; 688 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 689 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 690 #endif 691 } 692 693 /** 694 * @brief Process received data 695 */ 696 #define ANSI_COLOR_RED "\x1b[31m" 697 #define ANSI_COLOR_GREEN "\x1b[32m" 698 #define ANSI_COLOR_YELLOW "\x1b[33m" 699 #define ANSI_COLOR_BLUE "\x1b[34m" 700 #define ANSI_COLOR_MAGENTA "\x1b[35m" 701 #define ANSI_COLOR_CYAN "\x1b[36m" 702 #define ANSI_COLOR_RESET "\x1b[0m" 703 704 void sco_demo_receive(uint8_t * packet, uint16_t size){ 705 706 dump_data = 1; 707 708 count_received++; 709 static uint32_t packets = 0; 710 static uint32_t crc_errors = 0; 711 static uint32_t data_received = 0; 712 static uint32_t byte_errors = 0; 713 714 data_received += size - 3; 715 packets++; 716 if (data_received > 100000){ 717 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", data_received, packets, crc_errors, byte_errors); 718 crc_errors = 0; 719 byte_errors = 0; 720 data_received = 0; 721 packets = 0; 722 } 723 724 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 725 switch (negotiated_codec){ 726 case HFP_CODEC_MSBC: 727 sco_demo_receive_mSBC(packet, size); 728 break; 729 case HFP_CODEC_CVSD: 730 sco_demo_receive_CVSD(packet, size); 731 break; 732 default: 733 break; 734 } 735 dump_data = 0; 736 #endif 737 738 if (packet[1] & 0x30){ 739 crc_errors++; 740 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 741 // printf_hexdump(&packet[3], size-3); 742 return; 743 } 744 if (dump_data){ 745 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 746 printf("data: "); 747 int i; 748 for (i=3;i<size;i++){ 749 printf("%c", packet[i]); 750 } 751 printf("\n"); 752 dump_data = 0; 753 #endif 754 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 755 // colored hexdump with expected 756 static uint8_t expected_byte = 0; 757 int i; 758 printf("data: "); 759 for (i=3;i<size;i++){ 760 if (packet[i] != expected_byte){ 761 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 762 } else { 763 printf("%02x ", packet[i]); 764 } 765 expected_byte = packet[i]+1; 766 } 767 printf("\n"); 768 #endif 769 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 770 int i; 771 int contains_error = 0; 772 for (i=3;i<size;i++){ 773 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 774 contains_error = 1; 775 byte_errors++; 776 } 777 } 778 if (contains_error){ 779 printf("data: "); 780 for (i=0;i<3;i++){ 781 printf("%02x ", packet[i]); 782 } 783 for (i=3;i<size;i++){ 784 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 785 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 786 } else { 787 printf("%02x ", packet[i]); 788 } 789 } 790 printf("\n"); 791 } 792 #endif 793 } 794 } 795