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