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 BLUEKITCHEN 24 * GMBH 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 #define BTSTACK_FILE__ "sco_demo_util.c" 39 40 /* 41 * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo 42 */ 43 44 #include <stdio.h> 45 46 #include "sco_demo_util.h" 47 48 #include "btstack_audio.h" 49 #include "btstack_debug.h" 50 #include "btstack_ring_buffer.h" 51 #include "classic/btstack_cvsd_plc.h" 52 #include "classic/btstack_sbc.h" 53 #include "classic/hfp.h" 54 #include "classic/hfp_msbc.h" 55 56 #ifdef _MSC_VER 57 // ignore deprecated warning for fopen 58 #pragma warning(disable : 4996) 59 #endif 60 61 #ifdef HAVE_POSIX_FILE_IO 62 #include "wav_util.h" 63 #endif 64 65 // test modes 66 #define SCO_DEMO_MODE_SINE 0 67 #define SCO_DEMO_MODE_MICROPHONE 1 68 69 // SCO demo configuration 70 #define SCO_DEMO_MODE SCO_DEMO_MODE_MICROPHONE 71 72 // number of sco packets until 'report' on console 73 #define SCO_REPORT_PERIOD 100 74 75 76 #ifdef HAVE_POSIX_FILE_IO 77 // length and name of wav file on disk 78 #define SCO_WAV_DURATION_IN_SECONDS 15 79 #define SCO_WAV_FILENAME "sco_input.wav" 80 #endif 81 82 // constants 83 #define NUM_CHANNELS 1 84 #define SAMPLE_RATE_8KHZ 8000 85 #define SAMPLE_RATE_16KHZ 16000 86 #define BYTES_PER_FRAME 2 87 88 // pre-buffer for CVSD and mSBC - also defines latency 89 #define SCO_PREBUFFER_MS 50 90 #define PREBUFFER_BYTES_8KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_8KHZ/1000 * BYTES_PER_FRAME) 91 #define PREBUFFER_BYTES_16KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_16KHZ/1000 * BYTES_PER_FRAME) 92 93 #if defined(ENABLE_HFP_WIDE_BAND_SPEECH) 94 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_16KHZ 95 #else 96 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_8KHZ 97 #endif 98 99 static uint16_t audio_prebuffer_bytes; 100 101 // output 102 static int audio_output_paused = 0; 103 static uint8_t audio_output_ring_buffer_storage[2 * PREBUFFER_BYTES_MAX]; 104 static btstack_ring_buffer_t audio_output_ring_buffer; 105 106 // input 107 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 108 #define USE_AUDIO_INPUT 109 #else 110 #define USE_ADUIO_GENERATOR 111 static void (*sco_demo_audio_generator)(uint16_t num_samples, int16_t * data); 112 #endif 113 static int audio_input_paused = 0; 114 static uint8_t audio_input_ring_buffer_storage[2 * PREBUFFER_BYTES_MAX]; 115 static btstack_ring_buffer_t audio_input_ring_buffer; 116 117 static int count_sent = 0; 118 static int count_received = 0; 119 120 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 121 #define MSBC_MAX_NUM_SAMPLES (16*8) 122 static btstack_sbc_decoder_state_t decoder_state; 123 #endif 124 125 static btstack_cvsd_plc_state_t cvsd_plc_state; 126 127 int num_samples_to_write; 128 int num_audio_frames; 129 130 // generic codec support 131 typedef struct { 132 void (*init)(void); 133 void(*receive)(const uint8_t * packet, uint16_t size); 134 void (*fill_payload)(uint8_t * payload_buffer, uint16_t sco_payload_length); 135 void (*close)(void); 136 // 137 uint16_t sample_rate; 138 } codec_support_t; 139 140 // current configuration 141 static const codec_support_t * codec_current = NULL; 142 143 // sine generator 144 145 #ifdef USE_ADUIO_GENERATOR 146 static unsigned int phase; 147 148 // input signal: pre-computed sine wave, 266 Hz at 16000 kHz 149 static const int16_t sine_int16_at_16000hz[] = { 150 0, 3135, 6237, 9270, 12202, 14999, 17633, 20073, 22294, 24270, 151 25980, 27406, 28531, 29344, 29835, 30000, 29835, 29344, 28531, 27406, 152 25980, 24270, 22294, 20073, 17633, 14999, 12202, 9270, 6237, 3135, 153 0, -3135, -6237, -9270, -12202, -14999, -17633, -20073, -22294, -24270, 154 -25980, -27406, -28531, -29344, -29835, -30000, -29835, -29344, -28531, -27406, 155 -25980, -24270, -22294, -20073, -17633, -14999, -12202, -9270, -6237, -3135, 156 }; 157 158 // 8 kHz samples for CVSD/SCO packets in little endian 159 static void sco_demo_sine_wave_int16_at_8000_hz_host_endian(uint16_t num_samples, int16_t * data){ 160 unsigned int i; 161 for (i=0; i < num_samples; i++){ 162 data[i] = sine_int16_at_16000hz[phase]; 163 // ony use every second sample from 16khz table to get 8khz 164 phase += 2; 165 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 166 phase = 0; 167 } 168 } 169 } 170 171 // 16 kHz samples for mSBC encoder in host endianess 172 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 173 static void sco_demo_sine_wave_int16_at_16000_hz_host_endian(uint16_t num_samples, int16_t * data){ 174 unsigned int i; 175 for (i=0; i < num_samples; i++){ 176 data[i] = sine_int16_at_16000hz[phase++]; 177 if (phase >= (sizeof(sine_int16_at_16000hz) / sizeof(int16_t))){ 178 phase = 0; 179 } 180 } 181 } 182 #endif 183 #endif 184 185 // Audio Playback / Recording 186 187 static void audio_playback_callback(int16_t * buffer, uint16_t num_samples){ 188 189 // fill with silence while paused 190 if (audio_output_paused){ 191 if (btstack_ring_buffer_bytes_available(&audio_output_ring_buffer) < audio_prebuffer_bytes){ 192 memset(buffer, 0, num_samples * BYTES_PER_FRAME); 193 return; 194 } else { 195 // resume playback 196 audio_output_paused = 0; 197 } 198 } 199 200 // get data from ringbuffer 201 uint32_t bytes_read = 0; 202 btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 203 num_samples -= bytes_read / BYTES_PER_FRAME; 204 buffer += bytes_read / BYTES_PER_FRAME; 205 206 // fill with 0 if not enough 207 if (num_samples){ 208 memset(buffer, 0, num_samples * BYTES_PER_FRAME); 209 audio_output_paused = 1; 210 } 211 } 212 213 #ifdef USE_AUDIO_INPUT 214 static void audio_recording_callback(const int16_t * buffer, uint16_t num_samples){ 215 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)buffer, num_samples * 2); 216 } 217 #endif 218 219 // return 1 if ok 220 static int audio_initialize(int sample_rate){ 221 222 // -- output -- // 223 224 // init buffers 225 memset(audio_output_ring_buffer_storage, 0, sizeof(audio_output_ring_buffer_storage)); 226 btstack_ring_buffer_init(&audio_output_ring_buffer, audio_output_ring_buffer_storage, sizeof(audio_output_ring_buffer_storage)); 227 228 // config and setup audio playback 229 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance(); 230 if (!audio_sink) return 0; 231 232 audio_sink->init(1, sample_rate, &audio_playback_callback); 233 audio_sink->start_stream(); 234 235 audio_output_paused = 1; 236 237 // -- input -- // 238 239 // init buffers 240 memset(audio_input_ring_buffer_storage, 0, sizeof(audio_input_ring_buffer_storage)); 241 btstack_ring_buffer_init(&audio_input_ring_buffer, audio_input_ring_buffer_storage, sizeof(audio_input_ring_buffer_storage)); 242 audio_input_paused = 1; 243 244 #ifdef USE_AUDIO_INPUT 245 // config and setup audio recording 246 const btstack_audio_source_t * audio_source = btstack_audio_source_get_instance(); 247 if (!audio_source) return 0; 248 249 audio_source->init(1, sample_rate, &audio_recording_callback); 250 audio_source->start_stream(); 251 #endif 252 253 return 1; 254 } 255 256 static void audio_terminate(void){ 257 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance(); 258 if (!audio_sink) return; 259 audio_sink->close(); 260 261 #ifdef USE_AUDIO_INPUT 262 const btstack_audio_source_t * audio_source= btstack_audio_source_get_instance(); 263 if (!audio_source) return; 264 audio_source->close(); 265 #endif 266 } 267 268 269 // CVSD - 8 kHz 270 271 static void sco_demo_cvsd_init(void){ 272 printf("SCO Demo: Init CVSD\n"); 273 274 btstack_cvsd_plc_init(&cvsd_plc_state); 275 276 audio_prebuffer_bytes = PREBUFFER_BYTES_8KHZ; 277 278 #ifdef USE_ADUIO_GENERATOR 279 sco_demo_audio_generator = &sco_demo_sine_wave_int16_at_8000_hz_host_endian; 280 #endif 281 282 #ifdef SCO_WAV_FILENAME 283 num_samples_to_write = SAMPLE_RATE_8KHZ * SCO_WAV_DURATION_IN_SECONDS; 284 wav_writer_open(SCO_WAV_FILENAME, 1, SAMPLE_RATE_8KHZ); 285 #endif 286 287 audio_initialize(SAMPLE_RATE_8KHZ); 288 } 289 290 static void sco_demo_cvsd_receive(const uint8_t * packet, uint16_t size){ 291 292 int16_t audio_frame_out[128]; // 293 294 if (size > sizeof(audio_frame_out)){ 295 printf("sco_demo_cvsd_receive: SCO packet larger than local output buffer - dropping data.\n"); 296 return; 297 } 298 299 const int audio_bytes_read = size - 3; 300 const int num_samples = audio_bytes_read / BYTES_PER_FRAME; 301 302 // convert into host endian 303 int16_t audio_frame_in[128]; 304 int i; 305 for (i=0;i<num_samples;i++){ 306 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2); 307 } 308 309 // treat packet as bad frame if controller does not report 'all good' 310 bool bad_frame = (packet[1] & 0x30) != 0; 311 312 btstack_cvsd_plc_process_data(&cvsd_plc_state, bad_frame, audio_frame_in, num_samples, audio_frame_out); 313 314 #ifdef SCO_WAV_FILENAME 315 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 316 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 317 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 318 num_samples_to_write -= samples_to_write; 319 if (num_samples_to_write == 0){ 320 wav_writer_close(); 321 } 322 #endif 323 324 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 325 } 326 327 static void sco_demo_cvsd_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){ 328 uint16_t bytes_to_copy = sco_payload_length; 329 330 // get data from ringbuffer 331 uint16_t pos = 0; 332 if (!audio_input_paused){ 333 uint16_t samples_to_copy = sco_payload_length / 2; 334 uint32_t bytes_read = 0; 335 btstack_ring_buffer_read(&audio_input_ring_buffer, payload_buffer, bytes_to_copy, &bytes_read); 336 // flip 16 on big endian systems 337 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 338 if (btstack_is_big_endian()){ 339 uint16_t i; 340 for (i=0;i<samples_to_copy/2;i+=2){ 341 uint8_t tmp = payload_buffer[i*2]; 342 payload_buffer[i*2] = payload_buffer[i*2+1]; 343 payload_buffer[i*2+1] = tmp; 344 } 345 } 346 bytes_to_copy -= bytes_read; 347 pos += bytes_read; 348 } 349 350 // fill with 0 if not enough 351 if (bytes_to_copy){ 352 memset(payload_buffer + pos, 0, bytes_to_copy); 353 audio_input_paused = 1; 354 } 355 } 356 357 static void sco_demo_cvsd_close(void){ 358 printf("Used CVSD with PLC, number of proccesed frames: \n - %d good frames, \n - %d bad frames.\n", cvsd_plc_state.good_frames_nr, cvsd_plc_state.bad_frames_nr); 359 } 360 361 static const codec_support_t codec_cvsd = { 362 .init = &sco_demo_cvsd_init, 363 .receive = &sco_demo_cvsd_receive, 364 .fill_payload = &sco_demo_cvsd_fill_payload, 365 .close = &sco_demo_cvsd_close, 366 .sample_rate = SAMPLE_RATE_8KHZ 367 }; 368 369 // mSBC - 16 kHz 370 371 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 372 373 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 374 UNUSED(context); 375 UNUSED(sample_rate); 376 UNUSED(data); 377 UNUSED(num_samples); 378 UNUSED(num_channels); 379 380 // samples in callback in host endianess, ready for playback 381 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 382 383 #ifdef SCO_WAV_FILENAME 384 if (!num_samples_to_write) return; 385 num_samples = btstack_min(num_samples, num_samples_to_write); 386 num_samples_to_write -= num_samples; 387 wav_writer_write_int16(num_samples, data); 388 if (num_samples_to_write == 0){ 389 wav_writer_close(); 390 } 391 #endif /* SCO_WAV_FILENAME */ 392 } 393 394 static void sco_demo_msbc_init(void){ 395 printf("SCO Demo: Init mSBC\n"); 396 397 btstack_sbc_decoder_init(&decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 398 hfp_msbc_init(); 399 400 audio_prebuffer_bytes = PREBUFFER_BYTES_16KHZ; 401 402 #ifdef USE_ADUIO_GENERATOR 403 sco_demo_audio_generator = &sco_demo_sine_wave_int16_at_16000_hz_host_endian; 404 #endif 405 406 #ifdef SCO_WAV_FILENAME 407 num_samples_to_write = SAMPLE_RATE_16KHZ * SCO_WAV_DURATION_IN_SECONDS; 408 wav_writer_open(SCO_WAV_FILENAME, 1, SAMPLE_RATE_16KHZ); 409 #endif 410 411 audio_initialize(SAMPLE_RATE_16KHZ); 412 } 413 414 static void sco_demo_msbc_receive(const uint8_t * packet, uint16_t size){ 415 btstack_sbc_decoder_process_data(&decoder_state, (packet[1] >> 4) & 3, packet+3, size-3); 416 } 417 418 void sco_demo_msbc_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){ 419 if (!audio_input_paused){ 420 int num_samples = hfp_msbc_num_audio_samples_per_frame(); 421 btstack_assert(num_samples <= MSBC_MAX_NUM_SAMPLES); 422 if (hfp_msbc_can_encode_audio_frame_now() && btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= (unsigned int)(num_samples * BYTES_PER_FRAME)){ 423 int16_t sample_buffer[MSBC_MAX_NUM_SAMPLES]; 424 uint32_t bytes_read; 425 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 426 hfp_msbc_encode_audio_frame(sample_buffer); 427 num_audio_frames++; 428 } 429 btstack_assert(hfp_msbc_num_bytes_in_stream() >= sco_payload_length); 430 } 431 432 // get data from encoder, fill with 0 if not enough 433 if (audio_input_paused || hfp_msbc_num_bytes_in_stream() < sco_payload_length){ 434 // just send '0's 435 memset(payload_buffer, 0, sco_payload_length); 436 audio_input_paused = 1; 437 } else { 438 hfp_msbc_read_from_stream(payload_buffer, sco_payload_length); 439 } 440 } 441 442 static void sco_demo_msbc_close(void){ 443 printf("Used mSBC with PLC, number of processed frames: \n - %d good frames, \n - %d zero frames, \n - %d bad frames.\n", decoder_state.good_frames_nr, decoder_state.zero_frames_nr, decoder_state.bad_frames_nr); 444 } 445 446 static const codec_support_t codec_msbc = { 447 .init = &sco_demo_msbc_init, 448 .receive = &sco_demo_msbc_receive, 449 .fill_payload = &sco_demo_msbc_fill_payload, 450 .close = &sco_demo_msbc_close, 451 .sample_rate = SAMPLE_RATE_16KHZ 452 }; 453 454 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */ 455 456 void sco_demo_init(void){ 457 458 #ifdef ENABLE_CLASSIC_LEGACY_CONNECTIONS_FOR_SCO_DEMOS 459 printf("Disable BR/EDR Secure Connctions due to incompatibilities with SCO connections\n"); 460 gap_secure_connections_enable(false); 461 #endif 462 463 // status 464 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 465 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n"); 466 #endif 467 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 468 if (btstack_audio_sink_get_instance()){ 469 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n"); 470 } else { 471 printf("SCO Demo: Sending sine wave, hexdump received data.\n"); 472 } 473 #endif 474 475 // Set SCO for CVSD (mSBC or other codecs automatically use 8-bit transparent mode) 476 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 477 } 478 479 void sco_demo_set_codec(uint8_t negotiated_codec){ 480 switch (negotiated_codec){ 481 case HFP_CODEC_CVSD: 482 codec_current = &codec_cvsd; 483 break; 484 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 485 case HFP_CODEC_MSBC: 486 codec_current = &codec_msbc; 487 break; 488 #endif 489 default: 490 btstack_assert(false); 491 break; 492 } 493 494 codec_current->init(); 495 } 496 497 void sco_demo_receive(uint8_t * packet, uint16_t size){ 498 static uint32_t packets = 0; 499 static uint32_t crc_errors = 0; 500 static uint32_t data_received = 0; 501 static uint32_t byte_errors = 0; 502 503 count_received++; 504 505 data_received += size - 3; 506 packets++; 507 if (data_received > 100000){ 508 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", (unsigned int) data_received, (unsigned int) packets, (unsigned int) crc_errors, (unsigned int) byte_errors); 509 crc_errors = 0; 510 byte_errors = 0; 511 data_received = 0; 512 packets = 0; 513 } 514 515 codec_current->receive(packet, size); 516 } 517 518 void sco_demo_send(hci_con_handle_t sco_handle){ 519 520 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 521 522 int sco_packet_length = hci_get_sco_packet_length(); 523 int sco_payload_length = sco_packet_length - 3; 524 525 hci_reserve_packet_buffer(); 526 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 527 528 #ifdef USE_ADUIO_GENERATOR 529 #define REFILL_SAMPLES 16 530 // re-fill audio buffer 531 uint16_t samples_free = btstack_ring_buffer_bytes_free(&audio_input_ring_buffer) / 2; 532 while (samples_free > 0){ 533 int16_t samples_buffer[REFILL_SAMPLES]; 534 uint16_t samples_to_add = btstack_min(samples_free, REFILL_SAMPLES); 535 (*sco_demo_audio_generator)(samples_to_add, samples_buffer); 536 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)samples_buffer, samples_to_add * 2); 537 samples_free -= samples_to_add; 538 } 539 #endif 540 541 // resume if pre-buffer is filled 542 if (audio_input_paused){ 543 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= audio_prebuffer_bytes){ 544 // resume sending 545 audio_input_paused = 0; 546 } 547 } 548 549 // fill payload by codec 550 codec_current->fill_payload(&sco_packet[3], sco_payload_length); 551 552 // set handle + flags 553 little_endian_store_16(sco_packet, 0, sco_handle); 554 // set len 555 sco_packet[2] = sco_payload_length; 556 // finally send packet 557 hci_send_sco_packet_buffer(sco_packet_length); 558 559 // request another send event 560 hci_request_sco_can_send_now_event(); 561 562 count_sent++; 563 if ((count_sent % SCO_REPORT_PERIOD) == 0) { 564 printf("SCO: sent %u, received %u\n", count_sent, count_received); 565 } 566 } 567 568 void sco_demo_close(void){ 569 printf("SCO demo close\n"); 570 571 printf("SCO demo statistics: "); 572 codec_current->close(); 573 codec_current = NULL; 574 575 #if defined(SCO_WAV_FILENAME) 576 wav_writer_close(); 577 #endif 578 579 audio_terminate(); 580 } 581