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 <stdio.h>
41 #include <inttypes.h>
42
43 #include "le_audio_demo_util_sink.h"
44
45 #include "btstack_bool.h"
46 #include "btstack_config.h"
47 #include <btstack_debug.h>
48 #include <stdio.h>
49
50 #include "hci.h"
51 #include "btstack_audio.h"
52 #include "btstack_lc3_google.h"
53 #include "btstack_lc3plus_fraunhofer.h"
54
55 #include "btstack_sample_rate_compensation.h"
56 #include "btstack_resample.h"
57 #include "btstack_fsm.h"
58
59 #include "hxcmod.h"
60 #include "mods/mod.h"
61
62 #include "btstack_ring_buffer.h"
63 #ifdef HAVE_POSIX_FILE_IO
64 #include "wav_util.h"
65 #endif
66
67 #define MAX_CHANNELS 2
68 #define MAX_SAMPLES_PER_FRAME 480
69 #define MAX_LC3_FRAME_BYTES 155
70
71 // playback
72 #define MAX_NUM_LC3_FRAMES (15*2)
73 #define MAX_BYTES_PER_SAMPLE 4
74 #define PLAYBACK_BUFFER_SIZE (MAX_NUM_LC3_FRAMES * MAX_SAMPLES_PER_FRAME * MAX_CHANNELS * MAX_BYTES_PER_SAMPLE)
75 #define PLAYBACK_START_MS (MAX_NUM_LC3_FRAMES * 20 / 3)
76
77 // analysis
78 #define PACKET_PREFIX_LEN 10
79
80 #define ANSI_COLOR_RED "\x1b[31m"
81 #define ANSI_COLOR_GREEN "\x1b[32m"
82 #define ANSI_COLOR_YELLOW "\x1b[33m"
83 #define ANSI_COLOR_BLUE "\x1b[34m"
84 #define ANSI_COLOR_MAGENTA "\x1b[35m"
85 #define ANSI_COLOR_CYAN "\x1b[36m"
86 #define ANSI_COLOR_RESET "\x1b[0m"
87
88 // statistics
89 static uint16_t last_packet_sequence[MAX_CHANNELS];
90 static uint32_t last_packet_time_ms[MAX_CHANNELS];
91 static uint8_t last_packet_prefix[MAX_CHANNELS * PACKET_PREFIX_LEN];
92
93 // SINK
94
95 static enum {
96 LE_AUDIO_SINK_IDLE,
97 LE_AUDIO_SINK_INIT,
98 LE_AUDIO_SINK_CONFIGURED,
99 } le_audio_demo_util_sink_state = LE_AUDIO_SINK_IDLE;
100
101 static const char * le_audio_demo_sink_filename_wav;
102 static btstack_sample_rate_compensation_t sample_rate_compensation;
103 static uint32_t le_audio_demo_sink_received_samples;
104 static btstack_resample_t resample_instance;
105 static bool sink_receive_streaming;
106
107 static int16_t pcm_resample[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME * 2];
108
109
110 static btstack_lc3_frame_duration_t le_audio_demo_sink_frame_duration;
111 static hci_iso_type_t le_audio_demo_sink_type;
112
113 static uint32_t le_audio_demo_sink_sampling_frequency_hz;
114 static uint16_t le_audio_demo_sink_num_samples_per_frame;
115 static uint8_t le_audio_demo_sink_num_streams;
116 static uint8_t le_audio_demo_sink_num_channels_per_stream;
117 static uint8_t le_audio_demo_sink_num_channels;
118 static uint16_t le_audio_demo_sink_octets_per_frame;
119 static uint16_t le_audio_demo_sink_iso_interval_1250us;
120 static uint8_t le_audio_demo_sink_flush_timeout;
121 static uint8_t le_audio_demo_sink_pre_transmission_offset;
122
123 // playback
124 static uint16_t playback_start_threshold_bytes;
125 static bool playback_active;
126 static uint8_t playback_buffer_storage[PLAYBACK_BUFFER_SIZE];
127 static btstack_ring_buffer_t playback_buffer;
128
129 // PLC
130 static uint32_t le_audio_demo_sink_lc3_frames;
131 static uint32_t samples_received;
132 static uint32_t samples_played;
133 static uint32_t samples_dropped;
134
135 // Audio FSM
136 #define TRAN( target ) btstack_fsm_transit( &me->super, (btstack_fsm_state_handler_t)target )
137
138 typedef struct {
139 btstack_fsm_t super;
140 uint32_t receive_time_ms;
141 uint32_t last_receive_time_ms;
142 uint32_t zero_frames;
143 uint32_t have_pcm;
144 uint32_t received_samples;
145 } audio_processing_t;
146
147 typedef struct {
148 btstack_fsm_event_t super;
149 uint16_t sequence_number;
150 uint16_t size;
151 uint32_t receive_time_ms;
152 uint8_t *data;
153 uint8_t stream;
154 } data_event_t;
155
156 typedef struct {
157 btstack_fsm_event_t super;
158 uint32_t time_ms;
159 } time_event_t;
160
161 audio_processing_t audio_processing;
162
163 enum EventSignals {
164 DATA_SIG = BTSTACK_FSM_USER_SIG,
165 TIME_SIG
166 };
167
168 #define AUDIO_FSM_DEBUGx
169 #ifdef AUDIO_FSM_DEBUG
170 #define ENUM_TO_TEXT(sig) [sig] = #sig
171 #define audio_fsm_debug(format, ...) \
172 printf( format __VA_OPT__(,) __VA_ARGS__)
173
174 const char * const sigToString[] = {
175 ENUM_TO_TEXT(BTSTACK_FSM_INIT_SIG),
176 ENUM_TO_TEXT(BTSTACK_FSM_ENTRY_SIG),
177 ENUM_TO_TEXT(BTSTACK_FSM_EXIT_SIG),
178 ENUM_TO_TEXT(DATA_SIG),
179 ENUM_TO_TEXT(TIME_SIG),
180 };
181 #else
182 #define audio_fsm_debug(...)
183 #endif
184
185 static btstack_fsm_state_t audio_processing_initial( audio_processing_t * const me, btstack_fsm_event_t const * const e );
186 static btstack_fsm_state_t audio_processing_waiting( audio_processing_t * const me, btstack_fsm_event_t const * const e );
187 static btstack_fsm_state_t audio_processing_streaming( audio_processing_t * const me, btstack_fsm_event_t const * const e );
188 static bool audio_processing_is_streaming( audio_processing_t * const me );
189
190 static btstack_timer_source_t next_packet_timer;
191
192 // lc3 decoder
193 static bool le_audio_demo_lc3plus_decoder_requested = false;
194 static const btstack_lc3_decoder_t * lc3_decoder;
195 static int16_t pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME];
196
197 static btstack_lc3_decoder_google_t google_decoder_contexts[MAX_CHANNELS];
198 #ifdef HAVE_LC3PLUS
199 static btstack_lc3plus_fraunhofer_decoder_t fraunhofer_decoder_contexts[MAX_CHANNELS];
200 #endif
201 static void * decoder_contexts[MAX_CHANNELS];
202
le_audio_connection_sink_playback(int16_t * buffer,uint16_t num_samples)203 static void le_audio_connection_sink_playback(int16_t * buffer, uint16_t num_samples){
204 // called from lower-layer but guaranteed to be on main thread
205 log_info("Playback: need %u, have %" PRIu32 "", num_samples, btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2));
206
207 samples_played += num_samples;
208
209 uint32_t bytes_needed = num_samples * le_audio_demo_sink_num_channels * 2;
210 if (playback_active == false){
211 if (btstack_ring_buffer_bytes_available(&playback_buffer) >= playback_start_threshold_bytes) {
212 log_info("Playback started");
213 printf("Playback started\n");
214 playback_active = true;
215 }
216 } else {
217 if (bytes_needed > btstack_ring_buffer_bytes_available(&playback_buffer)) {
218 if( audio_processing_is_streaming( &audio_processing ) ) {
219 log_info("Playback underrun");
220 printf("Playback Underrun\n");
221 } else {
222 log_info("Playback stopped");
223 printf("Playback stopped\n");
224 }
225 // empty buffer
226 uint32_t bytes_read;
227 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read);
228 playback_active = false;
229 }
230 }
231
232 if (playback_active){
233 uint32_t bytes_read;
234 btstack_ring_buffer_read(&playback_buffer, (uint8_t *) buffer, bytes_needed, &bytes_read);
235 btstack_assert(bytes_read == bytes_needed);
236 } else {
237 memset(buffer, 0, bytes_needed);
238 }
239 }
240
le_audio_demo_util_sink_enable_lc3plus(bool enable)241 void le_audio_demo_util_sink_enable_lc3plus(bool enable){
242 le_audio_demo_lc3plus_decoder_requested = enable;
243 }
244
setup_lc3_decoder(bool use_lc3plus_decoder)245 static void setup_lc3_decoder(bool use_lc3plus_decoder){
246 UNUSED(use_lc3plus_decoder);
247
248 uint8_t channel;
249 for (channel = 0 ; channel < le_audio_demo_sink_num_channels ; channel++){
250 // pick decoder
251 void * decoder_context = NULL;
252 #ifdef HAVE_LC3PLUS
253 if (use_lc3plus_decoder){
254 decoder_context = &fraunhofer_decoder_contexts[channel];
255 lc3_decoder = btstack_lc3plus_fraunhofer_decoder_init_instance(decoder_context);
256 }
257 else
258 #endif
259 {
260 decoder_context = &google_decoder_contexts[channel];
261 lc3_decoder = btstack_lc3_decoder_google_init_instance(decoder_context);
262 }
263 decoder_contexts[channel] = decoder_context;
264 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);
265 }
266 btstack_assert(le_audio_demo_sink_num_samples_per_frame <= MAX_SAMPLES_PER_FRAME);
267 }
268
le_audio_demo_util_sink_configure_general(uint8_t num_streams,uint8_t num_channels_per_stream,uint32_t sampling_frequency_hz,btstack_lc3_frame_duration_t frame_duration,uint16_t octets_per_frame,uint32_t iso_interval_1250us)269 void le_audio_demo_util_sink_configure_general(uint8_t num_streams, uint8_t num_channels_per_stream,
270 uint32_t sampling_frequency_hz,
271 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame,
272 uint32_t iso_interval_1250us) {
273 le_audio_demo_sink_sampling_frequency_hz = sampling_frequency_hz;
274 le_audio_demo_sink_frame_duration = frame_duration;
275 le_audio_demo_sink_octets_per_frame = octets_per_frame;
276 le_audio_demo_sink_iso_interval_1250us = iso_interval_1250us;
277 le_audio_demo_sink_num_streams = num_streams;
278 le_audio_demo_sink_num_channels_per_stream = num_channels_per_stream;
279
280 sink_receive_streaming = false;
281 le_audio_demo_util_sink_state = LE_AUDIO_SINK_CONFIGURED;
282
283 le_audio_demo_sink_num_channels = le_audio_demo_sink_num_streams * le_audio_demo_sink_num_channels_per_stream;
284 btstack_assert((le_audio_demo_sink_num_channels == 1) || (le_audio_demo_sink_num_channels == 2));
285
286 le_audio_demo_sink_lc3_frames = 0;
287
288 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);
289
290 // switch to lc3plus if requested and possible
291 bool use_lc3plus_decoder = le_audio_demo_lc3plus_decoder_requested && (frame_duration == BTSTACK_LC3_FRAME_DURATION_10000US);
292
293 // init decoder
294 setup_lc3_decoder(use_lc3plus_decoder);
295
296 printf("Configure: %u streams, %u channels per stream, sampling rate %" PRIu32 ", samples per frame %u, lc3plus %u\n",
297 num_streams, num_channels_per_stream, sampling_frequency_hz, le_audio_demo_sink_num_samples_per_frame, use_lc3plus_decoder);
298
299 #ifdef HAVE_POSIX_FILE_IO
300 // create wav file
301 printf("WAV file: %s\n", le_audio_demo_sink_filename_wav);
302 wav_writer_open(le_audio_demo_sink_filename_wav, le_audio_demo_sink_num_channels, le_audio_demo_sink_sampling_frequency_hz);
303 #endif
304
305 // init playback buffer
306 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE);
307
308 // calc start threshold in bytes for PLAYBACK_START_MS
309 playback_start_threshold_bytes = (sampling_frequency_hz / 1000 * PLAYBACK_START_MS) * le_audio_demo_sink_num_channels * 2;
310
311 // sample rate compensation
312 le_audio_demo_sink_received_samples = 0;
313
314 // start playback
315 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance();
316 if (sink != NULL){
317 btstack_sample_rate_compensation_reset( &sample_rate_compensation, btstack_run_loop_get_time_ms() );
318 btstack_resample_init(&resample_instance, le_audio_demo_sink_num_channels);
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
le_audio_demo_util_sink_configure_unicast(uint8_t num_streams,uint8_t num_channels_per_stream,uint32_t sampling_frequency_hz,btstack_lc3_frame_duration_t frame_duration,uint16_t octets_per_frame,uint32_t iso_interval_1250us,uint8_t flush_timeout)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
330 // set playback start: FT * ISO Interval + max(10 ms, 1/2 ISO Interval)
331 uint16_t playback_start_ms = flush_timeout * (iso_interval_1250us * 5 / 4) + btstack_max(10, iso_interval_1250us * 5 / 8);
332 uint16_t playback_start_samples = sampling_frequency_hz / 1000 * playback_start_ms;
333 playback_start_threshold_bytes = playback_start_samples * num_streams * num_channels_per_stream * 2;
334 printf("Playback: start %u ms (%u samples, %u bytes)\n", playback_start_ms, playback_start_samples, playback_start_threshold_bytes);
335
336 le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz,
337 frame_duration, octets_per_frame, iso_interval_1250us);
338 }
339
le_audio_demo_util_sink_configure_broadcast(uint8_t num_streams,uint8_t num_channels_per_stream,uint32_t sampling_frequency_hz,btstack_lc3_frame_duration_t frame_duration,uint16_t octets_per_frame,uint32_t iso_interval_1250us,uint8_t pre_transmission_offset)340 void le_audio_demo_util_sink_configure_broadcast(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
341 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame,
342 uint32_t iso_interval_1250us, uint8_t pre_transmission_offset) {
343 le_audio_demo_sink_type = HCI_ISO_TYPE_BIS;
344 le_audio_demo_sink_pre_transmission_offset = pre_transmission_offset;
345
346 // set playback start: ISO Interval + 10 ms
347 uint16_t playback_start_ms = (iso_interval_1250us * 5 / 4) + 10;
348 uint16_t playback_start_samples = sampling_frequency_hz / 1000 * playback_start_ms;
349 playback_start_threshold_bytes = playback_start_samples * num_streams * num_channels_per_stream * 2;
350 printf("Playback: start %u ms (%u samples, %u bytes)\n", playback_start_ms, playback_start_samples, playback_start_threshold_bytes);
351
352 le_audio_demo_util_sink_configure_general(num_streams, num_channels_per_stream, sampling_frequency_hz, frame_duration, octets_per_frame, iso_interval_1250us);
353 }
354
le_audio_demo_util_sink_count(uint8_t stream_index,uint8_t * packet,uint16_t size)355 void le_audio_demo_util_sink_count(uint8_t stream_index, uint8_t *packet, uint16_t size) {
356 UNUSED(size);
357 // check for missing packet
358 uint16_t header = little_endian_read_16(packet, 0);
359 uint8_t ts_flag = (header >> 14) & 1;
360
361 uint16_t offset = 4;
362 uint32_t time_stamp = 0;
363 if (ts_flag){
364 time_stamp = little_endian_read_32(packet, offset);
365 offset += 4;
366 }
367
368 UNUSED(time_stamp);
369 uint32_t receive_time_ms = btstack_run_loop_get_time_ms();
370
371 uint16_t packet_sequence_number = little_endian_read_16(packet, offset);
372 offset += 4;
373
374 uint16_t last_seq_no = last_packet_sequence[stream_index];
375 bool packet_missed = (last_seq_no != 0) && ((last_seq_no + 1) != packet_sequence_number);
376 if (packet_missed){
377 // print last packet
378 printf("\n");
379 printf("%04x %10"PRIu32" %u ", last_seq_no, last_packet_time_ms[stream_index], stream_index);
380 printf_hexdump(&last_packet_prefix[le_audio_demo_sink_num_streams*PACKET_PREFIX_LEN], PACKET_PREFIX_LEN);
381 last_seq_no++;
382
383 printf(ANSI_COLOR_RED);
384 while (last_seq_no < packet_sequence_number){
385 printf("%04x %u MISSING\n", last_seq_no, stream_index);
386 last_seq_no++;
387 }
388 printf(ANSI_COLOR_RESET);
389
390 // print current packet
391 printf("%04x %10"PRIu32" %u ", packet_sequence_number, receive_time_ms, stream_index);
392 printf_hexdump(&packet[offset], PACKET_PREFIX_LEN);
393 }
394
395 // cache current packet
396 memcpy(&last_packet_prefix[le_audio_demo_sink_num_streams*PACKET_PREFIX_LEN], &packet[offset], PACKET_PREFIX_LEN);
397 }
398
audio_processing_initial(audio_processing_t * const me,btstack_fsm_event_t const * const e)399 static btstack_fsm_state_t audio_processing_initial( audio_processing_t * const me, btstack_fsm_event_t const * const e ) {
400 UNUSED(e);
401 audio_fsm_debug("%s\n", __FUNCTION__ );
402 return TRAN(audio_processing_waiting);
403 }
404
audio_processing_waiting(audio_processing_t * const me,btstack_fsm_event_t const * const e)405 static btstack_fsm_state_t audio_processing_waiting( audio_processing_t * const me, btstack_fsm_event_t const * const e ) {
406 audio_fsm_debug("%s - %s\n", __FUNCTION__, sigToString[e->sig]);
407 btstack_fsm_state_t status;
408 switch(e->sig) {
409 case BTSTACK_FSM_ENTRY_SIG: {
410 status = BTSTACK_FSM_HANDLED_STATUS;
411 break;
412 }
413 case BTSTACK_FSM_EXIT_SIG: {
414 btstack_ring_buffer_init(&playback_buffer, playback_buffer_storage, PLAYBACK_BUFFER_SIZE);
415
416 btstack_sample_rate_compensation_init(&sample_rate_compensation, me->last_receive_time_ms,
417 le_audio_demo_sink_sampling_frequency_hz, FLOAT_TO_Q15(1.f));
418 me->zero_frames = 0;
419 me->received_samples = 0;
420 btstack_resample_init( &resample_instance, le_audio_demo_sink_num_channels );
421 me->have_pcm = 0;
422 status = BTSTACK_FSM_HANDLED_STATUS;
423 break;
424 }
425 case DATA_SIG: {
426 data_event_t *data_event = (data_event_t*)e;
427 // nothing to do here
428 if( data_event->data == NULL ) {
429 status = BTSTACK_FSM_IGNORED_STATUS;
430 break;
431 }
432
433 // ignore empty data at start
434 if( data_event->size == 0 ) {
435 status = BTSTACK_FSM_IGNORED_STATUS;
436 break;
437 }
438
439 // always start at first stream
440 if( data_event->stream > 0 ) {
441 status = BTSTACK_FSM_IGNORED_STATUS;
442 break;
443 }
444
445 me->last_receive_time_ms = data_event->receive_time_ms;
446 status = TRAN(audio_processing_streaming);
447 break;
448 }
449 default: {
450 status = BTSTACK_FSM_IGNORED_STATUS;
451 break;
452 }
453 }
454 return status;
455 }
456
audio_processing_resample(audio_processing_t * const me,data_event_t * e)457 static void audio_processing_resample( audio_processing_t * const me, data_event_t *e ) {
458 // mark current packet as handled
459 e->data = NULL;
460 if( me->have_pcm != (uint32_t)((1<<(le_audio_demo_sink_num_streams*le_audio_demo_sink_num_channels_per_stream))-1) ) {
461 return;
462 }
463
464 int16_t *data_in = pcm;
465 int16_t *data_out = pcm_resample;
466 #ifdef HAVE_POSIX_FILE_IO
467 // write wav samples
468 wav_writer_write_int16(le_audio_demo_sink_num_channels * le_audio_demo_sink_num_samples_per_frame, data_in);
469 #endif
470
471 // count for samplerate compensation
472 me->received_samples += le_audio_demo_sink_num_samples_per_frame;
473
474 // store samples in playback buffer
475 samples_received += le_audio_demo_sink_num_samples_per_frame;
476 uint32_t resampled_frames = btstack_resample_block(&resample_instance, data_in, le_audio_demo_sink_num_samples_per_frame, data_out);
477 uint32_t bytes_to_store = resampled_frames * le_audio_demo_sink_num_channels * 2;
478
479 if (btstack_ring_buffer_bytes_free(&playback_buffer) >= bytes_to_store) {
480 btstack_ring_buffer_write(&playback_buffer, (uint8_t *)data_out, bytes_to_store);
481 log_info("Samples in playback buffer %5" PRIu32 "", btstack_ring_buffer_bytes_available(&playback_buffer) / (le_audio_demo_sink_num_channels * 2));
482 } else {
483 printf("Samples dropped\n");
484 samples_dropped += le_audio_demo_sink_num_samples_per_frame;
485 }
486 me->have_pcm = 0;
487 }
488
audio_processing_decode(audio_processing_t * const me,btstack_fsm_event_t const * const e)489 static btstack_fsm_state_t audio_processing_decode( audio_processing_t * const me, btstack_fsm_event_t const * const e ) {
490 audio_fsm_debug("%s - %s\n", __FUNCTION__, sigToString[e->sig]);
491 btstack_fsm_state_t status;
492 switch(e->sig) {
493 case BTSTACK_FSM_ENTRY_SIG: {
494 btstack_assert( (le_audio_demo_sink_num_streams*le_audio_demo_sink_num_channels_per_stream) < (sizeof(me->have_pcm)*8));
495 status = BTSTACK_FSM_HANDLED_STATUS;
496 break;
497 }
498 case BTSTACK_FSM_EXIT_SIG: {
499 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance();
500 if( sink == NULL ) {
501
502 status = BTSTACK_FSM_HANDLED_STATUS;
503 break;
504 }
505 uint32_t resampling_factor = btstack_sample_rate_compensation_update( &sample_rate_compensation, me->receive_time_ms,
506 me->received_samples, sink->get_samplerate() );
507 btstack_resample_set_factor(&resample_instance, resampling_factor);
508 me->received_samples = 0;
509
510 status = BTSTACK_FSM_HANDLED_STATUS;
511 break;
512 }
513 case DATA_SIG: {
514 data_event_t *data_event = (data_event_t*)e;
515 uint8_t *data_in = data_event->data;
516 int16_t *data_out = pcm;
517 uint16_t offset = 0;
518 uint8_t BFI = 0;
519 if (data_event->size != le_audio_demo_sink_num_channels_per_stream * le_audio_demo_sink_octets_per_frame) {
520 // incorrect size. we assume that we received this packet on time but cannot decode it, so we use PLC
521 BFI = 1;
522 printf("predict audio\n");
523 }
524 uint8_t i;
525 for (i = 0 ; i < le_audio_demo_sink_num_channels_per_stream ; i++){
526 uint8_t tmp_BEC_detect;
527 uint8_t effective_channel = (data_event->stream * le_audio_demo_sink_num_channels_per_stream) + i;
528 (void) lc3_decoder->decode_signed_16(decoder_contexts[effective_channel], &data_in[offset], BFI,
529 &data_out[effective_channel], le_audio_demo_sink_num_channels,
530 &tmp_BEC_detect);
531 offset += le_audio_demo_sink_octets_per_frame;
532 audio_fsm_debug("effective_channel: %d\n", effective_channel );
533 if( (me->have_pcm & (1<<effective_channel)) ) {
534 audio_fsm_debug("de-syncroniced, resync\n");
535 status = TRAN(audio_processing_waiting);
536 break;
537 }
538 me->have_pcm |= (1<<effective_channel);
539 }
540 audio_processing_resample( me, data_event );
541 status = TRAN(audio_processing_streaming);
542 break;
543 }
544 default: {
545 status = BTSTACK_FSM_IGNORED_STATUS;
546 break;
547 }
548 }
549 return status;
550 }
551
audio_processing_streaming(audio_processing_t * const me,btstack_fsm_event_t const * const e)552 static btstack_fsm_state_t audio_processing_streaming( audio_processing_t * const me, btstack_fsm_event_t const * const e ) {
553 audio_fsm_debug("%s - %s\n", __FUNCTION__, sigToString[e->sig]);
554
555 btstack_fsm_state_t status;
556 switch(e->sig) {
557 case BTSTACK_FSM_ENTRY_SIG: {
558 status = BTSTACK_FSM_HANDLED_STATUS;
559 break;
560 }
561 case BTSTACK_FSM_EXIT_SIG: {
562 me->last_receive_time_ms = me->receive_time_ms;
563 status = BTSTACK_FSM_HANDLED_STATUS;
564 break;
565 }
566 case TIME_SIG: {
567 time_event_t *time = (time_event_t*)e;
568 printf("time: %" PRId32 " - %" PRId32 " %" PRId32 "\n", time->time_ms, me->last_receive_time_ms, time->time_ms-me->last_receive_time_ms );
569 // we were last called ages ago, so just start waiting again
570 if( btstack_time_delta( time->time_ms, me->last_receive_time_ms ) > 100) {
571 status = TRAN(audio_processing_waiting);
572 break;
573 }
574 status = BTSTACK_FSM_HANDLED_STATUS;
575 break;
576 }
577 case DATA_SIG: {
578 data_event_t *data_event = (data_event_t*)e;
579 me->receive_time_ms = data_event->receive_time_ms;
580
581 // done processing this data
582 if( data_event->data == NULL ) {
583 status = BTSTACK_FSM_HANDLED_STATUS;
584 break;
585 }
586
587 if( btstack_time_delta( data_event->receive_time_ms, me->last_receive_time_ms ) > 100) {
588 status = TRAN(audio_processing_waiting);
589 break;
590 }
591
592 if( me->zero_frames > 10 ) {
593 status = TRAN(audio_processing_waiting);
594 break;
595 }
596
597 // track consecutive audio frames without data
598 if( data_event->size == 0 ) {
599 me->zero_frames++;
600 } else {
601 me->zero_frames = 0;
602 }
603
604 // will decode and/or predict missing data
605 status = TRAN(audio_processing_decode);
606 break;
607 }
608 default: {
609 status = BTSTACK_FSM_IGNORED_STATUS;
610 break;
611 }
612 }
613 return status;
614 }
615
audio_processing_constructor(audio_processing_t * me)616 static void audio_processing_constructor( audio_processing_t *me) {
617 btstack_fsm_constructor(&me->super, (btstack_fsm_state_handler_t)&audio_processing_initial);
618 btstack_fsm_init(&me->super, NULL);
619 }
620
audio_processing_task(audio_processing_t * me,btstack_fsm_event_t const * e)621 static void audio_processing_task( audio_processing_t *me, btstack_fsm_event_t const *e ) {
622 btstack_fsm_dispatch_until(&me->super, e);
623 }
624
audio_processing_is_streaming(audio_processing_t * me)625 static bool audio_processing_is_streaming( audio_processing_t *me ) {
626 btstack_fsm_t *fsm = &me->super;
627 time_event_t const time_event = { { TIME_SIG }, btstack_run_loop_get_time_ms() };
628 audio_processing_task( me, &time_event.super );
629 return fsm->state == (btstack_fsm_state_handler_t)&audio_processing_streaming;
630 }
631
le_audio_demo_util_sink_receive(uint8_t stream_index,uint8_t * packet,uint16_t size)632 void le_audio_demo_util_sink_receive(uint8_t stream_index, uint8_t *packet, uint16_t size) {
633 UNUSED(size);
634
635 if (le_audio_demo_util_sink_state != LE_AUDIO_SINK_CONFIGURED) return;
636
637 uint16_t header = little_endian_read_16(packet, 0);
638 hci_con_handle_t con_handle = header & 0x0fff;
639 uint8_t pb_flag = (header >> 12) & 3;
640 uint8_t ts_flag = (header >> 14) & 1;
641 uint16_t iso_load_len = little_endian_read_16(packet, 2);
642
643 uint16_t offset = 4;
644 uint32_t time_stamp = 0;
645 if (ts_flag){
646 time_stamp = little_endian_read_32(packet, offset);
647 offset += 4;
648 }
649
650 uint32_t receive_time_ms = btstack_run_loop_get_time_ms();
651
652 uint16_t packet_sequence_number = little_endian_read_16(packet, offset);
653 offset += 2;
654
655 uint16_t header_2 = little_endian_read_16(packet, offset);
656 uint16_t iso_sdu_length = header_2 & 0x3fff;
657 uint8_t packet_status_flag = (uint8_t) (header_2 >> 14);
658 offset += 2;
659
660 // avoid warning for (yet) unused fields
661 UNUSED(con_handle);
662 UNUSED(pb_flag);
663 UNUSED(iso_load_len);
664 UNUSED(packet_status_flag);
665 UNUSED(time_stamp);
666
667 data_event_t const data_event = {
668 .super.sig = DATA_SIG,
669 .sequence_number = packet_sequence_number,
670 .stream = stream_index,
671 .data = &packet[offset],
672 .size = iso_sdu_length,
673 .receive_time_ms = receive_time_ms,
674 };
675
676 audio_fsm_debug("new data\n stream_index: %d\n", stream_index);
677 audio_processing_task( &audio_processing, &data_event.super );
678
679 le_audio_demo_sink_lc3_frames++;
680
681 if (samples_received >= 10 * le_audio_demo_sink_sampling_frequency_hz){
682 printf("LC3 Frames: %4" PRIu32 " - samples received %5" PRIu32 ", played %5" PRIu32 ", dropped %5" PRIu32 "\n", le_audio_demo_sink_lc3_frames, samples_received, samples_played, samples_dropped);
683 samples_received = 0;
684 samples_dropped = 0;
685 samples_played = 0;
686 }
687 }
688
le_audio_demo_util_sink_init(const char * filename_wav)689 void le_audio_demo_util_sink_init(const char * filename_wav){
690 le_audio_demo_sink_filename_wav = filename_wav;
691 le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT;
692 audio_processing_constructor( &audio_processing );
693 }
694
695 /**
696 * @brief Close sink: close wav file, stop playback
697 */
le_audio_demo_util_sink_close(void)698 void le_audio_demo_util_sink_close(void){
699 #ifdef HAVE_POSIX_FILE_IO
700 printf("Close WAV file\n");
701 wav_writer_close();
702 #endif
703 // stop playback
704 const btstack_audio_sink_t * sink = btstack_audio_sink_get_instance();
705 if (sink != NULL){
706 sink->stop_stream();
707 }
708 le_audio_demo_util_sink_state = LE_AUDIO_SINK_INIT;
709 sink_receive_streaming = false;
710 // stop timer
711 btstack_run_loop_remove_timer(&next_packet_timer);
712 }
713