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