1f7c85330SMatthias Ringwald /* 2f7c85330SMatthias Ringwald * Copyright (C) 2016 BlueKitchen GmbH 3f7c85330SMatthias Ringwald * 4f7c85330SMatthias Ringwald * Redistribution and use in source and binary forms, with or without 5f7c85330SMatthias Ringwald * modification, are permitted provided that the following conditions 6f7c85330SMatthias Ringwald * are met: 7f7c85330SMatthias Ringwald * 8f7c85330SMatthias Ringwald * 1. Redistributions of source code must retain the above copyright 9f7c85330SMatthias Ringwald * notice, this list of conditions and the following disclaimer. 10f7c85330SMatthias Ringwald * 2. Redistributions in binary form must reproduce the above copyright 11f7c85330SMatthias Ringwald * notice, this list of conditions and the following disclaimer in the 12f7c85330SMatthias Ringwald * documentation and/or other materials provided with the distribution. 13f7c85330SMatthias Ringwald * 3. Neither the name of the copyright holders nor the names of 14f7c85330SMatthias Ringwald * contributors may be used to endorse or promote products derived 15f7c85330SMatthias Ringwald * from this software without specific prior written permission. 16f7c85330SMatthias Ringwald * 4. Any redistribution, use, or modification is done solely for 17f7c85330SMatthias Ringwald * personal benefit and not for any commercial purpose or for 18f7c85330SMatthias Ringwald * monetary gain. 19f7c85330SMatthias Ringwald * 20f7c85330SMatthias Ringwald * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21f7c85330SMatthias Ringwald * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22f7c85330SMatthias Ringwald * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23f7c85330SMatthias Ringwald * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24f7c85330SMatthias Ringwald * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25f7c85330SMatthias Ringwald * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26f7c85330SMatthias Ringwald * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27f7c85330SMatthias Ringwald * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28f7c85330SMatthias Ringwald * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29f7c85330SMatthias Ringwald * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30f7c85330SMatthias Ringwald * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31f7c85330SMatthias Ringwald * SUCH DAMAGE. 32f7c85330SMatthias Ringwald * 33f7c85330SMatthias Ringwald * Please inquire about commercial licensing options at 34f7c85330SMatthias Ringwald * [email protected] 35f7c85330SMatthias Ringwald * 36f7c85330SMatthias Ringwald */ 37f7c85330SMatthias Ringwald 38f7c85330SMatthias Ringwald /* 39f7c85330SMatthias Ringwald * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo 40f7c85330SMatthias Ringwald */ 41f7c85330SMatthias Ringwald 422ec72fbbSMilanka Ringwald #include <stdio.h> 432ec72fbbSMilanka Ringwald 44f7c85330SMatthias Ringwald #include "sco_demo_util.h" 45fcb08cdbSMilanka Ringwald #include "btstack_debug.h" 4635fd3fb9SMatthias Ringwald #include "classic/btstack_sbc.h" 4735fd3fb9SMatthias Ringwald #include "classic/btstack_cvsd_plc.h" 4835fd3fb9SMatthias Ringwald #include "classic/hfp_msbc.h" 4935fd3fb9SMatthias Ringwald #include "classic/hfp.h" 50fcb08cdbSMilanka Ringwald 5135fd3fb9SMatthias Ringwald #ifdef HAVE_POSIX_FILE_IO 52fbc7c9f2SMilanka Ringwald #include "wav_util.h" 5335fd3fb9SMatthias Ringwald #endif 54fbc7c9f2SMilanka Ringwald 55*c4e666bcSMatthias Ringwald #ifdef HAVE_PORTAUDIO 56*c4e666bcSMatthias Ringwald #include <portaudio.h> 57*c4e666bcSMatthias Ringwald #include "btstack_ring_buffer.h" 58*c4e666bcSMatthias Ringwald #endif 59*c4e666bcSMatthias Ringwald 60*c4e666bcSMatthias Ringwald 61*c4e666bcSMatthias Ringwald // test modes 62f7c85330SMatthias Ringwald #define SCO_DEMO_MODE_SINE 0 63f7c85330SMatthias Ringwald #define SCO_DEMO_MODE_ASCII 1 64f7c85330SMatthias Ringwald #define SCO_DEMO_MODE_COUNTER 2 651a919128SMatthias Ringwald #define SCO_DEMO_MODE_55 3 661a919128SMatthias Ringwald #define SCO_DEMO_MODE_00 4 67f7c85330SMatthias Ringwald 68f7c85330SMatthias Ringwald // SCO demo configuration 69fcb08cdbSMilanka Ringwald #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE 70*c4e666bcSMatthias Ringwald 71*c4e666bcSMatthias Ringwald // number of sco packets until 'report' on console 72f7c85330SMatthias Ringwald #define SCO_REPORT_PERIOD 100 73f7c85330SMatthias Ringwald 74*c4e666bcSMatthias Ringwald // length and name of wav file on disc 75*c4e666bcSMatthias Ringwald #define SCO_WAV_DURATION_IN_SECONDS 15 768b29cfc6SMatthias Ringwald #define SCO_WAV_FILENAME "sco_input.wav" 77*c4e666bcSMatthias Ringwald 78*c4e666bcSMatthias Ringwald // name of sbc test files 79d5e5f834SMatthias Ringwald #define SCO_MSBC_OUT_FILENAME "sco_output.msbc" 802308e108SMilanka Ringwald #define SCO_MSBC_IN_FILENAME "sco_input.msbc" 81220eb563SMilanka Ringwald 82*c4e666bcSMatthias Ringwald // pre-buffer for CVSD and mSBC - also defines latency 83*c4e666bcSMatthias Ringwald #define SCO_CVSD_PA_PREBUFFER_MS 50 84*c4e666bcSMatthias Ringwald #define SCO_MSBC_PA_PREBUFFER_MS 50 858b29cfc6SMatthias Ringwald 86*c4e666bcSMatthias Ringwald // constants 87*c4e666bcSMatthias Ringwald #define NUM_CHANNELS 1 88*c4e666bcSMatthias Ringwald #define CVSD_BYTES_PER_FRAME (2*NUM_CHANNELS) 89*c4e666bcSMatthias Ringwald #define CVSD_SAMPLE_RATE 8000 90*c4e666bcSMatthias Ringwald #define MSBC_SAMPLE_RATE 16000 91*c4e666bcSMatthias Ringwald #define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) 92f7c85330SMatthias Ringwald 93f7c85330SMatthias Ringwald #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) 94f7c85330SMatthias Ringwald #define USE_PORTAUDIO 95*c4e666bcSMatthias Ringwald #define CVSD_PA_PREBUFFER_BYTES (SCO_CVSD_PA_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * CVSD_BYTES_PER_FRAME) 96*c4e666bcSMatthias Ringwald #define MSBC_PA_PREBUFFER_BYTES (SCO_MSBC_PA_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * MSBC_BYTES_PER_FRAME) 97f7c85330SMatthias Ringwald #endif 98f7c85330SMatthias Ringwald 99f7c85330SMatthias Ringwald #ifdef USE_PORTAUDIO 100f7c85330SMatthias Ringwald static PaStream * stream; 101*c4e666bcSMatthias Ringwald static int pa_stream_started = 0; 102*c4e666bcSMatthias Ringwald static int pa_stream_paused = 0; 103*c4e666bcSMatthias Ringwald static uint8_t ring_buffer_storage[2*MSBC_PA_PREBUFFER_BYTES]; 104dbb41bfeSMilanka Ringwald static btstack_ring_buffer_t ring_buffer; 105f7c85330SMatthias Ringwald #endif 106f7c85330SMatthias Ringwald 107fcb08cdbSMilanka Ringwald static int dump_data = 1; 108fcb08cdbSMilanka Ringwald static int count_sent = 0; 109fcb08cdbSMilanka Ringwald static int count_received = 0; 110*c4e666bcSMatthias Ringwald static int negotiated_codec = -1; 1111a919128SMatthias Ringwald static int phase = 0; 112*c4e666bcSMatthias Ringwald static int num_audio_frames = 0; 113*c4e666bcSMatthias Ringwald 114*c4e666bcSMatthias Ringwald static btstack_sbc_decoder_state_t decoder_state; 115*c4e666bcSMatthias Ringwald static btstack_cvsd_plc_state_t cvsd_plc_state; 116*c4e666bcSMatthias Ringwald static int num_samples_to_write; 117fcb08cdbSMilanka Ringwald 118d5e5f834SMatthias Ringwald FILE * msbc_file_in; 119d5e5f834SMatthias Ringwald FILE * msbc_file_out; 1207294d009SMatthias Ringwald 121f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 122d6a06398SMatthias Ringwald 12335fd3fb9SMatthias Ringwald // input signal: pre-computed sine wave, 160 Hz at 16000 kHz 124*c4e666bcSMatthias Ringwald static const int16_t sine_int16_at_16000hz[] = { 12535fd3fb9SMatthias Ringwald 0, 2057, 4107, 6140, 8149, 10126, 12062, 13952, 15786, 17557, 12635fd3fb9SMatthias Ringwald 19260, 20886, 22431, 23886, 25247, 26509, 27666, 28714, 29648, 30466, 12735fd3fb9SMatthias Ringwald 31163, 31738, 32187, 32509, 32702, 32767, 32702, 32509, 32187, 31738, 12835fd3fb9SMatthias Ringwald 31163, 30466, 29648, 28714, 27666, 26509, 25247, 23886, 22431, 20886, 12935fd3fb9SMatthias Ringwald 19260, 17557, 15786, 13952, 12062, 10126, 8149, 6140, 4107, 2057, 13035fd3fb9SMatthias Ringwald 0, -2057, -4107, -6140, -8149, -10126, -12062, -13952, -15786, -17557, 13135fd3fb9SMatthias Ringwald -19260, -20886, -22431, -23886, -25247, -26509, -27666, -28714, -29648, -30466, 13235fd3fb9SMatthias Ringwald -31163, -31738, -32187, -32509, -32702, -32767, -32702, -32509, -32187, -31738, 13335fd3fb9SMatthias Ringwald -31163, -30466, -29648, -28714, -27666, -26509, -25247, -23886, -22431, -20886, 13435fd3fb9SMatthias Ringwald -19260, -17557, -15786, -13952, -12062, -10126, -8149, -6140, -4107, -2057, 13535fd3fb9SMatthias Ringwald }; 13635fd3fb9SMatthias Ringwald 137*c4e666bcSMatthias Ringwald // ony use every second value from 16khz table 138*c4e666bcSMatthias Ringwald static void sco_demo_sine_wave_int16_at_8000_hz(int num_samples, int16_t * data){ 13935fd3fb9SMatthias Ringwald int i; 14035fd3fb9SMatthias Ringwald for (i=0; i < num_samples; i++){ 141*c4e666bcSMatthias Ringwald data[i] = sine_int16_at_16000hz[phase++]; 14235fd3fb9SMatthias Ringwald phase++; 143*c4e666bcSMatthias Ringwald if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 14435fd3fb9SMatthias Ringwald phase = 0; 14535fd3fb9SMatthias Ringwald } 14635fd3fb9SMatthias Ringwald } 14735fd3fb9SMatthias Ringwald } 14835fd3fb9SMatthias Ringwald 14935fd3fb9SMatthias Ringwald static void sco_demo_fill_audio_frame(void){ 15035fd3fb9SMatthias Ringwald if (!hfp_msbc_can_encode_audio_frame_now()) return; 15135fd3fb9SMatthias Ringwald int num_samples = hfp_msbc_num_audio_samples_per_frame(); 15235fd3fb9SMatthias Ringwald int16_t sample_buffer[num_samples]; 153*c4e666bcSMatthias Ringwald sco_demo_sine_wave_int16_at_8000_hz(num_samples, sample_buffer); 15435fd3fb9SMatthias Ringwald hfp_msbc_encode_audio_frame(sample_buffer); 15535fd3fb9SMatthias Ringwald num_audio_frames++; 15635fd3fb9SMatthias Ringwald } 157dbb41bfeSMilanka Ringwald 158*c4e666bcSMatthias Ringwald #ifdef SCO_WAV_FILENAME 159dbb41bfeSMilanka Ringwald #ifdef USE_PORTAUDIO 160*c4e666bcSMatthias Ringwald static int portaudio_callback( const void *inputBuffer, void *outputBuffer, 161dbb41bfeSMilanka Ringwald unsigned long framesPerBuffer, 162dbb41bfeSMilanka Ringwald const PaStreamCallbackTimeInfo* timeInfo, 163dbb41bfeSMilanka Ringwald PaStreamCallbackFlags statusFlags, 164dbb41bfeSMilanka Ringwald void *userData ) { 165dbb41bfeSMilanka Ringwald (void) timeInfo; /* Prevent unused variable warnings. */ 166dbb41bfeSMilanka Ringwald (void) statusFlags; 167dbb41bfeSMilanka Ringwald (void) inputBuffer; 1683963d036SMatthias Ringwald (void) userData; 169dbb41bfeSMilanka Ringwald 170*c4e666bcSMatthias Ringwald // config based on codec 171*c4e666bcSMatthias Ringwald int bytes_to_copy; 172*c4e666bcSMatthias Ringwald int prebuffer_bytes; 173*c4e666bcSMatthias Ringwald switch (negotiated_codec){ 174*c4e666bcSMatthias Ringwald case HFP_CODEC_MSBC: 175*c4e666bcSMatthias Ringwald bytes_to_copy = framesPerBuffer * MSBC_BYTES_PER_FRAME; 176*c4e666bcSMatthias Ringwald prebuffer_bytes = MSBC_PA_PREBUFFER_BYTES; 177*c4e666bcSMatthias Ringwald break; 178*c4e666bcSMatthias Ringwald case HFP_CODEC_CVSD: 179*c4e666bcSMatthias Ringwald bytes_to_copy = framesPerBuffer * CVSD_BYTES_PER_FRAME; 180*c4e666bcSMatthias Ringwald prebuffer_bytes = CVSD_PA_PREBUFFER_BYTES; 181*c4e666bcSMatthias Ringwald break; 182*c4e666bcSMatthias Ringwald default: 183*c4e666bcSMatthias Ringwald bytes_to_copy = framesPerBuffer * 2; // assume 1 channel / 16 bit audio samples 184*c4e666bcSMatthias Ringwald prebuffer_bytes = 0xfffffff; 185*c4e666bcSMatthias Ringwald break; 186dbb41bfeSMilanka Ringwald } 187dbb41bfeSMilanka Ringwald 188*c4e666bcSMatthias Ringwald // fill with silence while paused 189*c4e666bcSMatthias Ringwald if (pa_stream_paused){ 190*c4e666bcSMatthias Ringwald if (btstack_ring_buffer_bytes_available(&ring_buffer) < prebuffer_bytes){ 191*c4e666bcSMatthias Ringwald memset(outputBuffer, 0, bytes_to_copy); 192*c4e666bcSMatthias Ringwald return 0; 193dbb41bfeSMilanka Ringwald } else { 194*c4e666bcSMatthias Ringwald // resume playback 195*c4e666bcSMatthias Ringwald pa_stream_paused = 0; 196dbb41bfeSMilanka Ringwald } 197*c4e666bcSMatthias Ringwald } 198*c4e666bcSMatthias Ringwald 199*c4e666bcSMatthias Ringwald // get data from ringbuffer 200*c4e666bcSMatthias Ringwald uint32_t bytes_read = 0; 201*c4e666bcSMatthias Ringwald btstack_ring_buffer_read(&ring_buffer, outputBuffer, bytes_to_copy, &bytes_read); 202*c4e666bcSMatthias Ringwald bytes_to_copy -= bytes_read; 203*c4e666bcSMatthias Ringwald 204*c4e666bcSMatthias Ringwald // fill with 0 if not enough 205*c4e666bcSMatthias Ringwald if (bytes_to_copy){ 206*c4e666bcSMatthias Ringwald memset(outputBuffer + bytes_read, 0, bytes_to_copy); 207*c4e666bcSMatthias Ringwald pa_stream_paused = 1; 208*c4e666bcSMatthias Ringwald } 209dbb41bfeSMilanka Ringwald return 0; 210dbb41bfeSMilanka Ringwald } 2118b29cfc6SMatthias Ringwald 212*c4e666bcSMatthias Ringwald static void portaudio_start(void){ 213*c4e666bcSMatthias Ringwald if (!pa_stream_started){ 214dbb41bfeSMilanka Ringwald /* -- start stream -- */ 215dbb41bfeSMilanka Ringwald PaError err = Pa_StartStream(stream); 216dbb41bfeSMilanka Ringwald if (err != paNoError){ 217dbb41bfeSMilanka Ringwald printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 218dbb41bfeSMilanka Ringwald return; 219dbb41bfeSMilanka Ringwald } 220dbb41bfeSMilanka Ringwald pa_stream_started = 1; 221*c4e666bcSMatthias Ringwald pa_stream_paused = 1; 222dbb41bfeSMilanka Ringwald } 223*c4e666bcSMatthias Ringwald } 224*c4e666bcSMatthias Ringwald 225*c4e666bcSMatthias Ringwald // return 1 if ok 226*c4e666bcSMatthias Ringwald static int portaudio_initialize(int sample_rate){ 227*c4e666bcSMatthias Ringwald PaError err; 228*c4e666bcSMatthias Ringwald PaStreamParameters outputParameters; 229*c4e666bcSMatthias Ringwald 230*c4e666bcSMatthias Ringwald /* -- initialize PortAudio -- */ 231*c4e666bcSMatthias Ringwald printf("PortAudio: Initialize\n"); 232*c4e666bcSMatthias Ringwald err = Pa_Initialize(); 233*c4e666bcSMatthias Ringwald if( err != paNoError ) return 0; 234*c4e666bcSMatthias Ringwald /* -- setup input and output -- */ 235*c4e666bcSMatthias Ringwald outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 236*c4e666bcSMatthias Ringwald outputParameters.channelCount = NUM_CHANNELS; 237*c4e666bcSMatthias Ringwald outputParameters.sampleFormat = paInt16; 238*c4e666bcSMatthias Ringwald outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 239*c4e666bcSMatthias Ringwald outputParameters.hostApiSpecificStreamInfo = NULL; 240*c4e666bcSMatthias Ringwald /* -- setup stream -- */ 241*c4e666bcSMatthias Ringwald printf("PortAudio: Open stream\n"); 242*c4e666bcSMatthias Ringwald err = Pa_OpenStream( 243*c4e666bcSMatthias Ringwald &stream, 244*c4e666bcSMatthias Ringwald NULL, // &inputParameters, 245*c4e666bcSMatthias Ringwald &outputParameters, 246*c4e666bcSMatthias Ringwald sample_rate, 247*c4e666bcSMatthias Ringwald 0, 248*c4e666bcSMatthias Ringwald paClipOff, /* we won't output out of range samples so don't bother clipping them */ 249*c4e666bcSMatthias Ringwald portaudio_callback, 250*c4e666bcSMatthias Ringwald NULL ); 251*c4e666bcSMatthias Ringwald if (err != paNoError){ 252*c4e666bcSMatthias Ringwald printf("Error opening portaudio stream: \"%s\"\n", Pa_GetErrorText(err)); 253*c4e666bcSMatthias Ringwald return 0; 254*c4e666bcSMatthias Ringwald } 255*c4e666bcSMatthias Ringwald memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); 256*c4e666bcSMatthias Ringwald btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); 257*c4e666bcSMatthias Ringwald pa_stream_started = 0; 258*c4e666bcSMatthias Ringwald return 1; 259*c4e666bcSMatthias Ringwald } 260*c4e666bcSMatthias Ringwald #endif 261*c4e666bcSMatthias Ringwald 262*c4e666bcSMatthias Ringwald 263*c4e666bcSMatthias Ringwald static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 264*c4e666bcSMatthias Ringwald UNUSED(context); 265*c4e666bcSMatthias Ringwald UNUSED(sample_rate); 266*c4e666bcSMatthias Ringwald 267*c4e666bcSMatthias Ringwald // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 268*c4e666bcSMatthias Ringwald #ifdef HAVE_PORTAUDIO 269*c4e666bcSMatthias Ringwald portaudio_start(); 270dbb41bfeSMilanka Ringwald btstack_ring_buffer_write(&ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 2719ec2630cSMatthias Ringwald #else 2729ec2630cSMatthias Ringwald UNUSED(num_channels); 273dbb41bfeSMilanka Ringwald #endif 274dbb41bfeSMilanka Ringwald 275fcb08cdbSMilanka Ringwald if (!num_samples_to_write) return; 276fcb08cdbSMilanka Ringwald 277fcb08cdbSMilanka Ringwald num_samples = btstack_min(num_samples, num_samples_to_write); 278fcb08cdbSMilanka Ringwald num_samples_to_write -= num_samples; 279fcb08cdbSMilanka Ringwald 280fbc7c9f2SMilanka Ringwald wav_writer_write_int16(num_samples, data); 281fcb08cdbSMilanka Ringwald 282fcb08cdbSMilanka Ringwald if (num_samples_to_write == 0){ 283fcb08cdbSMilanka Ringwald sco_demo_close(); 284fcb08cdbSMilanka Ringwald } 285fcb08cdbSMilanka Ringwald } 286fcb08cdbSMilanka Ringwald 287fcb08cdbSMilanka Ringwald static void sco_demo_init_mSBC(void){ 288*c4e666bcSMatthias Ringwald printf("SCO Demo: Init mSBC\n"); 289*c4e666bcSMatthias Ringwald 290*c4e666bcSMatthias Ringwald wav_writer_open(SCO_WAV_FILENAME, 1, MSBC_SAMPLE_RATE); 291fbc7c9f2SMilanka Ringwald btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 292fcb08cdbSMilanka Ringwald 293*c4e666bcSMatthias Ringwald num_samples_to_write = MSBC_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 294220eb563SMilanka Ringwald 295220eb563SMilanka Ringwald hfp_msbc_init(); 296220eb563SMilanka Ringwald sco_demo_fill_audio_frame(); 297973d7173SMatthias Ringwald 298d5e5f834SMatthias Ringwald #ifdef SCO_MSBC_IN_FILENAME 299d5e5f834SMatthias Ringwald msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 300d5e5f834SMatthias Ringwald printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 301d5e5f834SMatthias Ringwald #endif 3027294d009SMatthias Ringwald #ifdef SCO_MSBC_OUT_FILENAME 303d5e5f834SMatthias Ringwald msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 304d5e5f834SMatthias Ringwald printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 3057294d009SMatthias Ringwald #endif 306dbb41bfeSMilanka Ringwald 307dbb41bfeSMilanka Ringwald #ifdef USE_PORTAUDIO 308*c4e666bcSMatthias Ringwald portaudio_initialize(MSBC_SAMPLE_RATE); 309dbb41bfeSMilanka Ringwald #endif 310fcb08cdbSMilanka Ringwald } 311fcb08cdbSMilanka Ringwald 312fcb08cdbSMilanka Ringwald static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 313fcb08cdbSMilanka Ringwald if (num_samples_to_write){ 314d5e5f834SMatthias Ringwald if (msbc_file_in){ 315d5e5f834SMatthias Ringwald // log incoming mSBC data for testing 316d5e5f834SMatthias Ringwald fwrite(packet+3, size-3, 1, msbc_file_in); 317d5e5f834SMatthias Ringwald } 318fcb08cdbSMilanka Ringwald } 319dbb41bfeSMilanka Ringwald btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 320fcb08cdbSMilanka Ringwald } 321fcb08cdbSMilanka Ringwald 322fbc7c9f2SMilanka Ringwald static void sco_demo_init_CVSD(void){ 323*c4e666bcSMatthias Ringwald printf("SCO Demo: Init CVSD\n"); 324*c4e666bcSMatthias Ringwald 325*c4e666bcSMatthias Ringwald wav_writer_open(SCO_WAV_FILENAME, 1, CVSD_SAMPLE_RATE); 326fbc7c9f2SMilanka Ringwald btstack_cvsd_plc_init(&cvsd_plc_state); 327*c4e666bcSMatthias Ringwald 328*c4e666bcSMatthias Ringwald num_samples_to_write = CVSD_SAMPLE_RATE * SCO_WAV_DURATION_IN_SECONDS; 329dbb41bfeSMilanka Ringwald 330dbb41bfeSMilanka Ringwald #ifdef USE_PORTAUDIO 331*c4e666bcSMatthias Ringwald portaudio_initialize(CVSD_SAMPLE_RATE); 332dbb41bfeSMilanka Ringwald #endif 333fbc7c9f2SMilanka Ringwald } 334fbc7c9f2SMilanka Ringwald 335fcb08cdbSMilanka Ringwald static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 336dbb41bfeSMilanka Ringwald if (!num_samples_to_write) return; 337*c4e666bcSMatthias Ringwald int16_t audio_frame_out[255]; // 3381f8694ccSMatthias Ringwald 3391f8694ccSMatthias Ringwald if (size > sizeof(audio_frame_out)){ 3401f8694ccSMatthias Ringwald printf("sco_demo_receive_CVSD: SCO packet larger than local output buffer - dropping data.\n"); 3411f8694ccSMatthias Ringwald return; 3421f8694ccSMatthias Ringwald } 343*c4e666bcSMatthias Ringwald const int audio_bytes_read = size - 3; 344*c4e666bcSMatthias Ringwald const int num_samples = audio_bytes_read / CVSD_BYTES_PER_FRAME; 345fcb08cdbSMilanka Ringwald const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 346379d044eSMilanka Ringwald 347*c4e666bcSMatthias Ringwald #if 0 348fbc7c9f2SMilanka Ringwald btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 349*c4e666bcSMatthias Ringwald #else 350*c4e666bcSMatthias Ringwald memcpy(audio_frame_out, packet+3, audio_bytes_read); 351*c4e666bcSMatthias Ringwald #endif 352379d044eSMilanka Ringwald 353*c4e666bcSMatthias Ringwald wav_writer_write_int16(samples_to_write, audio_frame_out); 354fcb08cdbSMilanka Ringwald num_samples_to_write -= samples_to_write; 355fcb08cdbSMilanka Ringwald if (num_samples_to_write == 0){ 356fcb08cdbSMilanka Ringwald sco_demo_close(); 357fcb08cdbSMilanka Ringwald } 358dbb41bfeSMilanka Ringwald #ifdef USE_PORTAUDIO 359*c4e666bcSMatthias Ringwald portaudio_start(); 360*c4e666bcSMatthias Ringwald btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 361dbb41bfeSMilanka Ringwald #endif 362fcb08cdbSMilanka Ringwald } 363fcb08cdbSMilanka Ringwald 3648b29cfc6SMatthias Ringwald #endif 3654a96141eSMatthias Ringwald #endif 3668b29cfc6SMatthias Ringwald 367fcb08cdbSMilanka Ringwald void sco_demo_close(void){ 368*c4e666bcSMatthias Ringwald printf("SCO demo close\n"); 369fcb08cdbSMilanka Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 37026463303SMilanka Ringwald #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 371fbc7c9f2SMilanka Ringwald wav_writer_close(); 37226463303SMilanka Ringwald printf("SCO demo statistics: "); 37326463303SMilanka Ringwald if (negotiated_codec == HFP_CODEC_MSBC){ 37426463303SMilanka Ringwald 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); 37526463303SMilanka Ringwald } else { 37626463303SMilanka Ringwald 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); 37726463303SMilanka Ringwald } 37826463303SMilanka Ringwald #endif 37926463303SMilanka Ringwald 380dbb41bfeSMilanka Ringwald #ifdef HAVE_PORTAUDIO 381dbb41bfeSMilanka Ringwald if (pa_stream_started){ 382*c4e666bcSMatthias Ringwald printf("PortAudio: Stop Stream\n"); 383dbb41bfeSMilanka Ringwald PaError err = Pa_StopStream(stream); 384dbb41bfeSMilanka Ringwald if (err != paNoError){ 385dbb41bfeSMilanka Ringwald printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); 386dbb41bfeSMilanka Ringwald return; 387dbb41bfeSMilanka Ringwald } 388dbb41bfeSMilanka Ringwald pa_stream_started = 0; 389*c4e666bcSMatthias Ringwald printf("PortAudio: Close Stream\n"); 390dbb41bfeSMilanka Ringwald err = Pa_CloseStream(stream); 391dbb41bfeSMilanka Ringwald if (err != paNoError){ 392dbb41bfeSMilanka Ringwald printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); 393dbb41bfeSMilanka Ringwald return; 394dbb41bfeSMilanka Ringwald } 395dbb41bfeSMilanka Ringwald 396*c4e666bcSMatthias Ringwald printf("PortAudio: Terminate\n"); 397dbb41bfeSMilanka Ringwald err = Pa_Terminate(); 398dbb41bfeSMilanka Ringwald if (err != paNoError){ 399dbb41bfeSMilanka Ringwald printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); 400dbb41bfeSMilanka Ringwald return; 401dbb41bfeSMilanka Ringwald } 402dbb41bfeSMilanka Ringwald } 403dbb41bfeSMilanka Ringwald #endif 404fcb08cdbSMilanka Ringwald 405*c4e666bcSMatthias Ringwald #ifdef SCO_WAV_FILENAME 406613518d1SMilanka Ringwald #if 0 407fcb08cdbSMilanka Ringwald printf("SCO Demo: closing wav file\n"); 408220eb563SMilanka Ringwald if (negotiated_codec == HFP_CODEC_MSBC){ 4096e046a36SMatthias Ringwald wav_writer_state_t * writer_state = &wav_writer_state; 410fcb08cdbSMilanka Ringwald if (!writer_state->wav_file) return; 411fcb08cdbSMilanka Ringwald rewind(writer_state->wav_file); 4122afeea7fSMilanka Ringwald 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); 413fcb08cdbSMilanka Ringwald fclose(writer_state->wav_file); 414fcb08cdbSMilanka Ringwald writer_state->wav_file = NULL; 415fcb08cdbSMilanka Ringwald } 416613518d1SMilanka Ringwald #endif 417fcb08cdbSMilanka Ringwald #endif 418*c4e666bcSMatthias Ringwald 419*c4e666bcSMatthias Ringwald negotiated_codec = -1; 420*c4e666bcSMatthias Ringwald 421fcb08cdbSMilanka Ringwald #endif 422fcb08cdbSMilanka Ringwald } 423fcb08cdbSMilanka Ringwald 424fcb08cdbSMilanka Ringwald void sco_demo_set_codec(uint8_t codec){ 425fcb08cdbSMilanka Ringwald if (negotiated_codec == codec) return; 426fcb08cdbSMilanka Ringwald negotiated_codec = codec; 427fcb08cdbSMilanka Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 42817cd946eSMatthias Ringwald #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 429220eb563SMilanka Ringwald if (negotiated_codec == HFP_CODEC_MSBC){ 430fcb08cdbSMilanka Ringwald sco_demo_init_mSBC(); 431fcb08cdbSMilanka Ringwald } else { 432fcb08cdbSMilanka Ringwald sco_demo_init_CVSD(); 433fcb08cdbSMilanka Ringwald } 434fcb08cdbSMilanka Ringwald #endif 435fcb08cdbSMilanka Ringwald #endif 436fcb08cdbSMilanka Ringwald } 437fcb08cdbSMilanka Ringwald 438f7c85330SMatthias Ringwald void sco_demo_init(void){ 439f7c85330SMatthias Ringwald 440f7c85330SMatthias Ringwald // status 441f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 442f7c85330SMatthias Ringwald #ifdef HAVE_PORTAUDIO 443f7c85330SMatthias Ringwald printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 444f7c85330SMatthias Ringwald #else 445f7c85330SMatthias Ringwald printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 446f7c85330SMatthias Ringwald #endif 447f7c85330SMatthias Ringwald #endif 448f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 449f7c85330SMatthias Ringwald printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 450f7c85330SMatthias Ringwald #endif 451f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 452f7c85330SMatthias Ringwald printf("SCO Demo: Sending counter value, hexdump received data.\n"); 453f7c85330SMatthias Ringwald #endif 454f7c85330SMatthias Ringwald 455*c4e666bcSMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 456*c4e666bcSMatthias Ringwald hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 457*c4e666bcSMatthias Ringwald #else 458f7c85330SMatthias Ringwald hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 4597294d009SMatthias Ringwald #endif 460f7c85330SMatthias Ringwald 461f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 462f7c85330SMatthias Ringwald phase = 'a'; 463f7c85330SMatthias Ringwald #endif 464f7c85330SMatthias Ringwald } 465f7c85330SMatthias Ringwald 4661a919128SMatthias Ringwald void sco_report(void); 4671a919128SMatthias Ringwald void sco_report(void){ 4684a96141eSMatthias Ringwald printf("SCO: sent %u, received %u\n", count_sent, count_received); 4694a96141eSMatthias Ringwald } 470f7c85330SMatthias Ringwald 471f7c85330SMatthias Ringwald void sco_demo_send(hci_con_handle_t sco_handle){ 472f7c85330SMatthias Ringwald 473f7c85330SMatthias Ringwald if (!sco_handle) return; 474f7c85330SMatthias Ringwald 475*c4e666bcSMatthias Ringwald int sco_packet_length = hci_get_sco_packet_length(); 476*c4e666bcSMatthias Ringwald int sco_payload_length = sco_packet_length - 3; 477f7c85330SMatthias Ringwald 478f7c85330SMatthias Ringwald hci_reserve_packet_buffer(); 479f7c85330SMatthias Ringwald uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 480f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 481220eb563SMilanka Ringwald if (negotiated_codec == HFP_CODEC_MSBC){ 482220eb563SMilanka Ringwald 483*c4e666bcSMatthias Ringwald // overwrite 484*c4e666bcSMatthias Ringwald sco_payload_length = 24; 485*c4e666bcSMatthias Ringwald sco_packet_length = sco_payload_length + 3; 486*c4e666bcSMatthias Ringwald 487220eb563SMilanka Ringwald if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 488220eb563SMilanka Ringwald log_error("mSBC stream is empty."); 489220eb563SMilanka Ringwald } 490220eb563SMilanka Ringwald hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 491d5e5f834SMatthias Ringwald if (msbc_file_out){ 492d76591efSMatthias Ringwald // log outgoing mSBC data for testing 493d5e5f834SMatthias Ringwald fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 494d76591efSMatthias Ringwald } 4957294d009SMatthias Ringwald 496220eb563SMilanka Ringwald sco_demo_fill_audio_frame(); 497220eb563SMilanka Ringwald } else { 498*c4e666bcSMatthias Ringwald const int audio_samples_per_packet = sco_payload_length / CVSD_BYTES_PER_FRAME; 499*c4e666bcSMatthias Ringwald sco_demo_sine_wave_int16_at_8000_hz(audio_samples_per_packet, (int16_t *) (sco_packet+3)); 500220eb563SMilanka Ringwald } 5011a919128SMatthias Ringwald #endif 502f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 503*c4e666bcSMatthias Ringwald memset(&sco_packet[3], phase++, sco_payload_length); 504f7c85330SMatthias Ringwald if (phase > 'z') phase = 'a'; 5051a919128SMatthias Ringwald #endif 5061a919128SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 50738b2eaafSMatthias Ringwald int j; 508*c4e666bcSMatthias Ringwald for (j=0;j<sco_payload_length;j++){ 50938b2eaafSMatthias Ringwald sco_packet[3+j] = phase++; 510f7c85330SMatthias Ringwald } 511f7c85330SMatthias Ringwald #endif 5121a919128SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 5131a919128SMatthias Ringwald int j; 514*c4e666bcSMatthias Ringwald for (j=0;j<sco_payload_length;j++){ 5151a919128SMatthias Ringwald // sco_packet[3+j] = j & 1 ? 0x35 : 0x53; 5161a919128SMatthias Ringwald sco_packet[3+j] = 0x55; 5171a919128SMatthias Ringwald } 5181a919128SMatthias Ringwald #endif 5191a919128SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_00 5201a919128SMatthias Ringwald int j; 521*c4e666bcSMatthias Ringwald for (j=0;j<sco_payload_length;j++){ 5221a919128SMatthias Ringwald sco_packet[3+j] = 0x00; 5231a919128SMatthias Ringwald } 5241a919128SMatthias Ringwald // additional hack 5251a919128SMatthias Ringwald // big_endian_store_16(sco_packet, 5, phase++); 5261a919128SMatthias Ringwald (void) phase; 527f7c85330SMatthias Ringwald #endif 528220eb563SMilanka Ringwald 529*c4e666bcSMatthias Ringwald // set handle + flags 530*c4e666bcSMatthias Ringwald little_endian_store_16(sco_packet, 0, sco_handle); 531*c4e666bcSMatthias Ringwald // set len 532*c4e666bcSMatthias Ringwald sco_packet[2] = sco_payload_length; 533*c4e666bcSMatthias Ringwald // finally send packet 534f7c85330SMatthias Ringwald hci_send_sco_packet_buffer(sco_packet_length); 535f7c85330SMatthias Ringwald 536f7c85330SMatthias Ringwald // request another send event 537f7c85330SMatthias Ringwald hci_request_sco_can_send_now_event(); 538f7c85330SMatthias Ringwald 5394a96141eSMatthias Ringwald count_sent++; 5401a919128SMatthias Ringwald #if SCO_DEMO_MODE != SCO_DEMO_MODE_55 5414a96141eSMatthias Ringwald if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 5421a919128SMatthias Ringwald #endif 543f7c85330SMatthias Ringwald } 544f7c85330SMatthias Ringwald 545f7c85330SMatthias Ringwald /** 546f7c85330SMatthias Ringwald * @brief Process received data 547f7c85330SMatthias Ringwald */ 5481a919128SMatthias Ringwald #define ANSI_COLOR_RED "\x1b[31m" 5491a919128SMatthias Ringwald #define ANSI_COLOR_GREEN "\x1b[32m" 5501a919128SMatthias Ringwald #define ANSI_COLOR_YELLOW "\x1b[33m" 5511a919128SMatthias Ringwald #define ANSI_COLOR_BLUE "\x1b[34m" 5521a919128SMatthias Ringwald #define ANSI_COLOR_MAGENTA "\x1b[35m" 5531a919128SMatthias Ringwald #define ANSI_COLOR_CYAN "\x1b[36m" 5541a919128SMatthias Ringwald #define ANSI_COLOR_RESET "\x1b[0m" 5551a919128SMatthias Ringwald 556f7c85330SMatthias Ringwald void sco_demo_receive(uint8_t * packet, uint16_t size){ 557f7c85330SMatthias Ringwald 558fcb08cdbSMilanka Ringwald dump_data = 1; 5598b29cfc6SMatthias Ringwald 5604a96141eSMatthias Ringwald count_received++; 5611a919128SMatthias Ringwald static uint32_t packets = 0; 5621a919128SMatthias Ringwald static uint32_t crc_errors = 0; 5631a919128SMatthias Ringwald static uint32_t data_received = 0; 5641a919128SMatthias Ringwald static uint32_t byte_errors = 0; 5654a96141eSMatthias Ringwald 5661a919128SMatthias Ringwald data_received += size - 3; 5671a919128SMatthias Ringwald packets++; 5681a919128SMatthias Ringwald if (data_received > 100000){ 5691a919128SMatthias Ringwald printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", data_received, packets, crc_errors, byte_errors); 5701a919128SMatthias Ringwald crc_errors = 0; 5711a919128SMatthias Ringwald byte_errors = 0; 5721a919128SMatthias Ringwald data_received = 0; 5731a919128SMatthias Ringwald packets = 0; 5741a919128SMatthias Ringwald } 5754a96141eSMatthias Ringwald 5764a96141eSMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 5778b29cfc6SMatthias Ringwald #ifdef SCO_WAV_FILENAME 578*c4e666bcSMatthias Ringwald switch (negotiated_codec){ 579*c4e666bcSMatthias Ringwald case HFP_CODEC_MSBC: 580fcb08cdbSMilanka Ringwald sco_demo_receive_mSBC(packet, size); 581*c4e666bcSMatthias Ringwald break; 582*c4e666bcSMatthias Ringwald case HFP_CODEC_CVSD: 583fcb08cdbSMilanka Ringwald sco_demo_receive_CVSD(packet, size); 584*c4e666bcSMatthias Ringwald break; 585*c4e666bcSMatthias Ringwald default: 586*c4e666bcSMatthias Ringwald break; 5878b29cfc6SMatthias Ringwald } 588dbb41bfeSMilanka Ringwald dump_data = 0; 5898b29cfc6SMatthias Ringwald #endif 5904a96141eSMatthias Ringwald #endif 5918b29cfc6SMatthias Ringwald 592b3f76298SMilanka Ringwald if (packet[1] & 0x30){ 5931a919128SMatthias Ringwald crc_errors++; 5941a919128SMatthias Ringwald // printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 5951a919128SMatthias Ringwald // printf_hexdump(&packet[3], size-3); 596f7c85330SMatthias Ringwald return; 597f7c85330SMatthias Ringwald } 5988b29cfc6SMatthias Ringwald if (dump_data){ 599f7c85330SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 6001a919128SMatthias Ringwald printf("data: "); 601f7c85330SMatthias Ringwald int i; 602f7c85330SMatthias Ringwald for (i=3;i<size;i++){ 603f7c85330SMatthias Ringwald printf("%c", packet[i]); 604f7c85330SMatthias Ringwald } 605f7c85330SMatthias Ringwald printf("\n"); 6068b29cfc6SMatthias Ringwald dump_data = 0; 6071a919128SMatthias Ringwald #endif 6081a919128SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 6091a919128SMatthias Ringwald // colored hexdump with expected 6101a919128SMatthias Ringwald static uint8_t expected_byte = 0; 6111a919128SMatthias Ringwald int i; 6121a919128SMatthias Ringwald printf("data: "); 6131a919128SMatthias Ringwald for (i=3;i<size;i++){ 6141a919128SMatthias Ringwald if (packet[i] != expected_byte){ 6151a919128SMatthias Ringwald printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 6161a919128SMatthias Ringwald } else { 6171a919128SMatthias Ringwald printf("%02x ", packet[i]); 6181a919128SMatthias Ringwald } 6191a919128SMatthias Ringwald expected_byte = packet[i]+1; 6201a919128SMatthias Ringwald } 6211a919128SMatthias Ringwald printf("\n"); 6221a919128SMatthias Ringwald #endif 623a11bf416SMatthias Ringwald #if SCO_DEMO_MODE == SCO_DEMO_MODE_55 || SCO_DEMO_MODE == SCO_DEMO_MODE_00 6241a919128SMatthias Ringwald int i; 6251a919128SMatthias Ringwald int contains_error = 0; 6261a919128SMatthias Ringwald for (i=3;i<size;i++){ 6271a919128SMatthias Ringwald if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 6281a919128SMatthias Ringwald contains_error = 1; 6291a919128SMatthias Ringwald byte_errors++; 6301a919128SMatthias Ringwald } 6311a919128SMatthias Ringwald } 6321a919128SMatthias Ringwald if (contains_error){ 6331a919128SMatthias Ringwald printf("data: "); 6341a919128SMatthias Ringwald for (i=0;i<3;i++){ 6351a919128SMatthias Ringwald printf("%02x ", packet[i]); 6361a919128SMatthias Ringwald } 6371a919128SMatthias Ringwald for (i=3;i<size;i++){ 6381a919128SMatthias Ringwald if (packet[i] != 0x00 && packet[i] != 0x35 && packet[i] != 0x53 && packet[i] != 0x55){ 6391a919128SMatthias Ringwald printf(ANSI_COLOR_RED "%02x " ANSI_COLOR_RESET, packet[i]); 6401a919128SMatthias Ringwald } else { 6411a919128SMatthias Ringwald printf("%02x ", packet[i]); 6421a919128SMatthias Ringwald } 6431a919128SMatthias Ringwald } 6441a919128SMatthias Ringwald printf("\n"); 6451a919128SMatthias Ringwald } 646f7c85330SMatthias Ringwald #endif 6478b29cfc6SMatthias Ringwald } 648f7c85330SMatthias Ringwald } 649