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 printf("sco_demo_receive_mSBC size %u, need %u\n", size, num_samples_to_write); 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 write_wav_data_uint8(wav_writer_state.wav_file, samples_to_write, &packet[3]); 231 num_samples_to_write -= samples_to_write; 232 if (num_samples_to_write == 0){ 233 sco_demo_close(); 234 } 235 dump_data = 0; 236 } 237 } 238 239 #endif 240 #endif 241 242 void sco_demo_close(void){ 243 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 244 #ifdef SCO_WAV_FILENAME 245 246 printf("SCO Demo: closing wav file\n"); 247 if (negotiated_codec == 0x02){ 248 wav_writer_state_t * writer_state = (wav_writer_state_t*) decoder_state.context; 249 if (!writer_state->wav_file) return; 250 rewind(writer_state->wav_file); 251 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); 252 fclose(writer_state->wav_file); 253 writer_state->wav_file = NULL; 254 } 255 256 #endif 257 #endif 258 } 259 260 void sco_demo_set_codec(uint8_t codec){ 261 if (negotiated_codec == codec) return; 262 negotiated_codec = codec; 263 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 264 #ifdef SCO_WAV_FILENAME 265 if (negotiated_codec == 0x02){ 266 sco_demo_init_mSBC(); 267 } else { 268 sco_demo_init_CVSD(); 269 } 270 #endif 271 #endif 272 } 273 274 void sco_demo_init(void){ 275 276 // status 277 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 278 #ifdef HAVE_PORTAUDIO 279 printf("SCO Demo: Sending sine wave, audio output via portaudio.\n"); 280 #else 281 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 282 #endif 283 #endif 284 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 285 printf("SCO Demo: Sending ASCII blocks, print received data.\n"); 286 #endif 287 #if SCO_DEMO_MODE == SCO_DEMO_MODE_COUNTER 288 printf("SCO Demo: Sending counter value, hexdump received data.\n"); 289 #endif 290 291 #ifdef USE_PORTAUDIO 292 int err; 293 PaStreamParameters outputParameters; 294 295 /* -- initialize PortAudio -- */ 296 err = Pa_Initialize(); 297 if( err != paNoError ) return; 298 /* -- setup input and output -- */ 299 outputParameters.device = Pa_GetDefaultOutputDevice(); /* default output device */ 300 outputParameters.channelCount = NUM_CHANNELS; 301 outputParameters.sampleFormat = PA_SAMPLE_TYPE; 302 outputParameters.suggestedLatency = Pa_GetDeviceInfo( outputParameters.device )->defaultHighOutputLatency; 303 outputParameters.hostApiSpecificStreamInfo = NULL; 304 /* -- setup stream -- */ 305 err = Pa_OpenStream( 306 &stream, 307 NULL, // &inputParameters, 308 &outputParameters, 309 SAMPLE_RATE, 310 FRAMES_PER_BUFFER, 311 paClipOff, /* we won't output out of range samples so don't bother clipping them */ 312 NULL, /* no callback, use blocking API */ 313 NULL ); /* no callback, so no callback userData */ 314 if( err != paNoError ) return; 315 /* -- start stream -- */ 316 err = Pa_StartStream( stream ); 317 if( err != paNoError ) return; 318 #endif 319 320 //#if SCO_DEMO_MODE != SCO_DEMO_MODE_SINE 321 hci_set_sco_voice_setting(0x03); // linear, unsigned, 8-bit, transparent 322 //#endif 323 324 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 325 phase = 'a'; 326 #endif 327 } 328 329 static void sco_report(void){ 330 printf("SCO: sent %u, received %u\n", count_sent, count_received); 331 } 332 333 void sco_demo_send(hci_con_handle_t sco_handle){ 334 335 if (!sco_handle) return; 336 337 const int sco_packet_length = 24 + 3; // hci_get_sco_packet_length(); 338 const int sco_payload_length = sco_packet_length - 3; 339 340 hci_reserve_packet_buffer(); 341 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 342 // set handle + flags 343 little_endian_store_16(sco_packet, 0, sco_handle); 344 // set len 345 sco_packet[2] = sco_payload_length; 346 const int frames_per_packet = sco_payload_length; // for 8-bit data. for 16-bit data it's /2 347 348 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 349 int i; 350 for (i=0;i<frames_per_packet;i++){ 351 sco_packet[3+i] = sine[phase]; 352 phase++; 353 if (phase >= sizeof(sine)) phase = 0; 354 } 355 #else 356 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 357 memset(&sco_packet[3], phase++, frames_per_packet); 358 if (phase > 'z') phase = 'a'; 359 #else 360 int j; 361 for (j=0;j<frames_per_packet;j++){ 362 sco_packet[3+j] = phase++; 363 } 364 #endif 365 #endif 366 hci_send_sco_packet_buffer(sco_packet_length); 367 368 // request another send event 369 hci_request_sco_can_send_now_event(); 370 371 count_sent++; 372 if ((count_sent % SCO_REPORT_PERIOD) == 0) sco_report(); 373 } 374 375 376 /** 377 * @brief Process received data 378 */ 379 void sco_demo_receive(uint8_t * packet, uint16_t size){ 380 381 382 dump_data = 1; 383 384 count_received++; 385 // if ((count_received % SCO_REPORT_PERIOD) == 0) sco_report(); 386 387 388 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 389 #ifdef SCO_WAV_FILENAME 390 if (negotiated_codec == 0x02){ 391 sco_demo_receive_mSBC(packet, size); 392 } else { 393 sco_demo_receive_CVSD(packet, size); 394 } 395 #endif 396 #endif 397 398 if (packet[1] & 0xf0){ 399 printf("SCO CRC Error: %x - data: ", packet[1] >> 4); 400 printf_hexdump(&packet[3], size-3); 401 return; 402 } 403 404 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 405 #ifdef USE_PORTAUDIO 406 uint32_t start = btstack_run_loop_get_time_ms(); 407 Pa_WriteStream( stream, &packet[3], size -3); 408 uint32_t end = btstack_run_loop_get_time_ms(); 409 if (end - start > 5){ 410 printf("Portaudio: write stream took %u ms\n", end - start); 411 } 412 dump_data = 0; 413 #endif 414 #endif 415 416 if (dump_data){ 417 printf("data: "); 418 #if SCO_DEMO_MODE == SCO_DEMO_MODE_ASCII 419 int i; 420 for (i=3;i<size;i++){ 421 printf("%c", packet[i]); 422 } 423 printf("\n"); 424 dump_data = 0; 425 #else 426 printf_hexdump(&packet[3], size-3); 427 #endif 428 } 429 } 430