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