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 "btstack_bool.h" 43 #include "btstack_config.h" 44 #include <btstack_debug.h> 45 #include <stdio.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 // analysis 87 #define PACKET_PREFIX_LEN 10 88 89 // SOURCE 90 91 // input signal: pre-computed int16 sine wave, 96000 Hz at 300 Hz 92 static const int16_t sine_int16[] = { 93 0, 643, 1286, 1929, 2571, 3212, 3851, 4489, 5126, 5760, 94 6393, 7022, 7649, 8273, 8894, 9512, 10126, 10735, 11341, 11943, 95 12539, 13131, 13718, 14300, 14876, 15446, 16011, 16569, 17121, 17666, 96 18204, 18736, 19260, 19777, 20286, 20787, 21280, 21766, 22242, 22710, 97 23170, 23620, 24062, 24494, 24916, 25329, 25732, 26126, 26509, 26882, 98 27245, 27597, 27938, 28269, 28589, 28898, 29196, 29482, 29757, 30021, 99 30273, 30513, 30742, 30958, 31163, 31356, 31537, 31705, 31862, 32006, 100 32137, 32257, 32364, 32458, 32540, 32609, 32666, 32710, 32742, 32761, 101 32767, 32761, 32742, 32710, 32666, 32609, 32540, 32458, 32364, 32257, 102 32137, 32006, 31862, 31705, 31537, 31356, 31163, 30958, 30742, 30513, 103 30273, 30021, 29757, 29482, 29196, 28898, 28589, 28269, 27938, 27597, 104 27245, 26882, 26509, 26126, 25732, 25329, 24916, 24494, 24062, 23620, 105 23170, 22710, 22242, 21766, 21280, 20787, 20286, 19777, 19260, 18736, 106 18204, 17666, 17121, 16569, 16011, 15446, 14876, 14300, 13718, 13131, 107 12539, 11943, 11341, 10735, 10126, 9512, 8894, 8273, 7649, 7022, 108 6393, 5760, 5126, 4489, 3851, 3212, 2571, 1929, 1286, 643, 109 0, -643, -1286, -1929, -2571, -3212, -3851, -4489, -5126, -5760, 110 -6393, -7022, -7649, -8273, -8894, -9512, -10126, -10735, -11341, -11943, 111 -12539, -13131, -13718, -14300, -14876, -15446, -16011, -16569, -17121, -17666, 112 -18204, -18736, -19260, -19777, -20286, -20787, -21280, -21766, -22242, -22710, 113 -23170, -23620, -24062, -24494, -24916, -25329, -25732, -26126, -26509, -26882, 114 -27245, -27597, -27938, -28269, -28589, -28898, -29196, -29482, -29757, -30021, 115 -30273, -30513, -30742, -30958, -31163, -31356, -31537, -31705, -31862, -32006, 116 -32137, -32257, -32364, -32458, -32540, -32609, -32666, -32710, -32742, -32761, 117 -32767, -32761, -32742, -32710, -32666, -32609, -32540, -32458, -32364, -32257, 118 -32137, -32006, -31862, -31705, -31537, -31356, -31163, -30958, -30742, -30513, 119 -30273, -30021, -29757, -29482, -29196, -28898, -28589, -28269, -27938, -27597, 120 -27245, -26882, -26509, -26126, -25732, -25329, -24916, -24494, -24062, -23620, 121 -23170, -22710, -22242, -21766, -21280, -20787, -20286, -19777, -19260, -18736, 122 -18204, -17666, -17121, -16569, -16011, -15446, -14876, -14300, -13718, -13131, 123 -12539, -11943, -11341, -10735, -10126, -9512, -8894, -8273, -7649, -7022, 124 -6393, -5760, -5126, -4489, -3851, -3212, -2571, -1929, -1286, -643, 125 }; 126 127 static uint16_t le_audio_demo_source_octets_per_frame; 128 static uint16_t le_audio_demo_source_packet_sequence_numbers[MAX_CHANNELS]; 129 static uint8_t le_audio_demo_source_iso_frame_counter; 130 static uint32_t le_audio_demo_source_sampling_frequency_hz; 131 static btstack_lc3_frame_duration_t le_audio_demo_source_frame_duration; 132 static uint8_t le_audio_demo_source_num_channels; 133 static uint8_t le_audio_demo_source_num_streams; 134 static uint8_t le_audio_demo_source_num_channels_per_stream; 135 static uint16_t le_audio_demo_source_num_samples_per_frame; 136 137 138 static uint8_t le_audio_demo_source_iso_payload[MAX_CHANNELS * MAX_LC3_FRAME_BYTES]; 139 140 // lc3 encoder 141 static const btstack_lc3_encoder_t * le_audio_demo_source_lc3_encoder; 142 static btstack_lc3_encoder_google_t le_audio_demo_source_encoder_contexts[MAX_CHANNELS]; 143 static int16_t le_audio_demo_source_pcm[MAX_CHANNELS * MAX_SAMPLES_PER_FRAME]; 144 145 // sine generator 146 static uint8_t le_audio_demo_source_sine_step; 147 static uint16_t le_audio_demo_source_sine_phases[MAX_CHANNELS]; 148 149 // mod player 150 static bool le_audio_demo_source_hxcmod_initialized; 151 static modcontext le_audio_demo_source_hxcmod_context; 152 static tracker_buffer_state le_audio_demo_source_hxcmod_trkbuf; 153 154 void le_audio_demo_util_source_init(void){ 155 } 156 157 static void le_audio_demo_source_setup_lc3_encoder(void){ 158 uint8_t channel; 159 for (channel = 0 ; channel < le_audio_demo_source_num_channels ; channel++){ 160 btstack_lc3_encoder_google_t * context = &le_audio_demo_source_encoder_contexts[channel]; 161 le_audio_demo_source_lc3_encoder = btstack_lc3_encoder_google_init_instance(context); 162 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); 163 } 164 165 printf("LC3 Encoder config: %u hz, frame duration %s ms, num samples %u, num octets %u\n", 166 le_audio_demo_source_sampling_frequency_hz, le_audio_demo_source_frame_duration == BTSTACK_LC3_FRAME_DURATION_7500US ? "7.5" : "10", 167 le_audio_demo_source_num_samples_per_frame, le_audio_demo_source_octets_per_frame); 168 } 169 170 static void le_audio_demo_source_setup_mod_player(void){ 171 if (!le_audio_demo_source_hxcmod_initialized) { 172 le_audio_demo_source_hxcmod_initialized = hxcmod_init(&le_audio_demo_source_hxcmod_context); 173 btstack_assert(le_audio_demo_source_hxcmod_initialized != 0); 174 } 175 hxcmod_unload(&le_audio_demo_source_hxcmod_context); 176 hxcmod_setcfg(&le_audio_demo_source_hxcmod_context, le_audio_demo_source_sampling_frequency_hz, 16, 1, 1, 1); 177 hxcmod_load(&le_audio_demo_source_hxcmod_context, (void *) &mod_data, mod_len); 178 } 179 180 void le_audio_demo_util_source_configure(uint8_t num_streams, uint8_t num_channels_per_stream, uint32_t sampling_frequency_hz, 181 btstack_lc3_frame_duration_t frame_duration, uint16_t octets_per_frame) { 182 le_audio_demo_source_sampling_frequency_hz = sampling_frequency_hz; 183 le_audio_demo_source_frame_duration = frame_duration; 184 le_audio_demo_source_octets_per_frame = octets_per_frame; 185 le_audio_demo_source_num_streams = num_streams; 186 le_audio_demo_source_num_channels_per_stream = num_channels_per_stream; 187 188 le_audio_demo_source_num_channels = num_streams * num_channels_per_stream; 189 btstack_assert((le_audio_demo_source_num_channels == 1) || (le_audio_demo_source_num_channels == 2)); 190 191 le_audio_demo_source_num_samples_per_frame = btstack_lc3_samples_per_frame(sampling_frequency_hz, frame_duration); 192 btstack_assert(le_audio_demo_source_num_samples_per_frame <= MAX_SAMPLES_PER_FRAME); 193 194 // setup encoder 195 le_audio_demo_source_setup_lc3_encoder(); 196 197 // setup sine generator 198 if (sampling_frequency_hz == 44100){ 199 le_audio_demo_source_sine_step = 2; 200 } else { 201 le_audio_demo_source_sine_step = 96000 / sampling_frequency_hz; 202 } 203 204 // setup mod player 205 le_audio_demo_source_setup_mod_player(); 206 }; 207 208 void le_audio_demo_util_source_generate_iso_frame(le_audio_demo_source_generator generator) { 209 btstack_assert(le_audio_demo_source_octets_per_frame != 0); 210 uint16_t sample; 211 bool encode_pcm = true; 212 switch (generator){ 213 case AUDIO_SOURCE_COUNTER: 214 encode_pcm = false; 215 memset(le_audio_demo_source_iso_payload, le_audio_demo_source_iso_frame_counter++, sizeof(le_audio_demo_source_iso_payload)); 216 break; 217 case AUDIO_SOURCE_SINE: 218 // generate sine wave for all channels 219 for (sample = 0 ; sample < le_audio_demo_source_num_samples_per_frame ; sample++){ 220 uint8_t i; 221 for (i = 0; i < le_audio_demo_source_num_channels; i++) { 222 int16_t value = sine_int16[le_audio_demo_source_sine_phases[i]] / 4; 223 le_audio_demo_source_pcm[sample * le_audio_demo_source_num_channels + i] = value; 224 le_audio_demo_source_sine_phases[i] += le_audio_demo_source_sine_step * (1 + i); // second channel, double frequency 225 if (le_audio_demo_source_sine_phases[i] >= (sizeof(sine_int16) / sizeof(int16_t))) { 226 le_audio_demo_source_sine_phases[i] = 0; 227 } 228 } 229 } 230 break; 231 case AUDIO_SOURCE_MODPLAYER: 232 // mod player configured for stereo 233 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); 234 if (le_audio_demo_source_num_channels == 1) { 235 // stereo -> mono 236 uint16_t i; 237 for (i=0;i<le_audio_demo_source_num_samples_per_frame;i++){ 238 le_audio_demo_source_pcm[i] = (le_audio_demo_source_pcm[2*i] / 2) + (le_audio_demo_source_pcm[2*i+1] / 2); 239 } 240 } 241 break; 242 default: 243 btstack_unreachable(); 244 break; 245 } 246 247 if (encode_pcm){ 248 uint8_t i; 249 for (i=0;i<le_audio_demo_source_num_channels;i++){ 250 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]); 251 } 252 } 253 }; 254 255 void le_audio_demo_util_source_send(uint8_t stream_index, hci_con_handle_t con_handle){ 256 btstack_assert(le_audio_demo_source_octets_per_frame != 0); 257 258 hci_reserve_packet_buffer(); 259 260 uint8_t * buffer = hci_get_outgoing_packet_buffer(); 261 // complete SDU, no TimeStamp 262 little_endian_store_16(buffer, 0, ((uint16_t) con_handle) | (2 << 12)); 263 // len 264 little_endian_store_16(buffer, 2, 0 + 4 + le_audio_demo_source_num_channels_per_stream * le_audio_demo_source_octets_per_frame); 265 // TimeStamp if TS flag is set 266 // packet seq nr 267 little_endian_store_16(buffer, 4, le_audio_demo_source_packet_sequence_numbers[stream_index]); 268 // iso sdu len 269 little_endian_store_16(buffer, 6, le_audio_demo_source_num_channels_per_stream * le_audio_demo_source_octets_per_frame); 270 uint16_t offset = 8; 271 // copy encoded payload 272 uint8_t i; 273 for (i=0; i<le_audio_demo_source_num_channels_per_stream;i++) { 274 uint8_t effective_channel = (stream_index * le_audio_demo_source_num_channels_per_stream) + i; 275 memcpy(&buffer[offset], &le_audio_demo_source_iso_payload[effective_channel * MAX_LC3_FRAME_BYTES], le_audio_demo_source_octets_per_frame); 276 offset += le_audio_demo_source_octets_per_frame; 277 } 278 // send 279 hci_send_iso_packet_buffer(offset); 280 281 le_audio_demo_source_packet_sequence_numbers[stream_index]++; 282 } 283 284 void le_audio_demo_util_source_close(void){ 285 } 286