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