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 "btstack_sbc.h" 48 #include "btstack_cvsd_plc.h" 49 #include "hfp_msbc.h" 50 #include "hfp.h" 51 52 #include "wav_util.h" 53 54 // configure test mode 55 #define SCO_DEMO_MODE_SINE 0 56 #define SCO_DEMO_MODE_ASCII 1 57 #define SCO_DEMO_MODE_COUNTER 2 58 59 60 // SCO demo configuration 61 #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE 62 #define SCO_REPORT_PERIOD 100 63 64 #ifdef HAVE_POSIX_FILE_IO 65 #define SCO_WAV_FILENAME "sco_input.wav" 66 #define SCO_MSBC_OUT_FILENAME "sco_output.msbc" 67 #define SCO_MSBC_IN_FILENAME "sco_input.mscb" 68 69 #define SCO_WAV_DURATION_IN_SECONDS 15 70 #endif 71 72 73 #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) 74 #define USE_PORTAUDIO 75 #endif 76 77 #ifdef USE_PORTAUDIO 78 #include <portaudio.h> 79 // portaudio config 80 #define NUM_CHANNELS 1 81 #define SAMPLE_RATE 8000 82 #define FRAMES_PER_BUFFER 24 83 #define PA_SAMPLE_TYPE paInt8 84 // portaudio globals 85 static PaStream * stream; 86 #endif 87 88 89 static int dump_data = 1; 90 91 static int count_sent = 0; 92 static int count_received = 0; 93 static uint8_t negotiated_codec = 0; 94 static int num_audio_frames = 0; 95 96 FILE * msbc_file_in; 97 FILE * msbc_file_out; 98 99 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 100 101 102 #ifdef SCO_WAV_FILENAME 103 104 static int num_samples_to_write; 105 106 static btstack_sbc_decoder_state_t decoder_state; 107 static btstack_cvsd_plc_state_t cvsd_plc_state; 108 109 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 110 log_info("handle_pcm_data num samples %u / %u", num_samples, num_samples_to_write); 111 if (!num_samples_to_write) return; 112 113 num_samples = btstack_min(num_samples, num_samples_to_write); 114 num_samples_to_write -= num_samples; 115 116 wav_writer_write_int16(num_samples, data); 117 118 if (num_samples_to_write == 0){ 119 sco_demo_close(); 120 } 121 } 122 123 static void sco_demo_fill_audio_frame(void){ 124 if (!hfp_msbc_can_encode_audio_frame_now()) return; 125 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 126 int16_t sample_buffer[num_samples]; 127 wav_synthesize_sine_wave_int16(num_samples, sample_buffer); 128 hfp_msbc_encode_audio_frame(sample_buffer); 129 num_audio_frames++; 130 } 131 132 static void sco_demo_init_mSBC(void){ 133 int sample_rate = 16000; 134 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 135 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 136 137 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 138 139 hfp_msbc_init(); 140 sco_demo_fill_audio_frame(); 141 142 #ifdef SCO_MSBC_IN_FILENAME 143 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 144 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 145 #endif 146 #ifdef SCO_MSBC_OUT_FILENAME 147 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 148 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 149 #endif 150 } 151 152 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 153 if (num_samples_to_write){ 154 if (msbc_file_in){ 155 // log incoming mSBC data for testing 156 fwrite(packet+3, size-3, 1, msbc_file_in); 157 } 158 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 159 dump_data = 0; 160 } 161 } 162 163 static void sco_demo_init_CVSD(void){ 164 int sample_rate = 8000; 165 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 166 btstack_cvsd_plc_init(&cvsd_plc_state); 167 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 168 } 169 170 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 171 if (num_samples_to_write){ 172 const int num_samples = size - 3; 173 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 174 int8_t audio_frame_out[24]; 175 176 // memcpy(audio_frame_out, (int8_t*)(packet+3), 24); 177 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 178 179 wav_writer_write_int8(samples_to_write, audio_frame_out); 180 num_samples_to_write -= samples_to_write; 181 if (num_samples_to_write == 0){ 182 sco_demo_close(); 183 } 184 dump_data = 0; 185 } 186 } 187 188 #endif 189 #endif 190 191 void sco_demo_close(void){ 192 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 193 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 194 wav_writer_close(); 195 printf("SCO demo statistics: "); 196 if (negotiated_codec == HFP_CODEC_MSBC){ 197 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); 198 } else { 199 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); 200 } 201 #endif 202 #endif 203 204 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 205 #ifdef SCO_WAV_FILENAME 206 207 #if 0 208 printf("SCO Demo: closing wav file\n"); 209 if (negotiated_codec == HFP_CODEC_MSBC){ 210 wav_writer_state_t * writer_state = &wav_writer_state; 211 if (!writer_state->wav_file) return; 212 rewind(writer_state->wav_file); 213 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); 214 fclose(writer_state->wav_file); 215 writer_state->wav_file = NULL; 216 } 217 #endif 218 #endif 219 #endif 220 } 221 222 void sco_demo_set_codec(uint8_t codec){ 223 if (negotiated_codec == codec) return; 224 negotiated_codec = codec; 225 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 226 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 227 if (negotiated_codec == HFP_CODEC_MSBC){ 228 sco_demo_init_mSBC(); 229 } else { 230 sco_demo_init_CVSD(); 231 } 232 #endif 233 #endif 234 } 235 236 void sco_demo_init(void){ 237 238 // status 239 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 240 #ifdef HAVE_PORTAUDIO 241 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 242 #else 243 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 244 #endif 245 #endif 246 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 247 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 248 #endif 249 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 250 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 251 #endif 252 253 #ifdef USE_PORTAUDIO 254 int err; 255 PaStreamParameters outputParameters; 256 257 /* -- initialize PortAudio -- */ 258 err = Pa_Initialize(); 259 if( err != paNoError ) return; 260 /* -- setup input and output -- */ 261 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 262 outputParameters.channelCount = NUM_CHANNELS; 263 outputParameters.sampleFormat = PA_SAMPLE_TYPE; 264 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 265 outputParameters.hostApiSpecificStreamInfo = NULL; 266 /* -- setup stream -- */ 267 err = Pa_OpenStream( 268 &stream, 269 NULL, // &inputParameters, 270 &outputParameters, 271 SAMPLE_RATE, 272 FRAMES_PER_BUFFER, 273 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 274 NULL, /* no callback, use blocking API */ 275 NULL ); /* no callback, so no callback userData */ 276 if( err != paNoError ) return; 277 /* -- start stream -- */ 278 err = Pa_StartStream( stream ); 279 if( err != paNoError ) return; 280 #endif 281 282 #if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 283 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 284 #endif 285 286 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 287 phase = 'a'; 288 #endif 289 } 290 291 static void sco_report(void){ 292 printf("SCO: sent %u, received %u\n", count_sent, count_received); 293 } 294 295 void sco_demo_send(hci_con_handle_t sco_handle){ 296 297 if (!sco_handle) return; 298 299 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 300 const int sco_payload_length = sco_packet_length - 3; 301 302 hci_reserve_packet_buffer(); 303 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 304 // set handle + flags 305 little_endian_store_16(sco_packet, 0, sco_handle); 306 // set len 307 sco_packet[2] = sco_payload_length; 308 const int audio_samples_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 309 310 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 311 if (negotiated_codec == HFP_CODEC_MSBC){ 312 313 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 314 log_error("mSBC stream is empty."); 315 } 316 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 317 if (msbc_file_out){ 318 // log outgoing mSBC data for testing 319 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 320 } 321 322 sco_demo_fill_audio_frame(); 323 } else { 324 wav_synthesize_sine_wave_int8(audio_samples_per_packet, (int8_t *) (sco_packet+3)); 325 } 326 #else 327 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 328 memset(&sco_packet[3], phase++, audio_samples_per_packet); 329 if (phase > 'z') phase = 'a'; 330 #else 331 int j; 332 for (j=0;j<audio_samples_per_packet;j++){ 333 sco_packet[3+j] = phase++; 334 } 335 #endif 336 #endif 337 338 hci_send_sco_packet_buffer(sco_packet_length); 339 340 // request another send event 341 hci_request_sco_can_send_now_event(); 342 343 count_sent++; 344 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 345 } 346 347 /** 348 * @brief Process received data 349 */ 350 void sco_demo_receive(uint8_t * packet, uint16_t size){ 351 352 dump_data = 1; 353 354 count_received++; 355 // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); 356 357 358 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 359 #ifdef SCO_WAV_FILENAME 360 if (negotiated_codec == HFP_CODEC_MSBC){ 361 sco_demo_receive_mSBC(packet, size); 362 } else { 363 sco_demo_receive_CVSD(packet, size); 364 } 365 #endif 366 #endif 367 368 if (packet[1] & 0x30){ 369 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 370 log_info("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 371 printf_hexdump(&packet[3], size-3); 372 373 return; 374 } 375 376 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 377 #ifdef USE_PORTAUDIO 378 uint32_t start = btstack_run_loop_get_time_ms(); 379 Pa_WriteStream( stream, &packet[3], size -3); 380 uint32_t end = btstack_run_loop_get_time_ms(); 381 if (end - start > 5){ 382 printf("Portaudio: write stream took %u ms\n", end - start); 383 } 384 dump_data = 0; 385 #endif 386 #endif 387 388 if (dump_data){ 389 printf("data: "); 390 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 391 int i; 392 for (i=3;i<size;i++){ 393 printf("%c", packet[i]); 394 } 395 printf("\n"); 396 dump_data = 0; 397 #else 398 printf_hexdump(&packet[3], size-3); 399 #endif 400 } 401 } 402