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_demo_send(hci_con_handle_t sco_handle){ 490 491 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 492 493 int sco_packet_length = hci_get_sco_packet_length(); 494 int sco_payload_length = sco_packet_length - 3; 495 496 hci_reserve_packet_buffer(); 497 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 498 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 499 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 500 if (negotiated_codec == HFP_CODEC_MSBC){ 501 502 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 503 log_error("mSBC stream is empty."); 504 } 505 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 506 #ifdef HAVE_POSIX_FILE_IO 507 if (msbc_file_out){ 508 // log outgoing mSBC data for testing 509 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 510 } 511 #endif 512 sco_demo_msbc_fill_sine_audio_frame(); 513 } else 514 #endif 515 { 516 const int audio_samples_per_packet = sco_payload_length / BYTES_PER_FRAME; 517 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]); 518 } 519 #endif 520 521 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 522 523 if (btstack_audio_source_get_instance()){ 524 525 if (negotiated_codec == HFP_CODEC_MSBC){ 526 // MSBC 527 528 if (audio_input_paused){ 529 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 530 // resume sending 531 audio_input_paused = 0; 532 } 533 } 534 535 if (!audio_input_paused){ 536 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 537 if (num_samples > MAX_NUM_MSBC_SAMPLES) return; // assert 538 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){ 539 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES]; 540 uint32_t bytes_read; 541 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 542 hfp_msbc_encode_audio_frame(sample_buffer); 543 num_audio_frames++; 544 } 545 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 546 log_error("mSBC stream should not be empty."); 547 } 548 } 549 550 if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 551 memset(sco_packet + 3, 0, sco_payload_length); 552 audio_input_paused = 1; 553 } else { 554 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 555 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 556 #ifdef HAVE_POSIX_FILE_IO 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 #endif 562 #endif 563 } 564 565 } else { 566 // CVSD 567 568 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)); 569 // fill with silence while paused 570 int bytes_to_copy = sco_payload_length; 571 if (audio_input_paused){ 572 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 573 // resume sending 574 audio_input_paused = 0; 575 } 576 } 577 578 // get data from ringbuffer 579 uint16_t pos = 0; 580 uint8_t * sample_data = &sco_packet[3]; 581 if (!audio_input_paused){ 582 uint32_t bytes_read = 0; 583 btstack_ring_buffer_read(&audio_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read); 584 // flip 16 on big endian systems 585 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 586 if (btstack_is_big_endian()){ 587 unsigned int i; 588 for (i=0;i<bytes_read;i+=2){ 589 uint8_t tmp = sample_data[i*2]; 590 sample_data[i*2] = sample_data[i*2+1]; 591 sample_data[i*2+1] = tmp; 592 } 593 } 594 bytes_to_copy -= bytes_read; 595 pos += bytes_read; 596 } 597 598 // fill with 0 if not enough 599 if (bytes_to_copy){ 600 memset(sample_data + pos, 0, bytes_to_copy); 601 audio_input_paused = 1; 602 } 603 } 604 } 605 else { 606 // just send '0's 607 memset(sco_packet + 3, 0, sco_payload_length); 608 } 609 #endif 610 611 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 612 // store packet counter-xxxx 613 snprintf((char *)&sco_packet[3], 5, "%04u", phase++); 614 uint8_t ascii = (phase & 0x0f) + 'a'; 615 sco_packet[3+4] = '-'; 616 memset(&sco_packet[3+5], ascii, sco_payload_length-5); 617 #endif 618 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 619 int j; 620 for (j=0;j<sco_payload_length;j++){ 621 sco_packet[3+j] = phase++; 622 } 623 #endif 624 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 625 int j; 626 for (j=0;j<sco_payload_length;j++){ 627 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 628 sco_packet[3+j] = 0x55; 629 } 630 #endif 631 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 632 int j; 633 for (j=0;j<sco_payload_length;j++){ 634 sco_packet[3+j] = 0x00; 635 } 636 // additional hack 637 // big_endian_store_16(sco_packet, 5, phase++); 638 (void) phase; 639 #endif 640 641 // test silence 642 // memset(sco_packet+3, 0, sco_payload_length); 643 644 // set handle + flags 645 little_endian_store_16(sco_packet, 0, sco_handle); 646 // set len 647 sco_packet[2] = sco_payload_length; 648 // finally send packet 649 hci_send_sco_packet_buffer(sco_packet_length); 650 651 // request another send event 652 hci_request_sco_can_send_now_event(); 653 654 count_sent++; 655 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 656 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 657 #endif 658 } 659 660 /** 661 * @brief Process received data 662 */ 663 #define ANSI_COLOR_RED "\x1b[31m" 664 #define ANSI_COLOR_GREEN "\x1b[32m" 665 #define ANSI_COLOR_YELLOW "\x1b[33m" 666 #define ANSI_COLOR_BLUE "\x1b[34m" 667 #define ANSI_COLOR_MAGENTA "\x1b[35m" 668 #define ANSI_COLOR_CYAN "\x1b[36m" 669 #define ANSI_COLOR_RESET "\x1b[0m" 670 671 void sco_demo_receive(uint8_t * packet, uint16_t size){ 672 673 dump_data = 1; 674 675 count_received++; 676 static uint32_t packets = 0; 677 static uint32_t crc_errors = 0; 678 static uint32_t data_received = 0; 679 static uint32_t byte_errors = 0; 680 681 data_received += size - 3; 682 packets++; 683 if (data_received > 100000){ 684 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); 685 crc_errors = 0; 686 byte_errors = 0; 687 data_received = 0; 688 packets = 0; 689 } 690 691 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 692 switch (negotiated_codec){ 693 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 694 case HFP_CODEC_MSBC: 695 sco_demo_receive_mSBC(packet, size); 696 break; 697 #endif 698 case HFP_CODEC_CVSD: 699 sco_demo_receive_CVSD(packet, size); 700 break; 701 default: 702 break; 703 } 704 dump_data = 0; 705 #endif 706 707 #if 0 708 if (packet[1] & 0x30){ 709 crc_errors++; 710 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 711 printf_hexdump(&packet[3], size-3); 712 return; 713 } 714 #endif 715 716 if (dump_data){ 717 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 718 printf("data: "); 719 int i; 720 for (i=3;i<size;i++){ 721 printf("%c", packet[i]); 722 } 723 printf("\n"); 724 dump_data = 0; 725 #endif 726 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 727 // colored hexdump with expected 728 static uint8_t expected_byte = 0; 729 int i; 730 printf("data: "); 731 for (i=3;i<size;i++){ 732 if (packet[i] != expected_byte){ 733 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 734 } else { 735 printf("%02x ", packet[i]); 736 } 737 expected_byte = packet[i]+1; 738 } 739 printf("\n"); 740 #endif 741 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 742 int i; 743 int contains_error = 0; 744 for (i=3;i<size;i++){ 745 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 746 contains_error = 1; 747 byte_errors++; 748 } 749 } 750 if (contains_error){ 751 printf("data: "); 752 for (i=0;i<3;i++){ 753 printf("%02x ", packet[i]); 754 } 755 for (i=3;i<size;i++){ 756 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 757 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 758 } else { 759 printf("%02x ", packet[i]); 760 } 761 } 762 printf("\n"); 763 } 764 #endif 765 } 766 } 767