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 "hfp_msbc.h" 49 #include "hfp.h" 50 51 // configure test mode 52 #define SCO_DEMO_MODE_SINE 0 53 #define SCO_DEMO_MODE_ASCII 1 54 #define SCO_DEMO_MODE_COUNTER 2 55 56 57 // SCO demo configuration 58 #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE 59 #define SCO_REPORT_PERIOD 100 60 61 #ifdef HAVE_POSIX_FILE_IO 62 #define SCO_WAV_FILENAME "sco_input.wav" 63 // #define SCO_MSBC_OUT_FILENAME "sco_output.msbc" 64 65 #define SCO_WAV_DURATION_IN_SECONDS 30 66 #endif 67 68 69 #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) 70 #define USE_PORTAUDIO 71 #endif 72 73 #ifdef USE_PORTAUDIO 74 #include <portaudio.h> 75 // portaudio config 76 #define NUM_CHANNELS 1 77 #define SAMPLE_RATE 8000 78 #define FRAMES_PER_BUFFER 24 79 #define PA_SAMPLE_TYPE paInt8 80 // portaudio globals 81 static PaStream * stream; 82 #endif 83 84 typedef struct wav_writer_state { 85 FILE * wav_file; 86 int total_num_samples; 87 int frame_count; 88 } wav_writer_state_t; 89 90 static int dump_data = 1; 91 92 static int phase = 0; 93 static int count_sent = 0; 94 static int count_received = 0; 95 static uint8_t negotiated_codec = 0; 96 static int num_audio_frames = 0; 97 98 FILE * msbc_file; 99 100 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 101 102 // input signal: pre-computed sine wave, 160 Hz at 8 kHz 103 static const uint8_t sine[] = { 104 0, 15, 31, 46, 61, 74, 86, 97, 107, 114, 105 120, 124, 126, 126, 124, 120, 114, 107, 97, 86, 106 74, 61, 46, 31, 15, 0, 241, 225, 210, 195, 107 182, 170, 159, 149, 142, 136, 132, 130, 130, 132, 108 136, 142, 149, 159, 170, 182, 195, 210, 225, 241, 109 }; 110 111 // input signal: pre-computed sine wave, 160 Hz at 16000 kHz 112 static const int16_t sine_int16[] = { 113 0, 2057, 4107, 6140, 8149, 10126, 12062, 13952, 15786, 17557, 114 19260, 20886, 22431, 23886, 25247, 26509, 27666, 28714, 29648, 30466, 115 31163, 31738, 32187, 32509, 32702, 32767, 32702, 32509, 32187, 31738, 116 31163, 30466, 29648, 28714, 27666, 26509, 25247, 23886, 22431, 20886, 117 19260, 17557, 15786, 13952, 12062, 10126, 8149, 6140, 4107, 2057, 118 0, -2057, -4107, -6140, -8149, -10126, -12062, -13952, -15786, -17557, 119 -19260, -20886, -22431, -23886, -25247, -26509, -27666, -28714, -29648, -30466, 120 -31163, -31738, -32187, -32509, -32702, -32767, -32702, -32509, -32187, -31738, 121 -31163, -30466, -29648, -28714, -27666, -26509, -25247, -23886, -22431, -20886, 122 -19260, -17557, -15786, -13952, -12062, -10126, -8149, -6140, -4107, -2057, 123 }; 124 125 #ifdef SCO_WAV_FILENAME 126 127 static int num_samples_to_write; 128 static wav_writer_state_t wav_writer_state; 129 130 static btstack_sbc_decoder_state_t decoder_state; 131 132 static void little_endian_fstore_16(FILE * file, uint16_t value){ 133 uint8_t buf[2]; 134 little_endian_store_32(buf, 0, value); 135 fwrite(&buf, 1, 2, file); 136 } 137 138 static void little_endian_fstore_32(FILE * file, uint32_t value){ 139 uint8_t buf[4]; 140 little_endian_store_32(buf, 0, value); 141 fwrite(&buf, 1, 4, file); 142 } 143 144 static FILE * wav_init(const char * filename){ 145 FILE * f = fopen(filename, "wb"); 146 printf("SCO Demo: creating wav file %s, %p\n", filename, f); 147 return f; 148 } 149 150 static void write_wav_header(FILE * file, int sample_rate, int num_channels, int num_samples, int bytes_per_sample){ 151 /* write RIFF header */ 152 fwrite("RIFF", 1, 4, file); 153 // num_samples = blocks * subbands 154 uint32_t data_bytes = (uint32_t) (bytes_per_sample * num_samples * num_channels); 155 little_endian_fstore_32(file, data_bytes + 36); 156 fwrite("WAVE", 1, 4, file); 157 158 int byte_rate = sample_rate * num_channels * bytes_per_sample; 159 int bits_per_sample = 8 * bytes_per_sample; 160 int block_align = num_channels * bits_per_sample; 161 int fmt_length = 16; 162 int fmt_format_tag = 1; // PCM 163 164 /* write fmt chunk */ 165 fwrite("fmt ", 1, 4, file); 166 little_endian_fstore_32(file, fmt_length); 167 little_endian_fstore_16(file, fmt_format_tag); 168 little_endian_fstore_16(file, num_channels); 169 little_endian_fstore_32(file, sample_rate); 170 little_endian_fstore_32(file, byte_rate); 171 little_endian_fstore_16(file, block_align); 172 little_endian_fstore_16(file, bits_per_sample); 173 174 /* write data chunk */ 175 fwrite("data", 1, 4, file); 176 little_endian_fstore_32(file, data_bytes); 177 } 178 179 static void write_wav_data_uint8(FILE * file, unsigned long num_samples, uint8_t * data){ 180 fwrite(data, num_samples, 1, file); 181 } 182 183 static void write_wav_data_int16(FILE * file, int num_samples, int16_t * data){ 184 fwrite(data, num_samples, 2, file); 185 } 186 187 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 188 log_info("handle_pcm_data num samples %u / %u", num_samples, num_samples_to_write); 189 if (!num_samples_to_write) return; 190 191 wav_writer_state_t * writer_state = (wav_writer_state_t*) context; 192 num_samples = btstack_min(num_samples, num_samples_to_write); 193 num_samples_to_write -= num_samples; 194 195 write_wav_data_int16(writer_state->wav_file, num_samples, data); 196 writer_state->total_num_samples+=num_samples; 197 writer_state->frame_count++; 198 199 if (num_samples_to_write == 0){ 200 sco_demo_close(); 201 } 202 } 203 204 static void sco_demo_fill_audio_frame(void){ 205 if (!hfp_msbc_can_encode_audio_frame_now()) return; 206 int i; 207 int16_t sample_buffer[8*16*2]; 208 for (i=0; i < hfp_msbc_num_audio_samples_per_frame(); i++){ 209 sample_buffer[i] = sine_int16[phase++]; 210 if (phase >= (sizeof(sine_int16) / sizeof(int16_t))){ 211 phase = 0; 212 } 213 } 214 hfp_msbc_encode_audio_frame(sample_buffer); 215 num_audio_frames++; 216 } 217 218 static void sco_demo_init_mSBC(void){ 219 wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); 220 wav_writer_state.frame_count = 0; 221 wav_writer_state.total_num_samples = 0; 222 223 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, (void*)&wav_writer_state); 224 225 const int sample_rate = 16000; 226 const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 227 const int bytes_per_sample = 2; 228 const int num_channels = 1; 229 num_samples_to_write = num_samples; 230 231 write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); 232 233 hfp_msbc_init(); 234 sco_demo_fill_audio_frame(); 235 236 #ifdef SCO_MSBC_OUT_FILENAME 237 msbc_file = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 238 printf("SCO Demo: creating mSBC file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file); 239 #endif 240 } 241 242 static void sco_demo_init_CVSD(void){ 243 wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); 244 wav_writer_state.frame_count = 0; 245 wav_writer_state.total_num_samples = 0; 246 247 const int sample_rate = 8000; 248 const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 249 const int num_channels = 1; 250 const int bytes_per_sample = 1; 251 num_samples_to_write = num_samples; 252 write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); 253 } 254 255 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 256 if (num_samples_to_write){ 257 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 258 dump_data = 0; 259 } 260 } 261 262 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 263 if (num_samples_to_write){ 264 const int num_samples = size - 3; 265 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 266 // convert 8 bit signed to 8 bit unsigned 267 int i; 268 for (i=0;i<samples_to_write;i++){ 269 packet[3+i] += 128; 270 } 271 272 wav_writer_state_t * writer_state = &wav_writer_state; 273 write_wav_data_uint8(writer_state->wav_file, samples_to_write, &packet[3]); 274 num_samples_to_write -= samples_to_write; 275 if (num_samples_to_write == 0){ 276 sco_demo_close(); 277 } 278 dump_data = 0; 279 } 280 } 281 282 #endif 283 #endif 284 285 void sco_demo_close(void){ 286 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 287 #ifdef SCO_WAV_FILENAME 288 289 #if 0 290 printf("SCO Demo: closing wav file\n"); 291 if (negotiated_codec == HFP_CODEC_MSBC){ 292 wav_writer_state_t * writer_state = &wav_writer_state; 293 if (!writer_state->wav_file) return; 294 rewind(writer_state->wav_file); 295 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); 296 fclose(writer_state->wav_file); 297 writer_state->wav_file = NULL; 298 } 299 #endif 300 #endif 301 #endif 302 } 303 304 void sco_demo_set_codec(uint8_t codec){ 305 if (negotiated_codec == codec) return; 306 negotiated_codec = codec; 307 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 308 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 309 if (negotiated_codec == HFP_CODEC_MSBC){ 310 sco_demo_init_mSBC(); 311 } else { 312 sco_demo_init_CVSD(); 313 } 314 #endif 315 #endif 316 } 317 318 void sco_demo_init(void){ 319 320 // status 321 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 322 #ifdef HAVE_PORTAUDIO 323 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 324 #else 325 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 326 #endif 327 #endif 328 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 329 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 330 #endif 331 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 332 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 333 #endif 334 335 #ifdef USE_PORTAUDIO 336 int err; 337 PaStreamParameters outputParameters; 338 339 /* -- initialize PortAudio -- */ 340 err = Pa_Initialize(); 341 if( err != paNoError ) return; 342 /* -- setup input and output -- */ 343 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 344 outputParameters.channelCount = NUM_CHANNELS; 345 outputParameters.sampleFormat = PA_SAMPLE_TYPE; 346 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 347 outputParameters.hostApiSpecificStreamInfo = NULL; 348 /* -- setup stream -- */ 349 err = Pa_OpenStream( 350 &stream, 351 NULL, // &inputParameters, 352 &outputParameters, 353 SAMPLE_RATE, 354 FRAMES_PER_BUFFER, 355 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 356 NULL, /* no callback, use blocking API */ 357 NULL ); /* no callback, so no callback userData */ 358 if( err != paNoError ) return; 359 /* -- start stream -- */ 360 err = Pa_StartStream( stream ); 361 if( err != paNoError ) return; 362 #endif 363 364 #if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 365 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 366 #endif 367 368 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 369 phase = 'a'; 370 #endif 371 } 372 373 static void sco_report(void){ 374 printf("SCO: sent %u, received %u\n", count_sent, count_received); 375 } 376 377 static void sco_assert_codec_set(void){ 378 // if SCO is open but we didn't hear about the codec yet, we fall back to CVSD 379 if (!negotiated_codec){ 380 sco_demo_set_codec(HFP_CODEC_CVSD); 381 } 382 } 383 384 void sco_demo_send(hci_con_handle_t sco_handle){ 385 386 if (!sco_handle) return; 387 388 sco_assert_codec_set(); 389 390 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 391 const int sco_payload_length = sco_packet_length - 3; 392 393 hci_reserve_packet_buffer(); 394 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 395 // set handle + flags 396 little_endian_store_16(sco_packet, 0, sco_handle); 397 // set len 398 sco_packet[2] = sco_payload_length; 399 const int audio_samples_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 400 401 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 402 if (negotiated_codec == HFP_CODEC_MSBC){ 403 404 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 405 log_error("mSBC stream is empty."); 406 } 407 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 408 if (msbc_file){ 409 // log outgoing mSBC data for testing 410 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file); 411 } 412 413 sco_demo_fill_audio_frame(); 414 } else { 415 int i; 416 for (i=0;i<audio_samples_per_packet;i++){ 417 sco_packet[3+i] = sine[phase]; 418 phase++; 419 if (phase >= sizeof(sine)) phase = 0; 420 } 421 } 422 #else 423 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 424 memset(&sco_packet[3], phase++, audio_samples_per_packet); 425 if (phase > 'z') phase = 'a'; 426 #else 427 int j; 428 for (j=0;j<audio_samples_per_packet;j++){ 429 sco_packet[3+j] = phase++; 430 } 431 #endif 432 #endif 433 434 hci_send_sco_packet_buffer(sco_packet_length); 435 436 // request another send event 437 hci_request_sco_can_send_now_event(); 438 439 count_sent++; 440 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 441 } 442 443 /** 444 * @brief Process received data 445 */ 446 void sco_demo_receive(uint8_t * packet, uint16_t size){ 447 448 sco_assert_codec_set(); 449 450 dump_data = 1; 451 452 count_received++; 453 // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); 454 455 456 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 457 #ifdef SCO_WAV_FILENAME 458 if (negotiated_codec == HFP_CODEC_MSBC){ 459 sco_demo_receive_mSBC(packet, size); 460 } else { 461 sco_demo_receive_CVSD(packet, size); 462 } 463 #endif 464 #endif 465 466 if (packet[1] & 0xf0){ 467 printf("SCO CRC Error: %x - data: ", packet[1] >> 4); 468 printf_hexdump(&packet[3], size-3); 469 return; 470 } 471 472 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 473 #ifdef USE_PORTAUDIO 474 uint32_t start = btstack_run_loop_get_time_ms(); 475 Pa_WriteStream( stream, &packet[3], size -3); 476 uint32_t end = btstack_run_loop_get_time_ms(); 477 if (end - start > 5){ 478 printf("Portaudio: write stream took %u ms\n", end - start); 479 } 480 dump_data = 0; 481 #endif 482 #endif 483 484 if (dump_data){ 485 printf("data: "); 486 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 487 int i; 488 for (i=3;i<size;i++){ 489 printf("%c", packet[i]); 490 } 491 printf("\n"); 492 dump_data = 0; 493 #else 494 printf_hexdump(&packet[3], size-3); 495 #endif 496 } 497 } 498