1 /*
2 * Copyright (C) 2016 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__ "sco_demo_util.c"
39
40 /*
41 * sco_demo_util.c - send/receive test data via SCO, used by hfp_*_demo and hsp_*_demo
42 */
43
44 #include <stdio.h>
45
46 #include "sco_demo_util.h"
47
48 #include "btstack_audio.h"
49 #include "btstack_debug.h"
50 #include "btstack_ring_buffer.h"
51 #include "classic/btstack_cvsd_plc.h"
52 #include "classic/btstack_sbc.h"
53 #include "classic/btstack_sbc_bluedroid.h"
54 #include "classic/hfp.h"
55 #include "classic/hfp_codec.h"
56
57 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
58 #include "btstack_lc3.h"
59 #include "btstack_lc3_google.h"
60 #endif
61
62
63 #ifdef _MSC_VER
64 // ignore deprecated warning for fopen
65 #pragma warning(disable : 4996)
66 #endif
67
68 #ifdef HAVE_POSIX_FILE_IO
69 #include "wav_util.h"
70 #endif
71
72 // test modes
73 #define SCO_DEMO_MODE_SINE 0
74 #define SCO_DEMO_MODE_MICROPHONE 1
75 #define SCO_DEMO_MODE_MODPLAYER 2
76
77 // SCO demo configuration
78 #define SCO_DEMO_MODE SCO_DEMO_MODE_MICROPHONE
79
80 // number of sco packets until 'report' on console
81 #define SCO_REPORT_PERIOD 100
82
83
84 #ifdef HAVE_POSIX_FILE_IO
85 // length and name of wav file on disk
86 #define SCO_WAV_DURATION_IN_SECONDS 15
87 #define SCO_WAV_FILENAME "sco_input.wav"
88 #endif
89
90 // constants
91 #define NUM_CHANNELS 1
92 #define SAMPLE_RATE_8KHZ 8000
93 #define SAMPLE_RATE_16KHZ 16000
94 #define SAMPLE_RATE_32KHZ 32000
95 #define BYTES_PER_FRAME 2
96
97 // audio pre-buffer - also defines latency
98 #define SCO_PREBUFFER_MS 50
99 #define PREBUFFER_BYTES_8KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_8KHZ/1000 * BYTES_PER_FRAME)
100 #define PREBUFFER_BYTES_16KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_16KHZ/1000 * BYTES_PER_FRAME)
101 #define PREBUFFER_BYTES_32KHZ (SCO_PREBUFFER_MS * SAMPLE_RATE_32KHZ/1000 * BYTES_PER_FRAME)
102
103 #if defined(ENABLE_HFP_SUPER_WIDE_BAND_SPEECH)
104 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_32KHZ
105 #define SAMPLES_PER_FRAME_MAX 240
106 #elif defined(ENABLE_HFP_WIDE_BAND_SPEECH)
107 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_16KHZ
108 #define SAMPLES_PER_FRAME_MAX 120
109 #else
110 #define PREBUFFER_BYTES_MAX PREBUFFER_BYTES_8KHZ
111 #define SAMPLES_PER_FRAME_MAX 60
112 #endif
113
114 static uint16_t audio_prebuffer_bytes;
115
116 // output
117 static int audio_output_paused = 0;
118 static uint8_t audio_output_ring_buffer_storage[2 * PREBUFFER_BYTES_MAX];
119 static btstack_ring_buffer_t audio_output_ring_buffer;
120
121 // input
122 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE
123 #define USE_AUDIO_INPUT
124 #else
125 #define USE_ADUIO_GENERATOR
126 static void (*sco_demo_audio_generator)(uint16_t num_samples, int16_t * data);
127 #endif
128
129 static int audio_input_paused = 0;
130 static uint8_t audio_input_ring_buffer_storage[2 * PREBUFFER_BYTES_MAX];
131 static btstack_ring_buffer_t audio_input_ring_buffer;
132
133 // mod player
134 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER
135 #include "hxcmod.h"
136 #include "mods/mod.h"
137 static modcontext mod_context;
138 #endif
139
140 static int count_sent = 0;
141 static int count_received = 0;
142
143 static btstack_cvsd_plc_state_t cvsd_plc_state;
144
145 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
146 static const btstack_sbc_decoder_t * sbc_decoder_instance;
147 static btstack_sbc_decoder_bluedroid_t sbc_decoder_context;
148 static const btstack_sbc_encoder_t * sbc_encoder_instance;
149 static btstack_sbc_encoder_bluedroid_t sbc_encoder_context;
150 #endif
151
152 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
153 static const btstack_lc3_decoder_t * lc3_decoder;
154 static btstack_lc3_decoder_google_t lc3_decoder_context;
155 static btstack_lc3_encoder_google_t lc3_encoder_context;
156 static hfp_h2_sync_t hfp_h2_sync;
157 #endif
158
159 int num_samples_to_write;
160 int num_audio_frames;
161
162 // generic codec support
163 typedef struct {
164 void (*init)(void);
165 void(*receive)(const uint8_t * packet, uint16_t size);
166 void (*fill_payload)(uint8_t * payload_buffer, uint16_t sco_payload_length);
167 void (*close)(void);
168 //
169 uint16_t sample_rate;
170 } codec_support_t;
171
172 // current configuration
173 static const codec_support_t * codec_current = NULL;
174
175 // hfp_codec
176 #if defined(ENABLE_HFP_WIDE_BAND_SPEECH) || defined(ENABLE_HFP_SUPER_WIDE_BAND_SPEECH)
177 static hfp_codec_t hfp_codec;
178 #endif
179
180 // Sine Wave
181
182 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
183 static uint16_t sine_wave_phase;
184 static uint16_t sine_wave_steps_per_sample;
185 #define SINE_WAVE_SAMPLE_RATE SAMPLE_RATE_32KHZ
186
187 // input signal: pre-computed int16 sine wave, 32000 Hz at 266 Hz
188 static const int16_t sine_int16[] = {
189 0, 1715, 3425, 5126, 6813, 8481, 10126, 11743, 13328, 14876,
190 16383, 17846, 19260, 20621, 21925, 23170, 24351, 25465, 26509, 27481,
191 28377, 29196, 29934, 30591, 31163, 31650, 32051, 32364, 32587, 32722,
192 32767, 32722, 32587, 32364, 32051, 31650, 31163, 30591, 29934, 29196,
193 28377, 27481, 26509, 25465, 24351, 23170, 21925, 20621, 19260, 17846,
194 16383, 14876, 13328, 11743, 10126, 8481, 6813, 5126, 3425, 1715,
195 0, -1715, -3425, -5126, -6813, -8481, -10126, -11743, -13328, -14876,
196 -16384, -17846, -19260, -20621, -21925, -23170, -24351, -25465, -26509, -27481,
197 -28377, -29196, -29934, -30591, -31163, -31650, -32051, -32364, -32587, -32722,
198 -32767, -32722, -32587, -32364, -32051, -31650, -31163, -30591, -29934, -29196,
199 -28377, -27481, -26509, -25465, -24351, -23170, -21925, -20621, -19260, -17846,
200 -16384, -14876, -13328, -11743, -10126, -8481, -6813, -5126, -3425, -1715,
201 };
202
sco_demo_sine_wave_host_endian(uint16_t num_samples,int16_t * data)203 static void sco_demo_sine_wave_host_endian(uint16_t num_samples, int16_t * data){
204 unsigned int i;
205 for (i=0; i < num_samples; i++){
206 data[i] = sine_int16[sine_wave_phase];
207 sine_wave_phase += sine_wave_steps_per_sample;
208 if (sine_wave_phase >= (sizeof(sine_int16) / sizeof(int16_t))){
209 sine_wave_phase = 0;
210 }
211 }
212 }
213 #endif
214
215 // Mod Player
216 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER
217 #define NUM_SAMPLES_GENERATOR_BUFFER 30
sco_demo_modplayer(uint16_t num_samples,int16_t * data)218 static void sco_demo_modplayer(uint16_t num_samples, int16_t * data){
219 // mix down stereo
220 signed short samples[NUM_SAMPLES_GENERATOR_BUFFER * 2];
221 while (num_samples > 0){
222 uint16_t next_samples = btstack_min(num_samples, NUM_SAMPLES_GENERATOR_BUFFER);
223 hxcmod_fillbuffer(&mod_context, (unsigned short *) samples, next_samples, NULL);
224 num_samples -= next_samples;
225 uint16_t i;
226 for (i=0;i<next_samples;i++){
227 int32_t left = samples[2*i + 0];
228 int32_t right = samples[2*i + 1];
229 data[i] = (int16_t)((left + right) / 2);
230 }
231 }
232 }
233 #endif
234
235 // Audio Playback / Recording
236
audio_playback_callback(int16_t * buffer,uint16_t num_samples)237 static void audio_playback_callback(int16_t * buffer, uint16_t num_samples){
238
239 // fill with silence while paused
240 if (audio_output_paused){
241 if (btstack_ring_buffer_bytes_available(&audio_output_ring_buffer) < audio_prebuffer_bytes){
242 memset(buffer, 0, num_samples * BYTES_PER_FRAME);
243 return;
244 } else {
245 // resume playback
246 audio_output_paused = 0;
247 }
248 }
249
250 // get data from ringbuffer
251 uint32_t bytes_read = 0;
252 btstack_ring_buffer_read(&audio_output_ring_buffer, (uint8_t *) buffer, num_samples * BYTES_PER_FRAME, &bytes_read);
253 num_samples -= bytes_read / BYTES_PER_FRAME;
254 buffer += bytes_read / BYTES_PER_FRAME;
255
256 // fill with 0 if not enough
257 if (num_samples){
258 memset(buffer, 0, num_samples * BYTES_PER_FRAME);
259 audio_output_paused = 1;
260 }
261 }
262
263 #ifdef USE_AUDIO_INPUT
audio_recording_callback(const int16_t * buffer,uint16_t num_samples)264 static void audio_recording_callback(const int16_t * buffer, uint16_t num_samples){
265 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)buffer, num_samples * 2);
266 }
267 #endif
268
269 // return 1 if ok
audio_initialize(int sample_rate)270 static int audio_initialize(int sample_rate){
271
272 // -- output -- //
273
274 // init buffers
275 memset(audio_output_ring_buffer_storage, 0, sizeof(audio_output_ring_buffer_storage));
276 btstack_ring_buffer_init(&audio_output_ring_buffer, audio_output_ring_buffer_storage, sizeof(audio_output_ring_buffer_storage));
277
278 // config and setup audio playback
279 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance();
280 if (audio_sink != NULL){
281 audio_sink->init(1, sample_rate, &audio_playback_callback);
282 audio_sink->start_stream();
283
284 audio_output_paused = 1;
285 }
286
287 // -- input -- //
288
289 // init buffers
290 memset(audio_input_ring_buffer_storage, 0, sizeof(audio_input_ring_buffer_storage));
291 btstack_ring_buffer_init(&audio_input_ring_buffer, audio_input_ring_buffer_storage, sizeof(audio_input_ring_buffer_storage));
292 audio_input_paused = 1;
293
294 #ifdef USE_AUDIO_INPUT
295 // config and setup audio recording
296 const btstack_audio_source_t * audio_source = btstack_audio_source_get_instance();
297 if (audio_source != NULL){
298 audio_source->init(1, sample_rate, &audio_recording_callback);
299 audio_source->start_stream();
300 }
301 #endif
302
303 return 1;
304 }
305
audio_terminate(void)306 static void audio_terminate(void){
307 const btstack_audio_sink_t * audio_sink = btstack_audio_sink_get_instance();
308 if (!audio_sink) return;
309 audio_sink->close();
310
311 #ifdef USE_AUDIO_INPUT
312 const btstack_audio_source_t * audio_source= btstack_audio_source_get_instance();
313 if (!audio_source) return;
314 audio_source->close();
315 #endif
316 }
317
318
319 // CVSD - 8 kHz
320
sco_demo_cvsd_init(void)321 static void sco_demo_cvsd_init(void){
322 printf("SCO Demo: Init CVSD\n");
323 btstack_cvsd_plc_init(&cvsd_plc_state);
324 }
325
sco_demo_cvsd_receive(const uint8_t * packet,uint16_t size)326 static void sco_demo_cvsd_receive(const uint8_t * packet, uint16_t size){
327
328 int16_t audio_frame_out[128]; //
329
330 if (size > sizeof(audio_frame_out)){
331 printf("sco_demo_cvsd_receive: SCO packet larger than local output buffer - dropping data.\n");
332 return;
333 }
334
335 const int audio_bytes_read = size - 3;
336 const int num_samples = audio_bytes_read / BYTES_PER_FRAME;
337
338 // convert into host endian
339 int16_t audio_frame_in[128];
340 int i;
341 for (i=0;i<num_samples;i++){
342 audio_frame_in[i] = little_endian_read_16(packet, 3 + i * 2);
343 }
344
345 // treat packet as bad frame if controller does not report 'all good'
346 bool bad_frame = (packet[1] & 0x30) != 0;
347
348 btstack_cvsd_plc_process_data(&cvsd_plc_state, bad_frame, audio_frame_in, num_samples, audio_frame_out);
349
350 #ifdef SCO_WAV_FILENAME
351 // Samples in CVSD SCO packet are in little endian, ready for wav files (take shortcut)
352 const int samples_to_write = btstack_min(num_samples, num_samples_to_write);
353 wav_writer_write_le_int16(samples_to_write, audio_frame_out);
354 num_samples_to_write -= samples_to_write;
355 if (num_samples_to_write == 0){
356 wav_writer_close();
357 }
358 #endif
359
360 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)audio_frame_out, audio_bytes_read);
361 }
362
sco_demo_cvsd_fill_payload(uint8_t * payload_buffer,uint16_t sco_payload_length)363 static void sco_demo_cvsd_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){
364 uint16_t bytes_to_copy = sco_payload_length;
365
366 // get data from ringbuffer
367 uint16_t pos = 0;
368 if (!audio_input_paused){
369 uint16_t samples_to_copy = sco_payload_length / 2;
370 uint32_t bytes_read = 0;
371 btstack_ring_buffer_read(&audio_input_ring_buffer, payload_buffer, bytes_to_copy, &bytes_read);
372 // flip 16 on big endian systems
373 // @note We don't use (uint16_t *) casts since all sample addresses are odd which causes crahses on some systems
374 if (btstack_is_big_endian()){
375 uint16_t i;
376 for (i=0;i<samples_to_copy/2;i+=2){
377 uint8_t tmp = payload_buffer[i*2];
378 payload_buffer[i*2] = payload_buffer[i*2+1];
379 payload_buffer[i*2+1] = tmp;
380 }
381 }
382 bytes_to_copy -= bytes_read;
383 pos += bytes_read;
384 }
385
386 // fill with 0 if not enough
387 if (bytes_to_copy){
388 memset(payload_buffer + pos, 0, bytes_to_copy);
389 audio_input_paused = 1;
390 }
391 }
392
sco_demo_cvsd_close(void)393 static void sco_demo_cvsd_close(void){
394 printf("Used CVSD with PLC, number of proccesed frames: \n - %d good frames, \n - %d bad frames.\n", cvsd_plc_state.good_frames_nr, cvsd_plc_state.bad_frames_nr);
395 }
396
397 static const codec_support_t codec_cvsd = {
398 .init = &sco_demo_cvsd_init,
399 .receive = &sco_demo_cvsd_receive,
400 .fill_payload = &sco_demo_cvsd_fill_payload,
401 .close = &sco_demo_cvsd_close,
402 .sample_rate = SAMPLE_RATE_8KHZ
403 };
404
405 // encode using hfp_codec
406 #if defined(ENABLE_HFP_WIDE_BAND_SPEECH) || defined(ENABLE_HFP_SUPER_WIDE_BAND_SPEECH)
sco_demo_codec_fill_payload(uint8_t * payload_buffer,uint16_t sco_payload_length)407 static void sco_demo_codec_fill_payload(uint8_t * payload_buffer, uint16_t sco_payload_length){
408 if (!audio_input_paused){
409 int num_samples = hfp_codec_num_audio_samples_per_frame(&hfp_codec);
410 btstack_assert(num_samples <= SAMPLES_PER_FRAME_MAX);
411 uint16_t samples_available = btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) / BYTES_PER_FRAME;
412 if (hfp_codec_can_encode_audio_frame_now(&hfp_codec) && samples_available >= num_samples){
413 int16_t sample_buffer[SAMPLES_PER_FRAME_MAX];
414 uint32_t bytes_read;
415 btstack_ring_buffer_read(&audio_input_ring_buffer, (uint8_t*) sample_buffer, num_samples * BYTES_PER_FRAME, &bytes_read);
416 hfp_codec_encode_audio_frame(&hfp_codec, sample_buffer);
417 num_audio_frames++;
418 }
419 }
420 // get data from encoder, fill with 0 if not enough
421 if (audio_input_paused || hfp_codec_num_bytes_available(&hfp_codec) < sco_payload_length){
422 // just send '0's
423 memset(payload_buffer, 0, sco_payload_length);
424 audio_input_paused = 1;
425 } else {
426 hfp_codec_read_from_stream(&hfp_codec, payload_buffer, sco_payload_length);
427 }
428 }
429 #endif
430
431 // mSBC - 16 kHz
432
433 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
434
handle_pcm_data(int16_t * data,int num_samples,int num_channels,int sample_rate,void * context)435 static void handle_pcm_data(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context){
436 UNUSED(context);
437 UNUSED(sample_rate);
438 UNUSED(data);
439 UNUSED(num_samples);
440 UNUSED(num_channels);
441
442 // samples in callback in host endianess, ready for playback
443 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)data, num_samples*num_channels*2);
444
445 #ifdef SCO_WAV_FILENAME
446 if (!num_samples_to_write) return;
447 num_samples = btstack_min(num_samples, num_samples_to_write);
448 num_samples_to_write -= num_samples;
449 wav_writer_write_int16(num_samples, data);
450 if (num_samples_to_write == 0){
451 wav_writer_close();
452 }
453 #endif /* SCO_WAV_FILENAME */
454 }
455
sco_demo_msbc_init(void)456 static void sco_demo_msbc_init(void){
457 printf("SCO Demo: Init mSBC\n");
458 sbc_decoder_instance = btstack_sbc_decoder_bluedroid_init_instance(&sbc_decoder_context);
459 sbc_decoder_instance->configure(&sbc_decoder_context, SBC_MODE_mSBC, &handle_pcm_data, NULL);
460 sbc_encoder_instance = btstack_sbc_encoder_bluedroid_init_instance(&sbc_encoder_context);
461 hfp_codec_init_msbc_with_codec(&hfp_codec, sbc_encoder_instance, &sbc_encoder_context);
462 }
463
sco_demo_msbc_receive(const uint8_t * packet,uint16_t size)464 static void sco_demo_msbc_receive(const uint8_t * packet, uint16_t size){
465 sbc_decoder_instance->decode_signed_16(&sbc_decoder_context, (packet[1] >> 4) & 3, packet + 3, size - 3);
466 }
467
sco_demo_msbc_close(void)468 static void sco_demo_msbc_close(void){
469 printf("Used mSBC with PLC, number of processed frames: \n - %d good frames, \n - %d zero frames, \n - %d bad frames.\n", sbc_decoder_context.good_frames_nr, sbc_decoder_context.zero_frames_nr, sbc_decoder_context.bad_frames_nr);
470 }
471
472 static const codec_support_t codec_msbc = {
473 .init = &sco_demo_msbc_init,
474 .receive = &sco_demo_msbc_receive,
475 .fill_payload = &sco_demo_codec_fill_payload,
476 .close = &sco_demo_msbc_close,
477 .sample_rate = SAMPLE_RATE_16KHZ
478 };
479
480 #endif /* ENABLE_HFP_WIDE_BAND_SPEECH */
481
482 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
483
484 #define LC3_SWB_SAMPLES_PER_FRAME 240
485 #define LC3_SWB_OCTETS_PER_FRAME 58
486
sco_demo_lc3swb_frame_callback(bool bad_frame,const uint8_t * frame_data,uint16_t frame_len)487 static bool sco_demo_lc3swb_frame_callback(bool bad_frame, const uint8_t * frame_data, uint16_t frame_len){
488
489 // skip H2 header for good frames
490 if (bad_frame == false){
491 btstack_assert(frame_data != NULL);
492 frame_data += 2;
493 }
494
495 uint8_t tmp_BEC_detect = 0;
496 uint8_t BFI = bad_frame ? 1 : 0;
497 int16_t samples[LC3_SWB_SAMPLES_PER_FRAME];
498 (void) lc3_decoder->decode_signed_16(&lc3_decoder_context, frame_data, BFI,
499 samples, 1, &tmp_BEC_detect);
500
501 // samples in callback in host endianess, ready for playback
502 btstack_ring_buffer_write(&audio_output_ring_buffer, (uint8_t *)samples, LC3_SWB_SAMPLES_PER_FRAME*2);
503
504 #ifdef SCO_WAV_FILENAME
505 if (num_samples_to_write > 0){
506 uint16_t num_samples = btstack_min(LC3_SWB_SAMPLES_PER_FRAME, num_samples_to_write);
507 num_samples_to_write -= num_samples;
508 wav_writer_write_int16(num_samples, samples);
509 if (num_samples_to_write == 0){
510 wav_writer_close();
511 }
512 }
513 #endif /* SCO_WAV_FILENAME */
514
515 // frame is good, if it isn't a bad frame and we didn't detect other errors
516 return (bad_frame == false) && (tmp_BEC_detect == 0);
517 }
518
sco_demo_lc3swb_init(void)519 static void sco_demo_lc3swb_init(void){
520
521 printf("SCO Demo: Init LC3-SWB\n");
522
523 hfp_codec.lc3_encoder_context = &lc3_encoder_context;
524 const btstack_lc3_encoder_t * lc3_encoder = btstack_lc3_encoder_google_init_instance( &lc3_encoder_context);
525 hfp_codec_init_lc3_swb(&hfp_codec, lc3_encoder, &lc3_encoder_context);
526
527 // init lc3 decoder
528 lc3_decoder = btstack_lc3_decoder_google_init_instance(&lc3_decoder_context);
529 lc3_decoder->configure(&lc3_decoder_context, SAMPLE_RATE_32KHZ, BTSTACK_LC3_FRAME_DURATION_7500US, LC3_SWB_OCTETS_PER_FRAME);
530
531 // init HPF H2 framing
532 hfp_h2_sync_init(&hfp_h2_sync, &sco_demo_lc3swb_frame_callback);
533 }
534
sco_demo_lc3swb_receive(const uint8_t * packet,uint16_t size)535 static void sco_demo_lc3swb_receive(const uint8_t * packet, uint16_t size){
536 uint8_t packet_status = (packet[1] >> 4) & 3;
537 bool bad_frame = packet_status != 0;
538 hfp_h2_sync_process(&hfp_h2_sync, bad_frame, &packet[3], size-3);
539 }
540
sco_demo_lc3swb_close(void)541 static void sco_demo_lc3swb_close(void){
542 // TODO: report
543 }
544
545 static const codec_support_t codec_lc3swb = {
546 .init = &sco_demo_lc3swb_init,
547 .receive = &sco_demo_lc3swb_receive,
548 .fill_payload = &sco_demo_codec_fill_payload,
549 .close = &sco_demo_lc3swb_close,
550 .sample_rate = SAMPLE_RATE_32KHZ
551 };
552 #endif
553
sco_demo_init(void)554 void sco_demo_init(void){
555
556 #ifdef ENABLE_CLASSIC_LEGACY_CONNECTIONS_FOR_SCO_DEMOS
557 printf("Disable BR/EDR Secure Connctions due to incompatibilities with SCO connections\n");
558 gap_secure_connections_enable(false);
559 #endif
560
561 // Set SCO for CVSD (mSBC or other codecs automatically use 8-bit transparent mode)
562 hci_set_sco_voice_setting(0x60); // linear, unsigned, 16-bit, CVSD
563
564 // status
565 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MICROPHONE
566 printf("SCO Demo: Sending and receiving audio via btstack_audio.\n");
567 #endif
568 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
569 printf("SCO Demo: Sending sine wave, audio output via btstack_audio.\n");
570 #endif
571 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER
572 printf("SCO Demo: Sending modplayer wave, audio output via btstack_audio.\n");
573 // init mod
574 int hxcmod_initialized = hxcmod_init(&mod_context);
575 btstack_assert(hxcmod_initialized != 0);
576 #endif
577 }
578
sco_demo_set_codec(uint8_t negotiated_codec)579 void sco_demo_set_codec(uint8_t negotiated_codec){
580 switch (negotiated_codec){
581 case HFP_CODEC_CVSD:
582 codec_current = &codec_cvsd;
583 break;
584 #ifdef ENABLE_HFP_WIDE_BAND_SPEECH
585 case HFP_CODEC_MSBC:
586 codec_current = &codec_msbc;
587 break;
588 #endif
589 #ifdef ENABLE_HFP_SUPER_WIDE_BAND_SPEECH
590 case HFP_CODEC_LC3_SWB:
591 codec_current = &codec_lc3swb;
592 break;
593 #endif
594 default:
595 btstack_assert(false);
596 break;
597 }
598
599 codec_current->init();
600
601 audio_initialize(codec_current->sample_rate);
602
603 audio_prebuffer_bytes = SCO_PREBUFFER_MS * (codec_current->sample_rate/1000) * BYTES_PER_FRAME;
604
605 #ifdef SCO_WAV_FILENAME
606 num_samples_to_write = codec_current->sample_rate * SCO_WAV_DURATION_IN_SECONDS;
607 wav_writer_open(SCO_WAV_FILENAME, 1, codec_current->sample_rate);
608 #endif
609
610 #if SCO_DEMO_MODE == SCO_DEMO_MODE_SINE
611 sine_wave_steps_per_sample = SINE_WAVE_SAMPLE_RATE / codec_current->sample_rate;
612 sco_demo_audio_generator = &sco_demo_sine_wave_host_endian;
613 #endif
614
615 #if SCO_DEMO_MODE == SCO_DEMO_MODE_MODPLAYER
616 // load mod
617 hxcmod_setcfg(&mod_context, codec_current->sample_rate, 16, 1, 1, 1);
618 hxcmod_load(&mod_context, (void *) &mod_data, mod_len);
619 sco_demo_audio_generator = &sco_demo_modplayer;
620 #endif
621 }
622
sco_demo_receive(uint8_t * packet,uint16_t size)623 void sco_demo_receive(uint8_t * packet, uint16_t size){
624 static uint32_t packets = 0;
625 static uint32_t crc_errors = 0;
626 static uint32_t data_received = 0;
627 static uint32_t byte_errors = 0;
628
629 count_received++;
630
631 data_received += size - 3;
632 packets++;
633 if (data_received > 100000){
634 printf("Summary: data %07u, packets %04u, packet with crc errors %0u, byte errors %04u\n", (unsigned int) data_received, (unsigned int) packets, (unsigned int) crc_errors, (unsigned int) byte_errors);
635 crc_errors = 0;
636 byte_errors = 0;
637 data_received = 0;
638 packets = 0;
639 }
640
641 codec_current->receive(packet, size);
642 }
643
sco_demo_send(hci_con_handle_t sco_handle)644 void sco_demo_send(hci_con_handle_t sco_handle){
645
646 if (sco_handle == HCI_CON_HANDLE_INVALID) return;
647
648 int sco_packet_length = hci_get_sco_packet_length_for_connection(sco_handle);
649 int sco_payload_length = sco_packet_length - 3;
650
651 hci_reserve_packet_buffer();
652 uint8_t * sco_packet = hci_get_outgoing_packet_buffer();
653
654 #ifdef USE_ADUIO_GENERATOR
655 #define REFILL_SAMPLES 16
656 // re-fill audio buffer
657 uint16_t samples_free = btstack_ring_buffer_bytes_free(&audio_input_ring_buffer) / 2;
658 while (samples_free > 0){
659 int16_t samples_buffer[REFILL_SAMPLES];
660 uint16_t samples_to_add = btstack_min(samples_free, REFILL_SAMPLES);
661 (*sco_demo_audio_generator)(samples_to_add, samples_buffer);
662 btstack_ring_buffer_write(&audio_input_ring_buffer, (uint8_t *)samples_buffer, samples_to_add * 2);
663 samples_free -= samples_to_add;
664 }
665 #endif
666
667 // resume if pre-buffer is filled
668 if (audio_input_paused){
669 if (btstack_ring_buffer_bytes_available(&audio_input_ring_buffer) >= audio_prebuffer_bytes){
670 // resume sending
671 audio_input_paused = 0;
672 }
673 }
674
675 // fill payload by codec
676 codec_current->fill_payload(&sco_packet[3], sco_payload_length);
677
678 // set handle + flags
679 little_endian_store_16(sco_packet, 0, sco_handle);
680 // set len
681 sco_packet[2] = sco_payload_length;
682 // finally send packet
683 hci_send_sco_packet_buffer(sco_packet_length);
684
685 // request another send event
686 hci_request_sco_can_send_now_event();
687
688 count_sent++;
689 if ((count_sent % SCO_REPORT_PERIOD) == 0) {
690 printf("SCO: sent %u, received %u\n", count_sent, count_received);
691 }
692 }
693
sco_demo_close(void)694 void sco_demo_close(void){
695 printf("SCO demo close\n");
696
697 printf("SCO demo statistics: ");
698 codec_current->close();
699 codec_current = NULL;
700
701 #if defined(SCO_WAV_FILENAME)
702 wav_writer_close();
703 #endif
704
705 audio_terminate();
706 }
707