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 348 const int num_samples = size - 3; 349 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 350 int8_t audio_frame_out[24]; 351 352 353 // memcpy(audio_frame_out, (int8_t*)(packet+3), 24); 354 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 355 // int8_t * audio_frame_out = (int8_t*)&packet[3]; 356 357 wav_writer_write_int8(samples_to_write, audio_frame_out); 358 num_samples_to_write -= samples_to_write; 359 if (num_samples_to_write == 0){ 360 sco_demo_close(); 361 } 362 #ifdef USE_PORTAUDIO 363 if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= CVSD_PREBUFFER_BYTES){ 364 /* -- start stream -- */ 365 PaError err = Pa_StartStream(stream); 366 if (err != paNoError){ 367 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 368 return; 369 } 370 pa_stream_started = 1; 371 } 372 btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, samples_to_write); 373 #endif 374 } 375 376 #endif 377 #endif 378 379 void sco_demo_close(void){ 380 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 381 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 382 wav_writer_close(); 383 printf("SCO demo statistics: "); 384 if (negotiated_codec == HFP_CODEC_MSBC){ 385 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); 386 } else { 387 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); 388 } 389 #endif 390 391 #ifdef HAVE_PORTAUDIO 392 if (pa_stream_started){ 393 PaError err = Pa_StopStream(stream); 394 if (err != paNoError){ 395 printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); 396 return; 397 } 398 pa_stream_started = 0; 399 err = Pa_CloseStream(stream); 400 if (err != paNoError){ 401 printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); 402 return; 403 } 404 405 err = Pa_Terminate(); 406 if (err != paNoError){ 407 printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); 408 return; 409 } 410 } 411 #endif 412 #ifdef SCO_WAV_FILENAME 413 414 #if 0 415 printf("SCO Demo: closing wav file\n"); 416 if (negotiated_codec == HFP_CODEC_MSBC){ 417 wav_writer_state_t * writer_state = &wav_writer_state; 418 if (!writer_state->wav_file) return; 419 rewind(writer_state->wav_file); 420 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); 421 fclose(writer_state->wav_file); 422 writer_state->wav_file = NULL; 423 } 424 #endif 425 #endif 426 #endif 427 } 428 429 void sco_demo_set_codec(uint8_t codec){ 430 if (negotiated_codec == codec) return; 431 negotiated_codec = codec; 432 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 433 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 434 if (negotiated_codec == HFP_CODEC_MSBC){ 435 sco_demo_init_mSBC(); 436 } else { 437 sco_demo_init_CVSD(); 438 } 439 #endif 440 #endif 441 } 442 443 void sco_demo_init(void){ 444 445 // status 446 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 447 #ifdef HAVE_PORTAUDIO 448 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 449 #else 450 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 451 #endif 452 #endif 453 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 454 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 455 #endif 456 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 457 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 458 #endif 459 460 #if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 461 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 462 #endif 463 464 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 465 phase = 'a'; 466 #endif 467 } 468 469 void sco_report(void); 470 void sco_report(void){ 471 printf("SCO: sent %u, received %u\n", count_sent, count_received); 472 } 473 474 void sco_demo_send(hci_con_handle_t sco_handle){ 475 476 if (!sco_handle) return; 477 478 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 479 const int sco_payload_length = sco_packet_length - 3; 480 481 hci_reserve_packet_buffer(); 482 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 483 // set handle + flags 484 little_endian_store_16(sco_packet, 0, sco_handle); 485 // set len 486 sco_packet[2] = sco_payload_length; 487 const int audio_samples_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 488 489 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 490 if (negotiated_codec == HFP_CODEC_MSBC){ 491 492 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 493 log_error("mSBC stream is empty."); 494 } 495 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 496 if (msbc_file_out){ 497 // log outgoing mSBC data for testing 498 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 499 } 500 501 sco_demo_fill_audio_frame(); 502 } else { 503 sco_demo_sine_wave_int8(audio_samples_per_packet, (int8_t *) (sco_packet+3)); 504 } 505 #endif 506 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 507 memset(&sco_packet[3], phase++, audio_samples_per_packet); 508 if (phase > 'z') phase = 'a'; 509 #endif 510 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 511 int j; 512 for (j=0;j<audio_samples_per_packet;j++){ 513 sco_packet[3+j] = phase++; 514 } 515 #endif 516 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 517 int j; 518 for (j=0;j<audio_samples_per_packet;j++){ 519 // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 520 sco_packet[3+j] = 0x55; 521 } 522 #endif 523 #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 524 int j; 525 for (j=0;j<audio_samples_per_packet;j++){ 526 sco_packet[3+j] = 0x00; 527 } 528 // additional hack 529 // big_endian_store_16(sco_packet, 5, phase++); 530 (void) phase; 531 #endif 532 533 hci_send_sco_packet_buffer(sco_packet_length); 534 535 // request another send event 536 hci_request_sco_can_send_now_event(); 537 538 count_sent++; 539 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 540 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 541 #endif 542 } 543 544 /** 545 * @brief Process received data 546 */ 547 #define ANSI_COLOR_RED "\x1b[31m" 548 #define ANSI_COLOR_GREEN "\x1b[32m" 549 #define ANSI_COLOR_YELLOW "\x1b[33m" 550 #define ANSI_COLOR_BLUE "\x1b[34m" 551 #define ANSI_COLOR_MAGENTA "\x1b[35m" 552 #define ANSI_COLOR_CYAN "\x1b[36m" 553 #define ANSI_COLOR_RESET "\x1b[0m" 554 555 void sco_demo_receive(uint8_t * packet, uint16_t size){ 556 557 dump_data = 1; 558 559 count_received++; 560 static uint32_t packets = 0; 561 static uint32_t crc_errors = 0; 562 static uint32_t data_received = 0; 563 static uint32_t byte_errors = 0; 564 565 data_received += size - 3; 566 packets++; 567 if (data_received > 100000){ 568 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", data_received, packets, crc_errors, byte_errors); 569 crc_errors = 0; 570 byte_errors = 0; 571 data_received = 0; 572 packets = 0; 573 } 574 575 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 576 #ifdef SCO_WAV_FILENAME 577 if (negotiated_codec == HFP_CODEC_MSBC){ 578 sco_demo_receive_mSBC(packet, size); 579 } else { 580 sco_demo_receive_CVSD(packet, size); 581 } 582 dump_data = 0; 583 #endif 584 #endif 585 586 if (packet[1] & 0x30){ 587 crc_errors++; 588 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 589 // printf_hexdump(&packet[3], size-3); 590 return; 591 } 592 if (dump_data){ 593 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 594 printf("data: "); 595 int i; 596 for (i=3;i<size;i++){ 597 printf("%c", packet[i]); 598 } 599 printf("\n"); 600 dump_data = 0; 601 #endif 602 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 603 // colored hexdump with expected 604 static uint8_t expected_byte = 0; 605 int i; 606 printf("data: "); 607 for (i=3;i<size;i++){ 608 if (packet[i] != expected_byte){ 609 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 610 } else { 611 printf("%02x ", packet[i]); 612 } 613 expected_byte = packet[i]+1; 614 } 615 printf("\n"); 616 #endif 617 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 618 int i; 619 int contains_error = 0; 620 for (i=3;i<size;i++){ 621 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 622 contains_error = 1; 623 byte_errors++; 624 } 625 } 626 if (contains_error){ 627 printf("data: "); 628 for (i=0;i<3;i++){ 629 printf("%02x ", packet[i]); 630 } 631 for (i=3;i<size;i++){ 632 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 633 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 634 } else { 635 printf("%02x ", packet[i]); 636 } 637 } 638 printf("\n"); 639 } 640 #endif 641 } 642 } 643