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_source.c"
39
40 #include "le_audio_demo_util_source.h"
41
42 #include <stdio.h>
43 #include <inttypes.h>
44 #include <math.h>
45
46 #include "btstack_config.h"
47 #include "btstack_bool.h"
48 #include "btstack_debug.h"
49 #include "btstack_ring_buffer.h"
50
51 #include "hci.h"
52 #include "btstack_audio.h"
53 #include "btstack_lc3_google.h"
54 #include "btstack_lc3plus_fraunhofer.h"
55
56 #include "hxcmod.h"
57 #include "mods/mod.h"
58
59 #define MAX_CHANNELS 5
60 #define MAX_SAMPLES_PER_10MS_FRAME 480
61 #define MAX_LC3_FRAME_BYTES 155
62
63 // live recording
64 #define RECORDING_PREBUFFER__MS 20
65 #define RECORDING_BUFFER_MS 40
66
67 // SOURCE
68 static float sine_frequencies[MAX_CHANNELS] = {
69 261.63, // C-4
70 311.13, // Es-4
71 369.99, // G-4
72 493.88, // B-4
73 587.33, // D-5
74 };
75
76 // 48000 / 261.6 = 183.46
77 #define SINE_MAX_SAMPLES_AT_48KHZ 185
78 #define PI 3.14159265358979323846
79
80 static int16_t sine_table[MAX_CHANNELS * SINE_MAX_SAMPLES_AT_48KHZ];
81
82 static uint16_t le_audio_demo_source_octets_per_frame;
83 static uint16_t le_audio_demo_source_packet_sequence_numbers[MAX_CHANNELS];
84 static uint8_t le_audio_demo_source_iso_frame_counter;
85 static uint32_t le_audio_demo_source_sampling_frequency_hz;
86 static btstack_lc3_frame_duration_t le_audio_demo_source_frame_duration;
87 static uint8_t le_audio_demo_source_num_channels;
88 static uint8_t le_audio_demo_source_num_streams;
89 static uint8_t le_audio_demo_source_num_channels_per_stream;
90 static uint16_t le_audio_demo_source_num_samples_per_frame;
91
92
93 static uint8_t le_audio_demo_source_iso_payload[MAX_CHANNELS * MAX_LC3_FRAME_BYTES];
94
95 // lc3 encoder
96 static const btstack_lc3_encoder_t * le_audio_demo_source_lc3_encoder;
97 static btstack_lc3_encoder_google_t le_audio_demo_source_encoder_contexts[MAX_CHANNELS];
98 static int16_t le_audio_demo_source_pcm[MAX_CHANNELS * MAX_SAMPLES_PER_10MS_FRAME];
99
100 // sine generator
101 static uint16_t le_audio_demo_source_sine_samples[MAX_CHANNELS];
102 static uint16_t le_audio_demo_source_sine_phases[MAX_CHANNELS];
103
104 // mod player
105 static bool le_audio_demo_source_hxcmod_initialized;
106 static modcontext le_audio_demo_source_hxcmod_context;
107 static tracker_buffer_state le_audio_demo_source_hxcmod_trkbuf;
108
109 // recording / portaudio
110 #define SAMPLES_PER_CHANNEL_RECORDING (MAX_SAMPLES_PER_10MS_FRAME * RECORDING_BUFFER_MS / 10)
111 static uint8_t recording_storage[MAX_CHANNELS * 2 * SAMPLES_PER_CHANNEL_RECORDING];
112 static btstack_ring_buffer_t le_audio_demo_source_recording_buffer;
113 static uint16_t le_audio_demo_source_recording_stored_samples;
114 static uint16_t le_audio_demo_source_recording_prebuffer_samples;
115 static bool le_audio_demo_source_recording_streaming;
116 static btstack_audio_source_t const * le_audio_demo_audio_source;
117
118 // generation method
119 static le_audio_demo_source_generator le_audio_demo_util_source_generator = AUDIO_SOURCE_SINE;
120
121 // recording callback has channels interleaved, collect per channel
le_audio_util_source_recording_callback(const int16_t * buffer,uint16_t num_samples)122 static void le_audio_util_source_recording_callback(const int16_t * buffer, uint16_t num_samples){
123 log_info("store %u samples per channel", num_samples);
124 uint32_t bytes_to_store = le_audio_demo_source_num_channels * num_samples * 2;
125 if (bytes_to_store < btstack_ring_buffer_bytes_free(&le_audio_demo_source_recording_buffer)){
126 btstack_ring_buffer_write(&le_audio_demo_source_recording_buffer, (uint8_t *)buffer, bytes_to_store);
127 le_audio_demo_source_recording_stored_samples += num_samples;
128 }
129 }
130
le_audio_demo_util_source_init(void)131 void le_audio_demo_util_source_init(void) {
132 le_audio_demo_audio_source = btstack_audio_source_get_instance();
133 }
134
le_audio_demo_util_source_recording_start(void)135 static bool le_audio_demo_util_source_recording_start(void){
136 bool ok = false;
137 if (le_audio_demo_audio_source != NULL){
138 int init_ok = le_audio_demo_audio_source->init(le_audio_demo_source_num_channels, le_audio_demo_source_sampling_frequency_hz,
139 &le_audio_util_source_recording_callback);
140 log_info("recording initialized, ok %u", init_ok);
141 if (init_ok){
142 btstack_ring_buffer_init(&le_audio_demo_source_recording_buffer, recording_storage, sizeof(recording_storage));
143 le_audio_demo_source_recording_prebuffer_samples = le_audio_demo_source_num_channels * le_audio_demo_source_sampling_frequency_hz * RECORDING_PREBUFFER__MS / 1000;
144 le_audio_demo_source_recording_streaming = false;
145 le_audio_demo_source_recording_stored_samples = 0;
146 le_audio_demo_audio_source->start_stream();
147 log_info("recording start, %u prebuffer samples per channel", le_audio_demo_source_recording_prebuffer_samples / le_audio_demo_source_num_channels);
148 ok = true;
149 }
150 }
151 return ok;
152 }
153
le_audio_demo_util_source_recording_stop(void)154 static void le_audio_demo_util_source_recording_stop(void) {
155 le_audio_demo_audio_source->stop_stream();
156 }
157
le_audio_demo_source_setup_lc3_encoder(void)158 static void le_audio_demo_source_setup_lc3_encoder(void){
159 uint8_t channel;
160 for (channel = 0 ; channel < le_audio_demo_source_num_channels ; channel++){
161 btstack_lc3_encoder_google_t * context = &le_audio_demo_source_encoder_contexts[channel];
162 le_audio_demo_source_lc3_encoder = btstack_lc3_encoder_google_init_instance(context);
163 le_audio_demo_source_lc3_encoder->configure(context, le_audio_demo_source_sampling_frequency_hz, le_audio_demo_source_frame_duration, le_audio_demo_source_octets_per_frame);
164 }
165
166 printf("LC3 Encoder config: %" PRIu32 " hz, frame duration %s ms, num samples %u, num octets %u\n",
167 le_audio_demo_source_sampling_frequency_hz, le_audio_demo_source_frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? "7.5" : "10",
168 le_audio_demo_source_num_samples_per_frame, le_audio_demo_source_octets_per_frame);
169 }
170
le_audio_demo_source_setup_mod_player(void)171 static void le_audio_demo_source_setup_mod_player(void){
172 if (!le_audio_demo_source_hxcmod_initialized) {
173 le_audio_demo_source_hxcmod_initialized = hxcmod_init(&le_audio_demo_source_hxcmod_context);
174 btstack_assert(le_audio_demo_source_hxcmod_initialized != 0);
175 }
176 hxcmod_unload(&le_audio_demo_source_hxcmod_context);
177 hxcmod_setcfg(&le_audio_demo_source_hxcmod_context, le_audio_demo_source_sampling_frequency_hz, 1, 1);
178 hxcmod_load(&le_audio_demo_source_hxcmod_context, (void *) &mod_data, mod_len);
179 }
180
le_audio_demo_source_setup_sine_generator(void)181 static void le_audio_demo_source_setup_sine_generator(void){
182 // pre-compute sine for all channels
183 uint8_t channel;
184 for (channel = 0; channel < MAX_CHANNELS ; channel++){
185 float frequency_hz = (float) sine_frequencies[channel];
186 float samplerate_hz = (float) le_audio_demo_source_sampling_frequency_hz;
187 float period_samples =samplerate_hz / frequency_hz;
188 le_audio_demo_source_sine_samples[channel] = (uint16_t) period_samples;
189 le_audio_demo_source_sine_phases[channel] = 0;
190 uint16_t i;
191 for (i=0;i<le_audio_demo_source_sine_samples[channel] ;i++){
192 float sine_value = sin(2 * PI * i / period_samples);
193 sine_table[channel*SINE_MAX_SAMPLES_AT_48KHZ + i] = (int16_t)(sine_value * 32767);
194 }
195 }
196 }
197
le_audio_demo_util_source_configure(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)198 void le_audio_demo_util_source_configure(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz,
199 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame) {
200 le_audio_demo_source_sampling_frequency_hz = sampling_frequency_hz;
201 le_audio_demo_source_frame_duration = frame_duration;
202 le_audio_demo_source_octets_per_frame = octets_per_frame;
203 le_audio_demo_source_num_streams = num_streams;
204 le_audio_demo_source_num_channels_per_stream = num_channels_per_stream;
205
206 le_audio_demo_source_num_channels = num_streams * num_channels_per_stream;
207 btstack_assert((le_audio_demo_source_num_channels > 0) || (le_audio_demo_source_num_channels <= MAX_CHANNELS));
208
209 le_audio_demo_source_num_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration);
210 btstack_assert(le_audio_demo_source_num_samples_per_frame <= MAX_SAMPLES_PER_10MS_FRAME);
211
212 // setup encoder
213 le_audio_demo_source_setup_lc3_encoder();
214
215 // setup sine generator
216 le_audio_demo_source_setup_sine_generator();
217
218 // setup mod player
219 le_audio_demo_source_setup_mod_player();
220 }
221
le_audio_demo_util_source_generate_iso_frame(le_audio_demo_source_generator generator)222 void le_audio_demo_util_source_generate_iso_frame(le_audio_demo_source_generator generator) {
223 btstack_assert(le_audio_demo_source_octets_per_frame != 0);
224 uint16_t sample;
225 bool encode_pcm = true;
226
227 // lazy init of btstack_audio, fallback to sine
228 if ((generator == AUDIO_SOURCE_RECORDING) && (le_audio_demo_util_source_generator != AUDIO_SOURCE_RECORDING)){
229 bool ok = le_audio_demo_util_source_recording_start();
230 if (ok) {
231 le_audio_demo_util_source_generator = generator;
232 } else {
233 generator = AUDIO_SOURCE_SINE;
234 }
235 }
236
237 // stop recording
238 if ((generator != AUDIO_SOURCE_RECORDING) && (le_audio_demo_util_source_generator == AUDIO_SOURCE_RECORDING)) {
239 le_audio_demo_util_source_recording_stop();
240 }
241
242 switch (generator){
243 case AUDIO_SOURCE_COUNTER:
244 encode_pcm = false;
245 memset(le_audio_demo_source_iso_payload, le_audio_demo_source_iso_frame_counter++, sizeof(le_audio_demo_source_iso_payload));
246 break;
247 case AUDIO_SOURCE_SINE:
248 // use pre-computed sine for all channels
249 for (sample = 0 ; sample < le_audio_demo_source_num_samples_per_frame ; sample++){
250 uint8_t i;
251 for (i = 0; i < le_audio_demo_source_num_channels; i++) {
252 int16_t value = sine_table[i*SINE_MAX_SAMPLES_AT_48KHZ + le_audio_demo_source_sine_phases[i]];
253 le_audio_demo_source_pcm[sample * le_audio_demo_source_num_channels + i] = value;
254 le_audio_demo_source_sine_phases[i] += 1;
255 if (le_audio_demo_source_sine_phases[i] >= le_audio_demo_source_sine_samples[i]) {
256 le_audio_demo_source_sine_phases[i] = 0;
257 }
258 }
259 }
260 break;
261 case AUDIO_SOURCE_MODPLAYER:
262 // mod player configured for stereo
263 hxcmod_fillbuffer(&le_audio_demo_source_hxcmod_context, le_audio_demo_source_pcm, le_audio_demo_source_num_samples_per_frame, &le_audio_demo_source_hxcmod_trkbuf);
264 // stereo -> mono
265 if (le_audio_demo_source_num_channels == 1) {
266 uint16_t i;
267 for (i=0;i<le_audio_demo_source_num_samples_per_frame;i++){
268 le_audio_demo_source_pcm[i] = (le_audio_demo_source_pcm[2*i] / 2) + (le_audio_demo_source_pcm[2*i+1] / 2);
269 }
270 }
271 // duplicate stereo channels
272 if (le_audio_demo_source_num_channels > 2) {
273 int16_t i;
274 for (i = le_audio_demo_source_num_samples_per_frame - 1; i >= 0; i--) {
275 uint16_t channel_dst;
276 for (channel_dst=0; channel_dst < le_audio_demo_source_num_channels; channel_dst++){
277 uint16_t channel_src = channel_dst & 1;
278 le_audio_demo_source_pcm[i * le_audio_demo_source_num_channels + channel_dst] =
279 le_audio_demo_source_pcm[i * 2 + channel_src];
280 }
281 }
282 }
283 break;
284 case AUDIO_SOURCE_RECORDING:
285 if (le_audio_demo_source_recording_streaming == false){
286 if (le_audio_demo_source_recording_stored_samples >= le_audio_demo_source_recording_prebuffer_samples){
287 // start streaming audio
288 le_audio_demo_source_recording_streaming = true;
289 log_info("Streaming started");
290
291 }
292 }
293 if (le_audio_demo_source_recording_streaming){
294 uint32_t bytes_needed = le_audio_demo_source_num_channels * le_audio_demo_source_num_samples_per_frame * 2;
295 if (btstack_ring_buffer_bytes_available(&le_audio_demo_source_recording_buffer) >= bytes_needed){
296 uint32_t bytes_read;
297 btstack_ring_buffer_read(&le_audio_demo_source_recording_buffer, (uint8_t*) le_audio_demo_source_pcm, bytes_needed, &bytes_read);
298 btstack_assert(bytes_needed == bytes_needed);
299 log_info("Read %u samples per channel", le_audio_demo_source_num_samples_per_frame);
300 le_audio_demo_source_recording_stored_samples -= le_audio_demo_source_num_samples_per_frame;
301 } else {
302 le_audio_demo_source_recording_streaming = false;
303 log_info("Streaming underrun");
304 }
305 } else {
306 memset(le_audio_demo_source_pcm, 0, sizeof(le_audio_demo_source_pcm));
307 }
308 break;
309 default:
310 btstack_unreachable();
311 break;
312 }
313
314 if (encode_pcm){
315 uint8_t i;
316 for (i=0;i<le_audio_demo_source_num_channels;i++){
317 le_audio_demo_source_lc3_encoder->encode_signed_16(&le_audio_demo_source_encoder_contexts[i], &le_audio_demo_source_pcm[i], le_audio_demo_source_num_channels, &le_audio_demo_source_iso_payload[i * MAX_LC3_FRAME_BYTES]);
318 }
319 }
320
321 le_audio_demo_util_source_generator = generator;
322 }
323
le_audio_demo_util_source_send(uint8_t stream_index,hci_con_handle_t con_handle)324 void le_audio_demo_util_source_send(uint8_t stream_index, hci_con_handle_t con_handle){
325 btstack_assert(le_audio_demo_source_octets_per_frame != 0);
326
327 hci_reserve_packet_buffer();
328
329 uint8_t * buffer = hci_get_outgoing_packet_buffer();
330 // complete SDU, no TimeStamp
331 little_endian_store_16(buffer, 0, ((uint16_t) con_handle) | (2 << 12));
332 // len
333 little_endian_store_16(buffer, 2, 0 + 4 + le_audio_demo_source_num_channels_per_stream * le_audio_demo_source_octets_per_frame);
334 // TimeStamp if TS flag is set
335 // packet seq nr
336 little_endian_store_16(buffer, 4, le_audio_demo_source_packet_sequence_numbers[stream_index]);
337 // iso sdu len
338 little_endian_store_16(buffer, 6, le_audio_demo_source_num_channels_per_stream * le_audio_demo_source_octets_per_frame);
339 uint16_t offset = 8;
340 // copy encoded payload
341 uint8_t i;
342 for (i=0; i<le_audio_demo_source_num_channels_per_stream;i++) {
343 uint8_t effective_channel = (stream_index * le_audio_demo_source_num_channels_per_stream) + i;
344 memcpy(&buffer[offset], &le_audio_demo_source_iso_payload[effective_channel * MAX_LC3_FRAME_BYTES], le_audio_demo_source_octets_per_frame);
345 offset += le_audio_demo_source_octets_per_frame;
346 }
347 // send
348 hci_send_iso_packet_buffer(offset);
349
350 le_audio_demo_source_packet_sequence_numbers[stream_index]++;
351 }
352
le_audio_demo_util_source_close(void)353 void le_audio_demo_util_source_close(void){
354 if (le_audio_demo_audio_source != NULL){
355 le_audio_demo_audio_source->close();
356 }
357 }
358