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