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 btstack_cvsd_plc_process_data(&cvsd_plc_state, audio_frame_in, num_samples, audio_frame_out); 419 420 #ifdef SCO_WAV_FILENAME 421 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 422 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 423 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 424 num_samples_to_write -= samples_to_write; 425 if (num_samples_to_write == 0){ 426 wav_writer_close(); 427 } 428 #endif 429 430 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 431 } 432 433 #endif 434 435 436 void sco_demo_close(void){ 437 printf("SCO demo close\n"); 438 439 printf("SCO demo statistics: "); 440 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 441 if (negotiated_codec == HFP_CODEC_MSBC){ 442 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); 443 } else 444 #endif 445 { 446 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); 447 } 448 449 negotiated_codec = -1; 450 451 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 452 453 #if defined(SCO_WAV_FILENAME) 454 wav_writer_close(); 455 #endif 456 457 audio_terminate(); 458 459 #endif 460 } 461 462 void sco_demo_set_codec(uint8_t codec){ 463 if (negotiated_codec == codec) return; 464 negotiated_codec = codec; 465 466 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 467 if (negotiated_codec == HFP_CODEC_MSBC){ 468 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 469 sco_demo_init_mSBC(); 470 #endif 471 } else { 472 sco_demo_init_CVSD(); 473 } 474 #endif 475 } 476 477 void sco_demo_init(void){ 478 // status 479 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 480 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n"); 481 #endif 482 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 483 if (btstack_audio_sink_get_instance()){ 484 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n"); 485 } else { 486 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 487 } 488 #endif 489 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 490 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 491 #endif 492 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 493 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 494 #endif 495 496 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 497 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 498 #else 499 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 500 #endif 501 } 502 503 void sco_report(void); 504 void sco_report(void){ 505 printf("SCO: sent %u, received %u\n", count_sent, count_received); 506 } 507 508 void sco_demo_send(hci_con_handle_t sco_handle){ 509 510 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 511 512 int sco_packet_length = hci_get_sco_packet_length(); 513 int sco_payload_length = sco_packet_length - 3; 514 515 hci_reserve_packet_buffer(); 516 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 517 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 518 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 519 if (negotiated_codec == HFP_CODEC_MSBC){ 520 521 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 522 log_error("mSBC stream is empty."); 523 } 524 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 525 #ifdef HAVE_POSIX_FILE_IO 526 if (msbc_file_out){ 527 // log outgoing mSBC data for testing 528 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 529 } 530 #endif 531 sco_demo_msbc_fill_sine_audio_frame(); 532 } else 533 #endif 534 { 535 const int audio_samples_per_packet = sco_payload_length / BYTES_PER_FRAME; 536 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]); 537 } 538 #endif 539 540 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 541 542 if (btstack_audio_source_get_instance()){ 543 544 if (negotiated_codec == HFP_CODEC_MSBC){ 545 // MSBC 546 547 if (audio_input_paused){ 548 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= MSBC_PA_PREBUFFER_BYTES){ 549 // resume sending 550 audio_input_paused = 0; 551 } 552 } 553 554 if (!audio_input_paused){ 555 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 556 if (num_samples > MAX_NUM_MSBC_SAMPLES) return; // assert 557 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){ 558 int16_t sample_buffer[MAX_NUM_MSBC_SAMPLES]; 559 uint32_t bytes_read; 560 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 561 hfp_msbc_encode_audio_frame(sample_buffer); 562 num_audio_frames++; 563 } 564 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 565 log_error("mSBC stream should not be empty."); 566 } 567 } 568 569 if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 570 memset(sco_packet + 3, 0, sco_payload_length); 571 audio_input_paused = 1; 572 } else { 573 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 574 #ifdef HAVE_POSIX_FILE_IO 575 if (msbc_file_out){ 576 // log outgoing mSBC data for testing 577 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 578 } 579 #endif 580 } 581 582 } else { 583 // CVSD 584 585 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)); 586 // fill with silence while paused 587 int bytes_to_copy = sco_payload_length; 588 if (audio_input_paused){ 589 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= CVSD_PA_PREBUFFER_BYTES){ 590 // resume sending 591 audio_input_paused = 0; 592 } 593 } 594 595 // get data from ringbuffer 596 uint16_t pos = 0; 597 uint8_t * sample_data = &sco_packet[3]; 598 if (!audio_input_paused){ 599 uint32_t bytes_read = 0; 600 btstack_ring_buffer_read(&audio_input_ring_buffer, sample_data, bytes_to_copy, &bytes_read); 601 // flip 16 on big endian systems 602 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 603 if (btstack_is_big_endian()){ 604 unsigned int i; 605 for (i=0;i<bytes_read;i+=2){ 606 uint8_t tmp = sample_data[i*2]; 607 sample_data[i*2] = sample_data[i*2+1]; 608 sample_data[i*2+1] = tmp; 609 } 610 } 611 bytes_to_copy -= bytes_read; 612 pos += bytes_read; 613 } 614 615 // fill with 0 if not enough 616 if (bytes_to_copy){ 617 memset(sample_data + pos, 0, bytes_to_copy); 618 audio_input_paused = 1; 619 } 620 } 621 } 622 else { 623 // just send '0's 624 memset(sco_packet + 3, 0, sco_payload_length); 625 } 626 #endif 627 628 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 629 // store packet counter-xxxx 630 snprintf((char *)&sco_packet[3], 5, "%04u", phase++); 631 uint8_t ascii = (phase & 0x0f) + 'a'; 632 sco_packet[3+4] = '-'; 633 memset(&sco_packet[3+5], ascii, sco_payload_length-5); 634 #endif 635 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 636 int j; 637 for (j=0;j<sco_payload_length;j++){ 638 sco_packet[3+j] = phase++; 639 } 640 #endif 641 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 642 int j; 643 for (j=0;j<sco_payload_length;j++){ 644 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 645 sco_packet[3+j] = 0x55; 646 } 647 #endif 648 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 649 int j; 650 for (j=0;j<sco_payload_length;j++){ 651 sco_packet[3+j] = 0x00; 652 } 653 // additional hack 654 // big_endian_store_16(sco_packet, 5, phase++); 655 (void) phase; 656 #endif 657 658 // test silence 659 // memset(sco_packet+3, 0, sco_payload_length); 660 661 // set handle + flags 662 little_endian_store_16(sco_packet, 0, sco_handle); 663 // set len 664 sco_packet[2] = sco_payload_length; 665 // finally send packet 666 hci_send_sco_packet_buffer(sco_packet_length); 667 668 // request another send event 669 hci_request_sco_can_send_now_event(); 670 671 count_sent++; 672 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 673 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 674 #endif 675 } 676 677 /** 678 * @brief Process received data 679 */ 680 #define ANSI_COLOR_RED "\x1b[31m" 681 #define ANSI_COLOR_GREEN "\x1b[32m" 682 #define ANSI_COLOR_YELLOW "\x1b[33m" 683 #define ANSI_COLOR_BLUE "\x1b[34m" 684 #define ANSI_COLOR_MAGENTA "\x1b[35m" 685 #define ANSI_COLOR_CYAN "\x1b[36m" 686 #define ANSI_COLOR_RESET "\x1b[0m" 687 688 void sco_demo_receive(uint8_t * packet, uint16_t size){ 689 690 dump_data = 1; 691 692 count_received++; 693 static uint32_t packets = 0; 694 static uint32_t crc_errors = 0; 695 static uint32_t data_received = 0; 696 static uint32_t byte_errors = 0; 697 698 data_received += size - 3; 699 packets++; 700 if (data_received > 100000){ 701 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); 702 crc_errors = 0; 703 byte_errors = 0; 704 data_received = 0; 705 packets = 0; 706 } 707 708 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 709 switch (negotiated_codec){ 710 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 711 case HFP_CODEC_MSBC: 712 sco_demo_receive_mSBC(packet, size); 713 break; 714 #endif 715 case HFP_CODEC_CVSD: 716 sco_demo_receive_CVSD(packet, size); 717 break; 718 default: 719 break; 720 } 721 dump_data = 0; 722 #endif 723 724 #if 0 725 if (packet[1] & 0x30){ 726 crc_errors++; 727 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 728 printf_hexdump(&packet[3], size-3); 729 return; 730 } 731 #endif 732 733 if (dump_data){ 734 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 735 printf("data: "); 736 int i; 737 for (i=3;i<size;i++){ 738 printf("%c", packet[i]); 739 } 740 printf("\n"); 741 dump_data = 0; 742 #endif 743 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 744 // colored hexdump with expected 745 static uint8_t expected_byte = 0; 746 int i; 747 printf("data: "); 748 for (i=3;i<size;i++){ 749 if (packet[i] != expected_byte){ 750 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 751 } else { 752 printf("%02x ", packet[i]); 753 } 754 expected_byte = packet[i]+1; 755 } 756 printf("\n"); 757 #endif 758 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 759 int i; 760 int contains_error = 0; 761 for (i=3;i<size;i++){ 762 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 763 contains_error = 1; 764 byte_errors++; 765 } 766 } 767 if (contains_error){ 768 printf("data: "); 769 for (i=0;i<3;i++){ 770 printf("%02x ", packet[i]); 771 } 772 for (i=3;i<size;i++){ 773 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 774 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 775 } else { 776 printf("%02x ", packet[i]); 777 } 778 } 779 printf("\n"); 780 } 781 #endif 782 } 783 } 784