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 /* 39 * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo 40 */ 41 42 #include <stdio.h> 43 44 #include "sco_demo_util.h" 45 #include "btstack_debug.h" 46 #include "classic/btstack_sbc.h" 47 #include "classic/btstack_cvsd_plc.h" 48 #include "classic/hfp_msbc.h" 49 #include "classic/hfp.h" 50 51 #ifdef HAVE_POSIX_FILE_IO 52 #include "wav_util.h" 53 #endif 54 55 #ifdef HAVE_PORTAUDIO 56 #include <portaudio.h> 57 #include "btstack_ring_buffer.h" 58 #endif 59 60 61 // test modes 62 #define SCO_DEMO_MODE_SINE 0 63 #define SCO_DEMO_MODE_ASCII 1 64 #define SCO_DEMO_MODE_COUNTER 2 65 #define SCO_DEMO_MODE_55 3 66 #define SCO_DEMO_MODE_00 4 67 #define SCO_DEMO_MODE_MICROPHONE 5 68 69 // SCO demo configuration 70 #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE 71 72 // number of sco packets until 'report' on console 73 #define SCO_REPORT_PERIOD 100 74 75 // length and name of wav file on disc 76 #define SCO_WAV_DURATION_IN_SECONDS 15 77 #define SCO_WAV_FILENAME "sco_input.wav" 78 79 // name of sbc test files 80 #define SCO_MSBC_OUT_FILENAME "sco_output.msbc" 81 #define SCO_MSBC_IN_FILENAME "sco_input.msbc" 82 83 // pre-buffer for CVSD and mSBC - also defines latency 84 #define SCO_CVSD_PA_PREBUFFER_MS 50 85 #define SCO_MSBC_PA_PREBUFFER_MS 50 86 87 // constants 88 #define NUM_CHANNELS 1 89 #define CVSD_BYTES_PER_FRAME (2*NUM_CHANNELS) 90 #define CVSD_SAMPLE_RATE 8000 91 #define MSBC_SAMPLE_RATE 16000 92 #define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) 93 94 #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE || SCO_DEMO_MODE_MICROPHONE) 95 #define USE_PORTAUDIO 96 #define CVSD_PA_PREBUFFER_BYTES (SCO_CVSD_PA_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * CVSD_BYTES_PER_FRAME) 97 #define MSBC_PA_PREBUFFER_BYTES (SCO_MSBC_PA_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * MSBC_BYTES_PER_FRAME) 98 #endif 99 100 #ifdef USE_PORTAUDIO 101 static PaStream * pa_output_stream; 102 static int pa_output_started = 0; 103 static int pa_output_paused = 0; 104 static uint8_t pa_output_ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES]; 105 static btstack_ring_buffer_t pa_output_ring_buffer; 106 #endif 107 108 static int dump_data = 1; 109 static int count_sent = 0; 110 static int count_received = 0; 111 static int negotiated_codec = -1; 112 static int phase = 0; 113 static int num_audio_frames = 0; 114 115 static btstack_sbc_decoder_state_t decoder_state; 116 static btstack_cvsd_plc_state_t cvsd_plc_state; 117 static int num_samples_to_write; 118 119 FILE * msbc_file_in; 120 FILE * msbc_file_out; 121 122 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 123 124 // input signal: pre-computed sine wave, 160 Hz at 16000 kHz 125 static const int16_t sine_int16_at_16000hz[] = { 126 0, 2057, 4107, 6140, 8149, 10126, 12062, 13952, 15786, 17557, 127 19260, 20886, 22431, 23886, 25247, 26509, 27666, 28714, 29648, 30466, 128 31163, 31738, 32187, 32509, 32702, 32767, 32702, 32509, 32187, 31738, 129 31163, 30466, 29648, 28714, 27666, 26509, 25247, 23886, 22431, 20886, 130 19260, 17557, 15786, 13952, 12062, 10126, 8149, 6140, 4107, 2057, 131 0, -2057, -4107, -6140, -8149, -10126, -12062, -13952, -15786, -17557, 132 -19260, -20886, -22431, -23886, -25247, -26509, -27666, -28714, -29648, -30466, 133 -31163, -31738, -32187, -32509, -32702, -32767, -32702, -32509, -32187, -31738, 134 -31163, -30466, -29648, -28714, -27666, -26509, -25247, -23886, -22431, -20886, 135 -19260, -17557, -15786, -13952, -12062, -10126, -8149, -6140, -4107, -2057, 136 }; 137 138 // ony use every second value from 16khz table 139 static void sco_demo_sine_wave_int16_at_8000_hz(int num_samples, int16_t * data){ 140 int i; 141 for (i=0; i < num_samples; i++){ 142 data[i] = sine_int16_at_16000hz[phase++]; 143 phase++; 144 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 145 phase = 0; 146 } 147 } 148 } 149 150 static void sco_demo_fill_audio_frame(void){ 151 if (!hfp_msbc_can_encode_audio_frame_now()) return; 152 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 153 int16_t sample_buffer[num_samples]; 154 sco_demo_sine_wave_int16_at_8000_hz(num_samples, sample_buffer); 155 hfp_msbc_encode_audio_frame(sample_buffer); 156 num_audio_frames++; 157 } 158 159 #ifdef SCO_WAV_FILENAME 160 #ifdef USE_PORTAUDIO 161 static int portaudio_callback( const void *inputBuffer, void *outputBuffer, 162 unsigned long framesPerBuffer, 163 const PaStreamCallbackTimeInfo* timeInfo, 164 PaStreamCallbackFlags statusFlags, 165 void *userData ) { 166 (void) timeInfo; /* Prevent unused variable warnings. */ 167 (void) statusFlags; 168 (void) inputBuffer; 169 (void) userData; 170 171 // config based on codec 172 int bytes_to_copy; 173 int prebuffer_bytes; 174 switch (negotiated_codec){ 175 case HFP_CODEC_MSBC: 176 bytes_to_copy = framesPerBuffer * MSBC_BYTES_PER_FRAME; 177 prebuffer_bytes = MSBC_PA_PREBUFFER_BYTES; 178 break; 179 case HFP_CODEC_CVSD: 180 bytes_to_copy = framesPerBuffer * CVSD_BYTES_PER_FRAME; 181 prebuffer_bytes = CVSD_PA_PREBUFFER_BYTES; 182 break; 183 default: 184 bytes_to_copy = framesPerBuffer * 2; // assume 1 channel / 16 bit audio samples 185 prebuffer_bytes = 0xfffffff; 186 break; 187 } 188 189 // fill with silence while paused 190 if (pa_output_paused){ 191 if (btstack_ring_buffer_bytes_available(&pa_output_ring_buffer) < prebuffer_bytes){ 192 memset(outputBuffer, 0, bytes_to_copy); 193 return 0; 194 } else { 195 // resume playback 196 pa_output_paused = 0; 197 } 198 } 199 200 // get data from ringbuffer 201 uint32_t bytes_read = 0; 202 btstack_ring_buffer_read(&pa_output_ring_buffer, outputBuffer, bytes_to_copy, &bytes_read); 203 bytes_to_copy -= bytes_read; 204 205 // fill with 0 if not enough 206 if (bytes_to_copy){ 207 memset(outputBuffer + bytes_read, 0, bytes_to_copy); 208 pa_output_paused = 1; 209 } 210 return 0; 211 } 212 213 static void portaudio_start(void){ 214 if (!pa_output_started){ 215 /* -- start stream -- */ 216 PaError err = Pa_StartStream(pa_output_stream); 217 if (err != paNoError){ 218 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 219 return; 220 } 221 pa_output_started = 1; 222 pa_output_paused = 1; 223 } 224 } 225 226 // return 1 if ok 227 static int portaudio_initialize(int sample_rate){ 228 PaError err; 229 PaStreamParameters outputParameters; 230 231 /* -- initialize PortAudio -- */ 232 printf("PortAudio: Initialize\n"); 233 err = Pa_Initialize(); 234 if( err != paNoError ) return 0; 235 /* -- setup input and output -- */ 236 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 237 outputParameters.channelCount = NUM_CHANNELS; 238 outputParameters.sampleFormat = paInt16; 239 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 240 outputParameters.hostApiSpecificStreamInfo = NULL; 241 /* -- setup stream -- */ 242 printf("PortAudio: Open stream\n"); 243 err = Pa_OpenStream( 244 &stream, 245 NULL, // &inputParameters, 246 &outputParameters, 247 sample_rate, 248 0, 249 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 250 portaudio_callback, 251 NULL ); 252 if (err != paNoError){ 253 printf("Error opening portaudio stream: \"%s\"\n", Pa_GetErrorText(err)); 254 return 0; 255 } 256 memset(pa_output_ring_buffer_storage, 0, sizeof(pa_output_ring_buffer_storage)); 257 btstack_ring_buffer_init(&pa_output_ring_buffer, pa_output_ring_buffer_storage, sizeof(pa_output_ring_buffer_storage)); 258 pa_output_started = 0; 259 return 1; 260 } 261 #endif 262 263 264 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 265 UNUSED(context); 266 UNUSED(sample_rate); 267 268 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 269 #ifdef HAVE_PORTAUDIO 270 portaudio_start(); 271 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 272 #else 273 UNUSED(num_channels); 274 #endif 275 276 if (!num_samples_to_write) return; 277 278 num_samples = btstack_min(num_samples, num_samples_to_write); 279 num_samples_to_write -= num_samples; 280 281 wav_writer_write_int16(num_samples, data); 282 283 if (num_samples_to_write == 0){ 284 sco_demo_close(); 285 } 286 } 287 288 static void sco_demo_init_mSBC(void){ 289 printf("SCO Demo: Init mSBC\n"); 290 291 wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); 292 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 293 294 num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 295 296 hfp_msbc_init(); 297 sco_demo_fill_audio_frame(); 298 299 #ifdef SCO_MSBC_IN_FILENAME 300 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 301 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 302 #endif 303 #ifdef SCO_MSBC_OUT_FILENAME 304 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 305 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 306 #endif 307 308 #ifdef USE_PORTAUDIO 309 portaudio_initialize(MSBC_SAMPLE_RATE); 310 #endif 311 } 312 313 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 314 if (num_samples_to_write){ 315 if (msbc_file_in){ 316 // log incoming mSBC data for testing 317 fwrite(packet+3, size-3, 1, msbc_file_in); 318 } 319 } 320 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 321 } 322 323 static void sco_demo_init_CVSD(void){ 324 printf("SCO Demo: Init CVSD\n"); 325 326 wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); 327 btstack_cvsd_plc_init(&cvsd_plc_state); 328 329 num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 330 331 #ifdef USE_PORTAUDIO 332 portaudio_initialize(CVSD_SAMPLE_RATE); 333 #endif 334 } 335 336 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 337 if (!num_samples_to_write) return; 338 int16_t audio_frame_out[255]; // 339 340 if (size > sizeof(audio_frame_out)){ 341 printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 342 return; 343 } 344 const int audio_bytes_read = size - 3; 345 const int num_samples = audio_bytes_read / CVSD_BYTES_PER_FRAME; 346 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 347 348 #if 0 349 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 350 #else 351 memcpy(audio_frame_out, packet+3, audio_bytes_read); 352 #endif 353 354 wav_writer_write_int16(samples_to_write, audio_frame_out); 355 num_samples_to_write -= samples_to_write; 356 if (num_samples_to_write == 0){ 357 sco_demo_close(); 358 } 359 #ifdef USE_PORTAUDIO 360 portaudio_start(); 361 btstack_ring_buffer_write(&pa_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 362 #endif 363 } 364 365 #endif 366 #endif 367 368 void sco_demo_close(void){ 369 printf("SCO demo close\n"); 370 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 371 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 372 wav_writer_close(); 373 printf("SCO demo statistics: "); 374 if (negotiated_codec == HFP_CODEC_MSBC){ 375 printf("Used mSBC with PLC, number of processed frames: \n - %d good frames, \n - %d zero frames, \n - %d bad frames.", decoder_state.good_frames_nr, decoder_state.zero_frames_nr, decoder_state.bad_frames_nr); 376 } else { 377 printf("Used CVSD with PLC, number of proccesed frames: \n - %d good frames, \n - %d bad frames.", cvsd_plc_state.good_frames_nr, cvsd_plc_state.bad_frames_nr); 378 } 379 #endif 380 381 #ifdef HAVE_PORTAUDIO 382 if (pa_output_started){ 383 printf("PortAudio: Stop Stream\n"); 384 PaError err = Pa_StopStream(pa_output_stream); 385 if (err != paNoError){ 386 printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); 387 return; 388 } 389 pa_output_started = 0; 390 printf("PortAudio: Close Stream\n"); 391 err = Pa_CloseStream(pa_output_stream); 392 if (err != paNoError){ 393 printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); 394 return; 395 } 396 397 printf("PortAudio: Terminate\n"); 398 err = Pa_Terminate(); 399 if (err != paNoError){ 400 printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); 401 return; 402 } 403 } 404 #endif 405 406 #ifdef SCO_WAV_FILENAME 407 #if 0 408 printf("SCO Demo: closing wav file\n"); 409 if (negotiated_codec == HFP_CODEC_MSBC){ 410 wav_writer_state_t * writer_state = &wav_writer_state; 411 if (!writer_state->wav_file) return; 412 rewind(writer_state->wav_file); 413 write_wav_header(writer_state->wav_file, writer_state->total_num_samples, btstack_sbc_decoder_num_channels(&decoder_state), btstack_sbc_decoder_sample_rate(&decoder_state),2); 414 fclose(writer_state->wav_file); 415 writer_state->wav_file = NULL; 416 } 417 #endif 418 #endif 419 420 negotiated_codec = -1; 421 422 #endif 423 } 424 425 void sco_demo_set_codec(uint8_t codec){ 426 if (negotiated_codec == codec) return; 427 negotiated_codec = codec; 428 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 429 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 430 if (negotiated_codec == HFP_CODEC_MSBC){ 431 sco_demo_init_mSBC(); 432 } else { 433 sco_demo_init_CVSD(); 434 } 435 #endif 436 #endif 437 } 438 439 void sco_demo_init(void){ 440 441 // status 442 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 443 #ifdef HAVE_PORTAUDIO 444 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 445 #else 446 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 447 #endif 448 #endif 449 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 450 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 451 #endif 452 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 453 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 454 #endif 455 456 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 457 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 458 #else 459 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 460 #endif 461 462 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 463 phase = 'a'; 464 #endif 465 } 466 467 void sco_report(void); 468 void sco_report(void){ 469 printf("SCO: sent %u, received %u\n", count_sent, count_received); 470 } 471 472 void sco_demo_send(hci_con_handle_t sco_handle){ 473 474 if (!sco_handle) return; 475 476 int sco_packet_length = hci_get_sco_packet_length(); 477 int sco_payload_length = sco_packet_length - 3; 478 479 hci_reserve_packet_buffer(); 480 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 481 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 482 if (negotiated_codec == HFP_CODEC_MSBC){ 483 484 // overwrite 485 sco_payload_length = 24; 486 sco_packet_length = sco_payload_length + 3; 487 488 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 489 log_error("mSBC stream is empty."); 490 } 491 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 492 if (msbc_file_out){ 493 // log outgoing mSBC data for testing 494 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 495 } 496 497 sco_demo_fill_audio_frame(); 498 } else { 499 const int audio_samples_per_packet = sco_payload_length / CVSD_BYTES_PER_FRAME; 500 sco_demo_sine_wave_int16_at_8000_hz(audio_samples_per_packet, (int16_t *) (sco_packet+3)); 501 } 502 #endif 503 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 504 memset(&sco_packet[3], phase++, sco_payload_length); 505 if (phase > 'z') phase = 'a'; 506 #endif 507 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 508 int j; 509 for (j=0;j<sco_payload_length;j++){ 510 sco_packet[3+j] = phase++; 511 } 512 #endif 513 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 514 int j; 515 for (j=0;j<sco_payload_length;j++){ 516 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 517 sco_packet[3+j] = 0x55; 518 } 519 #endif 520 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 521 int j; 522 for (j=0;j<sco_payload_length;j++){ 523 sco_packet[3+j] = 0x00; 524 } 525 // additional hack 526 // big_endian_store_16(sco_packet, 5, phase++); 527 (void) phase; 528 #endif 529 530 // set handle + flags 531 little_endian_store_16(sco_packet, 0, sco_handle); 532 // set len 533 sco_packet[2] = sco_payload_length; 534 // finally send packet 535 hci_send_sco_packet_buffer(sco_packet_length); 536 537 // request another send event 538 hci_request_sco_can_send_now_event(); 539 540 count_sent++; 541 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 542 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 543 #endif 544 } 545 546 /** 547 * @brief Process received data 548 */ 549 #define ANSI_COLOR_RED "\x1b[31m" 550 #define ANSI_COLOR_GREEN "\x1b[32m" 551 #define ANSI_COLOR_YELLOW "\x1b[33m" 552 #define ANSI_COLOR_BLUE "\x1b[34m" 553 #define ANSI_COLOR_MAGENTA "\x1b[35m" 554 #define ANSI_COLOR_CYAN "\x1b[36m" 555 #define ANSI_COLOR_RESET "\x1b[0m" 556 557 void sco_demo_receive(uint8_t * packet, uint16_t size){ 558 559 dump_data = 1; 560 561 count_received++; 562 static uint32_t packets = 0; 563 static uint32_t crc_errors = 0; 564 static uint32_t data_received = 0; 565 static uint32_t byte_errors = 0; 566 567 data_received += size - 3; 568 packets++; 569 if (data_received > 100000){ 570 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", data_received, packets, crc_errors, byte_errors); 571 crc_errors = 0; 572 byte_errors = 0; 573 data_received = 0; 574 packets = 0; 575 } 576 577 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 578 #ifdef SCO_WAV_FILENAME 579 switch (negotiated_codec){ 580 case HFP_CODEC_MSBC: 581 sco_demo_receive_mSBC(packet, size); 582 break; 583 case HFP_CODEC_CVSD: 584 sco_demo_receive_CVSD(packet, size); 585 break; 586 default: 587 break; 588 } 589 dump_data = 0; 590 #endif 591 #endif 592 593 if (packet[1] & 0x30){ 594 crc_errors++; 595 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 596 // printf_hexdump(&packet[3], size-3); 597 return; 598 } 599 if (dump_data){ 600 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 601 printf("data: "); 602 int i; 603 for (i=3;i<size;i++){ 604 printf("%c", packet[i]); 605 } 606 printf("\n"); 607 dump_data = 0; 608 #endif 609 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 610 // colored hexdump with expected 611 static uint8_t expected_byte = 0; 612 int i; 613 printf("data: "); 614 for (i=3;i<size;i++){ 615 if (packet[i] != expected_byte){ 616 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 617 } else { 618 printf("%02x ", packet[i]); 619 } 620 expected_byte = packet[i]+1; 621 } 622 printf("\n"); 623 #endif 624 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 625 int i; 626 int contains_error = 0; 627 for (i=3;i<size;i++){ 628 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 629 contains_error = 1; 630 byte_errors++; 631 } 632 } 633 if (contains_error){ 634 printf("data: "); 635 for (i=0;i<3;i++){ 636 printf("%02x ", packet[i]); 637 } 638 for (i=3;i<size;i++){ 639 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 640 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 641 } else { 642 printf("%02x ", packet[i]); 643 } 644 } 645 printf("\n"); 646 } 647 #endif 648 } 649 } 650