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