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 // HACK: should be handled by HFP or HSP layer on (e)SCO connection request, not here 242 // transparent data 243 hci_set_sco_voice_setting(0x0003); 244 } 245 246 static void sco_demo_init_CVSD(void){ 247 wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); 248 wav_writer_state.frame_count = 0; 249 wav_writer_state.total_num_samples = 0; 250 251 const int sample_rate = 8000; 252 const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 253 const int num_channels = 1; 254 const int bytes_per_sample = 1; 255 num_samples_to_write = num_samples; 256 write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); 257 258 // HACK: should be handled by HFP or HSP layer on (e)SCO connection request, not here 259 // signed 8 bit pcm data with CVSD over the air 260 hci_set_sco_voice_setting(0x0040); 261 } 262 263 264 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 265 if (num_samples_to_write){ 266 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 267 dump_data = 0; 268 } 269 } 270 271 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 272 if (num_samples_to_write){ 273 const int num_samples = size - 3; 274 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 275 // convert 8 bit signed to 8 bit unsigned 276 int i; 277 for (i=0;i<samples_to_write;i++){ 278 packet[3+i] += 128; 279 } 280 281 wav_writer_state_t * writer_state = &wav_writer_state; 282 write_wav_data_uint8(writer_state->wav_file, samples_to_write, &packet[3]); 283 num_samples_to_write -= samples_to_write; 284 if (num_samples_to_write == 0){ 285 sco_demo_close(); 286 } 287 dump_data = 0; 288 } 289 } 290 291 #endif 292 #endif 293 294 void sco_demo_close(void){ 295 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 296 #ifdef SCO_WAV_FILENAME 297 298 #if 0 299 printf("SCO Demo: closing wav file\n"); 300 if (negotiated_codec == HFP_CODEC_MSBC){ 301 wav_writer_state_t * writer_state = &wav_writer_state; 302 if (!writer_state->wav_file) return; 303 rewind(writer_state->wav_file); 304 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); 305 fclose(writer_state->wav_file); 306 writer_state->wav_file = NULL; 307 } 308 #endif 309 #endif 310 #endif 311 } 312 313 void sco_demo_set_codec(uint8_t codec){ 314 if (negotiated_codec == codec) return; 315 negotiated_codec = codec; 316 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 317 #if defined(SCO_WAV_FILENAME) || defined(SCO_SBC_FILENAME) 318 if (negotiated_codec == HFP_CODEC_MSBC){ 319 sco_demo_init_mSBC(); 320 } else { 321 sco_demo_init_CVSD(); 322 } 323 #endif 324 #endif 325 } 326 327 void sco_demo_init(void){ 328 329 // status 330 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 331 #ifdef HAVE_PORTAUDIO 332 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 333 #else 334 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 335 #endif 336 #endif 337 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 338 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 339 #endif 340 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 341 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 342 #endif 343 344 #ifdef USE_PORTAUDIO 345 int err; 346 PaStreamParameters outputParameters; 347 348 /* -- initialize PortAudio -- */ 349 err = Pa_Initialize(); 350 if( err != paNoError ) return; 351 /* -- setup input and output -- */ 352 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 353 outputParameters.channelCount = NUM_CHANNELS; 354 outputParameters.sampleFormat = PA_SAMPLE_TYPE; 355 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 356 outputParameters.hostApiSpecificStreamInfo = NULL; 357 /* -- setup stream -- */ 358 err = Pa_OpenStream( 359 &stream, 360 NULL, // &inputParameters, 361 &outputParameters, 362 SAMPLE_RATE, 363 FRAMES_PER_BUFFER, 364 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 365 NULL, /* no callback, use blocking API */ 366 NULL ); /* no callback, so no callback userData */ 367 if( err != paNoError ) return; 368 /* -- start stream -- */ 369 err = Pa_StartStream( stream ); 370 if( err != paNoError ) return; 371 #endif 372 373 #if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 374 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 375 #endif 376 377 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 378 phase = 'a'; 379 #endif 380 } 381 382 static void sco_report(void){ 383 printf("SCO: sent %u, received %u\n", count_sent, count_received); 384 } 385 386 static void sco_assert_codec_set(void){ 387 // if SCO is open but we didn't hear about the codec yet, we fall back to CVSD 388 if (!negotiated_codec){ 389 sco_demo_set_codec(HFP_CODEC_CVSD); 390 } 391 } 392 393 void sco_demo_send(hci_con_handle_t sco_handle){ 394 395 if (!sco_handle) return; 396 397 sco_assert_codec_set(); 398 399 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 400 const int sco_payload_length = sco_packet_length - 3; 401 402 hci_reserve_packet_buffer(); 403 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 404 // set handle + flags 405 little_endian_store_16(sco_packet, 0, sco_handle); 406 // set len 407 sco_packet[2] = sco_payload_length; 408 const int audio_samples_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 409 410 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 411 if (negotiated_codec == HFP_CODEC_MSBC){ 412 413 if (hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 414 log_error("mSBC stream is empty."); 415 } 416 hfp_msbc_read_from_stream(sco_packet + 3, sco_payload_length); 417 if (msbc_file){ 418 // log outgoing mSBC data for testing 419 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file); 420 } 421 422 sco_demo_fill_audio_frame(); 423 } else { 424 int i; 425 for (i=0;i<audio_samples_per_packet;i++){ 426 sco_packet[3+i] = sine[phase]; 427 phase++; 428 if (phase >= sizeof(sine)) phase = 0; 429 } 430 } 431 #else 432 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 433 memset(&sco_packet[3], phase++, audio_samples_per_packet); 434 if (phase > 'z') phase = 'a'; 435 #else 436 int j; 437 for (j=0;j<audio_samples_per_packet;j++){ 438 sco_packet[3+j] = phase++; 439 } 440 #endif 441 #endif 442 443 hci_send_sco_packet_buffer(sco_packet_length); 444 445 // request another send event 446 hci_request_sco_can_send_now_event(); 447 448 count_sent++; 449 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 450 } 451 452 /** 453 * @brief Process received data 454 */ 455 void sco_demo_receive(uint8_t * packet, uint16_t size){ 456 457 sco_assert_codec_set(); 458 459 dump_data = 1; 460 461 count_received++; 462 // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); 463 464 465 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 466 #ifdef SCO_WAV_FILENAME 467 if (negotiated_codec == HFP_CODEC_MSBC){ 468 sco_demo_receive_mSBC(packet, size); 469 } else { 470 sco_demo_receive_CVSD(packet, size); 471 } 472 #endif 473 #endif 474 475 if (packet[1] & 0xf0){ 476 printf("SCO CRC Error: %x - data: ", packet[1] >> 4); 477 printf_hexdump(&packet[3], size-3); 478 return; 479 } 480 481 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 482 #ifdef USE_PORTAUDIO 483 uint32_t start = btstack_run_loop_get_time_ms(); 484 Pa_WriteStream( stream, &packet[3], size -3); 485 uint32_t end = btstack_run_loop_get_time_ms(); 486 if (end - start > 5){ 487 printf("Portaudio: write stream took %u ms\n", end - start); 488 } 489 dump_data = 0; 490 #endif 491 #endif 492 493 if (dump_data){ 494 printf("data: "); 495 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 496 int i; 497 for (i=3;i<size;i++){ 498 printf("%c", packet[i]); 499 } 500 printf("\n"); 501 dump_data = 0; 502 #else 503 printf_hexdump(&packet[3], size-3); 504 #endif 505 } 506 } 507