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