xref: /btstack/example/sco_demo_util.c (revision c4e666bc60ca1e15d942dd54f2d0968baa7ebb13)
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