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