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 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 131 void le_audio_demo_util_source_init(void) { 132 le_audio_demo_audio_source = btstack_audio_source_get_instance(); 133 } 134 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 154 static void le_audio_demo_util_source_recording_stop(void) { 155 le_audio_demo_audio_source->stop_stream(); 156 } 157 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 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, 16, 1, 1, 1); 178 hxcmod_load(&le_audio_demo_source_hxcmod_context, (void *) &mod_data, mod_len); 179 } 180 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 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 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, (unsigned short *) 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 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 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