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