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 191 uint32_t bytes_read = 0; 192 int bytes_per_buffer = framesPerBuffer; 193 if (negotiated_codec == HFP_CODEC_MSBC){ 194 bytes_per_buffer *= MSBC_BYTES_PER_FRAME; 195 } else { 196 bytes_per_buffer *= CVSD_BYTES_PER_FRAME; 197 } 198 199 if (btstack_ring_buffer_bytes_available(&ring_buffer) >= bytes_per_buffer){ 200 btstack_ring_buffer_read(&ring_buffer, outputBuffer, bytes_per_buffer, &bytes_read); 201 } else { 202 printf("NOT ENOUGH DATA!\n"); 203 memset(outputBuffer, 0, bytes_per_buffer); 204 } 205 // printf("bytes avail after read: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); 206 return 0; 207 } 208 #endif 209 210 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 211 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 212 #ifdef USE_PORTAUDIO 213 if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= MSBC_PREBUFFER_BYTES){ 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 } 222 btstack_ring_buffer_write(&ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 223 // printf("bytes avail after write: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); 224 #endif 225 226 if (!num_samples_to_write) return; 227 228 num_samples = btstack_min(num_samples, num_samples_to_write); 229 num_samples_to_write -= num_samples; 230 231 wav_writer_write_int16(num_samples, data); 232 233 if (num_samples_to_write == 0){ 234 sco_demo_close(); 235 } 236 } 237 238 static void sco_demo_init_mSBC(void){ 239 int sample_rate = 16000; 240 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 241 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 242 243 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 244 245 hfp_msbc_init(); 246 sco_demo_fill_audio_frame(); 247 248 #ifdef SCO_MSBC_IN_FILENAME 249 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 250 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 251 #endif 252 #ifdef SCO_MSBC_OUT_FILENAME 253 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 254 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 255 #endif 256 257 #ifdef USE_PORTAUDIO 258 PaError err; 259 PaStreamParameters outputParameters; 260 261 /* -- initialize PortAudio -- */ 262 err = Pa_Initialize(); 263 if( err != paNoError ) return; 264 /* -- setup input and output -- */ 265 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 266 outputParameters.channelCount = NUM_CHANNELS; 267 outputParameters.sampleFormat = MSBC_PA_SAMPLE_TYPE; 268 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 269 outputParameters.hostApiSpecificStreamInfo = NULL; 270 /* -- setup stream -- */ 271 err = Pa_OpenStream( 272 &stream, 273 NULL, // &inputParameters, 274 &outputParameters, 275 MSBC_SAMPLE_RATE, 276 MSBC_FRAMES_PER_BUFFER, 277 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 278 patestCallback, /* no callback, use blocking API */ 279 NULL ); /* no callback, so no callback userData */ 280 if (err != paNoError){ 281 printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); 282 return; 283 } 284 memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); 285 btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); 286 pa_stream_started = 0; 287 #endif 288 } 289 290 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 291 if (num_samples_to_write){ 292 if (msbc_file_in){ 293 // log incoming mSBC data for testing 294 fwrite(packet+3, size-3, 1, msbc_file_in); 295 } 296 } 297 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 298 } 299 300 static void sco_demo_init_CVSD(void){ 301 int sample_rate = 8000; 302 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 303 btstack_cvsd_plc_init(&cvsd_plc_state); 304 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 305 306 #ifdef USE_PORTAUDIO 307 PaError err; 308 PaStreamParameters outputParameters; 309 310 /* -- initialize PortAudio -- */ 311 err = Pa_Initialize(); 312 if( err != paNoError ) return; 313 /* -- setup input and output -- */ 314 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 315 outputParameters.channelCount = NUM_CHANNELS; 316 outputParameters.sampleFormat = CVSD_PA_SAMPLE_TYPE; 317 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 318 outputParameters.hostApiSpecificStreamInfo = NULL; 319 /* -- setup stream -- */ 320 err = Pa_OpenStream( 321 &stream, 322 NULL, // &inputParameters, 323 &outputParameters, 324 CVSD_SAMPLE_RATE, 325 CVSD_FRAMES_PER_BUFFER, 326 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 327 patestCallback, /* no callback, use blocking API */ 328 NULL ); /* no callback, so no callback userData */ 329 if (err != paNoError){ 330 printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); 331 return; 332 } 333 memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); 334 btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); 335 pa_stream_started = 0; 336 #endif 337 } 338 339 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 340 if (!num_samples_to_write) return; 341 342 const int num_samples = size - 3; 343 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 344 int8_t audio_frame_out[24]; 345 346 347 // memcpy(audio_frame_out, (int8_t*)(packet+3), 24); 348 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 349 // int8_t * audio_frame_out = (int8_t*)&packet[3]; 350 351 wav_writer_write_int8(samples_to_write, audio_frame_out); 352 num_samples_to_write -= samples_to_write; 353 if (num_samples_to_write == 0){ 354 sco_demo_close(); 355 } 356 #ifdef USE_PORTAUDIO 357 if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= CVSD_PREBUFFER_BYTES){ 358 /* -- start stream -- */ 359 PaError err = Pa_StartStream(stream); 360 if (err != paNoError){ 361 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 362 return; 363 } 364 pa_stream_started = 1; 365 } 366 btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, samples_to_write); 367 #endif 368 } 369 370 #endif 371 #endif 372 373 void sco_demo_close(void){ 374 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 375 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 376 wav_writer_close(); 377 printf("SCO demo statistics: "); 378 if (negotiated_codec == HFP_CODEC_MSBC){ 379 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); 380 } else { 381 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); 382 } 383 #endif 384 #endif 385 386 #ifdef HAVE_PORTAUDIO 387 if (pa_stream_started){ 388 PaError err = Pa_StopStream(stream); 389 if (err != paNoError){ 390 printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); 391 return; 392 } 393 pa_stream_started = 0; 394 err = Pa_CloseStream(stream); 395 if (err != paNoError){ 396 printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); 397 return; 398 } 399 400 err = Pa_Terminate(); 401 if (err != paNoError){ 402 printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); 403 return; 404 } 405 } 406 #endif 407 408 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 409 #ifdef SCO_WAV_FILENAME 410 411 #if 0 412 printf("SCO Demo: closing wav file\n"); 413 if (negotiated_codec == HFP_CODEC_MSBC){ 414 wav_writer_state_t * writer_state = &wav_writer_state; 415 if (!writer_state->wav_file) return; 416 rewind(writer_state->wav_file); 417 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); 418 fclose(writer_state->wav_file); 419 writer_state->wav_file = NULL; 420 } 421 #endif 422 #endif 423 #endif 424 } 425 426 void sco_demo_set_codec(uint8_t codec){ 427 if (negotiated_codec == codec) return; 428 negotiated_codec = codec; 429 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 430 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 431 if (negotiated_codec == HFP_CODEC_MSBC){ 432 sco_demo_init_mSBC(); 433 } else { 434 sco_demo_init_CVSD(); 435 } 436 #endif 437 #endif 438 } 439 440 void sco_demo_init(void){ 441 442 // status 443 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 444 #ifdef HAVE_PORTAUDIO 445 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 446 #else 447 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 448 #endif 449 #endif 450 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 451 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 452 #endif 453 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 454 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 455 #endif 456 457 #if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 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 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 476 const 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 // set handle + flags 481 little_endian_store_16(sco_packet, 0, sco_handle); 482 // set len 483 sco_packet[2] = sco_payload_length; 484 const int audio_samples_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 485 486 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 487 if (negotiated_codec == HFP_CODEC_MSBC){ 488 489 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 490 log_error("mSBC stream is empty."); 491 } 492 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 493 if (msbc_file_out){ 494 // log outgoing mSBC data for testing 495 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 496 } 497 498 sco_demo_fill_audio_frame(); 499 } else { 500 sco_demo_sine_wave_int8(audio_samples_per_packet, (int8_t *) (sco_packet+3)); 501 } 502 #endif 503 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 504 memset(&sco_packet[3], phase++, audio_samples_per_packet); 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<audio_samples_per_packet;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<audio_samples_per_packet;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<audio_samples_per_packet;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 hci_send_sco_packet_buffer(sco_packet_length); 531 532 // request another send event 533 hci_request_sco_can_send_now_event(); 534 535 count_sent++; 536 #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 537 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 538 #endif 539 } 540 541 /** 542 * @brief Process received data 543 */ 544 #define ANSI_COLOR_RED "\x1b[31m" 545 #define ANSI_COLOR_GREEN "\x1b[32m" 546 #define ANSI_COLOR_YELLOW "\x1b[33m" 547 #define ANSI_COLOR_BLUE "\x1b[34m" 548 #define ANSI_COLOR_MAGENTA "\x1b[35m" 549 #define ANSI_COLOR_CYAN "\x1b[36m" 550 #define ANSI_COLOR_RESET "\x1b[0m" 551 552 void sco_demo_receive(uint8_t * packet, uint16_t size){ 553 554 dump_data = 1; 555 556 count_received++; 557 static uint32_t packets = 0; 558 static uint32_t crc_errors = 0; 559 static uint32_t data_received = 0; 560 static uint32_t byte_errors = 0; 561 562 data_received += size - 3; 563 packets++; 564 if (data_received > 100000){ 565 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", data_received, packets, crc_errors, byte_errors); 566 crc_errors = 0; 567 byte_errors = 0; 568 data_received = 0; 569 packets = 0; 570 } 571 572 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 573 #ifdef SCO_WAV_FILENAME 574 if (negotiated_codec == HFP_CODEC_MSBC){ 575 sco_demo_receive_mSBC(packet, size); 576 } else { 577 sco_demo_receive_CVSD(packet, size); 578 } 579 dump_data = 0; 580 #endif 581 #endif 582 583 if (packet[1] & 0x30){ 584 crc_errors++; 585 // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 586 // printf_hexdump(&packet[3], size-3); 587 return; 588 } 589 if (dump_data){ 590 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 591 printf("data: "); 592 int i; 593 for (i=3;i<size;i++){ 594 printf("%c", packet[i]); 595 } 596 printf("\n"); 597 dump_data = 0; 598 #endif 599 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 600 // colored hexdump with expected 601 static uint8_t expected_byte = 0; 602 int i; 603 printf("data: "); 604 for (i=3;i<size;i++){ 605 if (packet[i] != expected_byte){ 606 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 607 } else { 608 printf("%02x ", packet[i]); 609 } 610 expected_byte = packet[i]+1; 611 } 612 printf("\n"); 613 #endif 614 #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE_00 615 int i; 616 int contains_error = 0; 617 for (i=3;i<size;i++){ 618 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 619 contains_error = 1; 620 byte_errors++; 621 } 622 } 623 if (contains_error){ 624 printf("data: "); 625 for (i=0;i<3;i++){ 626 printf("%02x ", packet[i]); 627 } 628 for (i=3;i<size;i++){ 629 if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 630 printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 631 } else { 632 printf("%02x ", packet[i]); 633 } 634 } 635 printf("\n"); 636 } 637 #endif 638 } 639 } 640