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