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_SINE 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 // init buffers 236 memset(audio_output_ring_buffer_storage, 0, sizeof(audio_output_ring_buffer_storage)); 237 btstack_ring_buffer_init(&audio_output_ring_buffer, audio_output_ring_buffer_storage, sizeof(audio_output_ring_buffer_storage)); 238 #ifdef USE_AUDIO_INPUT 239 memset(audio_input_ring_buffer_storage, 0, sizeof(audio_input_ring_buffer_storage)); 240 btstack_ring_buffer_init(&audio_input_ring_buffer, audio_input_ring_buffer_storage, sizeof(audio_input_ring_buffer_storage)); 241 printf("Audio: Input buffer size %u\n", btstack_ring_buffer_bytes_free(&audio_input_ring_buffer)); 242 #endif 243 244 // config and setup audio playback/recording 245 const btstack_audio_t * audio = btstack_audio_get_instance(); 246 if (!audio) return 0; 247 248 void (*recording)(const int16_t * buffer, uint16_t num_samples) = NULL; 249 #ifdef USE_AUDIO_INPUT 250 recording = &recording_callback; 251 #endif 252 audio->init(1, sample_rate, &playback_callback, recording); 253 audio->start_stream(); 254 255 audio_output_paused = 1; 256 #ifdef USE_AUDIO_INPUT 257 audio_input_paused = 1; 258 #endif 259 260 return 1; 261 } 262 263 static void audio_terminate(void){ 264 const btstack_audio_t * audio = btstack_audio_get_instance(); 265 if (!audio) return; 266 audio->close(); 267 } 268 269 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 270 271 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 272 UNUSED(context); 273 UNUSED(sample_rate); 274 UNUSED(data); 275 UNUSED(num_samples); 276 UNUSED(num_channels); 277 278 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 279 280 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 281 282 // samples in callback in host endianess, ready for playback 283 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 284 285 #ifdef SCO_WAV_FILENAME 286 if (!num_samples_to_write) return; 287 num_samples = btstack_min(num_samples, num_samples_to_write); 288 num_samples_to_write -= num_samples; 289 wav_writer_write_int16(num_samples, data); 290 if (num_samples_to_write == 0){ 291 wav_writer_close(); 292 } 293 #endif /* SCO_WAV_FILENAME */ 294 295 #endif /* Demo mode sine or microphone */ 296 } 297 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */ 298 299 300 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 301 302 static void sco_demo_init_mSBC(void){ 303 printf("SCO Demo: Init mSBC\n"); 304 305 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 306 hfp_msbc_init(); 307 308 #ifdef SCO_WAV_FILENAME 309 num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 310 wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); 311 #endif 312 313 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 314 sco_demo_msbc_fill_sine_audio_frame(); 315 #endif 316 317 #ifdef SCO_MSBC_IN_FILENAME 318 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 319 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 320 #endif 321 322 #ifdef SCO_MSBC_OUT_FILENAME 323 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 324 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 325 #endif 326 327 audio_initialize(MSBC_SAMPLE_RATE); 328 } 329 330 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 331 if (num_samples_to_write){ 332 if (msbc_file_in){ 333 // log incoming mSBC data for testing 334 fwrite(packet+3, size-3, 1, msbc_file_in); 335 } 336 } 337 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 338 } 339 #endif 340 341 static void sco_demo_init_CVSD(void){ 342 printf("SCO Demo: Init CVSD\n"); 343 344 btstack_cvsd_plc_init(&cvsd_plc_state); 345 346 #ifdef SCO_WAV_FILENAME 347 num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 348 wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); 349 #endif 350 351 audio_initialize(CVSD_SAMPLE_RATE); 352 } 353 354 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 355 if (!num_samples_to_write) return; 356 357 int16_t audio_frame_out[128]; // 358 359 if (size > sizeof(audio_frame_out)){ 360 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 361 return; 362 } 363 364 const int audio_bytes_read = size - 3; 365 const int num_samples = audio_bytes_read / BYTES_PER_FRAME; 366 367 // convert into host endian 368 int16_t audio_frame_in[128]; 369 int i; 370 for (i=0;i<num_samples;i++){ 371 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2); 372 } 373 374 btstack_cvsd_plc_process_data(&cvsd_plc_state, audio_frame_in, num_samples, audio_frame_out); 375 376 #ifdef SCO_WAV_FILENAME 377 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 378 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 379 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 380 num_samples_to_write -= samples_to_write; 381 if (num_samples_to_write == 0){ 382 wav_writer_close(); 383 } 384 #endif 385 386 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 387 } 388 389 #endif 390 391 392 void sco_demo_close(void){ 393 printf("SCO demo close\n"); 394 395 printf("SCO demo statistics: "); 396 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 397 if (negotiated_codec == HFP_CODEC_MSBC){ 398 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); 399 } else 400 #endif 401 { 402 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); 403 } 404 405 negotiated_codec = -1; 406 407 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 408 409 #if defined(SCO_WAV_FILENAME) 410 wav_writer_close(); 411 #endif 412 413 audio_terminate(); 414 415 #endif 416 } 417 418 void sco_demo_set_codec(uint8_t codec){ 419 if (negotiated_codec == codec) return; 420 negotiated_codec = codec; 421 422 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 423 if (negotiated_codec == HFP_CODEC_MSBC){ 424 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 425 sco_demo_init_mSBC(); 426 #endif 427 } else { 428 sco_demo_init_CVSD(); 429 } 430 #endif 431 } 432 433 void sco_demo_init(void){ 434 // status 435 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 436 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n"); 437 #endif 438 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 439 #ifdef HAVE_PORTAUDIO 440 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n"); 441 #else 442 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 443 #endif 444 #endif 445 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 446 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 447 #endif 448 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 449 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 450 #endif 451 452 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 453 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 454 #else 455 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 456 #endif 457 458 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 459 phase = 'a'; 460 #endif 461 } 462 463 void sco_report(void); 464 void sco_report(void){ 465 printf("SCO: sent %u, received %u\n", count_sent, count_received); 466 } 467 468 void sco_demo_send(hci_con_handle_t sco_handle){ 469 470 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 471 472 int sco_packet_length = hci_get_sco_packet_length(); 473 int sco_payload_length = sco_packet_length - 3; 474 475 hci_reserve_packet_buffer(); 476 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 477 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 478 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 479 if (negotiated_codec == HFP_CODEC_MSBC){ 480 // overwrite 481 sco_payload_length = 24; 482 sco_packet_length = sco_payload_length + 3; 483 484 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 485 log_error("mSBC stream is empty."); 486 } 487 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 488 if (msbc_file_out){ 489 // log outgoing mSBC data for testing 490 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 491 } 492 493 sco_demo_msbc_fill_sine_audio_frame(); 494 } else 495 #endif 496 { 497 const int audio_samples_per_packet = sco_payload_length / BYTES_PER_FRAME; 498 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]); 499 } 500 #endif 501 502 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 503 504 #ifdef HAVE_PORTAUDIO 505 if (negotiated_codec == HFP_CODEC_MSBC){ 506 // MSBC 507 508 // overwrite 509 sco_payload_length = 24; 510 sco_packet_length = sco_payload_length + 3; 511 512 if (audio_input_paused){ 513 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 514 // resume sending 515 audio_input_paused = 0; 516 } 517 } 518 519 if (!audio_input_paused){ 520 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 521 if (num_samples > MAX_NUM_MSBC_SAMPLES) return; // assert 522 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){ 523 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES]; 524 uint32_t bytes_read; 525 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 526 hfp_msbc_encode_audio_frame(sample_buffer); 527 num_audio_frames++; 528 } 529 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 530 log_error("mSBC stream should not be empty."); 531 } 532 } 533 534 if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 535 memset(sco_packet + 3, 0, sco_payload_length); 536 audio_input_paused = 1; 537 } else { 538 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 539 if (msbc_file_out){ 540 // log outgoing mSBC data for testing 541 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 542 } 543 } 544 545 } else { 546 // CVSD 547 548 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)); 549 // fill with silence while paused 550 int bytes_to_copy = sco_payload_length; 551 if (audio_input_paused){ 552 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 553 // resume sending 554 audio_input_paused = 0; 555 } 556 } 557 558 // get data from ringbuffer 559 uint16_t pos = 0; 560 uint8_t * sample_data = &sco_packet[3]; 561 if (!audio_input_paused){ 562 uint32_t bytes_read = 0; 563 btstack_ring_buffer_read(&audio_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read); 564 // flip 16 on big endian systems 565 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 566 if (btstack_is_big_endian()){ 567 unsigned int i; 568 for (i=0;i<bytes_read;i+=2){ 569 uint8_t tmp = sample_data[i*2]; 570 sample_data[i*2] = sample_data[i*2+1]; 571 sample_data[i*2+1] = tmp; 572 } 573 } 574 bytes_to_copy -= bytes_read; 575 pos += bytes_read; 576 } 577 578 // fill with 0 if not enough 579 if (bytes_to_copy){ 580 memset(sample_data + pos, 0, bytes_to_copy); 581 audio_input_paused = 1; 582 } 583 } 584 #else 585 // just send '0's 586 if (negotiated_codec == HFP_CODEC_MSBC){ 587 sco_payload_length = 24; 588 sco_packet_length = sco_payload_length + 3; 589 } 590 memset(sco_packet + 3, 0, sco_payload_length); 591 #endif 592 #endif 593 594 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 595 memset(&sco_packet[3], phase++, sco_payload_length); 596 if (phase > 'z') phase = 'a'; 597 #endif 598 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 599 int j; 600 for (j=0;j<sco_payload_length;j++){ 601 sco_packet[3+j] = phase++; 602 } 603 #endif 604 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 605 int j; 606 for (j=0;j<sco_payload_length;j++){ 607 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 608 sco_packet[3+j] = 0x55; 609 } 610 #endif 611 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 612 int j; 613 for (j=0;j<sco_payload_length;j++){ 614 sco_packet[3+j] = 0x00; 615 } 616 // additional hack 617 // big_endian_store_16(sco_packet, 5, phase++); 618 (void) phase; 619 #endif 620 621 // test silence 622 // memset(sco_packet+3, 0, sco_payload_length); 623 624 // set handle + flags 625 little_endian_store_16(sco_packet, 0, sco_handle); 626 // set len 627 sco_packet[2] = sco_payload_length; 628 // finally send packet 629 hci_send_sco_packet_buffer(sco_packet_length); 630 631 // request another send event 632 hci_request_sco_can_send_now_event(); 633 634 count_sent++; 635 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 636 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 637 #endif 638 } 639 640 /** 641 * @brief Process received data 642 */ 643 #define ANSI_COLOR_RED "\x1b[31m" 644 #define ANSI_COLOR_GREEN "\x1b[32m" 645 #define ANSI_COLOR_YELLOW "\x1b[33m" 646 #define ANSI_COLOR_BLUE "\x1b[34m" 647 #define ANSI_COLOR_MAGENTA "\x1b[35m" 648 #define ANSI_COLOR_CYAN "\x1b[36m" 649 #define ANSI_COLOR_RESET "\x1b[0m" 650 651 void sco_demo_receive(uint8_t * packet, uint16_t size){ 652 653 dump_data = 1; 654 655 count_received++; 656 static uint32_t packets = 0; 657 static uint32_t crc_errors = 0; 658 static uint32_t data_received = 0; 659 static uint32_t byte_errors = 0; 660 661 data_received += size - 3; 662 packets++; 663 if (data_received > 100000){ 664 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); 665 crc_errors = 0; 666 byte_errors = 0; 667 data_received = 0; 668 packets = 0; 669 } 670 671 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 672 switch (negotiated_codec){ 673 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 674 case HFP_CODEC_MSBC: 675 sco_demo_receive_mSBC(packet, size); 676 break; 677 #endif 678 case HFP_CODEC_CVSD: 679 sco_demo_receive_CVSD(packet, size); 680 break; 681 default: 682 break; 683 } 684 dump_data = 0; 685 #endif 686 687 if (packet[1] & 0x30){ 688 crc_errors++; 689 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 690 // printf_hexdump(&packet[3], size-3); 691 return; 692 } 693 if (dump_data){ 694 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 695 printf("data: "); 696 int i; 697 for (i=3;i<size;i++){ 698 printf("%c", packet[i]); 699 } 700 printf("\n"); 701 dump_data = 0; 702 #endif 703 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 704 // colored hexdump with expected 705 static uint8_t expected_byte = 0; 706 int i; 707 printf("data: "); 708 for (i=3;i<size;i++){ 709 if (packet[i] != expected_byte){ 710 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 711 } else { 712 printf("%02x ", packet[i]); 713 } 714 expected_byte = packet[i]+1; 715 } 716 printf("\n"); 717 #endif 718 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 719 int i; 720 int contains_error = 0; 721 for (i=3;i<size;i++){ 722 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 723 contains_error = 1; 724 byte_errors++; 725 } 726 } 727 if (contains_error){ 728 printf("data: "); 729 for (i=0;i<3;i++){ 730 printf("%02x ", packet[i]); 731 } 732 for (i=3;i<size;i++){ 733 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 734 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 735 } else { 736 printf("%02x ", packet[i]); 737 } 738 } 739 printf("\n"); 740 } 741 #endif 742 } 743 } 744