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_decoder.h" 48 #include "btstack_sbc_encoder.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 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 = HFP_CODEC_CVSD; 97 static int num_audio_frames = 0; 98 99 FILE * msbc_file; 100 101 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 102 103 // input signal: pre-computed sine wave, 160 Hz at 8 kHz 104 static const uint8_t sine[] = { 105 0, 15, 31, 46, 61, 74, 86, 97, 107, 114, 106 120, 124, 126, 126, 124, 120, 114, 107, 97, 86, 107 74, 61, 46, 31, 15, 0, 241, 225, 210, 195, 108 182, 170, 159, 149, 142, 136, 132, 130, 130, 132, 109 136, 142, 149, 159, 170, 182, 195, 210, 225, 241, 110 }; 111 112 // input signal: pre-computed sine wave, 160 Hz at 16000 kHz 113 static const int16_t sine_int16[] = { 114 0, 2057, 4107, 6140, 8149, 10126, 12062, 13952, 15786, 17557, 115 19260, 20886, 22431, 23886, 25247, 26509, 27666, 28714, 29648, 30466, 116 31163, 31738, 32187, 32509, 32702, 32767, 32702, 32509, 32187, 31738, 117 31163, 30466, 29648, 28714, 27666, 26509, 25247, 23886, 22431, 20886, 118 19260, 17557, 15786, 13952, 12062, 10126, 8149, 6140, 4107, 2057, 119 0, -2057, -4107, -6140, -8149, -10126, -12062, -13952, -15786, -17557, 120 -19260, -20886, -22431, -23886, -25247, -26509, -27666, -28714, -29648, -30466, 121 -31163, -31738, -32187, -32509, -32702, -32767, -32702, -32509, -32187, -31738, 122 -31163, -30466, -29648, -28714, -27666, -26509, -25247, -23886, -22431, -20886, 123 -19260, -17557, -15786, -13952, -12062, -10126, -8149, -6140, -4107, -2057, 124 }; 125 126 #ifdef SCO_WAV_FILENAME 127 128 static int num_samples_to_write; 129 static wav_writer_state_t wav_writer_state; 130 131 static sbc_decoder_state_t decoder_state; 132 133 static void little_endian_fstore_16(FILE * file, uint16_t value){ 134 uint8_t buf[2]; 135 little_endian_store_32(buf, 0, value); 136 fwrite(&buf, 1, 2, file); 137 } 138 139 static void little_endian_fstore_32(FILE * file, uint32_t value){ 140 uint8_t buf[4]; 141 little_endian_store_32(buf, 0, value); 142 fwrite(&buf, 1, 4, file); 143 } 144 145 static FILE * wav_init(const char * filename){ 146 FILE * f = fopen(filename, "wb"); 147 printf("SCO Demo: creating wav file %s, %p\n", filename, f); 148 return f; 149 } 150 151 static void write_wav_header(FILE * file, int sample_rate, int num_channels, int num_samples, int bytes_per_sample){ 152 /* write RIFF header */ 153 fwrite("RIFF", 1, 4, file); 154 // num_samples = blocks * subbands 155 uint32_t data_bytes = (uint32_t) (bytes_per_sample * num_samples * num_channels); 156 little_endian_fstore_32(file, data_bytes + 36); 157 fwrite("WAVE", 1, 4, file); 158 159 int byte_rate = sample_rate * num_channels * bytes_per_sample; 160 int bits_per_sample = 8 * bytes_per_sample; 161 int block_align = num_channels * bits_per_sample; 162 int fmt_length = 16; 163 int fmt_format_tag = 1; // PCM 164 165 /* write fmt chunk */ 166 fwrite("fmt ", 1, 4, file); 167 little_endian_fstore_32(file, fmt_length); 168 little_endian_fstore_16(file, fmt_format_tag); 169 little_endian_fstore_16(file, num_channels); 170 little_endian_fstore_32(file, sample_rate); 171 little_endian_fstore_32(file, byte_rate); 172 little_endian_fstore_16(file, block_align); 173 little_endian_fstore_16(file, bits_per_sample); 174 175 /* write data chunk */ 176 fwrite("data", 1, 4, file); 177 little_endian_fstore_32(file, data_bytes); 178 } 179 180 static void write_wav_data_uint8(FILE * file, unsigned long num_samples, uint8_t * data){ 181 fwrite(data, num_samples, 1, file); 182 } 183 184 static void write_wav_data_int16(FILE * file, int num_samples, int16_t * data){ 185 fwrite(data, num_samples, 2, file); 186 } 187 188 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 189 log_info("handle_pcm_data num samples %u / %u", num_samples, num_samples_to_write); 190 if (!num_samples_to_write) return; 191 192 wav_writer_state_t * writer_state = (wav_writer_state_t*) context; 193 num_samples = btstack_min(num_samples, num_samples_to_write); 194 num_samples_to_write -= num_samples; 195 196 write_wav_data_int16(writer_state->wav_file, num_samples, data); 197 writer_state->total_num_samples+=num_samples; 198 writer_state->frame_count++; 199 200 if (num_samples_to_write == 0){ 201 sco_demo_close(); 202 } 203 } 204 205 static void sco_demo_fill_audio_frame(void){ 206 if (!hfp_msbc_can_encode_audio_frame_now()) return; 207 int i; 208 int16_t sample_buffer[8*16*2]; 209 for (i=0; i < hfp_msbc_num_audio_samples_per_frame(); i++){ 210 sample_buffer[i] = sine_int16[phase++]; 211 if (phase >= (sizeof(sine_int16) / sizeof(int16_t))){ 212 phase = 0; 213 } 214 } 215 hfp_msbc_encode_audio_frame(sample_buffer); 216 num_audio_frames++; 217 } 218 219 static void sco_demo_init_mSBC(void){ 220 wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); 221 wav_writer_state.frame_count = 0; 222 wav_writer_state.total_num_samples = 0; 223 224 sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, (void*)&wav_writer_state); 225 226 const int sample_rate = 16000; 227 const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 228 const int bytes_per_sample = 2; 229 const int num_channels = 1; 230 num_samples_to_write = num_samples; 231 232 write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); 233 234 hfp_msbc_init(); 235 sco_demo_fill_audio_frame(); 236 237 #ifdef SCO_MSBC_OUT_FILENAME 238 msbc_file = fopen(SCO_MSBC_OUT_FILENAME, "wb"); 239 printf("SCO Demo: creating mSBC file %s, %p\n", SCO_MSBC_OUT_FILENAME, msbc_file); 240 #endif 241 242 // HACK: should be handled by HFP or HSP layer on (e)SCO connection request, not here 243 // transparent data 244 hci_set_sco_voice_setting(0x0003); 245 } 246 247 static void sco_demo_init_CVSD(void){ 248 wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); 249 wav_writer_state.frame_count = 0; 250 wav_writer_state.total_num_samples = 0; 251 252 const int sample_rate = 8000; 253 const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 254 const int num_channels = 1; 255 const int bytes_per_sample = 1; 256 num_samples_to_write = num_samples; 257 write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); 258 259 // HACK: should be handled by HFP or HSP layer on (e)SCO connection request, not here 260 // signed 8 bit pcm data with CVSD over the air 261 hci_set_sco_voice_setting(0x0040); 262 } 263 264 265 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 266 if (num_samples_to_write){ 267 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_t*) decoder_state.context; 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_t*) decoder_state.context; 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, sbc_decoder_num_channels(&decoder_state), 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 fwrite(sco_packet + 3, sco_payload_length, 1, msbc_file); 410 411 sco_demo_fill_audio_frame(); 412 } else { 413 int i; 414 for (i=0;i<audio_samples_per_packet;i++){ 415 sco_packet[3+i] = sine[phase]; 416 phase++; 417 if (phase >= sizeof(sine)) phase = 0; 418 } 419 } 420 #else 421 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 422 memset(&sco_packet[3], phase++, audio_samples_per_packet); 423 if (phase > 'z') phase = 'a'; 424 #else 425 int j; 426 for (j=0;j<audio_samples_per_packet;j++){ 427 sco_packet[3+j] = phase++; 428 } 429 #endif 430 #endif 431 432 hci_send_sco_packet_buffer(sco_packet_length); 433 434 // request another send event 435 hci_request_sco_can_send_now_event(); 436 437 count_sent++; 438 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 439 } 440 441 442 /** 443 * @brief Process received data 444 */ 445 void sco_demo_receive(uint8_t * packet, uint16_t size){ 446 447 448 dump_data = 1; 449 450 count_received++; 451 // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); 452 453 454 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 455 #ifdef SCO_WAV_FILENAME 456 if (negotiated_codec == HFP_CODEC_MSBC){ 457 sco_demo_receive_mSBC(packet, size); 458 } else { 459 sco_demo_receive_CVSD(packet, size); 460 } 461 #endif 462 #endif 463 464 if (packet[1] & 0xf0){ 465 printf("SCO CRC Error: %x - data: ", packet[1] >> 4); 466 printf_hexdump(&packet[3], size-3); 467 return; 468 } 469 470 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 471 #ifdef USE_PORTAUDIO 472 uint32_t start = btstack_run_loop_get_time_ms(); 473 Pa_WriteStream( stream, &packet[3], size -3); 474 uint32_t end = btstack_run_loop_get_time_ms(); 475 if (end - start > 5){ 476 printf("Portaudio: write stream took %u ms\n", end - start); 477 } 478 dump_data = 0; 479 #endif 480 #endif 481 482 if (dump_data){ 483 printf("data: "); 484 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 485 int i; 486 for (i=3;i<size;i++){ 487 printf("%c", packet[i]); 488 } 489 printf("\n"); 490 dump_data = 0; 491 #else 492 printf_hexdump(&packet[3], size-3); 493 #endif 494 } 495 } 496