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 BLUEKITCHEN 24 * GMBH 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 #define BTSTACK_FILE__ "sco_demo_util.c" 39 40 /* 41 * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo 42 */ 43 44 #include <stdio.h> 45 46 #include "sco_demo_util.h" 47 48 #include "btstack_audio.h" 49 #include "btstack_debug.h" 50 #include "btstack_ring_buffer.h" 51 #include "classic/btstack_cvsd_plc.h" 52 #include "classic/btstack_sbc.h" 53 #include "classic/hfp.h" 54 #include "classic/hfp_msbc.h" 55 56 #ifdef _MSC_VER 57 // ignore deprecated warning for fopen 58 #pragma warning(disable : 4996) 59 #endif 60 61 #ifdef HAVE_POSIX_FILE_IO 62 #include "wav_util.h" 63 #endif 64 65 // test modes 66 #define SCO_DEMO_MODE_SINE 0 67 #define SCO_DEMO_MODE_ASCII 1 68 #define SCO_DEMO_MODE_COUNTER 2 69 #define SCO_DEMO_MODE_55 3 70 #define SCO_DEMO_MODE_00 4 71 #define SCO_DEMO_MODE_MICROPHONE 5 72 73 // SCO demo configuration 74 #define SCO_DEMO_MODE SCO_DEMO_MODE_MICROPHONE 75 76 // number of sco packets until 'report' on console 77 #define SCO_REPORT_PERIOD 100 78 79 // #define ENABLE_SCO_STEREO_PLAYBACK 80 81 #ifdef HAVE_POSIX_FILE_IO 82 // length and name of wav file on disk 83 #define SCO_WAV_DURATION_IN_SECONDS 15 84 #define SCO_WAV_FILENAME "sco_input.wav" 85 86 // name of sbc test files 87 #define SCO_MSBC_OUT_FILENAME "sco_output.msbc" 88 #define SCO_MSBC_IN_FILENAME "sco_input.msbc" 89 #endif 90 91 92 // pre-buffer for CVSD and mSBC - also defines latency 93 #define SCO_CVSD_PA_PREBUFFER_MS 50 94 #define SCO_MSBC_PA_PREBUFFER_MS 50 95 96 // constants 97 #define NUM_CHANNELS 1 98 #define CVSD_SAMPLE_RATE 8000 99 #define MSBC_SAMPLE_RATE 16000 100 #define BYTES_PER_FRAME 2 101 102 #define CVSD_PA_PREBUFFER_BYTES (SCO_CVSD_PA_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * BYTES_PER_FRAME) 103 #define MSBC_PA_PREBUFFER_BYTES (SCO_MSBC_PA_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * BYTES_PER_FRAME) 104 105 // output 106 107 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 108 static int audio_output_paused = 0; 109 static uint8_t audio_output_ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES]; 110 static btstack_ring_buffer_t audio_output_ring_buffer; 111 #endif 112 113 114 // input 115 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 116 #define USE_AUDIO_INPUT 117 static int audio_input_paused = 0; 118 static uint8_t audio_input_ring_buffer_storage[2*8000]; // full second input buffer 119 static btstack_ring_buffer_t audio_input_ring_buffer; 120 #endif 121 122 static int dump_data = 1; 123 static int count_sent = 0; 124 static int count_received = 0; 125 static int negotiated_codec = -1; 126 127 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 128 static btstack_sbc_decoder_state_t decoder_state; 129 130 #ifdef HAVE_POSIX_FILE_IO 131 FILE * msbc_file_in; 132 FILE * msbc_file_out; 133 #endif 134 135 #endif 136 137 static btstack_cvsd_plc_state_t cvsd_plc_state; 138 139 #define MAX_NUM_MSBC_SAMPLES (16*8) 140 141 int num_samples_to_write; 142 int num_audio_frames; 143 unsigned int phase; 144 145 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 146 147 // input signal: pre-computed sine wave, 266 Hz at 16000 kHz 148 static const int16_t sine_int16_at_16000hz[] = { 149 0, 3135, 6237, 9270, 12202, 14999, 17633, 20073, 22294, 24270, 150 25980, 27406, 28531, 29344, 29835, 30000, 29835, 29344, 28531, 27406, 151 25980, 24270, 22294, 20073, 17633, 14999, 12202, 9270, 6237, 3135, 152 0, -3135, -6237, -9270, -12202, -14999, -17633, -20073, -22294, -24270, 153 -25980, -27406, -28531, -29344, -29835, -30000, -29835, -29344, -28531, -27406, 154 -25980, -24270, -22294, -20073, -17633, -14999, -12202, -9270, -6237, -3135, 155 }; 156 157 // 8 kHz samples for CVSD/SCO packets in little endian 158 static void sco_demo_sine_wave_int16_at_8000_hz_little_endian(unsigned int num_samples, uint8_t * data){ 159 unsigned int i; 160 for (i=0; i < num_samples; i++){ 161 int16_t sample = sine_int16_at_16000hz[phase]; 162 little_endian_store_16(data, i * 2, sample); 163 // ony use every second sample from 16khz table to get 8khz 164 phase += 2; 165 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 166 phase = 0; 167 } 168 } 169 } 170 171 // 16 kHz samples for mSBC encoder in host endianess 172 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 173 static void sco_demo_sine_wave_int16_at_16000_hz_host_endian(unsigned int num_samples, int16_t * data){ 174 unsigned int i; 175 for (i=0; i < num_samples; i++){ 176 data[i] = sine_int16_at_16000hz[phase++]; 177 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 178 phase = 0; 179 } 180 } 181 } 182 183 static void sco_demo_msbc_fill_sine_audio_frame(void){ 184 if (!hfp_msbc_can_encode_audio_frame_now()) return; 185 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 186 if (num_samples > MAX_NUM_MSBC_SAMPLES) return; 187 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES]; 188 sco_demo_sine_wave_int16_at_16000_hz_host_endian(num_samples, sample_buffer); 189 hfp_msbc_encode_audio_frame(sample_buffer); 190 num_audio_frames++; 191 } 192 #endif 193 #endif 194 195 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 196 197 static void playback_callback(int16_t * buffer, uint16_t num_samples){ 198 199 uint32_t prebuffer_bytes; 200 switch (negotiated_codec){ 201 case HFP_CODEC_MSBC: 202 prebuffer_bytes = MSBC_PA_PREBUFFER_BYTES; 203 break; 204 case HFP_CODEC_CVSD: 205 default: 206 prebuffer_bytes = CVSD_PA_PREBUFFER_BYTES; 207 break; 208 } 209 210 // fill with silence while paused 211 if (audio_output_paused){ 212 if (btstack_ring_buffer_bytes_available(&audio_output_ring_buffer) < prebuffer_bytes){ 213 #ifdef ENABLE_SCO_STEREO_PLAYBACK 214 memset(buffer, 0, num_samples * BYTES_PER_FRAME * 2); 215 #else 216 memset(buffer, 0, num_samples * BYTES_PER_FRAME); 217 #endif 218 return; 219 } else { 220 // resume playback 221 audio_output_paused = 0; 222 } 223 } 224 225 // get data from ringbuffer 226 uint32_t bytes_read = 0; 227 #ifdef ENABLE_SCO_STEREO_PLAYBACK 228 while (num_samples){ 229 int16_t temp[16]; 230 unsigned int bytes_to_read = btstack_min(num_samples * BYTES_PER_FRAME, sizeof(temp)); 231 btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) &temp[0], bytes_to_read, &bytes_read); 232 if (bytes_read == 0) break; 233 unsigned int i; 234 for (i=0;i<bytes_read / BYTES_PER_FRAME;i++){ 235 *buffer++ = temp[i]; 236 *buffer++ = temp[i]; 237 num_samples--; 238 } 239 } 240 #else 241 btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 242 num_samples -= bytes_read / BYTES_PER_FRAME; 243 buffer += bytes_read / BYTES_PER_FRAME; 244 #endif 245 246 // fill with 0 if not enough 247 if (num_samples){ 248 #ifdef ENABLE_SCO_STEREO_PLAYBACK 249 memset(buffer, 0, num_samples * BYTES_PER_FRAME * 2); 250 #else 251 memset(buffer, 0, num_samples * BYTES_PER_FRAME); 252 #endif 253 audio_output_paused = 1; 254 } 255 } 256 257 #ifdef USE_AUDIO_INPUT 258 static void recording_callback(const int16_t * buffer, uint16_t num_samples){ 259 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)buffer, num_samples * 2); 260 } 261 #endif 262 263 // return 1 if ok 264 static int audio_initialize(int sample_rate){ 265 266 // -- output -- // 267 268 // init buffers 269 memset(audio_output_ring_buffer_storage, 0, sizeof(audio_output_ring_buffer_storage)); 270 btstack_ring_buffer_init(&audio_output_ring_buffer, audio_output_ring_buffer_storage, sizeof(audio_output_ring_buffer_storage)); 271 272 // config and setup audio playback 273 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance(); 274 if (!audio_sink) return 0; 275 276 #ifdef ENABLE_SCO_STEREO_PLAYBACK 277 audio_sink->init(2, sample_rate, &playback_callback); 278 #else 279 audio_sink->init(1, sample_rate, &playback_callback); 280 #endif 281 audio_sink->start_stream(); 282 283 audio_output_paused = 1; 284 285 // -- input -- // 286 287 #ifdef USE_AUDIO_INPUT 288 // init buffers 289 memset(audio_input_ring_buffer_storage, 0, sizeof(audio_input_ring_buffer_storage)); 290 btstack_ring_buffer_init(&audio_input_ring_buffer, audio_input_ring_buffer_storage, sizeof(audio_input_ring_buffer_storage)); 291 292 // config and setup audio recording 293 const btstack_audio_source_t * audio_source = btstack_audio_source_get_instance(); 294 if (!audio_source) return 0; 295 296 audio_source->init(1, sample_rate, &recording_callback); 297 audio_source->start_stream(); 298 299 audio_input_paused = 1; 300 #endif 301 302 return 1; 303 } 304 305 static void audio_terminate(void){ 306 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance(); 307 if (!audio_sink) return; 308 audio_sink->close(); 309 310 #ifdef USE_AUDIO_INPUT 311 const btstack_audio_source_t * audio_source= btstack_audio_source_get_instance(); 312 if (!audio_source) return; 313 audio_source->close(); 314 #endif 315 } 316 317 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 318 319 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 320 UNUSED(context); 321 UNUSED(sample_rate); 322 UNUSED(data); 323 UNUSED(num_samples); 324 UNUSED(num_channels); 325 326 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 327 328 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 329 330 // samples in callback in host endianess, ready for playback 331 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 332 333 #ifdef SCO_WAV_FILENAME 334 if (!num_samples_to_write) return; 335 num_samples = btstack_min(num_samples, num_samples_to_write); 336 num_samples_to_write -= num_samples; 337 wav_writer_write_int16(num_samples, data); 338 if (num_samples_to_write == 0){ 339 wav_writer_close(); 340 } 341 #endif /* SCO_WAV_FILENAME */ 342 343 #endif /* Demo mode sine or microphone */ 344 } 345 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */ 346 347 348 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 349 350 static void sco_demo_init_mSBC(void){ 351 printf("SCO Demo: Init mSBC\n"); 352 353 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 354 hfp_msbc_init(); 355 356 #ifdef SCO_WAV_FILENAME 357 num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 358 wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); 359 #endif 360 361 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 362 sco_demo_msbc_fill_sine_audio_frame(); 363 #endif 364 365 #ifdef SCO_MSBC_IN_FILENAME 366 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 367 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 368 #endif 369 370 #ifdef SCO_MSBC_OUT_FILENAME 371 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 372 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 373 #endif 374 375 audio_initialize(MSBC_SAMPLE_RATE); 376 } 377 378 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 379 #ifdef HAVE_POSIX_FILE_IO 380 if (num_samples_to_write){ 381 if (msbc_file_in){ 382 // log incoming mSBC data for testing 383 fwrite(packet+3, size-3, 1, msbc_file_in); 384 } 385 } 386 #endif 387 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 388 } 389 #endif 390 391 static void sco_demo_init_CVSD(void){ 392 printf("SCO Demo: Init CVSD\n"); 393 394 btstack_cvsd_plc_init(&cvsd_plc_state); 395 396 #ifdef SCO_WAV_FILENAME 397 num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 398 wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); 399 #endif 400 401 audio_initialize(CVSD_SAMPLE_RATE); 402 } 403 404 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 405 406 int16_t audio_frame_out[128]; // 407 408 if (size > sizeof(audio_frame_out)){ 409 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 410 return; 411 } 412 413 const int audio_bytes_read = size - 3; 414 const int num_samples = audio_bytes_read / BYTES_PER_FRAME; 415 416 // convert into host endian 417 int16_t audio_frame_in[128]; 418 int i; 419 for (i=0;i<num_samples;i++){ 420 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2); 421 } 422 423 // treat packet as bad frame if controller does not report 'all good' 424 bool bad_frame = (packet[1] & 0x30) != 0; 425 426 btstack_cvsd_plc_process_data(&cvsd_plc_state, bad_frame, audio_frame_in, num_samples, audio_frame_out); 427 428 #ifdef SCO_WAV_FILENAME 429 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 430 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 431 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 432 num_samples_to_write -= samples_to_write; 433 if (num_samples_to_write == 0){ 434 wav_writer_close(); 435 } 436 #endif 437 438 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 439 } 440 441 #endif 442 443 444 void sco_demo_close(void){ 445 printf("SCO demo close\n"); 446 447 printf("SCO demo statistics: "); 448 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 449 if (negotiated_codec == HFP_CODEC_MSBC){ 450 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); 451 } else 452 #endif 453 { 454 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); 455 } 456 457 negotiated_codec = -1; 458 459 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 460 461 #if defined(SCO_WAV_FILENAME) 462 wav_writer_close(); 463 #endif 464 465 audio_terminate(); 466 467 #endif 468 } 469 470 void sco_demo_set_codec(uint8_t codec){ 471 if (negotiated_codec == codec) return; 472 negotiated_codec = codec; 473 474 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 475 if (negotiated_codec == HFP_CODEC_MSBC){ 476 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 477 sco_demo_init_mSBC(); 478 #endif 479 } else { 480 sco_demo_init_CVSD(); 481 } 482 #endif 483 } 484 485 void sco_demo_init(void){ 486 487 #ifdef ENABLE_CLASSIC_LEGACY_CONNECTIONS_FOR_SCO_DEMOS 488 printf("Disable BR/EDR Secure Connctions due to incompatibilities with SCO connections\n"); 489 gap_secure_connections_enable(false); 490 #endif 491 492 // status 493 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 494 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n"); 495 #endif 496 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 497 if (btstack_audio_sink_get_instance()){ 498 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n"); 499 } else { 500 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 501 } 502 #endif 503 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 504 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 505 #endif 506 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 507 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 508 #endif 509 510 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 511 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 512 #else 513 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 514 #endif 515 } 516 517 void sco_report(void); 518 void sco_report(void){ 519 printf("SCO: sent %u, received %u\n", count_sent, count_received); 520 } 521 522 void sco_demo_send(hci_con_handle_t sco_handle){ 523 524 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 525 526 int sco_packet_length = hci_get_sco_packet_length(); 527 int sco_payload_length = sco_packet_length - 3; 528 529 hci_reserve_packet_buffer(); 530 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 531 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 532 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 533 if (negotiated_codec == HFP_CODEC_MSBC){ 534 535 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 536 log_error("mSBC stream is empty."); 537 } 538 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 539 #ifdef HAVE_POSIX_FILE_IO 540 if (msbc_file_out){ 541 // log outgoing mSBC data for testing 542 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 543 } 544 #endif 545 sco_demo_msbc_fill_sine_audio_frame(); 546 } else 547 #endif 548 { 549 const int audio_samples_per_packet = sco_payload_length / BYTES_PER_FRAME; 550 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]); 551 } 552 #endif 553 554 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 555 556 if (btstack_audio_source_get_instance()){ 557 558 if (negotiated_codec == HFP_CODEC_MSBC){ 559 // MSBC 560 561 if (audio_input_paused){ 562 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 563 // resume sending 564 audio_input_paused = 0; 565 } 566 } 567 568 if (!audio_input_paused){ 569 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 570 if (num_samples > MAX_NUM_MSBC_SAMPLES) return; // assert 571 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){ 572 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES]; 573 uint32_t bytes_read; 574 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 575 hfp_msbc_encode_audio_frame(sample_buffer); 576 num_audio_frames++; 577 } 578 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 579 log_error("mSBC stream should not be empty."); 580 } 581 } 582 583 if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 584 memset(sco_packet + 3, 0, sco_payload_length); 585 audio_input_paused = 1; 586 } else { 587 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 588 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 589 #ifdef HAVE_POSIX_FILE_IO 590 if (msbc_file_out){ 591 // log outgoing mSBC data for testing 592 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 593 } 594 #endif 595 #endif 596 } 597 598 } else { 599 // CVSD 600 601 log_debug("send: bytes avail %u, free %u", btstack_ring_buffer_bytes_available(&audio_input_ring_buffer), btstack_ring_buffer_bytes_free(&audio_input_ring_buffer)); 602 // fill with silence while paused 603 int bytes_to_copy = sco_payload_length; 604 if (audio_input_paused){ 605 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 606 // resume sending 607 audio_input_paused = 0; 608 } 609 } 610 611 // get data from ringbuffer 612 uint16_t pos = 0; 613 uint8_t * sample_data = &sco_packet[3]; 614 if (!audio_input_paused){ 615 uint32_t bytes_read = 0; 616 btstack_ring_buffer_read(&audio_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read); 617 // flip 16 on big endian systems 618 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 619 if (btstack_is_big_endian()){ 620 unsigned int i; 621 for (i=0;i<bytes_read;i+=2){ 622 uint8_t tmp = sample_data[i*2]; 623 sample_data[i*2] = sample_data[i*2+1]; 624 sample_data[i*2+1] = tmp; 625 } 626 } 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(sample_data + pos, 0, bytes_to_copy); 634 audio_input_paused = 1; 635 } 636 } 637 } 638 else { 639 // just send '0's 640 memset(sco_packet + 3, 0, sco_payload_length); 641 } 642 #endif 643 644 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 645 // store packet counter-xxxx 646 snprintf((char *)&sco_packet[3], 5, "%04u", phase++); 647 uint8_t ascii = (phase & 0x0f) + 'a'; 648 sco_packet[3+4] = '-'; 649 memset(&sco_packet[3+5], ascii, sco_payload_length-5); 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", (unsigned int) data_received, (unsigned int) packets, (unsigned int) crc_errors, (unsigned int) 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 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 727 case HFP_CODEC_MSBC: 728 sco_demo_receive_mSBC(packet, size); 729 break; 730 #endif 731 case HFP_CODEC_CVSD: 732 sco_demo_receive_CVSD(packet, size); 733 break; 734 default: 735 break; 736 } 737 dump_data = 0; 738 #endif 739 740 #if 0 741 if (packet[1] & 0x30){ 742 crc_errors++; 743 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 744 printf_hexdump(&packet[3], size-3); 745 return; 746 } 747 #endif 748 749 if (dump_data){ 750 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 751 printf("data: "); 752 int i; 753 for (i=3;i<size;i++){ 754 printf("%c", packet[i]); 755 } 756 printf("\n"); 757 dump_data = 0; 758 #endif 759 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 760 // colored hexdump with expected 761 static uint8_t expected_byte = 0; 762 int i; 763 printf("data: "); 764 for (i=3;i<size;i++){ 765 if (packet[i] != expected_byte){ 766 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 767 } else { 768 printf("%02x ", packet[i]); 769 } 770 expected_byte = packet[i]+1; 771 } 772 printf("\n"); 773 #endif 774 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 775 int i; 776 int contains_error = 0; 777 for (i=3;i<size;i++){ 778 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 779 contains_error = 1; 780 byte_errors++; 781 } 782 } 783 if (contains_error){ 784 printf("data: "); 785 for (i=0;i<3;i++){ 786 printf("%02x ", packet[i]); 787 } 788 for (i=3;i<size;i++){ 789 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 790 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 791 } else { 792 printf("%02x ", packet[i]); 793 } 794 } 795 printf("\n"); 796 } 797 #endif 798 } 799 } 800