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 "sbc_decoder.h" 48 49 // configure test mode 50 #define SCO_DEMO_MODE_SINE 0 51 #define SCO_DEMO_MODE_ASCII 1 52 #define SCO_DEMO_MODE_COUNTER 2 53 54 55 // SCO demo configuration 56 #define SCO_DEMO_MODE SCO_DEMO_MODE_SINE 57 #define SCO_REPORT_PERIOD 100 58 59 #ifdef HAVE_POSIX_FILE_IO 60 #define SCO_WAV_FILENAME "sco_input.wav" 61 #define SCO_WAV_DURATION_IN_SECONDS 30 62 #endif 63 64 65 #if defined(HAVE_PORTAUDIO) && (SCO_DEMO_MODE == SCO_DEMO_MODE_SINE) 66 #define USE_PORTAUDIO 67 #endif 68 69 #ifdef USE_PORTAUDIO 70 #include <portaudio.h> 71 // portaudio config 72 #define NUM_CHANNELS 1 73 #define SAMPLE_RATE 8000 74 #define FRAMES_PER_BUFFER 24 75 #define PA_SAMPLE_TYPE paInt8 76 // portaudio globals 77 static PaStream * stream; 78 #endif 79 80 typedef struct wav_writer_state { 81 FILE * wav_file; 82 int total_num_samples; 83 int frame_count; 84 } wav_writer_state_t; 85 86 static int dump_data = 1; 87 88 static int phase; 89 static int count_sent = 0; 90 static int count_received = 0; 91 static uint8_t negotiated_codec = 0; // CVSD 92 93 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 94 // input signal: pre-computed sine wave, 160 Hz 95 static const uint8_t sine[] = { 96 0, 15, 31, 46, 61, 74, 86, 97, 107, 114, 97 120, 124, 126, 126, 124, 120, 114, 107, 97, 86, 98 74, 61, 46, 31, 15, 0, 241, 225, 210, 195, 99 182, 170, 159, 149, 142, 136, 132, 130, 130, 132, 100 136, 142, 149, 159, 170, 182, 195, 210, 225, 241, 101 }; 102 103 104 #ifdef SCO_WAV_FILENAME 105 106 static int num_samples_to_write; 107 static wav_writer_state_t wav_writer_state; 108 109 static sbc_decoder_state_t decoder_state; 110 111 static void little_endian_fstore_16(FILE * file, uint16_t value){ 112 uint8_t buf[2]; 113 little_endian_store_32(buf, 0, value); 114 fwrite(&buf, 1, 2, file); 115 } 116 117 static void little_endian_fstore_32(FILE * file, uint32_t value){ 118 uint8_t buf[4]; 119 little_endian_store_32(buf, 0, value); 120 fwrite(&buf, 1, 4, file); 121 } 122 123 static FILE * wav_init(const char * filename){ 124 FILE * f = fopen(filename, "wb"); 125 printf("SCO Demo: creating wav file %s, %p\n", filename, f); 126 return f; 127 } 128 129 static void write_wav_header(FILE * file, int sample_rate, int num_channels, int num_samples, int bytes_per_sample){ 130 /* write RIFF header */ 131 fwrite("RIFF", 1, 4, file); 132 // num_samples = blocks * subbands 133 uint32_t data_bytes = (uint32_t) (bytes_per_sample * num_samples * num_channels); 134 little_endian_fstore_32(file, data_bytes + 36); 135 fwrite("WAVE", 1, 4, file); 136 137 int byte_rate = sample_rate * num_channels * bytes_per_sample; 138 int bits_per_sample = 8 * bytes_per_sample; 139 int block_align = num_channels * bits_per_sample; 140 int fmt_length = 16; 141 int fmt_format_tag = 1; // PCM 142 143 /* write fmt chunk */ 144 fwrite("fmt ", 1, 4, file); 145 little_endian_fstore_32(file, fmt_length); 146 little_endian_fstore_16(file, fmt_format_tag); 147 little_endian_fstore_16(file, num_channels); 148 little_endian_fstore_32(file, sample_rate); 149 little_endian_fstore_32(file, byte_rate); 150 little_endian_fstore_16(file, block_align); 151 little_endian_fstore_16(file, bits_per_sample); 152 153 /* write data chunk */ 154 fwrite("data", 1, 4, file); 155 little_endian_fstore_32(file, data_bytes); 156 } 157 158 static void write_wav_data_uint8(FILE * file, unsigned long num_samples, uint8_t * data){ 159 fwrite(data, num_samples, 1, file); 160 } 161 162 static void write_wav_data_int16(FILE * file, int num_samples, int16_t * data){ 163 fwrite(data, num_samples, 2, file); 164 } 165 166 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 167 log_info("handle_pcm_data num samples %u / %u", num_samples, num_samples_to_write); 168 if (!num_samples_to_write) return; 169 170 wav_writer_state_t * writer_state = (wav_writer_state_t*) context; 171 num_samples = btstack_min(num_samples, num_samples_to_write); 172 num_samples_to_write -= num_samples; 173 174 write_wav_data_int16(writer_state->wav_file, num_samples, data); 175 writer_state->total_num_samples+=num_samples; 176 writer_state->frame_count++; 177 178 if (num_samples_to_write == 0){ 179 sco_demo_close(); 180 } 181 } 182 183 184 static void sco_demo_init_mSBC(void){ 185 wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); 186 wav_writer_state.frame_count = 0; 187 wav_writer_state.total_num_samples = 0; 188 189 sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, (void*)&wav_writer_state); 190 191 const int sample_rate = 16000; 192 const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 193 const int bytes_per_sample = 2; 194 const int num_channels = 1; 195 num_samples_to_write = num_samples; 196 197 write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); 198 } 199 200 static void sco_demo_init_CVSD(void){ 201 wav_writer_state.wav_file = wav_init(SCO_WAV_FILENAME); 202 wav_writer_state.frame_count = 0; 203 wav_writer_state.total_num_samples = 0; 204 205 const int sample_rate = 8000; 206 const int num_samples = sample_rate * SCO_WAV_DURATION_IN_SECONDS; 207 const int num_channels = 1; 208 const int bytes_per_sample = 1; 209 num_samples_to_write = num_samples; 210 write_wav_header(wav_writer_state.wav_file, sample_rate, num_channels, num_samples, bytes_per_sample); 211 } 212 213 214 static void sco_demo_receive_mSBC(uint8_t * packet, uint16_t size){ 215 if (num_samples_to_write){ 216 sbc_decoder_process_data(&decoder_state, packet+3, size-3); 217 dump_data = 0; 218 } 219 } 220 221 static void sco_demo_receive_CVSD(uint8_t * packet, uint16_t size){ 222 if (num_samples_to_write){ 223 const int num_samples = size - 3; 224 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 225 // convert 8 bit signed to 8 bit unsigned 226 int i; 227 for (i=0;i<samples_to_write;i++){ 228 packet[3+i] += 128; 229 } 230 231 wav_writer_state_t * writer_state = (wav_writer_state_t*) decoder_state.context; 232 write_wav_data_uint8(writer_state->wav_file, samples_to_write, &packet[3]); 233 num_samples_to_write -= samples_to_write; 234 if (num_samples_to_write == 0){ 235 sco_demo_close(); 236 } 237 dump_data = 0; 238 } 239 } 240 241 #endif 242 #endif 243 244 void sco_demo_close(void){ 245 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 246 #ifdef SCO_WAV_FILENAME 247 248 #if 0 249 printf("SCO Demo: closing wav file\n"); 250 if (negotiated_codec == 0x02){ 251 wav_writer_state_t * writer_state = (wav_writer_state_t*) decoder_state.context; 252 if (!writer_state->wav_file) return; 253 rewind(writer_state->wav_file); 254 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); 255 fclose(writer_state->wav_file); 256 writer_state->wav_file = NULL; 257 } 258 #endif 259 #endif 260 #endif 261 } 262 263 void sco_demo_set_codec(uint8_t codec){ 264 if (negotiated_codec == codec) return; 265 negotiated_codec = codec; 266 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 267 #ifdef SCO_WAV_FILENAME 268 if (negotiated_codec == 0x02){ 269 sco_demo_init_mSBC(); 270 } else { 271 sco_demo_init_CVSD(); 272 } 273 #endif 274 #endif 275 } 276 277 void sco_demo_init(void){ 278 279 // status 280 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 281 #ifdef HAVE_PORTAUDIO 282 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 283 #else 284 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 285 #endif 286 #endif 287 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 288 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 289 #endif 290 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 291 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 292 #endif 293 294 #ifdef USE_PORTAUDIO 295 int err; 296 PaStreamParameters outputParameters; 297 298 /* -- initialize PortAudio -- */ 299 err = Pa_Initialize(); 300 if( err != paNoError ) return; 301 /* -- setup input and output -- */ 302 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 303 outputParameters.channelCount = NUM_CHANNELS; 304 outputParameters.sampleFormat = PA_SAMPLE_TYPE; 305 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 306 outputParameters.hostApiSpecificStreamInfo = NULL; 307 /* -- setup stream -- */ 308 err = Pa_OpenStream( 309 &stream, 310 NULL, // &inputParameters, 311 &outputParameters, 312 SAMPLE_RATE, 313 FRAMES_PER_BUFFER, 314 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 315 NULL, /* no callback, use blocking API */ 316 NULL ); /* no callback, so no callback userData */ 317 if( err != paNoError ) return; 318 /* -- start stream -- */ 319 err = Pa_StartStream( stream ); 320 if( err != paNoError ) return; 321 #endif 322 323 //#if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 324 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 325 //#endif 326 327 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 328 phase = 'a'; 329 #endif 330 } 331 332 static void sco_report(void){ 333 printf("SCO: sent %u, received %u\n", count_sent, count_received); 334 } 335 336 void sco_demo_send(hci_con_handle_t sco_handle){ 337 338 if (!sco_handle) return; 339 340 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 341 const int sco_payload_length = sco_packet_length - 3; 342 343 hci_reserve_packet_buffer(); 344 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 345 // set handle + flags 346 little_endian_store_16(sco_packet, 0, sco_handle); 347 // set len 348 sco_packet[2] = sco_payload_length; 349 const int frames_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 350 351 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 352 int i; 353 for (i=0;i<frames_per_packet;i++){ 354 sco_packet[3+i] = sine[phase]; 355 phase++; 356 if (phase >= sizeof(sine)) phase = 0; 357 } 358 #else 359 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 360 memset(&sco_packet[3], phase++, frames_per_packet); 361 if (phase > 'z') phase = 'a'; 362 #else 363 int j; 364 for (j=0;j<frames_per_packet;j++){ 365 sco_packet[3+j] = phase++; 366 } 367 #endif 368 #endif 369 hci_send_sco_packet_buffer(sco_packet_length); 370 371 // request another send event 372 hci_request_sco_can_send_now_event(); 373 374 count_sent++; 375 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 376 } 377 378 379 /** 380 * @brief Process received data 381 */ 382 void sco_demo_receive(uint8_t * packet, uint16_t size){ 383 384 385 dump_data = 1; 386 387 count_received++; 388 // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); 389 390 391 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 392 #ifdef SCO_WAV_FILENAME 393 if (negotiated_codec == 0x02){ 394 sco_demo_receive_mSBC(packet, size); 395 } else { 396 sco_demo_receive_CVSD(packet, size); 397 } 398 #endif 399 #endif 400 401 if (packet[1] & 0xf0){ 402 printf("SCO CRC Error: %x - data: ", packet[1] >> 4); 403 printf_hexdump(&packet[3], size-3); 404 return; 405 } 406 407 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 408 #ifdef USE_PORTAUDIO 409 uint32_t start = btstack_run_loop_get_time_ms(); 410 Pa_WriteStream( stream, &packet[3], size -3); 411 uint32_t end = btstack_run_loop_get_time_ms(); 412 if (end - start > 5){ 413 printf("Portaudio: write stream took %u ms\n", end - start); 414 } 415 dump_data = 0; 416 #endif 417 #endif 418 419 if (dump_data){ 420 printf("data: "); 421 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 422 int i; 423 for (i=3;i<size;i++){ 424 printf("%c", packet[i]); 425 } 426 printf("\n"); 427 dump_data = 0; 428 #else 429 printf_hexdump(&packet[3], size-3); 430 #endif 431 } 432 } 433