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 397 int16_t audio_frame_out[128]; // 398 399 if (size > sizeof(audio_frame_out)){ 400 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 401 return; 402 } 403 404 const int audio_bytes_read = size - 3; 405 const int num_samples = audio_bytes_read / BYTES_PER_FRAME; 406 407 // convert into host endian 408 int16_t audio_frame_in[128]; 409 int i; 410 for (i=0;i<num_samples;i++){ 411 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2); 412 } 413 414 btstack_cvsd_plc_process_data(&cvsd_plc_state, audio_frame_in, num_samples, audio_frame_out); 415 416 #ifdef SCO_WAV_FILENAME 417 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 418 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 419 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 420 num_samples_to_write -= samples_to_write; 421 if (num_samples_to_write == 0){ 422 wav_writer_close(); 423 } 424 #endif 425 426 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 427 } 428 429 #endif 430 431 432 void sco_demo_close(void){ 433 printf("SCO demo close\n"); 434 435 printf("SCO demo statistics: "); 436 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 437 if (negotiated_codec == HFP_CODEC_MSBC){ 438 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); 439 } else 440 #endif 441 { 442 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); 443 } 444 445 negotiated_codec = -1; 446 447 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 448 449 #if defined(SCO_WAV_FILENAME) 450 wav_writer_close(); 451 #endif 452 453 audio_terminate(); 454 455 #endif 456 } 457 458 void sco_demo_set_codec(uint8_t codec){ 459 if (negotiated_codec == codec) return; 460 negotiated_codec = codec; 461 462 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 463 if (negotiated_codec == HFP_CODEC_MSBC){ 464 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 465 sco_demo_init_mSBC(); 466 #endif 467 } else { 468 sco_demo_init_CVSD(); 469 } 470 #endif 471 } 472 473 void sco_demo_init(void){ 474 // status 475 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 476 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n"); 477 #endif 478 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 479 if (btstack_audio_sink_get_instance()){ 480 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n"); 481 } else { 482 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 483 } 484 #endif 485 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 486 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 487 #endif 488 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 489 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 490 #endif 491 492 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 493 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 494 #else 495 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 496 #endif 497 } 498 499 void sco_report(void); 500 void sco_report(void){ 501 printf("SCO: sent %u, received %u\n", count_sent, count_received); 502 } 503 504 void sco_demo_send(hci_con_handle_t sco_handle){ 505 506 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 507 508 int sco_packet_length = hci_get_sco_packet_length(); 509 int sco_payload_length = sco_packet_length - 3; 510 511 hci_reserve_packet_buffer(); 512 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 513 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 514 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 515 if (negotiated_codec == HFP_CODEC_MSBC){ 516 517 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 518 log_error("mSBC stream is empty."); 519 } 520 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 521 if (msbc_file_out){ 522 // log outgoing mSBC data for testing 523 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 524 } 525 526 sco_demo_msbc_fill_sine_audio_frame(); 527 } else 528 #endif 529 { 530 const int audio_samples_per_packet = sco_payload_length / BYTES_PER_FRAME; 531 sco_demo_sine_wave_int16_at_8000_hz_little_endian(audio_samples_per_packet, &sco_packet[3]); 532 } 533 #endif 534 535 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 536 537 if (btstack_audio_source_get_instance()){ 538 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 } 615 else { 616 // just send '0's 617 memset(sco_packet + 3, 0, sco_payload_length); 618 } 619 #endif 620 621 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 622 // store packet counter-xxxx 623 snprintf((char *)&sco_packet[3], 5, "%04u", phase++); 624 uint8_t ascii = (phase & 0x0f) + 'a'; 625 sco_packet[3+4] = '-'; 626 memset(&sco_packet[3+5], ascii, sco_payload_length-5); 627 #endif 628 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 629 int j; 630 for (j=0;j<sco_payload_length;j++){ 631 sco_packet[3+j] = phase++; 632 } 633 #endif 634 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 635 int j; 636 for (j=0;j<sco_payload_length;j++){ 637 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 638 sco_packet[3+j] = 0x55; 639 } 640 #endif 641 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 642 int j; 643 for (j=0;j<sco_payload_length;j++){ 644 sco_packet[3+j] = 0x00; 645 } 646 // additional hack 647 // big_endian_store_16(sco_packet, 5, phase++); 648 (void) phase; 649 #endif 650 651 // test silence 652 // memset(sco_packet+3, 0, sco_payload_length); 653 654 // set handle + flags 655 little_endian_store_16(sco_packet, 0, sco_handle); 656 // set len 657 sco_packet[2] = sco_payload_length; 658 // finally send packet 659 hci_send_sco_packet_buffer(sco_packet_length); 660 661 // request another send event 662 hci_request_sco_can_send_now_event(); 663 664 count_sent++; 665 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 666 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 667 #endif 668 } 669 670 /** 671 * @brief Process received data 672 */ 673 #define ANSI_COLOR_RED "\x1b[31m" 674 #define ANSI_COLOR_GREEN "\x1b[32m" 675 #define ANSI_COLOR_YELLOW "\x1b[33m" 676 #define ANSI_COLOR_BLUE "\x1b[34m" 677 #define ANSI_COLOR_MAGENTA "\x1b[35m" 678 #define ANSI_COLOR_CYAN "\x1b[36m" 679 #define ANSI_COLOR_RESET "\x1b[0m" 680 681 void sco_demo_receive(uint8_t * packet, uint16_t size){ 682 683 dump_data = 1; 684 685 count_received++; 686 static uint32_t packets = 0; 687 static uint32_t crc_errors = 0; 688 static uint32_t data_received = 0; 689 static uint32_t byte_errors = 0; 690 691 data_received += size - 3; 692 packets++; 693 if (data_received > 100000){ 694 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); 695 crc_errors = 0; 696 byte_errors = 0; 697 data_received = 0; 698 packets = 0; 699 } 700 701 #if (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) || (SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE) 702 switch (negotiated_codec){ 703 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 704 case HFP_CODEC_MSBC: 705 sco_demo_receive_mSBC(packet, size); 706 break; 707 #endif 708 case HFP_CODEC_CVSD: 709 sco_demo_receive_CVSD(packet, size); 710 break; 711 default: 712 break; 713 } 714 dump_data = 0; 715 #endif 716 717 #if 0 718 if (packet[1] & 0x30){ 719 crc_errors++; 720 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 721 printf_hexdump(&packet[3], size-3); 722 return; 723 } 724 #endif 725 726 if (dump_data){ 727 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 728 printf("data: "); 729 int i; 730 for (i=3;i<size;i++){ 731 printf("%c", packet[i]); 732 } 733 printf("\n"); 734 dump_data = 0; 735 #endif 736 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 737 // colored hexdump with expected 738 static uint8_t expected_byte = 0; 739 int i; 740 printf("data: "); 741 for (i=3;i<size;i++){ 742 if (packet[i] != expected_byte){ 743 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 744 } else { 745 printf("%02x ", packet[i]); 746 } 747 expected_byte = packet[i]+1; 748 } 749 printf("\n"); 750 #endif 751 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 752 int i; 753 int contains_error = 0; 754 for (i=3;i<size;i++){ 755 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 756 contains_error = 1; 757 byte_errors++; 758 } 759 } 760 if (contains_error){ 761 printf("data: "); 762 for (i=0;i<3;i++){ 763 printf("%02x ", packet[i]); 764 } 765 for (i=3;i<size;i++){ 766 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 767 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 768 } else { 769 printf("%02x ", packet[i]); 770 } 771 } 772 printf("\n"); 773 } 774 #endif 775 } 776 } 777