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_codec.h" 55 56 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH 57 #include "btstack_lc3.h" 58 #include "btstack_lc3_google.h" 59 #endif 60 61 62 #ifdef _MSC_VER 63 // ignore deprecated warning for fopen 64 #pragma warning(disable : 4996) 65 #endif 66 67 #ifdef HAVE_POSIX_FILE_IO 68 #include "wav_util.h" 69 #endif 70 71 // test modes 72 #define SCO_DEMO_MODE_SINE 0 73 #define SCO_DEMO_MODE_MICROPHONE 1 74 #define SCO_DEMO_MODE_MODPLAYER 2 75 76 // SCO demo configuration 77 #define SCO_DEMO_MODE SCO_DEMO_MODE_MICROPHONE 78 79 // number of sco packets until 'report' on console 80 #define SCO_REPORT_PERIOD 100 81 82 83 #ifdef HAVE_POSIX_FILE_IO 84 // length and name of wav file on disk 85 #define SCO_WAV_DURATION_IN_SECONDS 15 86 #define SCO_WAV_FILENAME "sco_input.wav" 87 #endif 88 89 // constants 90 #define NUM_CHANNELS 1 91 #define SAMPLE_RATE_8KHZ 8000 92 #define SAMPLE_RATE_16KHZ 16000 93 #define SAMPLE_RATE_32KHZ 32000 94 #define BYTES_PER_FRAME 2 95 96 // audio pre-buffer - also defines latency 97 #define SCO_PREBUFFER_MS 50 98 #define PREBUFFER_BYTES_8KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_8KHZ/1000 * BYTES_PER_FRAME) 99 #define PREBUFFER_BYTES_16KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_16KHZ/1000 * BYTES_PER_FRAME) 100 #define PREBUFFER_BYTES_32KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_32KHZ/1000 * BYTES_PER_FRAME) 101 102 #if defined(ENABLE_HFP_SUPER_WIDE_BAND_SPEECH) 103 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_32KHZ 104 #define SAMPLES_PER_FRAME_MAX 240 105 #elif defined(ENABLE_HFP_WIDE_BAND_SPEECH) 106 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_16KHZ 107 #define SAMPLES_PER_FRAME_MAX 120 108 #else 109 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_8KHZ 110 #define SAMPLES_PER_FRAME_MAX 60 111 #endif 112 113 static uint16_t audio_prebuffer_bytes; 114 115 // output 116 static int audio_output_paused = 0; 117 static uint8_t audio_output_ring_buffer_storage[2 * PREBUFFER_BYTES_MAX]; 118 static btstack_ring_buffer_t audio_output_ring_buffer; 119 120 // input 121 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 122 #define USE_AUDIO_INPUT 123 #else 124 #define USE_ADUIO_GENERATOR 125 static void (*sco_demo_audio_generator)(uint16_t num_samples, int16_t * data); 126 #endif 127 128 static int audio_input_paused = 0; 129 static uint8_t audio_input_ring_buffer_storage[2 * PREBUFFER_BYTES_MAX]; 130 static btstack_ring_buffer_t audio_input_ring_buffer; 131 132 // mod player 133 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER 134 #include "hxcmod.h" 135 #include "mods/mod.h" 136 static modcontext mod_context; 137 #endif 138 139 static int count_sent = 0; 140 static int count_received = 0; 141 142 static btstack_cvsd_plc_state_t cvsd_plc_state; 143 144 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 145 static btstack_sbc_encoder_state_t msbc_encoder_state; 146 static btstack_sbc_decoder_state_t msbc_decoder_state; 147 #endif 148 149 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH 150 static const btstack_lc3_decoder_t * lc3_decoder; 151 static btstack_lc3_decoder_google_t lc3_decoder_context; 152 static btstack_lc3_encoder_google_t lc3_encoder_context; 153 static hfp_h2_sync_t hfp_h2_sync; 154 #endif 155 156 int num_samples_to_write; 157 int num_audio_frames; 158 159 // generic codec support 160 typedef struct { 161 void (*init)(void); 162 void(*receive)(const uint8_t * packet, uint16_t size); 163 void (*fill_payload)(uint8_t * payload_buffer, uint16_t sco_payload_length); 164 void (*close)(void); 165 // 166 uint16_t sample_rate; 167 } codec_support_t; 168 169 // current configuration 170 static const codec_support_t * codec_current = NULL; 171 172 // hfp_codec 173 #if defined(ENABLE_HFP_WIDE_BAND_SPEECH) || defined(ENABLE_HFP_SUPER_WIDE_BAND_SPEECH) 174 static hfp_codec_t hfp_codec; 175 #endif 176 177 // Sine Wave 178 179 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 180 static uint16_t sine_wave_phase; 181 static uint16_t sine_wave_steps_per_sample; 182 #define SINE_WAVE_SAMPLE_RATE SAMPLE_RATE_32KHZ 183 184 // input signal: pre-computed int16 sine wave, 32000 Hz at 266 Hz 185 static const int16_t sine_int16[] = { 186 0, 1715, 3425, 5126, 6813, 8481, 10126, 11743, 13328, 14876, 187 16383, 17846, 19260, 20621, 21925, 23170, 24351, 25465, 26509, 27481, 188 28377, 29196, 29934, 30591, 31163, 31650, 32051, 32364, 32587, 32722, 189 32767, 32722, 32587, 32364, 32051, 31650, 31163, 30591, 29934, 29196, 190 28377, 27481, 26509, 25465, 24351, 23170, 21925, 20621, 19260, 17846, 191 16383, 14876, 13328, 11743, 10126, 8481, 6813, 5126, 3425, 1715, 192 0, -1715, -3425, -5126, -6813, -8481, -10126, -11743, -13328, -14876, 193 -16384, -17846, -19260, -20621, -21925, -23170, -24351, -25465, -26509, -27481, 194 -28377, -29196, -29934, -30591, -31163, -31650, -32051, -32364, -32587, -32722, 195 -32767, -32722, -32587, -32364, -32051, -31650, -31163, -30591, -29934, -29196, 196 -28377, -27481, -26509, -25465, -24351, -23170, -21925, -20621, -19260, -17846, 197 -16384, -14876, -13328, -11743, -10126, -8481, -6813, -5126, -3425, -1715, 198 }; 199 200 static void sco_demo_sine_wave_host_endian(uint16_t num_samples, int16_t * data){ 201 unsigned int i; 202 for (i=0; i < num_samples; i++){ 203 data[i] = sine_int16[sine_wave_phase]; 204 sine_wave_phase += sine_wave_steps_per_sample; 205 if (sine_wave_phase >= (sizeof(sine_int16) / sizeof(int16_t))){ 206 sine_wave_phase = 0; 207 } 208 } 209 } 210 #endif 211 212 // Mod Player 213 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER 214 #define NUM_SAMPLES_GENERATOR_BUFFER 30 215 static void sco_demo_modplayer(uint16_t num_samples, int16_t * data){ 216 // mix down stereo 217 signed short samples[NUM_SAMPLES_GENERATOR_BUFFER * 2]; 218 while (num_samples > 0){ 219 uint16_t next_samples = btstack_min(num_samples, NUM_SAMPLES_GENERATOR_BUFFER); 220 hxcmod_fillbuffer(&mod_context, (unsigned short *) samples, next_samples, NULL); 221 num_samples -= next_samples; 222 uint16_t i; 223 for (i=0;i<next_samples;i++){ 224 int32_t left = samples[2*i + 0]; 225 int32_t right = samples[2*i + 1]; 226 data[i] = (int16_t)((left + right) / 2); 227 } 228 } 229 } 230 #endif 231 232 // Audio Playback / Recording 233 234 static void audio_playback_callback(int16_t * buffer, uint16_t num_samples){ 235 236 // fill with silence while paused 237 if (audio_output_paused){ 238 if (btstack_ring_buffer_bytes_available(&audio_output_ring_buffer) < audio_prebuffer_bytes){ 239 memset(buffer, 0, num_samples * BYTES_PER_FRAME); 240 return; 241 } else { 242 // resume playback 243 audio_output_paused = 0; 244 } 245 } 246 247 // get data from ringbuffer 248 uint32_t bytes_read = 0; 249 btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 250 num_samples -= bytes_read / BYTES_PER_FRAME; 251 buffer += bytes_read / BYTES_PER_FRAME; 252 253 // fill with 0 if not enough 254 if (num_samples){ 255 memset(buffer, 0, num_samples * BYTES_PER_FRAME); 256 audio_output_paused = 1; 257 } 258 } 259 260 #ifdef USE_AUDIO_INPUT 261 static void audio_recording_callback(const int16_t * buffer, uint16_t num_samples){ 262 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)buffer, num_samples * 2); 263 } 264 #endif 265 266 // return 1 if ok 267 static int audio_initialize(int sample_rate){ 268 269 // -- output -- // 270 271 // init buffers 272 memset(audio_output_ring_buffer_storage, 0, sizeof(audio_output_ring_buffer_storage)); 273 btstack_ring_buffer_init(&audio_output_ring_buffer, audio_output_ring_buffer_storage, sizeof(audio_output_ring_buffer_storage)); 274 275 // config and setup audio playback 276 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance(); 277 if (audio_sink != NULL){ 278 audio_sink->init(1, sample_rate, &audio_playback_callback); 279 audio_sink->start_stream(); 280 281 audio_output_paused = 1; 282 } 283 284 // -- input -- // 285 286 // init buffers 287 memset(audio_input_ring_buffer_storage, 0, sizeof(audio_input_ring_buffer_storage)); 288 btstack_ring_buffer_init(&audio_input_ring_buffer, audio_input_ring_buffer_storage, sizeof(audio_input_ring_buffer_storage)); 289 audio_input_paused = 1; 290 291 #ifdef USE_AUDIO_INPUT 292 // config and setup audio recording 293 const btstack_audio_source_t * audio_source = btstack_audio_source_get_instance(); 294 if (audio_source != NULL){ 295 audio_source->init(1, sample_rate, &audio_recording_callback); 296 audio_source->start_stream(); 297 } 298 #endif 299 300 return 1; 301 } 302 303 static void audio_terminate(void){ 304 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance(); 305 if (!audio_sink) return; 306 audio_sink->close(); 307 308 #ifdef USE_AUDIO_INPUT 309 const btstack_audio_source_t * audio_source= btstack_audio_source_get_instance(); 310 if (!audio_source) return; 311 audio_source->close(); 312 #endif 313 } 314 315 316 // CVSD - 8 kHz 317 318 static void sco_demo_cvsd_init(void){ 319 printf("SCO Demo: Init CVSD\n"); 320 btstack_cvsd_plc_init(&cvsd_plc_state); 321 } 322 323 static void sco_demo_cvsd_receive(const uint8_t * packet, uint16_t size){ 324 325 int16_t audio_frame_out[128]; // 326 327 if (size > sizeof(audio_frame_out)){ 328 printf("sco_demo_cvsd_receive: SCO packet larger than local output buffer - dropping data.\n"); 329 return; 330 } 331 332 const int audio_bytes_read = size - 3; 333 const int num_samples = audio_bytes_read / BYTES_PER_FRAME; 334 335 // convert into host endian 336 int16_t audio_frame_in[128]; 337 int i; 338 for (i=0;i<num_samples;i++){ 339 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2); 340 } 341 342 // treat packet as bad frame if controller does not report 'all good' 343 bool bad_frame = (packet[1] & 0x30) != 0; 344 345 btstack_cvsd_plc_process_data(&cvsd_plc_state, bad_frame, audio_frame_in, num_samples, audio_frame_out); 346 347 #ifdef SCO_WAV_FILENAME 348 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut) 349 const int samples_to_write = btstack_min(num_samples, num_samples_to_write); 350 wav_writer_write_le_int16(samples_to_write, audio_frame_out); 351 num_samples_to_write -= samples_to_write; 352 if (num_samples_to_write == 0){ 353 wav_writer_close(); 354 } 355 #endif 356 357 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read); 358 } 359 360 static void sco_demo_cvsd_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){ 361 uint16_t bytes_to_copy = sco_payload_length; 362 363 // get data from ringbuffer 364 uint16_t pos = 0; 365 if (!audio_input_paused){ 366 uint16_t samples_to_copy = sco_payload_length / 2; 367 uint32_t bytes_read = 0; 368 btstack_ring_buffer_read(&audio_input_ring_buffer, payload_buffer, bytes_to_copy, &bytes_read); 369 // flip 16 on big endian systems 370 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems 371 if (btstack_is_big_endian()){ 372 uint16_t i; 373 for (i=0;i<samples_to_copy/2;i+=2){ 374 uint8_t tmp = payload_buffer[i*2]; 375 payload_buffer[i*2] = payload_buffer[i*2+1]; 376 payload_buffer[i*2+1] = tmp; 377 } 378 } 379 bytes_to_copy -= bytes_read; 380 pos += bytes_read; 381 } 382 383 // fill with 0 if not enough 384 if (bytes_to_copy){ 385 memset(payload_buffer + pos, 0, bytes_to_copy); 386 audio_input_paused = 1; 387 } 388 } 389 390 static void sco_demo_cvsd_close(void){ 391 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); 392 } 393 394 static const codec_support_t codec_cvsd = { 395 .init = &sco_demo_cvsd_init, 396 .receive = &sco_demo_cvsd_receive, 397 .fill_payload = &sco_demo_cvsd_fill_payload, 398 .close = &sco_demo_cvsd_close, 399 .sample_rate = SAMPLE_RATE_8KHZ 400 }; 401 402 // encode using hfp_codec 403 #if defined(ENABLE_HFP_WIDE_BAND_SPEECH) || defined(ENABLE_HFP_SUPER_WIDE_BAND_SPEECH) 404 static void sco_demo_codec_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){ 405 if (!audio_input_paused){ 406 int num_samples = hfp_codec_num_audio_samples_per_frame(&hfp_codec); 407 btstack_assert(num_samples <= SAMPLES_PER_FRAME_MAX); 408 uint16_t samples_available = btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) / BYTES_PER_FRAME; 409 if (hfp_codec_can_encode_audio_frame_now(&hfp_codec) && samples_available >= num_samples){ 410 int16_t sample_buffer[SAMPLES_PER_FRAME_MAX]; 411 uint32_t bytes_read; 412 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read); 413 hfp_codec_encode_audio_frame(&hfp_codec, sample_buffer); 414 num_audio_frames++; 415 } 416 } 417 // get data from encoder, fill with 0 if not enough 418 if (audio_input_paused || hfp_codec_num_bytes_available(&hfp_codec) < sco_payload_length){ 419 // just send '0's 420 memset(payload_buffer, 0, sco_payload_length); 421 audio_input_paused = 1; 422 } else { 423 hfp_codec_read_from_stream(&hfp_codec, payload_buffer, sco_payload_length); 424 } 425 } 426 #endif 427 428 // mSBC - 16 kHz 429 430 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 431 432 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){ 433 UNUSED(context); 434 UNUSED(sample_rate); 435 UNUSED(data); 436 UNUSED(num_samples); 437 UNUSED(num_channels); 438 439 // samples in callback in host endianess, ready for playback 440 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2); 441 442 #ifdef SCO_WAV_FILENAME 443 if (!num_samples_to_write) return; 444 num_samples = btstack_min(num_samples, num_samples_to_write); 445 num_samples_to_write -= num_samples; 446 wav_writer_write_int16(num_samples, data); 447 if (num_samples_to_write == 0){ 448 wav_writer_close(); 449 } 450 #endif /* SCO_WAV_FILENAME */ 451 } 452 453 static void sco_demo_msbc_init(void){ 454 printf("SCO Demo: Init mSBC\n"); 455 btstack_sbc_decoder_init(&msbc_decoder_state, SBC_MODE_mSBC, &handle_pcm_data, NULL); 456 hfp_codec_init_msbc(&hfp_codec, &msbc_encoder_state); 457 } 458 459 static void sco_demo_msbc_receive(const uint8_t * packet, uint16_t size){ 460 btstack_sbc_decoder_process_data(&msbc_decoder_state, (packet[1] >> 4) & 3, packet + 3, size - 3); 461 } 462 463 static void sco_demo_msbc_close(void){ 464 printf("Used mSBC with PLC, number of processed frames: \n - %d good frames, \n - %d zero frames, \n - %d bad frames.\n", msbc_decoder_state.good_frames_nr, msbc_decoder_state.zero_frames_nr, msbc_decoder_state.bad_frames_nr); 465 } 466 467 static const codec_support_t codec_msbc = { 468 .init = &sco_demo_msbc_init, 469 .receive = &sco_demo_msbc_receive, 470 .fill_payload = &sco_demo_codec_fill_payload, 471 .close = &sco_demo_msbc_close, 472 .sample_rate = SAMPLE_RATE_16KHZ 473 }; 474 475 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */ 476 477 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH 478 479 #define LC3_SWB_SAMPLES_PER_FRAME 240 480 #define LC3_SWB_OCTETS_PER_FRAME 58 481 482 static bool sco_demo_lc3swb_frame_callback(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len){ 483 484 // skip H2 header for good frames 485 if (bad_frame == false){ 486 btstack_assert(frame_data != NULL); 487 frame_data += 2; 488 } 489 490 uint8_t tmp_BEC_detect = 0; 491 uint8_t BFI = bad_frame ? 1 : 0; 492 int16_t samples[LC3_SWB_SAMPLES_PER_FRAME]; 493 (void) lc3_decoder->decode_signed_16(&lc3_decoder_context, frame_data, BFI, 494 samples, 1, &tmp_BEC_detect); 495 496 // samples in callback in host endianess, ready for playback 497 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)samples, LC3_SWB_SAMPLES_PER_FRAME*2); 498 499 #ifdef SCO_WAV_FILENAME 500 if (num_samples_to_write > 0){ 501 uint16_t num_samples = btstack_min(LC3_SWB_SAMPLES_PER_FRAME, num_samples_to_write); 502 num_samples_to_write -= num_samples; 503 wav_writer_write_int16(num_samples, samples); 504 if (num_samples_to_write == 0){ 505 wav_writer_close(); 506 } 507 } 508 #endif /* SCO_WAV_FILENAME */ 509 510 // frame is good, if it isn't a bad frame and we didn't detect other errors 511 return (bad_frame == false) && (tmp_BEC_detect == 0); 512 } 513 514 static void sco_demo_lc3swb_init(void){ 515 516 printf("SCO Demo: Init LC3-SWB\n"); 517 518 hfp_codec.lc3_encoder_context = &lc3_encoder_context; 519 const btstack_lc3_encoder_t * lc3_encoder = btstack_lc3_encoder_google_init_instance((btstack_lc3_encoder_google_t *) hfp_codec.lc3_encoder_context); 520 hfp_codec_init_lc3_swb(&hfp_codec, lc3_encoder, &lc3_encoder_context); 521 522 // init lc3 decoder 523 lc3_decoder = btstack_lc3_decoder_google_init_instance(&lc3_decoder_context); 524 lc3_decoder->configure(&lc3_decoder_context, SAMPLE_RATE_32KHZ, BTSTACK_LC3_FRAME_DURATION_7500US, LC3_SWB_OCTETS_PER_FRAME); 525 526 // init HPF H2 framing 527 hfp_h2_sync_init(&hfp_h2_sync, &sco_demo_lc3swb_frame_callback); 528 } 529 530 static void sco_demo_lc3swb_receive(const uint8_t * packet, uint16_t size){ 531 uint8_t packet_status = (packet[1] >> 4) & 3; 532 bool bad_frame = packet_status != 0; 533 hfp_h2_sync_process(&hfp_h2_sync, bad_frame, &packet[3], size-3); 534 } 535 536 static void sco_demo_lc3swb_close(void){ 537 // TODO: report 538 } 539 540 static const codec_support_t codec_lc3swb = { 541 .init = &sco_demo_lc3swb_init, 542 .receive = &sco_demo_lc3swb_receive, 543 .fill_payload = &sco_demo_codec_fill_payload, 544 .close = &sco_demo_lc3swb_close, 545 .sample_rate = SAMPLE_RATE_32KHZ 546 }; 547 #endif 548 549 void sco_demo_init(void){ 550 551 #ifdef ENABLE_CLASSIC_LEGACY_CONNECTIONS_FOR_SCO_DEMOS 552 printf("Disable BR/EDR Secure Connctions due to incompatibilities with SCO connections\n"); 553 gap_secure_connections_enable(false); 554 #endif 555 556 // Set SCO for CVSD (mSBC or other codecs automatically use 8-bit transparent mode) 557 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD 558 559 // status 560 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE 561 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n"); 562 #endif 563 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 564 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n"); 565 #endif 566 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER 567 printf("SCO Demo: Sending modplayer wave, audio output via btstack_audio.\n"); 568 // init mod 569 int hxcmod_initialized = hxcmod_init(&mod_context); 570 btstack_assert(hxcmod_initialized != 0); 571 #endif 572 } 573 574 void sco_demo_set_codec(uint8_t negotiated_codec){ 575 switch (negotiated_codec){ 576 case HFP_CODEC_CVSD: 577 codec_current = &codec_cvsd; 578 break; 579 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH 580 case HFP_CODEC_MSBC: 581 codec_current = &codec_msbc; 582 break; 583 #endif 584 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH 585 case HFP_CODEC_LC3_SWB: 586 codec_current = &codec_lc3swb; 587 break; 588 #endif 589 default: 590 btstack_assert(false); 591 break; 592 } 593 594 codec_current->init(); 595 596 audio_initialize(codec_current->sample_rate); 597 598 audio_prebuffer_bytes = SCO_PREBUFFER_MS * (codec_current->sample_rate/1000) * BYTES_PER_FRAME; 599 600 #ifdef SCO_WAV_FILENAME 601 num_samples_to_write = codec_current->sample_rate * SCO_WAV_DURATION_IN_SECONDS; 602 wav_writer_open(SCO_WAV_FILENAME, 1, codec_current->sample_rate); 603 #endif 604 605 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE 606 sine_wave_steps_per_sample = SINE_WAVE_SAMPLE_RATE / codec_current->sample_rate; 607 sco_demo_audio_generator = &sco_demo_sine_wave_host_endian; 608 #endif 609 610 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER 611 // load mod 612 hxcmod_setcfg(&mod_context, codec_current->sample_rate, 16, 1, 1, 1); 613 hxcmod_load(&mod_context, (void *) &mod_data, mod_len); 614 sco_demo_audio_generator = &sco_demo_modplayer; 615 #endif 616 } 617 618 void sco_demo_receive(uint8_t * packet, uint16_t size){ 619 static uint32_t packets = 0; 620 static uint32_t crc_errors = 0; 621 static uint32_t data_received = 0; 622 static uint32_t byte_errors = 0; 623 624 count_received++; 625 626 data_received += size - 3; 627 packets++; 628 if (data_received > 100000){ 629 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); 630 crc_errors = 0; 631 byte_errors = 0; 632 data_received = 0; 633 packets = 0; 634 } 635 636 codec_current->receive(packet, size); 637 } 638 639 void sco_demo_send(hci_con_handle_t sco_handle){ 640 641 if (sco_handle == HCI_CON_HANDLE_INVALID) return; 642 643 int sco_packet_length = hci_get_sco_packet_length_for_connection(sco_handle); 644 int sco_payload_length = sco_packet_length - 3; 645 646 hci_reserve_packet_buffer(); 647 uint8_t * sco_packet = hci_get_outgoing_packet_buffer(); 648 649 #ifdef USE_ADUIO_GENERATOR 650 #define REFILL_SAMPLES 16 651 // re-fill audio buffer 652 uint16_t samples_free = btstack_ring_buffer_bytes_free(&audio_input_ring_buffer) / 2; 653 while (samples_free > 0){ 654 int16_t samples_buffer[REFILL_SAMPLES]; 655 uint16_t samples_to_add = btstack_min(samples_free, REFILL_SAMPLES); 656 (*sco_demo_audio_generator)(samples_to_add, samples_buffer); 657 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)samples_buffer, samples_to_add * 2); 658 samples_free -= samples_to_add; 659 } 660 #endif 661 662 // resume if pre-buffer is filled 663 if (audio_input_paused){ 664 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= audio_prebuffer_bytes){ 665 // resume sending 666 audio_input_paused = 0; 667 } 668 } 669 670 // fill payload by codec 671 codec_current->fill_payload(&sco_packet[3], sco_payload_length); 672 673 // set handle + flags 674 little_endian_store_16(sco_packet, 0, sco_handle); 675 // set len 676 sco_packet[2] = sco_payload_length; 677 // finally send packet 678 hci_send_sco_packet_buffer(sco_packet_length); 679 680 // request another send event 681 hci_request_sco_can_send_now_event(); 682 683 count_sent++; 684 if ((count_sent % SCO_REPORT_PERIOD) == 0) { 685 printf("SCO: sent %u, received %u\n", count_sent, count_received); 686 } 687 } 688 689 void sco_demo_close(void){ 690 printf("SCO demo close\n"); 691 692 printf("SCO demo statistics: "); 693 codec_current->close(); 694 codec_current = NULL; 695 696 #if defined(SCO_WAV_FILENAME) 697 wav_writer_close(); 698 #endif 699 700 audio_terminate(); 701 } 702