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