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