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