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 UNUSED(context); 212 UNUSED(sample_rate); 213 214 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 215 #ifdef USE_PORTAUDIO 216 if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= MSBC_PREBUFFER_BYTES){ 217 /* -- start stream -- */ 218 PaError err = Pa_StartStream(stream); 219 if (err != paNoError){ 220 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 221 return; 222 } 223 pa_stream_started = 1; 224 } 225 btstack_ring_buffer_write(&ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 226 // printf("bytes avail after write: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); 227 #else 228 UNUSED(num_channels); 229 #endif 230 231 if (!num_samples_to_write) return; 232 233 num_samples = btstack_min(num_samples, num_samples_to_write); 234 num_samples_to_write -= num_samples; 235 236 wav_writer_write_int16(num_samples, data); 237 238 if (num_samples_to_write == 0){ 239 sco_demo_close(); 240 } 241 } 242 243 static void sco_demo_init_mSBC(void){ 244 int sample_rate = 16000; 245 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 246 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 247 248 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 249 250 hfp_msbc_init(); 251 sco_demo_fill_audio_frame(); 252 253 #ifdef SCO_MSBC_IN_FILENAME 254 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 255 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 256 #endif 257 #ifdef SCO_MSBC_OUT_FILENAME 258 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 259 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 260 #endif 261 262 #ifdef USE_PORTAUDIO 263 PaError err; 264 PaStreamParameters outputParameters; 265 266 /* -- initialize PortAudio -- */ 267 err = Pa_Initialize(); 268 if( err != paNoError ) return; 269 /* -- setup input and output -- */ 270 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 271 outputParameters.channelCount = NUM_CHANNELS; 272 outputParameters.sampleFormat = MSBC_PA_SAMPLE_TYPE; 273 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 274 outputParameters.hostApiSpecificStreamInfo = NULL; 275 /* -- setup stream -- */ 276 err = Pa_OpenStream( 277 &stream, 278 NULL, // &inputParameters, 279 &outputParameters, 280 MSBC_SAMPLE_RATE, 281 MSBC_FRAMES_PER_BUFFER, 282 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 283 patestCallback, /* no callback, use blocking API */ 284 NULL ); /* no callback, so no callback userData */ 285 if (err != paNoError){ 286 printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); 287 return; 288 } 289 memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); 290 btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); 291 pa_stream_started = 0; 292 #endif 293 } 294 295 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 296 if (num_samples_to_write){ 297 if (msbc_file_in){ 298 // log incoming mSBC data for testing 299 fwrite(packet+3, size-3, 1, msbc_file_in); 300 } 301 } 302 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 303 } 304 305 static void sco_demo_init_CVSD(void){ 306 int sample_rate = 8000; 307 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 308 btstack_cvsd_plc_init(&cvsd_plc_state); 309 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 310 311 #ifdef USE_PORTAUDIO 312 PaError err; 313 PaStreamParameters outputParameters; 314 315 /* -- initialize PortAudio -- */ 316 err = Pa_Initialize(); 317 if( err != paNoError ) return; 318 /* -- setup input and output -- */ 319 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 320 outputParameters.channelCount = NUM_CHANNELS; 321 outputParameters.sampleFormat = CVSD_PA_SAMPLE_TYPE; 322 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 323 outputParameters.hostApiSpecificStreamInfo = NULL; 324 /* -- setup stream -- */ 325 err = Pa_OpenStream( 326 &stream, 327 NULL, // &inputParameters, 328 &outputParameters, 329 CVSD_SAMPLE_RATE, 330 CVSD_FRAMES_PER_BUFFER, 331 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 332 patestCallback, /* no callback, use blocking API */ 333 NULL ); /* no callback, so no callback userData */ 334 if (err != paNoError){ 335 printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); 336 return; 337 } 338 memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); 339 btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); 340 pa_stream_started = 0; 341 #endif 342 } 343 344 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 345 if (!num_samples_to_write) return; 346 347 const int num_samples = size - 3; 348 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 349 int8_t audio_frame_out[24]; 350 351 352 // memcpy(audio_frame_out, (int8_t*)(packet+3), 24); 353 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 354 // int8_t * audio_frame_out = (int8_t*)&packet[3]; 355 356 wav_writer_write_int8(samples_to_write, audio_frame_out); 357 num_samples_to_write -= samples_to_write; 358 if (num_samples_to_write == 0){ 359 sco_demo_close(); 360 } 361 #ifdef USE_PORTAUDIO 362 if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= CVSD_PREBUFFER_BYTES){ 363 /* -- start stream -- */ 364 PaError err = Pa_StartStream(stream); 365 if (err != paNoError){ 366 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 367 return; 368 } 369 pa_stream_started = 1; 370 } 371 btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, samples_to_write); 372 #endif 373 } 374 375 #endif 376 #endif 377 378 void sco_demo_close(void){ 379 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 380 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 381 wav_writer_close(); 382 printf("SCO demo statistics: "); 383 if (negotiated_codec == HFP_CODEC_MSBC){ 384 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); 385 } else { 386 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); 387 } 388 #endif 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 413 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 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_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