1 /* 2 * Copyright (C) {copyright_year} 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__ "le_audio_demo_util_sink.c" 39 40 #include "le_audio_demo_util_sink.h" 41 42 #include "btstack_bool.h" 43 #include "btstack_config.h" 44 #include <btstack_debug.h> 45 #include <printf.h> 46 47 #include "hci.h" 48 #include "btstack_audio.h" 49 #include "btstack_lc3_google.h" 50 #include "btstack_lc3plus_fraunhofer.h" 51 52 #include "hxcmod.h" 53 #include "mods/mod.h" 54 55 #ifdef HAVE_POSIX_FILE_IO 56 #include "wav_util.h" 57 #include "btstack_ring_buffer.h" 58 59 #endif 60 61 //#define DEBUG_PLC 62 #ifdef DEBUG_PLC 63 #define printf_plc(...) printf(__VA_ARGS__) 64 #else 65 #define printf_plc(...) (void)(0); 66 #endif 67 68 #define MAX_CHANNELS 2 69 #define MAX_SAMPLES_PER_FRAME 480 70 #define MAX_LC3_FRAME_BYTES 155 71 72 // playback 73 #define MAX_NUM_LC3_FRAMES 15 74 #define MAX_BYTES_PER_SAMPLE 4 75 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_BYTES_PER_SAMPLE) 76 #define PLAYBACK_START_MS (MAX_NUM_LC3_FRAMES * 20 / 3) 77 78 #define ANSI_COLOR_RED "\x1b[31m" 79 #define ANSI_COLOR_GREEN "\x1b[32m" 80 #define ANSI_COLOR_YELLOW "\x1b[33m" 81 #define ANSI_COLOR_BLUE "\x1b[34m" 82 #define ANSI_COLOR_MAGENTA "\x1b[35m" 83 #define ANSI_COLOR_CYAN "\x1b[36m" 84 #define ANSI_COLOR_RESET "\x1b[0m" 85 86 // SINK 87 88 static const char * le_audio_demo_sink_filename_wav; 89 90 static btstack_lc3_frame_duration_t le_audio_demo_sink_frame_duration; 91 static hci_iso_type_t le_audio_demo_sink_type; 92 93 static uint32_t le_audio_demo_sink_sampling_frequency_hz; 94 static uint16_t le_audio_demo_sink_num_samples_per_frame; 95 static uint8_t le_audio_demo_sink_num_streams; 96 static uint8_t le_audio_demo_sink_num_channels_per_stream; 97 static uint8_t le_audio_demo_sink_num_channels; 98 static uint16_t le_audio_demo_sink_octets_per_frame; 99 static uint16_t le_audio_demo_sink_iso_interval_1250us; 100 static uint8_t le_audio_demo_sink_flush_timeout; 101 static uint8_t le_audio_demo_sink_pre_transmission_offset; 102 103 // playback 104 static uint16_t playback_start_threshold_bytes; 105 static bool playback_active; 106 static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE]; 107 static btstack_ring_buffer_t playback_buffer; 108 109 // PLC 110 static bool stream_last_packet_received[MAX_CHANNELS]; 111 static uint16_t stream_last_packet_sequence[MAX_CHANNELS]; 112 static uint16_t group_last_packet_sequence; 113 static bool group_last_packet_received; 114 115 static uint32_t le_audio_demo_sink_lc3_frames; 116 static uint32_t samples_received; 117 static uint32_t samples_played; 118 static uint32_t samples_dropped; 119 120 static btstack_timer_source_t next_packet_timer; 121 122 // lc3 decoder 123 static bool le_audio_demo_lc3plus_decoder_requested = false; 124 static const btstack_lc3_decoder_t * lc3_decoder; 125 static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME]; 126 static bool have_pcm[MAX_CHANNELS]; 127 128 static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_CHANNELS]; 129 #ifdef HAVE_LC3PLUS 130 static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_CHANNELS]; 131 #endif 132 static void * decoder_contexts[MAX_CHANNELS]; 133 134 static void le_audio_connection_sink_playback(int16_t * buffer, uint16_t num_samples){ 135 // called from lower-layer but guaranteed to be on main thread 136 log_info("Playback: need %u, have %u", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); 137 138 samples_played += num_samples; 139 140 uint32_t bytes_needed = num_samples * le_audio_demo_sink_num_channels * 2; 141 if (playback_active == false){ 142 if (btstack_ring_buffer_bytes_available(&playback_buffer) >= playback_start_threshold_bytes) { 143 log_info("Playback started"); 144 playback_active = true; 145 } 146 } else { 147 if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)) { 148 log_info("Playback underrun"); 149 printf("Playback Underrun\n"); 150 // empty buffer 151 uint32_t bytes_read; 152 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 153 playback_active = false; 154 } 155 } 156 157 if (playback_active){ 158 uint32_t bytes_read; 159 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read); 160 btstack_assert(bytes_read == bytes_needed); 161 } else { 162 memset(buffer, 0, bytes_needed); 163 } 164 } 165 166 static void setup_lc3_decoder(void){ 167 uint8_t channel; 168 for (channel = 0 ; channel < le_audio_demo_sink_num_channels ; channel++){ 169 // pick decoder 170 void * decoder_context = NULL; 171 #ifdef HAVE_LC3PLUS 172 if (use_lc3plus_decoder){ 173 decoder_context = &fraunhofer_decoder_contexts[channel]; 174 lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context); 175 } 176 else 177 #endif 178 { 179 decoder_context = &google_decoder_contexts[channel]; 180 lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context); 181 } 182 decoder_contexts[channel] = decoder_context; 183 lc3_decoder->configure(decoder_context, le_audio_demo_sink_sampling_frequency_hz, le_audio_demo_sink_frame_duration, le_audio_demo_sink_octets_per_frame); 184 } 185 btstack_assert(le_audio_demo_sink_num_samples_per_frame <= MAX_SAMPLES_PER_FRAME); 186 } 187 188 static void store_samples_in_ringbuffer(void){ 189 // check if we have all channels 190 uint8_t channel; 191 for (channel = 0; channel < le_audio_demo_sink_num_channels; channel++){ 192 if (have_pcm[channel] == false) return; 193 } 194 #ifdef HAVE_POSIX_FILE_IO 195 // write wav samples 196 wav_writer_write_int16(le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame, pcm); 197 #endif 198 // store samples in playback buffer 199 uint32_t bytes_to_store = le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame * 2; 200 samples_received += le_audio_demo_sink_num_samples_per_frame; 201 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) { 202 btstack_ring_buffer_write(&playback_buffer, (uint8_t *) pcm, bytes_to_store); 203 } else { 204 printf("Samples dropped\n"); 205 samples_dropped += le_audio_demo_sink_num_samples_per_frame; 206 } 207 memset(have_pcm, 0, sizeof(have_pcm)); 208 } 209 210 static void plc_do(uint8_t stream_index) { 211 // inject packet 212 uint8_t tmp_BEC_detect; 213 uint8_t BFI = 1; 214 uint8_t i; 215 for (i = 0; i < le_audio_demo_sink_num_channels_per_stream; i++){ 216 uint8_t effective_channel = stream_index + i; 217 (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], NULL, BFI, 218 &pcm[effective_channel], le_audio_demo_sink_num_channels, 219 &tmp_BEC_detect); 220 } 221 // and store in ringbuffer when PCM for all channels is available 222 store_samples_in_ringbuffer(); 223 } 224 225 // 226 // Perform PLC for packets missing in previous intervals 227 // 228 // assumptions: 229 // - packet sequence number is monotonic increasing 230 // - if packet with seq nr x is received, all packets with smaller seq number are either received or missed 231 static void plc_check(uint16_t packet_sequence_number) { 232 while (group_last_packet_sequence != packet_sequence_number){ 233 uint8_t i; 234 for (i=0;i<le_audio_demo_sink_num_streams;i++){ 235 // deal with first packet missing. inject silent samples, pcm buffer is memset to zero at start 236 if (stream_last_packet_received[i] == false){ 237 printf_plc("- ISO #%u, very first packet missing\n", i); 238 have_pcm[i] = true; 239 store_samples_in_ringbuffer(); 240 241 stream_last_packet_received[i] = true; 242 stream_last_packet_sequence[i] = group_last_packet_sequence; 243 continue; 244 } 245 246 // missing packet if big sequence counter is higher than bis sequence counter 247 if (btstack_time16_delta(group_last_packet_sequence, stream_last_packet_sequence[i]) > 0) { 248 printf_plc("- ISO #%u, PLC for %u\n", i, group_last_packet_sequence); 249 plc_do(i); 250 btstack_assert((stream_last_packet_sequence[i] + 1) == group_last_packet_sequence); 251 stream_last_packet_sequence[i] = group_last_packet_sequence; 252 } 253 } 254 group_last_packet_sequence++; 255 } 256 } 257 258 static void plc_timeout(btstack_timer_source_t * timer) { 259 // Restart timer. This will loose sync with ISO interval, but if we stop caring if we loose that many packets 260 uint32_t frame_duration_ms = le_audio_demo_sink_frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 261 btstack_run_loop_set_timer(timer, frame_duration_ms); 262 btstack_run_loop_set_timer_handler(timer, plc_timeout); 263 btstack_run_loop_add_timer(timer); 264 265 // assume no packet received in iso interval 266 plc_check(group_last_packet_sequence + 1); 267 } 268 269 void le_audio_demo_util_sink_init(const char * filename_wav){ 270 le_audio_demo_sink_filename_wav = filename_wav; 271 } 272 273 void le_audio_demo_util_sink_enable_lc3plus(bool enable){ 274 le_audio_demo_lc3plus_decoder_requested = enable; 275 } 276 277 void le_audio_demo_util_sink_configure_general(uint8_t num_streams, uint8_t num_channels_per_stream, 278 uint32_t sampling_frequency_hz, 279 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 280 uint32_t iso_interval_1250us) { 281 le_audio_demo_sink_sampling_frequency_hz = sampling_frequency_hz; 282 le_audio_demo_sink_frame_duration = frame_duration; 283 le_audio_demo_sink_octets_per_frame = octets_per_frame; 284 le_audio_demo_sink_iso_interval_1250us = iso_interval_1250us; 285 le_audio_demo_sink_num_streams = num_streams; 286 le_audio_demo_sink_num_channels_per_stream = num_channels_per_stream; 287 288 le_audio_demo_sink_num_channels = le_audio_demo_sink_num_streams * le_audio_demo_sink_num_channels_per_stream; 289 btstack_assert((le_audio_demo_sink_num_channels == 1) || (le_audio_demo_sink_num_channels == 2)); 290 291 le_audio_demo_sink_lc3_frames = 0; 292 293 le_audio_demo_sink_num_samples_per_frame = btstack_lc3_samples_per_frame(le_audio_demo_sink_sampling_frequency_hz, le_audio_demo_sink_frame_duration); 294 295 // switch to lc3plus if requested and possible 296 bool use_lc3plus_decoder = le_audio_demo_lc3plus_decoder_requested && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US); 297 298 // init decoder 299 setup_lc3_decoder(); 300 301 printf("Configure: %u streams, %u channels per stream, sampling rate %u, samples per frame %u, lc3plus %u\n", 302 num_streams, num_channels_per_stream, sampling_frequency_hz, le_audio_demo_sink_num_samples_per_frame, use_lc3plus_decoder); 303 304 #ifdef HAVE_POSIX_FILE_IO 305 // create wav file 306 printf("WAV file: %s\n", le_audio_demo_sink_filename_wav); 307 wav_writer_open(le_audio_demo_sink_filename_wav, le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz); 308 #endif 309 310 // init playback buffer 311 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE); 312 313 // calc start threshold in bytes for PLAYBACK_START_MS 314 playback_start_threshold_bytes = (sampling_frequency_hz / 1000 * PLAYBACK_START_MS) * le_audio_demo_sink_num_channels * 2; 315 316 // start playback 317 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 318 if (sink != NULL){ 319 sink->init(le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz, le_audio_connection_sink_playback); 320 sink->start_stream(); 321 } 322 } 323 324 void le_audio_demo_util_sink_configure_unicast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz, 325 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 326 uint32_t iso_interval_1250us, uint8_t flush_timeout){ 327 le_audio_demo_sink_type = HCI_ISO_TYPE_CIS; 328 le_audio_demo_sink_flush_timeout = flush_timeout; 329 le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz, 330 frame_duration, octets_per_frame, iso_interval_1250us); 331 } 332 333 void le_audio_demo_util_sink_configure_broadcast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz, 334 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame, 335 uint32_t iso_interval_1250us, uint8_t pre_transmission_offset) { 336 le_audio_demo_sink_type = HCI_ISO_TYPE_BIS; 337 le_audio_demo_sink_pre_transmission_offset = pre_transmission_offset; 338 le_audio_demo_util_sink_configure_unicast(num_streams, num_channels_per_stream, sampling_frequency_hz, frame_duration, 339 octets_per_frame, iso_interval_1250us, pre_transmission_offset); 340 } 341 342 void le_audio_demo_util_sink_receive(uint8_t stream_index, uint8_t *packet, uint16_t size) { 343 uint16_t header = little_endian_read_16(packet, 0); 344 hci_con_handle_t con_handle = header & 0x0fff; 345 uint8_t pb_flag = (header >> 12) & 3; 346 uint8_t ts_flag = (header >> 14) & 1; 347 uint16_t iso_load_len = little_endian_read_16(packet, 2); 348 349 uint16_t offset = 4; 350 uint32_t time_stamp = 0; 351 if (ts_flag){ 352 uint32_t time_stamp = little_endian_read_32(packet, offset); 353 offset += 4; 354 } 355 356 uint32_t receive_time_ms = btstack_run_loop_get_time_ms(); 357 358 uint16_t packet_sequence_number = little_endian_read_16(packet, offset); 359 offset += 2; 360 361 uint16_t header_2 = little_endian_read_16(packet, offset); 362 uint16_t iso_sdu_length = header_2 & 0x3fff; 363 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14); 364 offset += 2; 365 366 if (iso_sdu_length == 0) return; 367 368 if (iso_sdu_length != le_audio_demo_sink_num_channels_per_stream * le_audio_demo_sink_octets_per_frame) { 369 printf("ISO Length %u != %u * %u\n", iso_sdu_length, le_audio_demo_sink_num_channels_per_stream, le_audio_demo_sink_octets_per_frame); 370 return; 371 } 372 373 // start with first packet on first stream 374 if (group_last_packet_received == false){ 375 if (stream_index != 0){ 376 printf("Ignore first packet for second stream\n"); 377 return; 378 } 379 group_last_packet_received = true; 380 group_last_packet_sequence = packet_sequence_number; 381 } 382 383 if (stream_last_packet_received[stream_index]) { 384 printf_plc("ISO #%u, receive %u\n", stream_index, packet_sequence_number); 385 386 int16_t packet_sequence_delta = btstack_time16_delta(packet_sequence_number, 387 stream_last_packet_sequence[stream_index]); 388 if (packet_sequence_delta < 1) { 389 // drop delayed packet that had already been generated by PLC 390 printf_plc("- dropping delayed packet. Current sequence number %u, last received or generated by PLC: %u\n", 391 packet_sequence_number, stream_last_packet_sequence[stream_index]); 392 return; 393 } 394 // simple check 395 if (packet_sequence_number != stream_last_packet_sequence[stream_index] + 1) { 396 printf_plc("- ISO #%u, missing %u\n", stream_index, stream_last_packet_sequence[stream_index] + 1); 397 } 398 } else { 399 printf_plc("ISO %u, first packet seq number %u\n", stream_index, packet_sequence_number); 400 stream_last_packet_received[stream_index] = true; 401 } 402 403 plc_check(packet_sequence_number); 404 405 uint8_t i; 406 for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++){ 407 // decode codec frame 408 uint8_t tmp_BEC_detect; 409 uint8_t BFI = 0; 410 uint8_t effective_channel = stream_index + i; 411 (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], &packet[offset], BFI, 412 &pcm[effective_channel], le_audio_demo_sink_num_channels, 413 &tmp_BEC_detect); 414 offset += le_audio_demo_sink_octets_per_frame; 415 have_pcm[stream_index + i] = true; 416 } 417 418 store_samples_in_ringbuffer(); 419 420 log_info("Samples in playback buffer %5u", btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2)); 421 422 le_audio_demo_sink_lc3_frames++; 423 424 // PLC 425 uint32_t frame_duration_ms = le_audio_demo_sink_frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? 8 : 10; 426 uint32_t timeout_ms = frame_duration_ms * 5 / 2; 427 btstack_run_loop_remove_timer(&next_packet_timer); 428 btstack_run_loop_set_timer(&next_packet_timer, timeout_ms); 429 btstack_run_loop_set_timer_handler(&next_packet_timer, plc_timeout); 430 btstack_run_loop_add_timer(&next_packet_timer); 431 432 if (samples_received >= le_audio_demo_sink_sampling_frequency_hz){ 433 printf("LC3 Frames: %4u - samples received %5u, played %5u, dropped %5u\n", le_audio_demo_sink_lc3_frames, samples_received, samples_played, samples_dropped); 434 samples_received = 0; 435 samples_dropped = 0; 436 samples_played = 0; 437 } 438 439 stream_last_packet_sequence[stream_index] = packet_sequence_number; 440 } 441 442 /** 443 * @brief Close sink: close wav file, stop playback 444 */ 445 void le_audio_demo_util_sink_close(void){ 446 #ifdef HAVE_POSIX_FILE_IO 447 printf("Close WAV file\n"); 448 wav_writer_close(); 449 #endif 450 // stop playback 451 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance(); 452 if (sink != NULL){ 453 sink->stop_stream(); 454 } 455 // stop timer 456 btstack_run_loop_remove_timer(&next_packet_timer); 457 }