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