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