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