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 #include "btstack_ring_buffer.h" 80 81 // portaudio config 82 #define NUM_CHANNELS 1 83 84 #define CVSD_SAMPLE_RATE 8000 85 #define CVSD_FRAMES_PER_BUFFER 24 86 #define CVSD_PA_SAMPLE_TYPE paInt8 87 #define CVSD_BYTES_PER_FRAME (1*NUM_CHANNELS) 88 #define CVSD_PREBUFFER_MS 5 89 #define CVSD_PREBUFFER_BYTES (CVSD_PREBUFFER_MS * CVSD_SAMPLE_RATE/1000 * CVSD_BYTES_PER_FRAME) 90 91 #define MSBC_SAMPLE_RATE 16000 92 #define MSBC_FRAMES_PER_BUFFER 120 93 #define MSBC_PA_SAMPLE_TYPE paInt16 94 #define MSBC_BYTES_PER_FRAME (2*NUM_CHANNELS) 95 #define MSBC_PREBUFFER_MS 50 96 #define MSBC_PREBUFFER_BYTES (MSBC_PREBUFFER_MS * MSBC_SAMPLE_RATE/1000 * MSBC_BYTES_PER_FRAME) 97 98 // portaudio globals 99 static PaStream * stream; 100 static uint8_t pa_stream_started = 0; 101 102 static uint8_t ring_buffer_storage[2*MSBC_PREBUFFER_BYTES]; 103 static btstack_ring_buffer_t ring_buffer; 104 #endif 105 106 107 static int dump_data = 1; 108 static int count_sent = 0; 109 static int count_received = 0; 110 static uint8_t negotiated_codec = 0; 111 static int num_audio_frames = 0; 112 113 FILE * msbc_file_in; 114 FILE * msbc_file_out; 115 116 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 117 118 119 #ifdef SCO_WAV_FILENAME 120 static btstack_sbc_decoder_state_t decoder_state; 121 static btstack_cvsd_plc_state_t cvsd_plc_state; 122 static int num_samples_to_write; 123 124 #ifdef USE_PORTAUDIO 125 static int patestCallback( const void *inputBuffer, void *outputBuffer, 126 unsigned long framesPerBuffer, 127 const PaStreamCallbackTimeInfo* timeInfo, 128 PaStreamCallbackFlags statusFlags, 129 void *userData ) { 130 (void) timeInfo; /* Prevent unused variable warnings. */ 131 (void) statusFlags; 132 (void) inputBuffer; 133 134 uint32_t bytes_read = 0; 135 int bytes_per_buffer = framesPerBuffer; 136 if (negotiated_codec == HFP_CODEC_MSBC){ 137 bytes_per_buffer *= MSBC_BYTES_PER_FRAME; 138 } else { 139 bytes_per_buffer *= CVSD_BYTES_PER_FRAME; 140 } 141 142 if (btstack_ring_buffer_bytes_available(&ring_buffer) >= bytes_per_buffer){ 143 btstack_ring_buffer_read(&ring_buffer, outputBuffer, bytes_per_buffer, &bytes_read); 144 } else { 145 printf("NOT ENOUGH DATA!\n"); 146 memset(outputBuffer, 0, bytes_per_buffer); 147 } 148 // printf("bytes avail after read: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); 149 return 0; 150 } 151 #endif 152 153 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 154 // printf("handle_pcm_data num samples %u, sample rate %d\n", num_samples, num_channels); 155 #ifdef USE_PORTAUDIO 156 if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= MSBC_PREBUFFER_BYTES){ 157 /* -- start stream -- */ 158 PaError err = Pa_StartStream(stream); 159 if (err != paNoError){ 160 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 161 return; 162 } 163 pa_stream_started = 1; 164 } 165 btstack_ring_buffer_write(&ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 166 // printf("bytes avail after write: %d\n", btstack_ring_buffer_bytes_available(&ring_buffer)); 167 #endif 168 169 if (!num_samples_to_write) return; 170 171 num_samples = btstack_min(num_samples, num_samples_to_write); 172 num_samples_to_write -= num_samples; 173 174 wav_writer_write_int16(num_samples, data); 175 176 if (num_samples_to_write == 0){ 177 sco_demo_close(); 178 } 179 } 180 181 static void sco_demo_fill_audio_frame(void){ 182 if (!hfp_msbc_can_encode_audio_frame_now()) return; 183 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 184 int16_t sample_buffer[num_samples]; 185 wav_synthesize_sine_wave_int16(num_samples, sample_buffer); 186 hfp_msbc_encode_audio_frame(sample_buffer); 187 num_audio_frames++; 188 } 189 190 static void sco_demo_init_mSBC(void){ 191 int sample_rate = 16000; 192 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 193 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 194 195 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 196 197 hfp_msbc_init(); 198 sco_demo_fill_audio_frame(); 199 200 #ifdef SCO_MSBC_IN_FILENAME 201 msbc_file_in = fopen(SCO_MSBC_IN_FILENAME, "wb"); 202 printf("SCO Demo: creating mSBC in file %s, %p\n", SCO_MSBC_IN_FILENAME, msbc_file_in); 203 #endif 204 #ifdef SCO_MSBC_OUT_FILENAME 205 msbc_file_out = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 206 printf("SCO Demo: creating mSBC out file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file_out); 207 #endif 208 209 #ifdef USE_PORTAUDIO 210 PaError err; 211 PaStreamParameters outputParameters; 212 213 /* -- initialize PortAudio -- */ 214 err = Pa_Initialize(); 215 if( err != paNoError ) return; 216 /* -- setup input and output -- */ 217 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 218 outputParameters.channelCount = NUM_CHANNELS; 219 outputParameters.sampleFormat = MSBC_PA_SAMPLE_TYPE; 220 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 221 outputParameters.hostApiSpecificStreamInfo = NULL; 222 /* -- setup stream -- */ 223 err = Pa_OpenStream( 224 &stream, 225 NULL, // &inputParameters, 226 &outputParameters, 227 MSBC_SAMPLE_RATE, 228 MSBC_FRAMES_PER_BUFFER, 229 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 230 patestCallback, /* no callback, use blocking API */ 231 NULL ); /* no callback, so no callback userData */ 232 if (err != paNoError){ 233 printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); 234 return; 235 } 236 memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); 237 btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); 238 pa_stream_started = 0; 239 #endif 240 } 241 242 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 243 if (num_samples_to_write){ 244 if (msbc_file_in){ 245 // log incoming mSBC data for testing 246 fwrite(packet+3, size-3, 1, msbc_file_in); 247 } 248 } 249 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 250 } 251 252 static void sco_demo_init_CVSD(void){ 253 int sample_rate = 8000; 254 wav_writer_open(SCO_WAV_FILENAME, 1, sample_rate); 255 btstack_cvsd_plc_init(&cvsd_plc_state); 256 num_samples_to_write = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 257 258 #ifdef USE_PORTAUDIO 259 PaError err; 260 PaStreamParameters outputParameters; 261 262 /* -- initialize PortAudio -- */ 263 err = Pa_Initialize(); 264 if( err != paNoError ) return; 265 /* -- setup input and output -- */ 266 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 267 outputParameters.channelCount = NUM_CHANNELS; 268 outputParameters.sampleFormat = CVSD_PA_SAMPLE_TYPE; 269 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 270 outputParameters.hostApiSpecificStreamInfo = NULL; 271 /* -- setup stream -- */ 272 err = Pa_OpenStream( 273 &stream, 274 NULL, // &inputParameters, 275 &outputParameters, 276 CVSD_SAMPLE_RATE, 277 CVSD_FRAMES_PER_BUFFER, 278 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 279 patestCallback, /* no callback, use blocking API */ 280 NULL ); /* no callback, so no callback userData */ 281 if (err != paNoError){ 282 printf("Error initializing portaudio: \"%s\"\n", Pa_GetErrorText(err)); 283 return; 284 } 285 memset(ring_buffer_storage, 0, sizeof(ring_buffer_storage)); 286 btstack_ring_buffer_init(&ring_buffer, ring_buffer_storage, sizeof(ring_buffer_storage)); 287 pa_stream_started = 0; 288 #endif 289 } 290 291 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 292 if (!num_samples_to_write) return; 293 294 const int num_samples = size - 3; 295 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 296 int8_t audio_frame_out[24]; 297 298 299 // memcpy(audio_frame_out, (int8_t*)(packet+3), 24); 300 btstack_cvsd_plc_process_data(&cvsd_plc_state, (int8_t *)(packet+3), num_samples, audio_frame_out); 301 302 wav_writer_write_int8(samples_to_write, audio_frame_out); 303 num_samples_to_write -= samples_to_write; 304 if (num_samples_to_write == 0){ 305 sco_demo_close(); 306 } 307 #ifdef USE_PORTAUDIO 308 if (!pa_stream_started && btstack_ring_buffer_bytes_available(&ring_buffer) >= CVSD_PREBUFFER_BYTES){ 309 /* -- start stream -- */ 310 PaError err = Pa_StartStream(stream); 311 if (err != paNoError){ 312 printf("Error starting the stream: \"%s\"\n", Pa_GetErrorText(err)); 313 return; 314 } 315 pa_stream_started = 1; 316 } 317 btstack_ring_buffer_write(&ring_buffer, (uint8_t *)audio_frame_out, samples_to_write); 318 #endif 319 } 320 321 #endif 322 #endif 323 324 void sco_demo_close(void){ 325 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 326 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 327 wav_writer_close(); 328 printf("SCO demo statistics: "); 329 if (negotiated_codec == HFP_CODEC_MSBC){ 330 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); 331 } else { 332 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); 333 } 334 #endif 335 #endif 336 337 #ifdef HAVE_PORTAUDIO 338 if (pa_stream_started){ 339 PaError err = Pa_StopStream(stream); 340 if (err != paNoError){ 341 printf("Error stopping the stream: \"%s\"\n", Pa_GetErrorText(err)); 342 return; 343 } 344 pa_stream_started = 0; 345 err = Pa_CloseStream(stream); 346 if (err != paNoError){ 347 printf("Error closing the stream: \"%s\"\n", Pa_GetErrorText(err)); 348 return; 349 } 350 351 err = Pa_Terminate(); 352 if (err != paNoError){ 353 printf("Error terminating portaudio: \"%s\"\n", Pa_GetErrorText(err)); 354 return; 355 } 356 } 357 #endif 358 359 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 360 #ifdef SCO_WAV_FILENAME 361 362 #if 0 363 printf("SCO Demo: closing wav file\n"); 364 if (negotiated_codec == HFP_CODEC_MSBC){ 365 wav_writer_state_t * writer_state = &wav_writer_state; 366 if (!writer_state->wav_file) return; 367 rewind(writer_state->wav_file); 368 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); 369 fclose(writer_state->wav_file); 370 writer_state->wav_file = NULL; 371 } 372 #endif 373 #endif 374 #endif 375 } 376 377 void sco_demo_set_codec(uint8_t codec){ 378 if (negotiated_codec == codec) return; 379 negotiated_codec = codec; 380 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 381 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 382 if (negotiated_codec == HFP_CODEC_MSBC){ 383 sco_demo_init_mSBC(); 384 } else { 385 sco_demo_init_CVSD(); 386 } 387 #endif 388 #endif 389 } 390 391 void sco_demo_init(void){ 392 393 // status 394 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 395 #ifdef HAVE_PORTAUDIO 396 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 397 #else 398 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 399 #endif 400 #endif 401 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 402 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 403 #endif 404 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 405 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 406 #endif 407 408 #if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 409 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 410 #endif 411 412 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 413 phase = 'a'; 414 #endif 415 } 416 417 static void sco_report(void){ 418 printf("SCO: sent %u, received %u\n", count_sent, count_received); 419 } 420 421 void sco_demo_send(hci_con_handle_t sco_handle){ 422 423 if (!sco_handle) return; 424 425 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 426 const int sco_payload_length = sco_packet_length - 3; 427 428 hci_reserve_packet_buffer(); 429 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 430 // set handle + flags 431 little_endian_store_16(sco_packet, 0, sco_handle); 432 // set len 433 sco_packet[2] = sco_payload_length; 434 const int audio_samples_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 435 436 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 437 if (negotiated_codec == HFP_CODEC_MSBC){ 438 439 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 440 log_error("mSBC stream is empty."); 441 } 442 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 443 if (msbc_file_out){ 444 // log outgoing mSBC data for testing 445 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file_out); 446 } 447 448 sco_demo_fill_audio_frame(); 449 } else { 450 wav_synthesize_sine_wave_int8(audio_samples_per_packet, (int8_t *) (sco_packet+3)); 451 } 452 #else 453 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 454 memset(&sco_packet[3], phase++, audio_samples_per_packet); 455 if (phase > 'z') phase = 'a'; 456 #else 457 int j; 458 for (j=0;j<audio_samples_per_packet;j++){ 459 sco_packet[3+j] = phase++; 460 } 461 #endif 462 #endif 463 464 hci_send_sco_packet_buffer(sco_packet_length); 465 466 // request another send event 467 hci_request_sco_can_send_now_event(); 468 469 count_sent++; 470 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 471 } 472 473 /** 474 * @brief Process received data 475 */ 476 void sco_demo_receive(uint8_t * packet, uint16_t size){ 477 478 dump_data = 1; 479 480 count_received++; 481 // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); 482 483 484 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 485 #ifdef SCO_WAV_FILENAME 486 if (negotiated_codec == HFP_CODEC_MSBC){ 487 sco_demo_receive_mSBC(packet, size); 488 } else { 489 sco_demo_receive_CVSD(packet, size); 490 } 491 dump_data = 0; 492 #endif 493 #endif 494 495 if (packet[1] & 0x30){ 496 printf("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 497 log_info("SCO CRC Error: %x - data: ", (packet[1] & 0x30) >> 4); 498 printf_hexdump(&packet[3], size-3); 499 500 return; 501 } 502 if (dump_data){ 503 printf("data: "); 504 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 505 int i; 506 for (i=3;i<size;i++){ 507 printf("%c", packet[i]); 508 } 509 printf("\n"); 510 dump_data = 0; 511 #else 512 printf_hexdump(&packet[3], size-3); 513 #endif 514 } 515 } 516