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_ASCII 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 459 void sco_report(void); 460 void sco_report(void){ 461 printf("SCO: sent %u, received %u\n", count_sent, count_received); 462 } 463 464 void sco_demo_send(hci_con_handle_t sco_handle){ 465 466 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 467 468 int sco_packet_length = hci_get_sco_packet_length(); 469 int sco_payload_length = sco_packet_length - 3; 470 471 hci_reserve_packet_buffer(); 472 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 473 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 474 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 475 if (negotiated_codec == HFP_CODEC_MSBC){ 476 477 // overwrite 478 sco_payload_length = 24; 479 sco_packet_length = sco_payload_length + 3; 480 481 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 482 log_error("mSBC stream is empty."); 483 } 484 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 485 if (msbc_file_out){ 486 // log outgoing mSBC data for testing 487 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 488 } 489 490 sco_demo_msbc_fill_sine_audio_frame(); 491 } else 492 #endif 493 { 494 const int audio_samples_per_packet = sco_payload_length / BYTES_PER_FRAME; 495 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]); 496 } 497 #endif 498 499 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 500 501 #ifdef HAVE_PORTAUDIO 502 if (negotiated_codec == HFP_CODEC_MSBC){ 503 // MSBC 504 505 // overwrite 506 sco_payload_length = 24; 507 sco_packet_length = sco_payload_length + 3; 508 509 if (audio_input_paused){ 510 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 511 // resume sending 512 audio_input_paused = 0; 513 } 514 } 515 516 if (!audio_input_paused){ 517 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 518 if (num_samples > MAX_NUM_MSBC_SAMPLES) return; // assert 519 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){ 520 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES]; 521 uint32_t bytes_read; 522 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 523 hfp_msbc_encode_audio_frame(sample_buffer); 524 num_audio_frames++; 525 } 526 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 527 log_error("mSBC stream should not be empty."); 528 } 529 } 530 531 if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 532 memset(sco_packet + 3, 0, sco_payload_length); 533 audio_input_paused = 1; 534 } else { 535 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 536 if (msbc_file_out){ 537 // log outgoing mSBC data for testing 538 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 539 } 540 } 541 542 } else { 543 // CVSD 544 545 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)); 546 // fill with silence while paused 547 int bytes_to_copy = sco_payload_length; 548 if (audio_input_paused){ 549 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 550 // resume sending 551 audio_input_paused = 0; 552 } 553 } 554 555 // get data from ringbuffer 556 uint16_t pos = 0; 557 uint8_t * sample_data = &sco_packet[3]; 558 if (!audio_input_paused){ 559 uint32_t bytes_read = 0; 560 btstack_ring_buffer_read(&audio_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read); 561 // flip 16 on big endian systems 562 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 563 if (btstack_is_big_endian()){ 564 unsigned int i; 565 for (i=0;i<bytes_read;i+=2){ 566 uint8_t tmp = sample_data[i*2]; 567 sample_data[i*2] = sample_data[i*2+1]; 568 sample_data[i*2+1] = tmp; 569 } 570 } 571 bytes_to_copy -= bytes_read; 572 pos += bytes_read; 573 } 574 575 // fill with 0 if not enough 576 if (bytes_to_copy){ 577 memset(sample_data + pos, 0, bytes_to_copy); 578 audio_input_paused = 1; 579 } 580 } 581 #else 582 // just send '0's 583 if (negotiated_codec == HFP_CODEC_MSBC){ 584 sco_payload_length = 24; 585 sco_packet_length = sco_payload_length + 3; 586 } 587 memset(sco_packet + 3, 0, sco_payload_length); 588 #endif 589 #endif 590 591 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 592 // store packet counter-xxxx 593 snprintf((char *)&sco_packet[3], 5, "%04u", phase++); 594 uint8_t ascii = (phase & 0x0f) + 'a'; 595 sco_packet[3+4] = '-'; 596 memset(&sco_packet[3+5], ascii, sco_payload_length-5); 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 0 688 if (packet[1] & 0x30){ 689 crc_errors++; 690 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 691 printf_hexdump(&packet[3], size-3); 692 return; 693 } 694 #endif 695 696 if (dump_data){ 697 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 698 printf("data: "); 699 int i; 700 for (i=3;i<size;i++){ 701 printf("%c", packet[i]); 702 } 703 printf("\n"); 704 dump_data = 0; 705 #endif 706 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 707 // colored hexdump with expected 708 static uint8_t expected_byte = 0; 709 int i; 710 printf("data: "); 711 for (i=3;i<size;i++){ 712 if (packet[i] != expected_byte){ 713 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 714 } else { 715 printf("%02x ", packet[i]); 716 } 717 expected_byte = packet[i]+1; 718 } 719 printf("\n"); 720 #endif 721 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 722 int i; 723 int contains_error = 0; 724 for (i=3;i<size;i++){ 725 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 726 contains_error = 1; 727 byte_errors++; 728 } 729 } 730 if (contains_error){ 731 printf("data: "); 732 for (i=0;i<3;i++){ 733 printf("%02x ", packet[i]); 734 } 735 for (i=3;i<size;i++){ 736 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 737 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 738 } else { 739 printf("%02x ", packet[i]); 740 } 741 } 742 printf("\n"); 743 } 744 #endif 745 } 746 } 747