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