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